5.0.0

* Pass goal handle to goal response callback instead of a future

This resolves an issue where `promise->set_value` is called before a potential call to `promise->set_exception`.
If there is an issue sending a result request, set the exception on the future to the result in the goal handle, instead of the future to the goal handle itself.

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* Do not remove goal handle from list if result request fails

This way the user can still interact with the goal (e.g. send a cancel request).

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* Set the result awareness to false if goal handle is invalidated

This will cause an exception when trying to get the future to result, in addition to the exception when trying to access values for existing references to the future.

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* Revert "Set the result awareness to false if goal handle is invalidated"

This reverts commit d444e09131c42d6eece1338443b8ffb4f5f17370.

* Throw from Client::async_get_result if the goal handle was invalidated due to a failed result request

Propagate error message from a failed result request.

Also set result awareness to false if the result request fails so the user can also check before
being hit with an exception.

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* Guard against mutliple calls to invalidate

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

rclcpp/CHANGELOG.rst

@@ -2,6 +2,70 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+5.0.0 (2020-09-18)
+------------------
+* Make node_graph::count_graph_users() const (`#1320 <https://github.com/ros2/rclcpp/issues/1320>`_)
+* Add coverage for wait_set_policies (`#1316 <https://github.com/ros2/rclcpp/issues/1316>`_)
+* Only exchange intra_process waitable if nonnull (`#1317 <https://github.com/ros2/rclcpp/issues/1317>`_)
+* Check waitable for nullptr during constructor (`#1315 <https://github.com/ros2/rclcpp/issues/1315>`_)
+* Call vector.erase with end iterator overload (`#1314 <https://github.com/ros2/rclcpp/issues/1314>`_)
+* Use best effort, keep last, history depth 1 QoS Profile for '/clock' subscriptions (`#1312 <https://github.com/ros2/rclcpp/issues/1312>`_)
+* Add tests type_support module (`#1308 <https://github.com/ros2/rclcpp/issues/1308>`_)
+* Replace std_msgs with test_msgs in executors test (`#1310 <https://github.com/ros2/rclcpp/issues/1310>`_)
+* Add set_level for rclcpp::Logger (`#1284 <https://github.com/ros2/rclcpp/issues/1284>`_)
+* Remove unused private function (rclcpp::Node and rclcpp_lifecycle::Node) (`#1294 <https://github.com/ros2/rclcpp/issues/1294>`_)
+* Adding tests basic getters (`#1291 <https://github.com/ros2/rclcpp/issues/1291>`_)
+* Adding callback groups in executor (`#1218 <https://github.com/ros2/rclcpp/issues/1218>`_)
+* Refactor Subscription Topic Statistics Tests (`#1281 <https://github.com/ros2/rclcpp/issues/1281>`_)
+* Add operator!= for duration (`#1236 <https://github.com/ros2/rclcpp/issues/1236>`_)
+* Fix clock thread issue (`#1266 <https://github.com/ros2/rclcpp/issues/1266>`_) (`#1267 <https://github.com/ros2/rclcpp/issues/1267>`_)
+* Fix topic stats test, wait for more messages, only check the ones with samples (`#1274 <https://github.com/ros2/rclcpp/issues/1274>`_)
+* Add get_domain_id method to rclcpp::Context (`#1271 <https://github.com/ros2/rclcpp/issues/1271>`_)
+* Fixes for unit tests that fail under cyclonedds (`#1270 <https://github.com/ros2/rclcpp/issues/1270>`_)
+* initialize_logging\_ should be copied (`#1272 <https://github.com/ros2/rclcpp/issues/1272>`_)
+* Use static_cast instead of C-style cast for instrumentation (`#1263 <https://github.com/ros2/rclcpp/issues/1263>`_)
+* Make parameter clients use template constructors (`#1249 <https://github.com/ros2/rclcpp/issues/1249>`_)
+* Ability to configure domain_id via InitOptions. (`#1165 <https://github.com/ros2/rclcpp/issues/1165>`_)
+* Simplify and fix allocator memory strategy unit test for connext (`#1252 <https://github.com/ros2/rclcpp/issues/1252>`_)
+* Use global namespace for parameter events subscription topic (`#1257 <https://github.com/ros2/rclcpp/issues/1257>`_)
+* Increase timeouts for connext for long tests (`#1253 <https://github.com/ros2/rclcpp/issues/1253>`_)
+* Adjust test_static_executor_entities_collector for rmw_connext_cpp (`#1251 <https://github.com/ros2/rclcpp/issues/1251>`_)
+* Fix failing test with Connext since it doesn't wait for discovery (`#1246 <https://github.com/ros2/rclcpp/issues/1246>`_)
+* Fix node graph test with Connext and CycloneDDS returning actual data (`#1245 <https://github.com/ros2/rclcpp/issues/1245>`_)
+* Warn about unused result of add_on_set_parameters_callback (`#1238 <https://github.com/ros2/rclcpp/issues/1238>`_)
+* Unittests for memory strategy files, except allocator_memory_strategy (`#1189 <https://github.com/ros2/rclcpp/issues/1189>`_)
+* EXPECT_THROW_EQ and ASSERT_THROW_EQ macros for unittests (`#1232 <https://github.com/ros2/rclcpp/issues/1232>`_)
+* Add unit test for static_executor_entities_collector (`#1221 <https://github.com/ros2/rclcpp/issues/1221>`_)
+* Parameterize test executors for all executor types (`#1222 <https://github.com/ros2/rclcpp/issues/1222>`_)
+* Unit tests for allocator_memory_strategy.cpp part 2 (`#1198 <https://github.com/ros2/rclcpp/issues/1198>`_)
+* Unit tests for allocator_memory_strategy.hpp (`#1197 <https://github.com/ros2/rclcpp/issues/1197>`_)
+* Derive and throw exception in spin_some spin_all for StaticSingleThreadedExecutor (`#1220 <https://github.com/ros2/rclcpp/issues/1220>`_)
+* Make ring buffer thread-safe (`#1213 <https://github.com/ros2/rclcpp/issues/1213>`_)
+* Add missing RCLCPP_PUBLIC to ~StaticExecutorEntitiesCollector (`#1227 <https://github.com/ros2/rclcpp/issues/1227>`_)
+* Document graph functions don't apply remap rules (`#1225 <https://github.com/ros2/rclcpp/issues/1225>`_)
+* Remove recreation of entities_collector (`#1217 <https://github.com/ros2/rclcpp/issues/1217>`_)
+* Contributors: Audrow Nash, Chen Lihui, Christophe Bedard, Daisuke Sato, Devin Bonnie, Dirk Thomas, Ivan Santiago Paunovic, Jacob Perron, Jannik Abbenseth, Jorge Perez, Pedro Pena, Shane Loretz, Stephen Brawner, Tomoya Fujita
+
+4.0.0 (2020-07-09)
+------------------
+* Fix rclcpp::NodeOptions::operator= (`#1211 <https://github.com/ros2/rclcpp/issues/1211>`_)
+* Link against thread library where necessary (`#1210 <https://github.com/ros2/rclcpp/issues/1210>`_)
+* Unit tests for node interfaces (`#1202 <https://github.com/ros2/rclcpp/issues/1202>`_)
+* Remove usage of domain id in node options (`#1205 <https://github.com/ros2/rclcpp/issues/1205>`_)
+* Remove deprecated set_on_parameters_set_callback function (`#1199 <https://github.com/ros2/rclcpp/issues/1199>`_)
+* Fix conversion of negative durations to messages (`#1188 <https://github.com/ros2/rclcpp/issues/1188>`_)
+* Fix implementation of NodeOptions::use_global_arguments() (`#1176 <https://github.com/ros2/rclcpp/issues/1176>`_)
+* Bump to QD to level 3 and fixed links (`#1158 <https://github.com/ros2/rclcpp/issues/1158>`_)
+* Fix pub/sub count API tests (`#1203 <https://github.com/ros2/rclcpp/issues/1203>`_)
+* Update tracetools' QL to 2 in rclcpp's QD (`#1187 <https://github.com/ros2/rclcpp/issues/1187>`_)
+* Fix exception message on rcl_clock_init (`#1182 <https://github.com/ros2/rclcpp/issues/1182>`_)
+* Throw exception if rcl_timer_init fails (`#1179 <https://github.com/ros2/rclcpp/issues/1179>`_)
+* Unit tests for some header-only functions/classes (`#1181 <https://github.com/ros2/rclcpp/issues/1181>`_)
+* Callback should be perfectly-forwarded (`#1183 <https://github.com/ros2/rclcpp/issues/1183>`_)
+* Add unit tests for logging functionality (`#1184 <https://github.com/ros2/rclcpp/issues/1184>`_)
+* Add create_publisher include to create_subscription (`#1180 <https://github.com/ros2/rclcpp/issues/1180>`_)
+* Contributors: Alejandro Hernández Cordero, Christophe Bedard, Claire Wang, Dirk Thomas, Ivan Santiago Paunovic, Johannes Meyer, Michel Hidalgo, Stephen Brawner, tomoya
+
 3.0.0 (2020-06-18)
 ------------------
 * Check period duration in create_wall_timer (`#1178 <https://github.com/ros2/rclcpp/issues/1178>`_)

rclcpp/include/rclcpp/any_service_callback.hpp

@@ -88,7 +88,7 @@ public:
     std::shared_ptr<typename ServiceT::Request> request,
     std::shared_ptr<typename ServiceT::Response> response)
   {
-    TRACEPOINT(callback_start, (const void *)this, false);
+    TRACEPOINT(callback_start, static_cast<const void *>(this), false);
     if (shared_ptr_callback_ != nullptr) {
       (void)request_header;
       shared_ptr_callback_(request, response);
@@ -97,7 +97,7 @@ public:
     } else {
       throw std::runtime_error("unexpected request without any callback set");
     }
-    TRACEPOINT(callback_end, (const void *)this);
+    TRACEPOINT(callback_end, static_cast<const void *>(this));
   }
 
   void register_callback_for_tracing()
@@ -106,12 +106,12 @@ public:
     if (shared_ptr_callback_) {
       TRACEPOINT(
         rclcpp_callback_register,
-        (const void *)this,
+        static_cast<const void *>(this),
         get_symbol(shared_ptr_callback_));
     } else if (shared_ptr_with_request_header_callback_) {
       TRACEPOINT(
         rclcpp_callback_register,
-        (const void *)this,
+        static_cast<const void *>(this),
         get_symbol(shared_ptr_with_request_header_callback_));
     }
 #endif  // TRACETOOLS_DISABLED

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -158,7 +158,7 @@ public:
   void dispatch(
     std::shared_ptr<MessageT> message, const rclcpp::MessageInfo & message_info)
   {
-    TRACEPOINT(callback_start, (const void *)this, false);
+    TRACEPOINT(callback_start, static_cast<const void *>(this), false);
     if (shared_ptr_callback_) {
       shared_ptr_callback_(message);
     } else if (shared_ptr_with_info_callback_) {
@@ -178,13 +178,13 @@ public:
     } else {
       throw std::runtime_error("unexpected message without any callback set");
     }
-    TRACEPOINT(callback_end, (const void *)this);
+    TRACEPOINT(callback_end, static_cast<const void *>(this));
   }
 
   void dispatch_intra_process(
     ConstMessageSharedPtr message, const rclcpp::MessageInfo & message_info)
   {
-    TRACEPOINT(callback_start, (const void *)this, true);
+    TRACEPOINT(callback_start, static_cast<const void *>(this), true);
     if (const_shared_ptr_callback_) {
       const_shared_ptr_callback_(message);
     } else if (const_shared_ptr_with_info_callback_) {
@@ -201,13 +201,13 @@ public:
         throw std::runtime_error("unexpected message without any callback set");
       }
     }
-    TRACEPOINT(callback_end, (const void *)this);
+    TRACEPOINT(callback_end, static_cast<const void *>(this));
   }
 
   void dispatch_intra_process(
     MessageUniquePtr message, const rclcpp::MessageInfo & message_info)
   {
-    TRACEPOINT(callback_start, (const void *)this, true);
+    TRACEPOINT(callback_start, static_cast<const void *>(this), true);
     if (shared_ptr_callback_) {
       typename std::shared_ptr<MessageT> shared_message = std::move(message);
       shared_ptr_callback_(shared_message);
@@ -225,7 +225,7 @@ public:
     } else {
       throw std::runtime_error("unexpected message without any callback set");
     }
-    TRACEPOINT(callback_end, (const void *)this);
+    TRACEPOINT(callback_end, static_cast<const void *>(this));
   }
 
   bool use_take_shared_method() const
@@ -239,22 +239,22 @@ public:
     if (shared_ptr_callback_) {
       TRACEPOINT(
         rclcpp_callback_register,
-        (const void *)this,
+        static_cast<const void *>(this),
         get_symbol(shared_ptr_callback_));
     } else if (shared_ptr_with_info_callback_) {
       TRACEPOINT(
         rclcpp_callback_register,
-        (const void *)this,
+        static_cast<const void *>(this),
         get_symbol(shared_ptr_with_info_callback_));
     } else if (unique_ptr_callback_) {
       TRACEPOINT(
         rclcpp_callback_register,
-        (const void *)this,
+        static_cast<const void *>(this),
         get_symbol(unique_ptr_callback_));
     } else if (unique_ptr_with_info_callback_) {
       TRACEPOINT(
         rclcpp_callback_register,
-        (const void *)this,
+        static_cast<const void *>(this),
         get_symbol(unique_ptr_with_info_callback_));
     }
 #endif  // TRACETOOLS_DISABLED

rclcpp/include/rclcpp/callback_group.hpp

@@ -56,8 +56,44 @@ class CallbackGroup
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup)
 
+  /// Constructor for CallbackGroup.
+  /**
+   * Callback Groups have a type, either 'Mutually Exclusive' or 'Reentrant'
+   * and when creating one the type must be specified.
+   *
+   * Callbacks in Reentrant Callback Groups must be able to:
+   *   - run at the same time as themselves (reentrant)
+   *   - run at the same time as other callbacks in their group
+   *   - run at the same time as other callbacks in other groups
+   *
+   * Callbacks in Mutually Exclusive Callback Groups:
+   *   - will not be run multiple times simultaneously (non-reentrant)
+   *   - will not be run at the same time as other callbacks in their group
+   *   - but must run at the same time as callbacks in other groups
+   *
+   * Additiionally, callback groups have a property which determines whether or
+   * not they are added to an executor with their associated node automatically.
+   * When creating a callback group the automatically_add_to_executor_with_node
+   * argument determines this behavior, and if true it will cause the newly
+   * created callback group to be added to an executor with the node when the
+   * Executor::add_node method is used.
+   * If false, this callback group will not be added automatically and would
+   * have to be added to an executor manually using the
+   * Executor::add_callback_group method.
+   *
+   * Whether the node was added to the executor before creating the callback
+   * group, or after, is irrelevant; the callback group will be automatically
+   * added to the executor in either case.
+   *
+   * \param[in] group_type They type of the callback group.
+   * \param[in] automatically_add_to_executor_with_node a boolean which
+   *   determines whether a callback group is automatically added to an executor
+   *   with the node with which it is associated.
+   */
   RCLCPP_PUBLIC
-  explicit CallbackGroup(CallbackGroupType group_type);
+  explicit CallbackGroup(
+    CallbackGroupType group_type,
+    bool automatically_add_to_executor_with_node = true);
 
   template<typename Function>
   rclcpp::SubscriptionBase::SharedPtr
@@ -102,6 +138,31 @@ public:
   const CallbackGroupType &
   type() const;
 
+  /// Return a reference to the 'associated with executor' atomic boolean.
+  /**
+   * When a callback group is added to an executor this boolean is checked
+   * to ensure it has not already been added to another executor.
+   * If it has not been, then this boolean is set to true to indicate it is
+   * now associated with an executor.
+   *
+   * When the callback group is removed from the executor, this atomic boolean
+   * is set back to false.
+   *
+   * \return the 'associated with executor' atomic boolean
+   */
+  RCLCPP_PUBLIC
+  std::atomic_bool &
+  get_associated_with_executor_atomic();
+
+  /// Return true if this callback group should be automatically added to an executor by the node.
+  /**
+   * \return boolean true if this callback group should be automatically added
+   *   to an executor when the associated node is added, otherwise false.
+   */
+  RCLCPP_PUBLIC
+  bool
+  automatically_add_to_executor_with_node() const;
+
 protected:
   RCLCPP_DISABLE_COPY(CallbackGroup)
 
@@ -136,12 +197,14 @@ protected:
   CallbackGroupType type_;
   // Mutex to protect the subsequent vectors of pointers.
   mutable std::mutex mutex_;
+  std::atomic_bool associated_with_executor_;
   std::vector<rclcpp::SubscriptionBase::WeakPtr> subscription_ptrs_;
   std::vector<rclcpp::TimerBase::WeakPtr> timer_ptrs_;
   std::vector<rclcpp::ServiceBase::WeakPtr> service_ptrs_;
   std::vector<rclcpp::ClientBase::WeakPtr> client_ptrs_;
   std::vector<rclcpp::Waitable::WeakPtr> waitable_ptrs_;
   std::atomic_bool can_be_taken_from_;
+  const bool automatically_add_to_executor_with_node_;
 
 private:
   template<typename TypeT, typename Function>

rclcpp/include/rclcpp/context.hpp

@@ -139,6 +139,11 @@ public:
   rclcpp::InitOptions
   get_init_options();
 
+  /// Return actual domain id.
+  RCLCPP_PUBLIC
+  size_t
+  get_domain_id() const;
+
   /// Return the shutdown reason, or empty string if not shutdown.
   /**
    * This function is thread-safe.

rclcpp/include/rclcpp/duration.hpp

@@ -77,6 +77,9 @@ public:
   bool
   operator==(const rclcpp::Duration & rhs) const;
 
+  bool
+  operator!=(const rclcpp::Duration & rhs) const;
+
   bool
   operator<(const rclcpp::Duration & rhs) const;
 

rclcpp/include/rclcpp/exceptions/exceptions.hpp

@@ -100,6 +100,15 @@ public:
   {}
 };
 
+class UnimplementedError : public std::runtime_error
+{
+public:
+  UnimplementedError()
+  : std::runtime_error("This code is unimplemented.") {}
+  explicit UnimplementedError(const std::string & msg)
+  : std::runtime_error(msg) {}
+};
+
 /// Throw a C++ std::exception which was created based on an rcl error.
 /**
  * Passing nullptr for reset_error is safe and will avoid calling any function

rclcpp/include/rclcpp/executor.hpp

@@ -21,6 +21,7 @@
 #include <cstdlib>
 #include <iostream>
 #include <list>
+#include <map>
 #include <memory>
 #include <mutex>
 #include <string>
@@ -42,6 +43,10 @@
 namespace rclcpp
 {
 
+typedef std::map<rclcpp::CallbackGroup::WeakPtr,
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+    std::owner_less<rclcpp::CallbackGroup::WeakPtr>> WeakCallbackGroupsToNodesMap;
+
 // Forward declaration is used in convenience method signature.
 class Node;
 
@@ -76,13 +81,118 @@ public:
   virtual void
   spin() = 0;
 
+  /// Add a callback group to an executor.
+  /**
+   * An executor can have zero or more callback groups which provide work during `spin` functions.
+   * When an executor attempts to add a callback group, the executor checks to see if it is already
+   * associated with another executor, and if it has been, then an exception is thrown.
+   * Otherwise, the callback group is added to the executor.
+   *
+   * Adding a callback group with this method does not associate its node with this executor
+   * in any way
+   *
+   * \param[in] group_ptr a shared ptr that points to a callback group
+   * \param[in] node_ptr a shared pointer that points to a node base interface
+   * \param[in] notify True to trigger the interrupt guard condition during this function. If
+   * the executor is blocked at the rmw layer while waiting for work and it is notified that a new
+   * callback group was added, it will wake up.
+   * \throw std::runtime_error if the callback group is associated to an executor
+   */
+  RCLCPP_PUBLIC
+  virtual void
+  add_callback_group(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    bool notify = true);
+
+  /// Get callback groups that belong to executor.
+  /**
+   * This function returns a vector of weak pointers that point to callback groups that were
+   * associated with the executor.
+   * The callback groups associated with this executor may have been added with
+   * `add_callback_group`, or added when a node was added to the executor with `add_node`, or
+   * automatically added when it created by a node already associated with this executor and the
+   * automatically_add_to_executor_with_node parameter was true.
+   *
+   * \return a vector of weak pointers that point to callback groups that are associated with
+   * the executor
+   */
+  RCLCPP_PUBLIC
+  virtual std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_all_callback_groups();
+
+  /// Get callback groups that belong to executor.
+  /**
+   * This function returns a vector of weak pointers that point to callback groups that were
+   * associated with the executor.
+   * The callback groups associated with this executor have been added with
+   * `add_callback_group`.
+   *
+   * \return a vector of weak pointers that point to callback groups that are associated with
+   * the executor
+   */
+  RCLCPP_PUBLIC
+  virtual std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_manually_added_callback_groups();
+
+  /// Get callback groups that belong to executor.
+  /**
+   * This function returns a vector of weak pointers that point to callback groups that were
+   * added from a node that is associated with the executor.
+   * The callback groups are added when a node is added to the executor with `add_node`, or
+   * automatically if they are created in the future by that node and have the
+   * automatically_add_to_executor_with_node argument set to true.
+   *
+   * \return a vector of weak pointers that point to callback groups from a node associated with
+   * the executor
+   */
+  RCLCPP_PUBLIC
+  virtual std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_automatically_added_callback_groups_from_nodes();
+
+  /// Remove a callback group from the executor.
+  /**
+   * The callback group is removed from and disassociated with the executor.
+   * If the callback group removed was the last callback group from the node
+   * that is associated with the executor, the interrupt guard condition
+   * is triggered and node's guard condition is removed from the executor.
+   *
+   * This function only removes a callback group that was manually added with
+   * rclcpp::Executor::add_callback_group.
+   * To remove callback groups that were added from a node using
+   * rclcpp::Executor::add_node, use rclcpp::Executor::remove_node instead.
+   *
+   * \param[in] group_ptr Shared pointer to the callback group to be added.
+   * \param[in] notify True to trigger the interrupt guard condition during this function. If
+   * the executor is blocked at the rmw layer while waiting for work and it is notified that a
+   * callback group was removed, it will wake up.
+   * \throw std::runtime_error if node is deleted before callback group
+   * \throw std::runtime_error if the callback group is not associated with the executor
+   */
+  RCLCPP_PUBLIC
+  virtual void
+  remove_callback_group(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    bool notify = true);
+
   /// Add a node to the executor.
   /**
-   * An executor can have zero or more nodes which provide work during `spin` functions.
+   * Nodes have associated callback groups, and this method adds any of those callback groups
+   * to this executor which have their automatically_add_to_executor_with_node parameter true.
+   * The node is also associated with the executor so that future callback groups which are
+   * created on the node with the automatically_add_to_executor_with_node parameter set to true
+   * are also automatically associated with this executor.
+   *
+   * Callback groups with the automatically_add_to_executor_with_node parameter set to false must
+   * be manually added to an executor using the rclcpp::Executor::add_callback_group method.
+   *
+   * If a node is already associated with an executor, this method throws an exception.
+   *
    * \param[in] node_ptr Shared pointer to the node to be added.
    * \param[in] notify True to trigger the interrupt guard condition during this function. If
    * the executor is blocked at the rmw layer while waiting for work and it is notified that a new
    * node was added, it will wake up.
+   * \throw std::runtime_error if a node is already associated to an executor
    */
   RCLCPP_PUBLIC
   virtual void
@@ -98,10 +208,19 @@ public:
 
   /// Remove a node from the executor.
   /**
+   * Any callback groups automatically added when this node was added with
+   * rclcpp::Executor::add_node are automatically removed, and the node is no longer associated
+   * with this executor.
+   *
+   * This also means that future callback groups created by the given node are no longer
+   * automatically added to this executor.
+   *
    * \param[in] node_ptr Shared pointer to the node to remove.
    * \param[in] notify True to trigger the interrupt guard condition and wake up the executor.
    * This is useful if the last node was removed from the executor while the executor was blocked
    * waiting for work in another thread, because otherwise the executor would never be notified.
+   * \throw std::runtime_error if the node is not associated with an executor.
+   * \throw std::runtime_error if the node is not associated with this executor.
    */
   RCLCPP_PUBLIC
   virtual void
@@ -327,22 +446,79 @@ protected:
 
   RCLCPP_PUBLIC
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
-  get_node_by_group(rclcpp::CallbackGroup::SharedPtr group);
+  get_node_by_group(
+    WeakCallbackGroupsToNodesMap weak_groups_to_nodes,
+    rclcpp::CallbackGroup::SharedPtr group);
+
+  /// Return true if the node has been added to this executor.
+  /**
+   * \param[in] node_ptr a shared pointer that points to a node base interface
+   * \return true if the node is associated with the executor, otherwise false
+   */
+  RCLCPP_PUBLIC
+  bool
+  has_node(
+    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    WeakCallbackGroupsToNodesMap weak_groups_to_nodes) const;
 
   RCLCPP_PUBLIC
   rclcpp::CallbackGroup::SharedPtr
   get_group_by_timer(rclcpp::TimerBase::SharedPtr timer);
 
+  /// Add a callback group to an executor
+  /**
+   * \see rclcpp::Executor::add_callback_group
+   */
+  RCLCPP_PUBLIC
+  virtual void
+  add_callback_group_to_map(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
+    bool notify = true);
+
+  /// Remove a callback group from the executor.
+  /**
+   * \see rclcpp::Executor::remove_callback_group
+   */
+  RCLCPP_PUBLIC
+  virtual void
+  remove_callback_group_from_map(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
+    bool notify = true);
+
   RCLCPP_PUBLIC
   bool
   get_next_ready_executable(AnyExecutable & any_executable);
 
+  RCLCPP_PUBLIC
+  bool
+  get_next_ready_executable_from_map(
+    AnyExecutable & any_executable,
+    WeakCallbackGroupsToNodesMap weak_groups_to_nodes);
+
   RCLCPP_PUBLIC
   bool
   get_next_executable(
     AnyExecutable & any_executable,
     std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
+  /// Add all callback groups that can be automatically added from associated nodes.
+  /**
+   * The executor, before collecting entities, verifies if any callback group from
+   * nodes associated with the executor, which is not already associated to an executor,
+   * can be automatically added to this executor.
+   * This takes care of any callback group that has been added to a node but not explicitly added
+   * to the executor.
+   * It is important to note that in order for the callback groups to be automatically added to an
+   * executor through this function, the node of the callback groups needs to have been added
+   * through the `add_node` method.
+   */
+  RCLCPP_PUBLIC
+  virtual void
+  add_callback_groups_from_nodes_associated_to_executor();
+
   /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins.
   std::atomic_bool spinning;
 
@@ -367,8 +543,25 @@ protected:
   void
   spin_once_impl(std::chrono::nanoseconds timeout);
 
+  typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+      const rcl_guard_condition_t *,
+      std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
+    WeakNodesToGuardConditionsMap;
+
+  /// maps nodes to guard conditions
+  WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_;
+
+  /// maps callback groups associated to nodes
+  WeakCallbackGroupsToNodesMap weak_groups_associated_with_executor_to_nodes_;
+
+  /// maps callback groups to nodes associated with executor
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes_associated_with_executor_;
+
+  /// maps all callback groups to nodes
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes_;
+
+  /// nodes that are associated with the executor
   std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
-  std::list<const rcl_guard_condition_t *> guard_conditions_;
 };
 
 namespace executor

rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp

@@ -17,7 +17,9 @@
 
 #include <chrono>
 #include <list>
+#include <map>
 #include <memory>
+#include <vector>
 
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
@@ -32,6 +34,9 @@ namespace rclcpp
 {
 namespace executors
 {
+typedef std::map<rclcpp::CallbackGroup::WeakPtr,
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+    std::owner_less<rclcpp::CallbackGroup::WeakPtr>> WeakCallbackGroupsToNodesMap;
 
 class StaticExecutorEntitiesCollector final
   : public rclcpp::Waitable,
@@ -45,6 +50,7 @@ public:
   StaticExecutorEntitiesCollector() = default;
 
   // Destructor
+  RCLCPP_PUBLIC
   ~StaticExecutorEntitiesCollector();
 
   /// Initialize StaticExecutorEntitiesCollector
@@ -85,7 +91,7 @@ public:
   /**
    * block until the wait set is ready or until the timeout has been exceeded.
    * \throws std::runtime_error if wait set couldn't be cleared or filled.
-   * \throws any rcl errors from rcl_wait, \sa rclcpp::exceptions::throw_from_rcl_error()
+   * \throws any rcl errors from rcl_wait, \see rclcpp::exceptions::throw_from_rcl_error()
    */
   RCLCPP_PUBLIC
   void
@@ -102,17 +108,58 @@ public:
   size_t
   get_number_of_ready_guard_conditions() override;
 
+  /// Add a callback group to an executor.
   /**
-   * \sa rclcpp::Executor::add_node()
+   * \see rclcpp::Executor::add_callback_group
+   */
+  RCLCPP_PUBLIC
+  bool
+  add_callback_group(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+  /// Add a callback group to an executor.
+  /**
+   * \see rclcpp::Executor::add_callback_group
+   * \return boolean whether the node from the callback group is new
+   */
+  RCLCPP_PUBLIC
+  bool
+  add_callback_group(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
+
+  /// Remove a callback group from the executor.
+  /**
+   * \see rclcpp::Executor::remove_callback_group
+   */
+  RCLCPP_PUBLIC
+  bool
+  remove_callback_group(
+    rclcpp::CallbackGroup::SharedPtr group_ptr);
+
+  /// Remove a callback group from the executor.
+  /**
+   * \see rclcpp::Executor::remove_callback_group_from_map
+   */
+  RCLCPP_PUBLIC
+  bool
+  remove_callback_group_from_map(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
+
+  /**
+   * \see rclcpp::Executor::add_node()
    * \throw std::runtime_error if node was already added
    */
   RCLCPP_PUBLIC
-  void
+  bool
   add_node(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
 
   /**
-   * \sa rclcpp::Executor::remove_node()
+   * \see rclcpp::Executor::remove_node()
    * \throw std::runtime_error if no guard condition is associated with node.
    */
   RCLCPP_PUBLIC
@@ -120,6 +167,26 @@ public:
   remove_node(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
 
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_all_callback_groups();
+
+  /// Get callback groups that belong to executor.
+  /**
+   * \see rclcpp::Executor::get_manually_added_callback_groups()
+   */
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_manually_added_callback_groups();
+
+  /// Get callback groups that belong to executor.
+  /**
+   * \see rclcpp::Executor::get_automatically_added_callback_groups_from_nodes()
+   */
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_automatically_added_callback_groups_from_nodes();
+
   /// Complete all available queued work without blocking.
   /**
    * This function checks if after the guard condition was triggered
@@ -215,13 +282,45 @@ public:
   rclcpp::Waitable::SharedPtr
   get_waitable(size_t i) {return exec_list_.waitable[i];}
 
+  /// Return true if the node belongs to the collector
+  /**
+   * \param[in] group_ptr a node base interface shared pointer
+   * \return boolean whether a node belongs the collector
+   */
+  bool
+  has_node(
+    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    WeakCallbackGroupsToNodesMap weak_groups_to_nodes) const;
+
 private:
-  /// Nodes guard conditions which trigger this waitable
-  std::list<const rcl_guard_condition_t *> guard_conditions_;
+  /// Add all callback groups that can be automatically added by any executor
+  /// and is not already associated with an executor from nodes
+  /// that are associated with executor
+  /**
+   * \see rclcpp::Executor::add_callback_groups_from_nodes_associated_to_executor()
+   */
+  RCLCPP_PUBLIC
+  void
+  add_callback_groups_from_nodes_associated_to_executor();
+
+  RCLCPP_PUBLIC
+  void
+  fill_executable_list_from_map(WeakCallbackGroupsToNodesMap weak_groups_to_nodes);
 
   /// Memory strategy: an interface for handling user-defined memory allocation strategies.
   rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;
 
+  // maps callback groups to nodes.
+  WeakCallbackGroupsToNodesMap weak_groups_associated_with_executor_to_nodes_;
+  // maps callback groups to nodes.
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes_associated_with_executor_;
+
+  typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+      const rcl_guard_condition_t *,
+      std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
+    WeakNodesToGuardConditionsMap;
+  WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_;
+
   /// List of weak nodes registered in the static executor
   std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
 

rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp

@@ -78,15 +78,30 @@ public:
   void
   spin() override;
 
+  /// Add a callback group to an executor.
+  /**
+   * \sa rclcpp::Executor::add_callback_group
+   */
+  RCLCPP_PUBLIC
+  void
+  add_callback_group(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    bool notify = true) override;
+
+  /// Remove callback group from the executor
+  /**
+   * \sa rclcpp::Executor::remove_callback_group
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_callback_group(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    bool notify = true) override;
+
   /// Add a node to the executor.
   /**
-   * An executor can have zero or more nodes which provide work during `spin` functions.
-   * \param[in] node_ptr Shared pointer to the node to be added.
-   * \param[in] notify True to trigger the interrupt guard condition during this function. If
-   * the executor is blocked at the rmw layer while waiting for work and it is notified that a new
-   * node was added, it will wake up.
-   * \throw std::runtime_error if node was already added or if rcl_trigger_guard_condition
-   * return an error
+   * \sa rclcpp::Executor::add_node
    */
   RCLCPP_PUBLIC
   void
@@ -96,8 +111,7 @@ public:
 
   /// Convenience function which takes Node and forwards NodeBaseInterface.
   /**
-   * \throw std::runtime_error if node was already added or if rcl_trigger_guard_condition
-   * returns an error
+   * \sa rclcpp::StaticSingleThreadedExecutor::add_node
    */
   RCLCPP_PUBLIC
   void
@@ -105,11 +119,7 @@ public:
 
   /// Remove a node from the executor.
   /**
-   * \param[in] node_ptr Shared pointer to the node to remove.
-   * \param[in] notify True to trigger the interrupt guard condition and wake up the executor.
-   * This is useful if the last node was removed from the executor while the executor was blocked
-   * waiting for work in another thread, because otherwise the executor would never be notified.
-   * \throw std::runtime_error if rcl_trigger_guard_condition returns an error
+   * \sa rclcpp::Executor::remove_node
    */
   RCLCPP_PUBLIC
   void
@@ -119,12 +129,32 @@ public:
 
   /// Convenience function which takes Node and forwards NodeBaseInterface.
   /**
-   * \throw std::runtime_error if rcl_trigger_guard_condition returns an error
+   * \sa rclcpp::Executor::remove_node
    */
   RCLCPP_PUBLIC
   void
   remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
 
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_all_callback_groups() override;
+
+  /// Get callback groups that belong to executor.
+  /**
+   * \sa rclcpp::Executor::get_manually_added_callback_groups()
+   */
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_manually_added_callback_groups() override;
+
+  /// Get callback groups that belong to executor.
+  /**
+   * \sa rclcpp::Executor::get_automatically_added_callback_groups_from_nodes()
+   */
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_automatically_added_callback_groups_from_nodes() override;
+
   /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
   /**
    * \param[in] future The future to wait on. If this function returns SUCCESS, the future can be
@@ -162,7 +192,6 @@ public:
     }
     std::chrono::nanoseconds timeout_left = timeout_ns;
 
-    entities_collector_ = std::make_shared<StaticExecutorEntitiesCollector>();
     entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_);
 
     while (rclcpp::ok(this->context_)) {
@@ -191,6 +220,38 @@ public:
     return rclcpp::FutureReturnCode::INTERRUPTED;
   }
 
+  /// Not yet implemented, see https://github.com/ros2/rclcpp/issues/1219 for tracking
+  RCLCPP_PUBLIC
+  void
+  spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)) override
+  {
+    (void)max_duration;
+    throw rclcpp::exceptions::UnimplementedError(
+            "spin_some is not implemented for StaticSingleThreadedExecutor, use spin or "
+            "spin_until_future_complete");
+  }
+
+  /// Not yet implemented, see https://github.com/ros2/rclcpp/issues/1219 for tracking
+  RCLCPP_PUBLIC
+  void
+  spin_all(std::chrono::nanoseconds) override
+  {
+    throw rclcpp::exceptions::UnimplementedError(
+            "spin_all is not implemented for StaticSingleThreadedExecutor, use spin or "
+            "spin_until_future_complete");
+  }
+
+  /// Not yet implemented, see https://github.com/ros2/rclcpp/issues/1219 for tracking
+  RCLCPP_PUBLIC
+  void
+  spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)) override
+  {
+    (void)timeout;
+    throw rclcpp::exceptions::UnimplementedError(
+            "spin_once is not implemented for StaticSingleThreadedExecutor, use spin or "
+            "spin_until_future_complete");
+  }
+
 protected:
   /// Check which executables in ExecutableList struct are ready from wait_set and execute them.
   /**

rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp

@@ -37,6 +37,10 @@ namespace experimental
 namespace buffers
 {
 
+/// Store elements in a fixed-size, FIFO buffer
+/**
+ * All public member functions are thread-safe.
+ */
 template<typename BufferT>
 class RingBufferImplementation : public BufferImplementationBase<BufferT>
 {
@@ -55,55 +59,125 @@ public:
 
   virtual ~RingBufferImplementation() {}
 
+  /// Add a new element to store in the ring buffer
+  /**
+   * This member function is thread-safe.
+   *
+   * \param request the element to be stored in the ring buffer
+   */
   void enqueue(BufferT request)
   {
     std::lock_guard<std::mutex> lock(mutex_);
 
-    write_index_ = next(write_index_);
+    write_index_ = next_(write_index_);
     ring_buffer_[write_index_] = std::move(request);
 
-    if (is_full()) {
-      read_index_ = next(read_index_);
+    if (is_full_()) {
+      read_index_ = next_(read_index_);
     } else {
       size_++;
     }
   }
 
+  /// Remove the oldest element from ring buffer
+  /**
+   * This member function is thread-safe.
+   *
+   * \return the element that is being removed from the ring buffer
+   */
   BufferT dequeue()
   {
     std::lock_guard<std::mutex> lock(mutex_);
 
-    if (!has_data()) {
+    if (!has_data_()) {
       RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Calling dequeue on empty intra-process buffer");
       throw std::runtime_error("Calling dequeue on empty intra-process buffer");
     }
 
     auto request = std::move(ring_buffer_[read_index_]);
-    read_index_ = next(read_index_);
+    read_index_ = next_(read_index_);
 
     size_--;
 
     return request;
   }
 
+  /// Get the next index value for the ring buffer
+  /**
+   * This member function is thread-safe.
+   *
+   * \param val the current index value
+   * \return the next index value
+   */
   inline size_t next(size_t val)
   {
-    return (val + 1) % capacity_;
+    std::lock_guard<std::mutex> lock(mutex_);
+    return next_(val);
   }
 
+  /// Get if the ring buffer has at least one element stored
+  /**
+   * This member function is thread-safe.
+   *
+   * \return `true` if there is data and `false` otherwise
+   */
   inline bool has_data() const
   {
-    return size_ != 0;
+    std::lock_guard<std::mutex> lock(mutex_);
+    return has_data_();
   }
 
-  inline bool is_full()
+  /// Get if the size of the buffer is equal to its capacity
+  /**
+   * This member function is thread-safe.
+   *
+   * \return `true` if the size of the buffer is equal is capacity
+   * and `false` otherwise
+   */
+  inline bool is_full() const
   {
-    return size_ == capacity_;
+    std::lock_guard<std::mutex> lock(mutex_);
+    return is_full_();
   }
 
   void clear() {}
 
 private:
+  /// Get the next index value for the ring buffer
+  /**
+   * This member function is not thread-safe.
+   *
+   * \param val the current index value
+   * \return the next index value
+   */
+  inline size_t next_(size_t val)
+  {
+    return (val + 1) % capacity_;
+  }
+
+  /// Get if the ring buffer has at least one element stored
+  /**
+   * This member function is not thread-safe.
+   *
+   * \return `true` if there is data and `false` otherwise
+   */
+  inline bool has_data_() const
+  {
+    return size_ != 0;
+  }
+
+  /// Get if the size of the buffer is equal to its capacity
+  /**
+   * This member function is not thread-safe.
+   *
+   * \return `true` if the size of the buffer is equal is capacity
+   * and `false` otherwise
+   */
+  inline bool is_full_() const
+  {
+    return size_ == capacity_;
+  }
+
   size_t capacity_;
 
   std::vector<BufferT> ring_buffer_;
@@ -112,7 +186,7 @@ private:
   size_t read_index_;
   size_t size_;
 
-  std::mutex mutex_;
+  mutable std::mutex mutex_;
 };
 
 }  // namespace buffers

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -92,8 +92,8 @@ public:
 
     TRACEPOINT(
       rclcpp_subscription_callback_added,
-      (const void *)this,
-      (const void *)&any_callback_);
+      static_cast<const void *>(this),
+      static_cast<const void *>(&any_callback_));
     // The callback object gets copied, so if registration is done too early/before this point
     // (e.g. in `AnySubscriptionCallback::set()`), its address won't match any address used later
     // in subsequent tracepoints.

rclcpp/include/rclcpp/init_options.hpp

@@ -16,6 +16,7 @@
 #define RCLCPP__INIT_OPTIONS_HPP_
 
 #include <memory>
+#include <mutex>
 
 #include "rcl/init_options.h"
 #include "rclcpp/visibility_control.hpp"
@@ -80,11 +81,30 @@ public:
   const rcl_init_options_t *
   get_rcl_init_options() const;
 
+  /// Retrieve default domain id and set.
+  RCLCPP_PUBLIC
+  void
+  use_default_domain_id();
+
+  /// Set the domain id.
+  RCLCPP_PUBLIC
+  void
+  set_domain_id(size_t domain_id);
+
+  /// Return domain id.
+  RCLCPP_PUBLIC
+  size_t
+  get_domain_id() const;
+
 protected:
   void
   finalize_init_options();
 
 private:
+  void
+  finalize_init_options_impl();
+
+  mutable std::mutex init_options_mutex_;
   std::unique_ptr<rcl_init_options_t> init_options_;
   bool initialize_logging_{true};
 };

rclcpp/include/rclcpp/logger.hpp

@@ -21,6 +21,7 @@
 #include "rclcpp/visibility_control.hpp"
 
 #include "rcl/node.h"
+#include "rcutils/logging.h"
 
 /**
  * \def RCLCPP_LOGGING_ENABLED
@@ -76,6 +77,18 @@ get_node_logger(const rcl_node_t * node);
 
 class Logger
 {
+public:
+  /// An enum for the type of logger level.
+  enum class Level
+  {
+    Unset = RCUTILS_LOG_SEVERITY_UNSET,  ///< The unset log level
+    Debug = RCUTILS_LOG_SEVERITY_DEBUG,  ///< The debug log level
+    Info = RCUTILS_LOG_SEVERITY_INFO,    ///< The info log level
+    Warn = RCUTILS_LOG_SEVERITY_WARN,    ///< The warn log level
+    Error = RCUTILS_LOG_SEVERITY_ERROR,  ///< The error log level
+    Fatal = RCUTILS_LOG_SEVERITY_FATAL,  ///< The fatal log level
+  };
+
 private:
   friend Logger rclcpp::get_logger(const std::string & name);
   friend ::rclcpp::node_interfaces::NodeLogging;
@@ -138,6 +151,16 @@ public:
     }
     return Logger(*name_ + "." + suffix);
   }
+
+  /// Set level for current logger.
+  /**
+   * \param[in] level the logger's level
+   * \throws rclcpp::exceptions::RCLInvalidArgument if level is invalid.
+   * \throws rclcpp::exceptions::RCLError if other error happens.
+   */
+  RCLCPP_PUBLIC
+  void
+  set_level(Level level);
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -16,6 +16,7 @@
 #define RCLCPP__MEMORY_STRATEGY_HPP_
 
 #include <list>
+#include <map>
 #include <memory>
 
 #include "rcl/allocator.h"
@@ -42,11 +43,13 @@ class RCLCPP_PUBLIC MemoryStrategy
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy)
-  using WeakNodeList = std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>;
+  using WeakCallbackGroupsToNodesMap = std::map<rclcpp::CallbackGroup::WeakPtr,
+      rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+      std::owner_less<rclcpp::CallbackGroup::WeakPtr>>;
 
   virtual ~MemoryStrategy() = default;
 
-  virtual bool collect_entities(const WeakNodeList & weak_nodes) = 0;
+  virtual bool collect_entities(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0;
 
   virtual size_t number_of_ready_subscriptions() const = 0;
   virtual size_t number_of_ready_services() const = 0;
@@ -68,27 +71,27 @@ public:
   virtual void
   get_next_subscription(
     rclcpp::AnyExecutable & any_exec,
-    const WeakNodeList & weak_nodes) = 0;
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0;
 
   virtual void
   get_next_service(
     rclcpp::AnyExecutable & any_exec,
-    const WeakNodeList & weak_nodes) = 0;
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0;
 
   virtual void
   get_next_client(
     rclcpp::AnyExecutable & any_exec,
-    const WeakNodeList & weak_nodes) = 0;
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0;
 
   virtual void
   get_next_timer(
     rclcpp::AnyExecutable & any_exec,
-    const WeakNodeList & weak_nodes) = 0;
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0;
 
   virtual void
   get_next_waitable(
     rclcpp::AnyExecutable & any_exec,
-    const WeakNodeList & weak_nodes) = 0;
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0;
 
   virtual rcl_allocator_t
   get_allocator() = 0;
@@ -96,52 +99,52 @@ public:
   static rclcpp::SubscriptionBase::SharedPtr
   get_subscription_by_handle(
     std::shared_ptr<const rcl_subscription_t> subscriber_handle,
-    const WeakNodeList & weak_nodes);
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::ServiceBase::SharedPtr
   get_service_by_handle(
     std::shared_ptr<const rcl_service_t> service_handle,
-    const WeakNodeList & weak_nodes);
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::ClientBase::SharedPtr
   get_client_by_handle(
     std::shared_ptr<const rcl_client_t> client_handle,
-    const WeakNodeList & weak_nodes);
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::TimerBase::SharedPtr
   get_timer_by_handle(
     std::shared_ptr<const rcl_timer_t> timer_handle,
-    const WeakNodeList & weak_nodes);
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
   get_node_by_group(
     rclcpp::CallbackGroup::SharedPtr group,
-    const WeakNodeList & weak_nodes);
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::CallbackGroup::SharedPtr
   get_group_by_subscription(
     rclcpp::SubscriptionBase::SharedPtr subscription,
-    const WeakNodeList & weak_nodes);
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::CallbackGroup::SharedPtr
   get_group_by_service(
     rclcpp::ServiceBase::SharedPtr service,
-    const WeakNodeList & weak_nodes);
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::CallbackGroup::SharedPtr
   get_group_by_client(
     rclcpp::ClientBase::SharedPtr client,
-    const WeakNodeList & weak_nodes);
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::CallbackGroup::SharedPtr
   get_group_by_timer(
     rclcpp::TimerBase::SharedPtr timer,
-    const WeakNodeList & weak_nodes);
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::CallbackGroup::SharedPtr
   get_group_by_waitable(
     rclcpp::Waitable::SharedPtr waitable,
-    const WeakNodeList & weak_nodes);
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 };
 
 }  // namespace memory_strategy

rclcpp/include/rclcpp/node.hpp

@@ -26,6 +26,8 @@
 #include <utility>
 #include <vector>
 
+#include "rcutils/macros.h"
+
 #include "rcl/error_handling.h"
 #include "rcl/node.h"
 
@@ -139,7 +141,9 @@ public:
   /// Create and return a callback group.
   RCLCPP_PUBLIC
   rclcpp::CallbackGroup::SharedPtr
-  create_callback_group(rclcpp::CallbackGroupType group_type);
+  create_callback_group(
+    rclcpp::CallbackGroupType group_type,
+    bool automatically_add_to_executor_with_node = true);
 
   /// Return the list of callback groups in the node.
   RCLCPP_PUBLIC
@@ -812,6 +816,7 @@ public:
    * \throws std::bad_alloc if the allocation of the OnSetParametersCallbackHandle fails.
    */
   RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
   OnSetParametersCallbackHandle::SharedPtr
   add_on_set_parameters_callback(OnParametersSetCallbackType callback);
 
@@ -882,15 +887,21 @@ public:
     const std::string & node_name,
     const std::string & namespace_) const;
 
+  /// Return the number of publishers created for a given topic.
+  /**
+   * \param[in] topic_name the actual topic name used; it will not be automatically remapped.
+   * \return number of publishers that have been created for the given topic.
+   * \throws std::runtime_error if publishers could not be counted
+   */
   RCLCPP_PUBLIC
   size_t
   count_publishers(const std::string & topic_name) const;
 
-  /// Return the number of subscribers who have created a subscription for a given topic.
+  /// Return the number of subscribers created for a given topic.
   /**
-   * \param[in] topic_name the topic_name on which to count the subscribers.
-   * \return number of subscribers who have created a subscription for a given topic.
-   * \throws std::runtime_error if publishers could not be counted
+   * \param[in] topic_name the actual topic name used; it will not be automatically remapped.
+   * \return number of subscribers that have been created for the given topic.
+   * \throws std::runtime_error if subscribers could not be counted
    */
   RCLCPP_PUBLIC
   size_t
@@ -911,7 +922,7 @@ public:
    * A relative or private topic will be expanded using this node's namespace and name.
    * The queried `topic_name` is not remapped.
    *
-   * \param[in] topic_name the topic_name on which to find the publishers.
+   * \param[in] topic_name the actual topic name used; it will not be automatically remapped.
    * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name,
    *   otherwise it should be a valid ROS topic name. Defaults to `false`.
    * \return a list of TopicEndpointInfo representing all the publishers on this topic.
@@ -937,7 +948,7 @@ public:
    * A relative or private topic will be expanded using this node's namespace and name.
    * The queried `topic_name` is not remapped.
    *
-   * \param[in] topic_name the topic_name on which to find the subscriptions.
+   * \param[in] topic_name the actual topic name used; it will not be automatically remapped.
    * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name,
    *   otherwise it should be a valid ROS topic name. Defaults to `false`.
    * \return a list of TopicEndpointInfo representing all the subscriptions on this topic.
@@ -1171,10 +1182,6 @@ protected:
 private:
   RCLCPP_DISABLE_COPY(Node)
 
-  RCLCPP_PUBLIC
-  bool
-  group_in_node(CallbackGroup::SharedPtr group);
-
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
   rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -83,7 +83,9 @@ public:
 
   RCLCPP_PUBLIC
   rclcpp::CallbackGroup::SharedPtr
-  create_callback_group(rclcpp::CallbackGroupType group_type) override;
+  create_callback_group(
+    rclcpp::CallbackGroupType group_type,
+    bool automatically_add_to_executor_with_node = true) override;
 
   RCLCPP_PUBLIC
   rclcpp::CallbackGroup::SharedPtr

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -106,7 +106,9 @@ public:
   RCLCPP_PUBLIC
   virtual
   rclcpp::CallbackGroup::SharedPtr
-  create_callback_group(rclcpp::CallbackGroupType group_type) = 0;
+  create_callback_group(
+    rclcpp::CallbackGroupType group_type,
+    bool automatically_add_to_executor_with_node = true) = 0;
 
   /// Return the default callback group.
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

@@ -110,7 +110,7 @@ public:
 
   RCLCPP_PUBLIC
   size_t
-  count_graph_users() override;
+  count_graph_users() const override;
 
   RCLCPP_PUBLIC
   std::vector<rclcpp::TopicEndpointInfo>

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

@@ -146,6 +146,7 @@ public:
   /**
    * A topic is considered to exist when at least one publisher or subscriber
    * exists for it, whether they be local or remote to this process.
+   * The returned names are the actual names used and do not have remap rules applied.
    *
    * \param[in] no_demangle if true, topic names and types are not demangled
    */
@@ -159,6 +160,7 @@ public:
    * A service is considered to exist when at least one service server or
    * service client exists for it, whether they be local or remote to this
    * process.
+   * The returned names are the actual names used and do not have remap rules applied.
    */
   RCLCPP_PUBLIC
   virtual
@@ -168,6 +170,7 @@ public:
   /// Return a map of existing service names to list of service types for a specific node.
   /**
    * 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
@@ -180,24 +183,36 @@ public:
     const std::string & namespace_) const = 0;
 
   /// Return a vector of existing node names (string).
+  /*
+   * The returned names are the actual names used and do not have remap rules applied.
+   */
   RCLCPP_PUBLIC
   virtual
   std::vector<std::string>
   get_node_names() const = 0;
 
   /// Return a vector of existing node names and namespaces (pair of string).
+  /*
+   * The returned names are the actual names used and do not have remap rules applied.
+   */
   RCLCPP_PUBLIC
   virtual
   std::vector<std::pair<std::string, std::string>>
   get_node_names_and_namespaces() const = 0;
 
   /// Return the number of publishers that are advertised on a given topic.
+  /*
+   * \param[in] topic_name the actual topic name used; it will not be automatically remapped.
+   */
   RCLCPP_PUBLIC
   virtual
   size_t
   count_publishers(const std::string & topic_name) const = 0;
 
   /// Return the number of subscribers who have created a subscription for a given topic.
+  /*
+   * \param[in] topic_name the actual topic name used; it will not be automatically remapped.
+   */
   RCLCPP_PUBLIC
   virtual
   size_t
@@ -263,10 +278,11 @@ public:
   RCLCPP_PUBLIC
   virtual
   size_t
-  count_graph_users() = 0;
+  count_graph_users() const = 0;
 
   /// Return the topic endpoint information about publishers on a given topic.
   /**
+   * \param[in] topic_name the actual topic name used; it will not be automatically remapped.
    * \sa rclcpp::Node::get_publishers_info_by_topic
    */
   RCLCPP_PUBLIC
@@ -276,6 +292,7 @@ public:
 
   /// Return the topic endpoint information about subscriptions on a given topic.
   /**
+   * \param[in] topic_name the actual topic name used; it will not be automatically remapped.
    * \sa rclcpp::Node::get_subscriptions_info_by_topic
    */
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -21,6 +21,8 @@
 #include <string>
 #include <vector>
 
+#include "rcutils/macros.h"
+
 #include "rcl_interfaces/msg/list_parameters_result.hpp"
 #include "rcl_interfaces/msg/parameter_descriptor.hpp"
 #include "rcl_interfaces/msg/parameter_event.hpp"
@@ -160,6 +162,7 @@ public:
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const override;
 
   RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
   OnSetParametersCallbackHandle::SharedPtr
   add_on_set_parameters_callback(OnParametersSetCallbackType callback) override;
 

rclcpp/include/rclcpp/parameter_client.hpp

@@ -73,12 +73,21 @@ public:
    * \param[in] qos_profile (optional) The rmw qos profile to use to subscribe
    * \param[in] group (optional) The async parameter client will be added to this callback group.
    */
-  RCLCPP_PUBLIC
+  template<typename NodeT>
   AsyncParametersClient(
-    const rclcpp::Node::SharedPtr node,
+    const std::shared_ptr<NodeT> node,
     const std::string & remote_node_name = "",
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters,
-    rclcpp::CallbackGroup::SharedPtr group = nullptr);
+    rclcpp::CallbackGroup::SharedPtr group = nullptr)
+  : AsyncParametersClient(
+      node->get_node_base_interface(),
+      node->get_node_topics_interface(),
+      node->get_node_graph_interface(),
+      node->get_node_services_interface(),
+      remote_node_name,
+      qos_profile,
+      group)
+  {}
 
   /// Constructor
   /**
@@ -87,12 +96,21 @@ public:
    * \param[in] qos_profile (optional) The rmw qos profile to use to subscribe
    * \param[in] group (optional) The async parameter client will be added to this callback group.
    */
-  RCLCPP_PUBLIC
+  template<typename NodeT>
   AsyncParametersClient(
-    rclcpp::Node * node,
+    NodeT * node,
     const std::string & remote_node_name = "",
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters,
-    rclcpp::CallbackGroup::SharedPtr group = nullptr);
+    rclcpp::CallbackGroup::SharedPtr group = nullptr)
+  : AsyncParametersClient(
+      node->get_node_base_interface(),
+      node->get_node_topics_interface(),
+      node->get_node_graph_interface(),
+      node->get_node_services_interface(),
+      remote_node_name,
+      qos_profile,
+      group)
+  {}
 
   RCLCPP_PUBLIC
   std::shared_future<std::vector<rclcpp::Parameter>>
@@ -177,7 +195,7 @@ public:
   {
     return rclcpp::create_subscription<rcl_interfaces::msg::ParameterEvent>(
       node,
-      "parameter_events",
+      "/parameter_events",
       qos,
       std::forward<CallbackT>(callback),
       options);
@@ -237,31 +255,61 @@ class SyncParametersClient
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient)
 
-  RCLCPP_PUBLIC
+  template<typename NodeT>
   explicit SyncParametersClient(
-    rclcpp::Node::SharedPtr node,
+    std::shared_ptr<NodeT> node,
     const std::string & remote_node_name = "",
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters)
+  : SyncParametersClient(
+      std::make_shared<rclcpp::executors::SingleThreadedExecutor>(),
+      node,
+      remote_node_name,
+      qos_profile)
+  {}
 
-  RCLCPP_PUBLIC
+  template<typename NodeT>
   SyncParametersClient(
     rclcpp::Executor::SharedPtr executor,
-    rclcpp::Node::SharedPtr node,
+    std::shared_ptr<NodeT> node,
     const std::string & remote_node_name = "",
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters)
+  : SyncParametersClient(
+      executor,
+      node->get_node_base_interface(),
+      node->get_node_topics_interface(),
+      node->get_node_graph_interface(),
+      node->get_node_services_interface(),
+      remote_node_name,
+      qos_profile)
+  {}
 
-  RCLCPP_PUBLIC
-  explicit SyncParametersClient(
-    rclcpp::Node * node,
+  template<typename NodeT>
+  SyncParametersClient(
+    NodeT * node,
     const std::string & remote_node_name = "",
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters)
+  : SyncParametersClient(
+      std::make_shared<rclcpp::executors::SingleThreadedExecutor>(),
+      node,
+      remote_node_name,
+      qos_profile)
+  {}
 
-  RCLCPP_PUBLIC
+  template<typename NodeT>
   SyncParametersClient(
     rclcpp::Executor::SharedPtr executor,
-    rclcpp::Node * node,
+    NodeT * node,
     const std::string & remote_node_name = "",
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters)
+  : SyncParametersClient(
+      executor,
+      node->get_node_base_interface(),
+      node->get_node_topics_interface(),
+      node->get_node_graph_interface(),
+      node->get_node_services_interface(),
+      remote_node_name,
+      qos_profile)
+  {}
 
   RCLCPP_PUBLIC
   SyncParametersClient(
@@ -271,7 +319,18 @@ public:
     const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
     const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
     const std::string & remote_node_name = "",
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters)
+  : executor_(executor), node_base_interface_(node_base_interface)
+  {
+    async_parameters_client_ =
+      std::make_shared<AsyncParametersClient>(
+      node_base_interface,
+      node_topics_interface,
+      node_graph_interface,
+      node_services_interface,
+      remote_node_name,
+      qos_profile);
+  }
 
   RCLCPP_PUBLIC
   std::vector<rclcpp::Parameter>

rclcpp/include/rclcpp/service.hpp

@@ -223,8 +223,8 @@ public:
     }
     TRACEPOINT(
       rclcpp_service_callback_added,
-      (const void *)get_service_handle().get(),
-      (const void *)&any_callback_);
+      static_cast<const void *>(get_service_handle().get()),
+      static_cast<const void *>(&any_callback_));
 #ifndef TRACETOOLS_DISABLED
     any_callback_.register_callback_for_tracing();
 #endif
@@ -258,8 +258,8 @@ public:
     service_handle_ = service_handle;
     TRACEPOINT(
       rclcpp_service_callback_added,
-      (const void *)get_service_handle().get(),
-      (const void *)&any_callback_);
+      static_cast<const void *>(get_service_handle().get()),
+      static_cast<const void *>(&any_callback_));
 #ifndef TRACETOOLS_DISABLED
     any_callback_.register_callback_for_tracing();
 #endif
@@ -295,8 +295,8 @@ public:
     service_handle_->impl = service_handle->impl;
     TRACEPOINT(
       rclcpp_service_callback_added,
-      (const void *)get_service_handle().get(),
-      (const void *)&any_callback_);
+      static_cast<const void *>(get_service_handle().get()),
+      static_cast<const void *>(&any_callback_));
 #ifndef TRACETOOLS_DISABLED
     any_callback_.register_callback_for_tracing();
 #endif

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -150,48 +150,47 @@ public:
     );
   }
 
-  bool collect_entities(const WeakNodeList & weak_nodes) override
+  bool collect_entities(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
   {
-    bool has_invalid_weak_nodes = false;
-    for (auto & weak_node : weak_nodes) {
-      auto node = weak_node.lock();
-      if (!node) {
-        has_invalid_weak_nodes = true;
+    bool has_invalid_weak_groups_or_nodes = false;
+    for (const auto & pair : weak_groups_to_nodes) {
+      auto group = pair.first.lock();
+      auto node = pair.second.lock();
+      if (group == nullptr || node == nullptr) {
+        has_invalid_weak_groups_or_nodes = true;
         continue;
       }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto group = weak_group.lock();
-        if (!group || !group->can_be_taken_from().load()) {
-          continue;
-        }
-        group->find_subscription_ptrs_if(
-          [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
-            subscription_handles_.push_back(subscription->get_subscription_handle());
-            return false;
-          });
-        group->find_service_ptrs_if(
-          [this](const rclcpp::ServiceBase::SharedPtr & service) {
-            service_handles_.push_back(service->get_service_handle());
-            return false;
-          });
-        group->find_client_ptrs_if(
-          [this](const rclcpp::ClientBase::SharedPtr & client) {
-            client_handles_.push_back(client->get_client_handle());
-            return false;
-          });
-        group->find_timer_ptrs_if(
-          [this](const rclcpp::TimerBase::SharedPtr & timer) {
-            timer_handles_.push_back(timer->get_timer_handle());
-            return false;
-          });
-        group->find_waitable_ptrs_if(
-          [this](const rclcpp::Waitable::SharedPtr & waitable) {
-            waitable_handles_.push_back(waitable);
-            return false;
-          });
+      if (!group || !group->can_be_taken_from().load()) {
+        continue;
       }
+      group->find_subscription_ptrs_if(
+        [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
+          subscription_handles_.push_back(subscription->get_subscription_handle());
+          return false;
+        });
+      group->find_service_ptrs_if(
+        [this](const rclcpp::ServiceBase::SharedPtr & service) {
+          service_handles_.push_back(service->get_service_handle());
+          return false;
+        });
+      group->find_client_ptrs_if(
+        [this](const rclcpp::ClientBase::SharedPtr & client) {
+          client_handles_.push_back(client->get_client_handle());
+          return false;
+        });
+      group->find_timer_ptrs_if(
+        [this](const rclcpp::TimerBase::SharedPtr & timer) {
+          timer_handles_.push_back(timer->get_timer_handle());
+          return false;
+        });
+      group->find_waitable_ptrs_if(
+        [this](const rclcpp::Waitable::SharedPtr & waitable) {
+          waitable_handles_.push_back(waitable);
+          return false;
+        });
     }
-    return has_invalid_weak_nodes;
+
+    return has_invalid_weak_groups_or_nodes;
   }
 
   void add_waitable_handle(const rclcpp::Waitable::SharedPtr & waitable) override
@@ -264,14 +263,14 @@ public:
   void
   get_next_subscription(
     rclcpp::AnyExecutable & any_exec,
-    const WeakNodeList & weak_nodes) override
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
   {
     auto it = subscription_handles_.begin();
     while (it != subscription_handles_.end()) {
-      auto subscription = get_subscription_by_handle(*it, weak_nodes);
+      auto subscription = get_subscription_by_handle(*it, weak_groups_to_nodes);
       if (subscription) {
         // Find the group for this handle and see if it can be serviced
-        auto group = get_group_by_subscription(subscription, weak_nodes);
+        auto group = get_group_by_subscription(subscription, weak_groups_to_nodes);
         if (!group) {
           // Group was not found, meaning the subscription is not valid...
           // Remove it from the ready list and continue looking
@@ -287,7 +286,7 @@ public:
         // Otherwise it is safe to set and return the any_exec
         any_exec.subscription = subscription;
         any_exec.callback_group = group;
-        any_exec.node_base = get_node_by_group(group, weak_nodes);
+        any_exec.node_base = get_node_by_group(group, weak_groups_to_nodes);
         subscription_handles_.erase(it);
         return;
       }
@@ -299,14 +298,14 @@ public:
   void
   get_next_service(
     rclcpp::AnyExecutable & any_exec,
-    const WeakNodeList & weak_nodes) override
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
   {
     auto it = service_handles_.begin();
     while (it != service_handles_.end()) {
-      auto service = get_service_by_handle(*it, weak_nodes);
+      auto service = get_service_by_handle(*it, weak_groups_to_nodes);
       if (service) {
         // Find the group for this handle and see if it can be serviced
-        auto group = get_group_by_service(service, weak_nodes);
+        auto group = get_group_by_service(service, weak_groups_to_nodes);
         if (!group) {
           // Group was not found, meaning the service is not valid...
           // Remove it from the ready list and continue looking
@@ -322,7 +321,7 @@ public:
         // Otherwise it is safe to set and return the any_exec
         any_exec.service = service;
         any_exec.callback_group = group;
-        any_exec.node_base = get_node_by_group(group, weak_nodes);
+        any_exec.node_base = get_node_by_group(group, weak_groups_to_nodes);
         service_handles_.erase(it);
         return;
       }
@@ -332,14 +331,16 @@ public:
   }
 
   void
-  get_next_client(rclcpp::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) override
+  get_next_client(
+    rclcpp::AnyExecutable & any_exec,
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
   {
     auto it = client_handles_.begin();
     while (it != client_handles_.end()) {
-      auto client = get_client_by_handle(*it, weak_nodes);
+      auto client = get_client_by_handle(*it, weak_groups_to_nodes);
       if (client) {
         // Find the group for this handle and see if it can be serviced
-        auto group = get_group_by_client(client, weak_nodes);
+        auto group = get_group_by_client(client, weak_groups_to_nodes);
         if (!group) {
           // Group was not found, meaning the service is not valid...
           // Remove it from the ready list and continue looking
@@ -355,7 +356,7 @@ public:
         // Otherwise it is safe to set and return the any_exec
         any_exec.client = client;
         any_exec.callback_group = group;
-        any_exec.node_base = get_node_by_group(group, weak_nodes);
+        any_exec.node_base = get_node_by_group(group, weak_groups_to_nodes);
         client_handles_.erase(it);
         return;
       }
@@ -367,14 +368,14 @@ public:
   void
   get_next_timer(
     rclcpp::AnyExecutable & any_exec,
-    const WeakNodeList & weak_nodes) override
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
   {
     auto it = timer_handles_.begin();
     while (it != timer_handles_.end()) {
-      auto timer = get_timer_by_handle(*it, weak_nodes);
+      auto timer = get_timer_by_handle(*it, weak_groups_to_nodes);
       if (timer) {
         // Find the group for this handle and see if it can be serviced
-        auto group = get_group_by_timer(timer, weak_nodes);
+        auto group = get_group_by_timer(timer, weak_groups_to_nodes);
         if (!group) {
           // Group was not found, meaning the timer is not valid...
           // Remove it from the ready list and continue looking
@@ -390,7 +391,7 @@ public:
         // Otherwise it is safe to set and return the any_exec
         any_exec.timer = timer;
         any_exec.callback_group = group;
-        any_exec.node_base = get_node_by_group(group, weak_nodes);
+        any_exec.node_base = get_node_by_group(group, weak_groups_to_nodes);
         timer_handles_.erase(it);
         return;
       }
@@ -400,14 +401,16 @@ public:
   }
 
   void
-  get_next_waitable(rclcpp::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) override
+  get_next_waitable(
+    rclcpp::AnyExecutable & any_exec,
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
   {
     auto it = waitable_handles_.begin();
     while (it != waitable_handles_.end()) {
       auto waitable = *it;
       if (waitable) {
         // Find the group for this handle and see if it can be serviced
-        auto group = get_group_by_waitable(waitable, weak_nodes);
+        auto group = get_group_by_waitable(waitable, weak_groups_to_nodes);
         if (!group) {
           // Group was not found, meaning the waitable is not valid...
           // Remove it from the ready list and continue looking
@@ -423,7 +426,7 @@ public:
         // Otherwise it is safe to set and return the any_exec
         any_exec.waitable = waitable;
         any_exec.callback_group = group;
-        any_exec.node_base = get_node_by_group(group, weak_nodes);
+        any_exec.node_base = get_node_by_group(group, weak_groups_to_nodes);
         waitable_handles_.erase(it);
         return;
       }

rclcpp/include/rclcpp/subscription.hpp

@@ -184,8 +184,8 @@ public:
         resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback));
       TRACEPOINT(
         rclcpp_subscription_init,
-        (const void *)get_subscription_handle().get(),
-        (const void *)subscription_intra_process.get());
+        static_cast<const void *>(get_subscription_handle().get()),
+        static_cast<const void *>(subscription_intra_process.get()));
 
       // Add it to the intra process manager.
       using rclcpp::experimental::IntraProcessManager;
@@ -200,12 +200,12 @@ public:
 
     TRACEPOINT(
       rclcpp_subscription_init,
-      (const void *)get_subscription_handle().get(),
-      (const void *)this);
+      static_cast<const void *>(get_subscription_handle().get()),
+      static_cast<const void *>(this));
     TRACEPOINT(
       rclcpp_subscription_callback_added,
-      (const void *)this,
-      (const void *)&any_callback_);
+      static_cast<const void *>(this),
+      static_cast<const void *>(&any_callback_));
     // The callback object gets copied, so if registration is done too early/before this point
     // (e.g. in `AnySubscriptionCallback::set()`), its address won't match any address used later
     // in subsequent tracepoints.

rclcpp/include/rclcpp/time_source.hpp

@@ -148,8 +148,6 @@ private:
   void disable_ros_time();
 
   // Internal helper functions used inside iterators
-  static void enable_ros_time(rclcpp::Clock::SharedPtr clock);
-  static void disable_ros_time(rclcpp::Clock::SharedPtr clock);
   static void set_clock(
     const builtin_interfaces::msg::Time::SharedPtr msg,
     bool set_ros_time_enabled,

rclcpp/include/rclcpp/timer.hpp

@@ -176,11 +176,11 @@ public:
   {
     TRACEPOINT(
       rclcpp_timer_callback_added,
-      (const void *)get_timer_handle().get(),
-      (const void *)&callback_);
+      static_cast<const void *>(get_timer_handle().get()),
+      static_cast<const void *>(&callback_));
     TRACEPOINT(
       rclcpp_callback_register,
-      (const void *)&callback_,
+      static_cast<const void *>(&callback_),
       get_symbol(callback_));
   }
 
@@ -205,9 +205,9 @@ public:
     if (ret != RCL_RET_OK) {
       throw std::runtime_error("Failed to notify timer that callback occurred");
     }
-    TRACEPOINT(callback_start, (const void *)&callback_, false);
+    TRACEPOINT(callback_start, static_cast<const void *>(&callback_), false);
     execute_callback_delegate<>();
-    TRACEPOINT(callback_end, (const void *)&callback_);
+    TRACEPOINT(callback_end, static_cast<const void *>(&callback_));
   }
 
   // void specialization

rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp

@@ -73,7 +73,17 @@ protected:
     size_t services_from_waitables = 0;
     size_t events_from_waitables = 0;
     for (const auto & waitable_entry : waitables) {
-      rclcpp::Waitable & waitable = *waitable_entry.waitable.get();
+      auto waitable_ptr_pair = get_raw_pointer_from_smart_pointer(waitable_entry.waitable);
+      if (nullptr == waitable_ptr_pair.second) {
+        if (HasStrongOwnership) {
+          throw std::runtime_error("unexpected condition, fixed storage policy needs pruning");
+        }
+        // Flag for pruning.
+        needs_pruning_ = true;
+        continue;
+      }
+
+      rclcpp::Waitable & waitable = *waitable_ptr_pair.second;
       subscriptions_from_waitables += waitable.get_number_of_ready_subscriptions();
       guard_conditions_from_waitables += waitable.get_number_of_ready_guard_conditions();
       timers_from_waitables += waitable.get_number_of_ready_timers();

rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp

@@ -382,11 +382,13 @@ public:
         return weak_ptr.expired();
       };
     // remove guard conditions which have been deleted
-    guard_conditions_.erase(std::remove_if(guard_conditions_.begin(), guard_conditions_.end(), p));
-    timers_.erase(std::remove_if(timers_.begin(), timers_.end(), p));
-    clients_.erase(std::remove_if(clients_.begin(), clients_.end(), p));
-    services_.erase(std::remove_if(services_.begin(), services_.end(), p));
-    waitables_.erase(std::remove_if(waitables_.begin(), waitables_.end(), p));
+    guard_conditions_.erase(
+      std::remove_if(guard_conditions_.begin(), guard_conditions_.end(), p),
+      guard_conditions_.end());
+    timers_.erase(std::remove_if(timers_.begin(), timers_.end(), p), timers_.end());
+    clients_.erase(std::remove_if(clients_.begin(), clients_.end(), p), clients_.end());
+    services_.erase(std::remove_if(services_.begin(), services_.end(), p), services_.end());
+    waitables_.erase(std::remove_if(waitables_.begin(), waitables_.end(), p), waitables_.end());
   }
 
   void

rclcpp/include/rclcpp/wait_set_template.hpp

@@ -233,9 +233,9 @@ public:
         }
         if (mask.include_intra_process_waitable) {
           auto local_waitable = inner_subscription->get_intra_process_waitable();
-          inner_subscription->exchange_in_use_by_wait_set_state(local_waitable.get(), false);
           if (nullptr != local_waitable) {
-            // This is the case when intra process is disabled for the subscription.
+            // This is the case when intra process is enabled for the subscription.
+            inner_subscription->exchange_in_use_by_wait_set_state(local_waitable.get(), false);
             this->storage_remove_waitable(std::move(local_waitable));
           }
         }

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>3.0.0</version>
+  <version>5.0.0</version>
   <description>The ROS client library in C++.</description>
   <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
   <license>Apache License 2.0</license>
@@ -35,6 +35,7 @@
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>mimick_vendor</test_depend>
   <test_depend>rmw</test_depend>
   <test_depend>rmw_implementation_cmake</test_depend>
   <test_depend>rosidl_default_generators</test_depend>

rclcpp/src/rclcpp/callback_group.cpp

@@ -19,8 +19,12 @@
 using rclcpp::CallbackGroup;
 using rclcpp::CallbackGroupType;
 
-CallbackGroup::CallbackGroup(CallbackGroupType group_type)
-: type_(group_type), can_be_taken_from_(true)
+CallbackGroup::CallbackGroup(
+  CallbackGroupType group_type,
+  bool automatically_add_to_executor_with_node)
+: type_(group_type), associated_with_executor_(false),
+  can_be_taken_from_(true),
+  automatically_add_to_executor_with_node_(automatically_add_to_executor_with_node)
 {}
 
 
@@ -36,6 +40,18 @@ CallbackGroup::type() const
   return type_;
 }
 
+std::atomic_bool &
+CallbackGroup::get_associated_with_executor_atomic()
+{
+  return associated_with_executor_;
+}
+
+bool
+CallbackGroup::automatically_add_to_executor_with_node() const
+{
+  return automatically_add_to_executor_with_node_;
+}
+
 void
 CallbackGroup::add_subscription(
   const rclcpp::SubscriptionBase::SharedPtr subscription_ptr)

rclcpp/src/rclcpp/context.cpp

@@ -275,6 +275,17 @@ Context::get_init_options()
   return init_options_;
 }
 
+size_t
+Context::get_domain_id() const
+{
+  size_t domain_id;
+  rcl_ret_t ret = rcl_context_get_domain_id(rcl_context_.get(), &domain_id);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get domain id from context");
+  }
+  return domain_id;
+}
+
 std::string
 Context::shutdown_reason()
 {

rclcpp/src/rclcpp/duration.cpp

@@ -94,6 +94,12 @@ Duration::operator==(const rclcpp::Duration & rhs) const
   return rcl_duration_.nanoseconds == rhs.rcl_duration_.nanoseconds;
 }
 
+bool
+Duration::operator!=(const rclcpp::Duration & rhs) const
+{
+  return rcl_duration_.nanoseconds != rhs.rcl_duration_.nanoseconds;
+}
+
 bool
 Duration::operator<(const rclcpp::Duration & rhs) const
 {

rclcpp/src/rclcpp/executor.cpp

@@ -14,8 +14,11 @@
 
 #include <algorithm>
 #include <memory>
+#include <map>
 #include <string>
 #include <type_traits>
+#include <utility>
+#include <vector>
 
 #include "rcl/allocator.h"
 #include "rcl/error_handling.h"
@@ -82,19 +85,33 @@ Executor::Executor(const rclcpp::ExecutorOptions & options)
 
 Executor::~Executor()
 {
-  // Disassocate all nodes
-  for (auto & weak_node : weak_nodes_) {
-    auto node = weak_node.lock();
-    if (node) {
-      std::atomic_bool & has_executor = node->get_associated_with_executor_atomic();
+  // Disassocate all callback groups.
+  for (auto & pair : weak_groups_to_nodes_) {
+    auto group = pair.first.lock();
+    if (group) {
+      std::atomic_bool & has_executor = group->get_associated_with_executor_atomic();
       has_executor.store(false);
     }
   }
+  // Disassocate all nodes.
+  std::for_each(
+    weak_nodes_.begin(), weak_nodes_.end(), []
+      (rclcpp::node_interfaces::NodeBaseInterface::WeakPtr weak_node_ptr) {
+      auto shared_node_ptr = weak_node_ptr.lock();
+      if (shared_node_ptr) {
+        std::atomic_bool & has_executor = shared_node_ptr->get_associated_with_executor_atomic();
+        has_executor.store(false);
+      }
+    });
   weak_nodes_.clear();
-  for (auto & guard_condition : guard_conditions_) {
+  weak_groups_associated_with_executor_to_nodes_.clear();
+  weak_groups_to_nodes_associated_with_executor_.clear();
+  weak_groups_to_nodes_.clear();
+  for (const auto & pair : weak_nodes_to_guard_conditions_) {
+    auto & guard_condition = pair.second;
     memory_strategy_->remove_guard_condition(guard_condition);
   }
-  guard_conditions_.clear();
+  weak_nodes_to_guard_conditions_.clear();
 
   // Finalize the wait set.
   if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) {
@@ -115,6 +132,116 @@ Executor::~Executor()
   context_->release_interrupt_guard_condition(&wait_set_, std::nothrow);
 }
 
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+Executor::get_all_callback_groups()
+{
+  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
+  for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
+    groups.push_back(group_node_ptr.first);
+  }
+  for (auto const & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) {
+    groups.push_back(group_node_ptr.first);
+  }
+  return groups;
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+Executor::get_manually_added_callback_groups()
+{
+  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
+  for (auto const & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
+    groups.push_back(group_node_ptr.first);
+  }
+  return groups;
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+Executor::get_automatically_added_callback_groups_from_nodes()
+{
+  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
+  for (auto const & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) {
+    groups.push_back(group_node_ptr.first);
+  }
+  return groups;
+}
+
+void
+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)
+        {
+          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())
+          {
+            this->add_callback_group_to_map(
+              shared_group_ptr,
+              node,
+              weak_groups_to_nodes_associated_with_executor_,
+              true);
+          }
+        });
+    }
+  }
+}
+
+void
+Executor::add_callback_group_to_map(
+  rclcpp::CallbackGroup::SharedPtr group_ptr,
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
+  bool notify)
+{
+  // If the callback_group already has an executor
+  std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic();
+  if (has_executor.exchange(true)) {
+    throw std::runtime_error("Callback group has already been added to an executor.");
+  }
+  bool is_new_node = !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) &&
+    !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_);
+  rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
+  auto insert_info =
+    weak_groups_to_nodes.insert(std::make_pair(weak_group_ptr, node_ptr));
+  bool was_inserted = insert_info.second;
+  if (!was_inserted) {
+    throw std::runtime_error("Callback group was already added to executor.");
+  }
+  // Also add to the map that contains all callback groups
+  weak_groups_to_nodes_.insert(std::make_pair(weak_group_ptr, node_ptr));
+  if (is_new_node) {
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr);
+    weak_nodes_to_guard_conditions_[node_weak_ptr] = node_ptr->get_notify_guard_condition();
+    if (notify) {
+      // Interrupt waiting to handle new node
+      if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+        throw std::runtime_error(rcl_get_error_string().str);
+      }
+    }
+    // Add the node's notify condition to the guard condition handles
+    std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
+    memory_strategy_->add_guard_condition(node_ptr->get_notify_guard_condition());
+  }
+}
+
+void
+Executor::add_callback_group(
+  rclcpp::CallbackGroup::SharedPtr group_ptr,
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  bool notify)
+{
+  this->add_callback_group_to_map(
+    group_ptr,
+    node_ptr,
+    weak_groups_associated_with_executor_to_nodes_,
+    notify);
+}
+
 void
 Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
 {
@@ -123,25 +250,67 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt
   if (has_executor.exchange(true)) {
     throw std::runtime_error("Node has already been added to an executor.");
   }
-  // Check to ensure node not already added
-  for (auto & weak_node : weak_nodes_) {
-    auto node = weak_node.lock();
-    if (node == node_ptr) {
-      // TODO(jacquelinekay): Use a different error here?
-      throw std::runtime_error("Cannot add node to executor, node already added.");
+  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())
+    {
+      this->add_callback_group_to_map(
+        group_ptr,
+        node_ptr,
+        weak_groups_to_nodes_associated_with_executor_,
+        notify);
     }
   }
   weak_nodes_.push_back(node_ptr);
-  guard_conditions_.push_back(node_ptr->get_notify_guard_condition());
-  if (notify) {
-    // Interrupt waiting to handle new node
-    if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-      throw std::runtime_error(rcl_get_error_string().str);
+}
+
+void
+Executor::remove_callback_group_from_map(
+  rclcpp::CallbackGroup::SharedPtr group_ptr,
+  WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
+  bool notify)
+{
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr;
+  rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
+  auto iter = weak_groups_to_nodes.find(weak_group_ptr);
+  if (iter != weak_groups_to_nodes.end()) {
+    node_ptr = iter->second.lock();
+    if (node_ptr == nullptr) {
+      throw std::runtime_error("Node must not be deleted before its callback group(s).");
     }
+    weak_groups_to_nodes.erase(iter);
+    weak_groups_to_nodes_.erase(group_ptr);
+    std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic();
+    has_executor.store(false);
+  } else {
+    throw std::runtime_error("Callback group needs to be associated with executor.");
   }
-  // Add the node's notify condition to the guard condition handles
-  std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
-  memory_strategy_->add_guard_condition(node_ptr->get_notify_guard_condition());
+  // If the node was matched and removed, interrupt waiting.
+  if (!has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) &&
+    !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_))
+  {
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr);
+    weak_nodes_to_guard_conditions_.erase(node_weak_ptr);
+    if (notify) {
+      if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+        throw std::runtime_error(rcl_get_error_string().str);
+      }
+    }
+    std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
+    memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition());
+  }
+}
+
+void
+Executor::remove_callback_group(
+  rclcpp::CallbackGroup::SharedPtr group_ptr,
+  bool notify)
+{
+  this->remove_callback_group_from_map(
+    group_ptr,
+    weak_groups_associated_with_executor_to_nodes_,
+    notify);
 }
 
 void
@@ -153,34 +322,47 @@ Executor::add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify)
 void
 Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
 {
-  bool node_removed = false;
-  {
-    auto node_it = weak_nodes_.begin();
-    auto gc_it = guard_conditions_.begin();
-    while (node_it != weak_nodes_.end()) {
-      bool matched = (node_it->lock() == node_ptr);
-      if (matched) {
-        node_it = weak_nodes_.erase(node_it);
-        gc_it = guard_conditions_.erase(gc_it);
-        node_removed = true;
-      } else {
-        ++node_it;
-        ++gc_it;
-      }
+  if (!node_ptr->get_associated_with_executor_atomic().load()) {
+    throw std::runtime_error("Node needs to be associated with an executor.");
+  }
+
+  bool found_node = false;
+  auto node_it = weak_nodes_.begin();
+  while (node_it != weak_nodes_.end()) {
+    bool matched = (node_it->lock() == node_ptr);
+    if (matched) {
+      found_node = true;
+      node_it = weak_nodes_.erase(node_it);
+    } else {
+      ++node_it;
     }
   }
+  if (!found_node) {
+    throw std::runtime_error("Node needs to be associated with this executor.");
+  }
+  std::vector<rclcpp::CallbackGroup::SharedPtr> found_group_ptrs;
+  std::for_each(
+    weak_groups_to_nodes_associated_with_executor_.begin(),
+    weak_groups_to_nodes_associated_with_executor_.end(),
+    [&found_group_ptrs, node_ptr](std::pair<rclcpp::CallbackGroup::WeakPtr,
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> key_value_pair) {
+      auto weak_node_ptr = key_value_pair.second;
+      auto shared_node_ptr = weak_node_ptr.lock();
+      auto group_ptr = key_value_pair.first.lock();
+      if (shared_node_ptr == node_ptr) {
+        found_group_ptrs.push_back(group_ptr);
+      }
+    });
+  std::for_each(
+    found_group_ptrs.begin(), found_group_ptrs.end(), [this, notify]
+      (rclcpp::CallbackGroup::SharedPtr group_ptr) {
+      remove_callback_group_from_map(
+        group_ptr,
+        weak_groups_to_nodes_associated_with_executor_,
+        notify);
+    });
   std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
   has_executor.store(false);
-  if (notify) {
-    // If the node was matched and removed, interrupt waiting
-    if (node_removed) {
-      if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-        throw std::runtime_error(rcl_get_error_string().str);
-      }
-    }
-  }
-  std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
-  memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition());
 }
 
 void
@@ -473,25 +655,46 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
   {
     std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
 
+    // Check weak_nodes_ to find any callback group that is not owned
+    // by an executor and add it to the list of callbackgroups for
+    // collect entities. Also exchange to false so it is not
+    // allowed to add to another executor
+    add_callback_groups_from_nodes_associated_to_executor();
+
     // Collect the subscriptions and timers to be waited on
     memory_strategy_->clear_handles();
-    bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_);
+    bool has_invalid_weak_groups_or_nodes =
+      memory_strategy_->collect_entities(weak_groups_to_nodes_);
 
-    // Clean up any invalid nodes, if they were detected
-    if (has_invalid_weak_nodes) {
-      auto node_it = weak_nodes_.begin();
-      auto gc_it = guard_conditions_.begin();
-      while (node_it != weak_nodes_.end()) {
-        if (node_it->expired()) {
-          node_it = weak_nodes_.erase(node_it);
-          memory_strategy_->remove_guard_condition(*gc_it);
-          gc_it = guard_conditions_.erase(gc_it);
-        } else {
-          ++node_it;
-          ++gc_it;
+    if (has_invalid_weak_groups_or_nodes) {
+      std::vector<rclcpp::CallbackGroup::WeakPtr> invalid_group_ptrs;
+      for (auto pair : weak_groups_to_nodes_) {
+        auto weak_group_ptr = pair.first;
+        auto weak_node_ptr = pair.second;
+        if (weak_group_ptr.expired() || weak_node_ptr.expired()) {
+          invalid_group_ptrs.push_back(weak_group_ptr);
+          auto node_guard_pair = weak_nodes_to_guard_conditions_.find(weak_node_ptr);
+          weak_nodes_to_guard_conditions_.erase(weak_node_ptr);
+          memory_strategy_->remove_guard_condition(node_guard_pair->second);
         }
       }
+      std::for_each(
+        invalid_group_ptrs.begin(), invalid_group_ptrs.end(),
+        [this](rclcpp::CallbackGroup::WeakPtr group_ptr) {
+          if (weak_groups_to_nodes_associated_with_executor_.find(group_ptr) !=
+          weak_groups_to_nodes_associated_with_executor_.end())
+          {
+            weak_groups_to_nodes_associated_with_executor_.erase(group_ptr);
+          }
+          if (weak_groups_associated_with_executor_to_nodes_.find(group_ptr) !=
+          weak_groups_associated_with_executor_to_nodes_.end())
+          {
+            weak_groups_associated_with_executor_to_nodes_.erase(group_ptr);
+          }
+          weak_groups_to_nodes_.erase(group_ptr);
+        });
     }
+
     // clear wait set
     if (rcl_wait_set_clear(&wait_set_) != RCL_RET_OK) {
       throw std::runtime_error("Couldn't clear wait set");
@@ -512,6 +715,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
       throw std::runtime_error("Couldn't fill wait set");
     }
   }
+
   rcl_ret_t status =
     rcl_wait(&wait_set_, std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count());
   if (status == RCL_RET_WAIT_SET_EMPTY) {
@@ -529,22 +733,18 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
 }
 
 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
-Executor::get_node_by_group(rclcpp::CallbackGroup::SharedPtr group)
+Executor::get_node_by_group(
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes,
+  rclcpp::CallbackGroup::SharedPtr group)
 {
   if (!group) {
     return nullptr;
   }
-  for (auto & weak_node : weak_nodes_) {
-    auto node = weak_node.lock();
-    if (!node) {
-      continue;
-    }
-    for (auto & weak_group : node->get_callback_groups()) {
-      auto callback_group = weak_group.lock();
-      if (callback_group == group) {
-        return node;
-      }
-    }
+  rclcpp::CallbackGroup::WeakPtr weak_group_ptr(group);
+  const auto finder = weak_groups_to_nodes.find(weak_group_ptr);
+  if (finder != weak_groups_to_nodes.end()) {
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr = finder->second.lock();
+    return node_ptr;
   }
   return nullptr;
 }
@@ -552,80 +752,103 @@ Executor::get_node_by_group(rclcpp::CallbackGroup::SharedPtr group)
 rclcpp::CallbackGroup::SharedPtr
 Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer)
 {
-  for (auto & weak_node : weak_nodes_) {
-    auto node = weak_node.lock();
-    if (!node) {
+  for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) {
+    auto group = pair.first.lock();
+    if (!group) {
       continue;
     }
-    for (auto & weak_group : node->get_callback_groups()) {
-      auto group = weak_group.lock();
-      if (!group) {
-        continue;
-      }
-      auto timer_ref = group->find_timer_ptrs_if(
-        [timer](const rclcpp::TimerBase::SharedPtr & timer_ptr) -> bool {
-          return timer_ptr == timer;
-        });
-      if (timer_ref) {
-        return group;
-      }
+    auto timer_ref = group->find_timer_ptrs_if(
+      [timer](const rclcpp::TimerBase::SharedPtr & timer_ptr) -> bool {
+        return timer_ptr == timer;
+      });
+    if (timer_ref) {
+      return group;
     }
   }
-  return rclcpp::CallbackGroup::SharedPtr();
+
+  for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) {
+    auto group = pair.first.lock();
+    if (!group) {
+      continue;
+    }
+    auto timer_ref = group->find_timer_ptrs_if(
+      [timer](const rclcpp::TimerBase::SharedPtr & timer_ptr) -> bool {
+        return timer_ptr == timer;
+      });
+    if (timer_ref) {
+      return group;
+    }
+  }
+  return nullptr;
 }
 
 bool
 Executor::get_next_ready_executable(AnyExecutable & any_executable)
+{
+  bool success = get_next_ready_executable_from_map(any_executable, weak_groups_to_nodes_);
+  return success;
+}
+
+bool
+Executor::get_next_ready_executable_from_map(
+  AnyExecutable & any_executable,
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes)
 {
   bool success = false;
   // Check the timers to see if there are any that are ready
-  memory_strategy_->get_next_timer(any_executable, weak_nodes_);
+  memory_strategy_->get_next_timer(any_executable, weak_groups_to_nodes);
   if (any_executable.timer) {
     success = true;
   }
   if (!success) {
     // Check the subscriptions to see if there are any that are ready
-    memory_strategy_->get_next_subscription(any_executable, weak_nodes_);
+    memory_strategy_->get_next_subscription(any_executable, weak_groups_to_nodes);
     if (any_executable.subscription) {
       success = true;
     }
   }
   if (!success) {
     // Check the services to see if there are any that are ready
-    memory_strategy_->get_next_service(any_executable, weak_nodes_);
+    memory_strategy_->get_next_service(any_executable, weak_groups_to_nodes);
     if (any_executable.service) {
       success = true;
     }
   }
   if (!success) {
     // Check the clients to see if there are any that are ready
-    memory_strategy_->get_next_client(any_executable, weak_nodes_);
+    memory_strategy_->get_next_client(any_executable, weak_groups_to_nodes);
     if (any_executable.client) {
       success = true;
     }
   }
   if (!success) {
     // Check the waitables to see if there are any that are ready
-    memory_strategy_->get_next_waitable(any_executable, weak_nodes_);
+    memory_strategy_->get_next_waitable(any_executable, weak_groups_to_nodes);
     if (any_executable.waitable) {
       success = true;
     }
   }
-  // At this point any_exec should be valid with either a valid subscription
+  // At this point any_executable should be valid with either a valid subscription
   // or a valid timer, or it should be a null shared_ptr
+  if (success) {
+    rclcpp::CallbackGroup::WeakPtr weak_group_ptr = any_executable.callback_group;
+    auto iter = weak_groups_to_nodes.find(weak_group_ptr);
+    if (iter == weak_groups_to_nodes.end()) {
+      success = false;
+    }
+  }
+
   if (success) {
     // If it is valid, check to see if the group is mutually exclusive or
-    // not, then mark it accordingly
-    using callback_group::CallbackGroupType;
-    if (
-      any_executable.callback_group &&
-      any_executable.callback_group->type() == CallbackGroupType::MutuallyExclusive)
+    // not, then mark it accordingly ..Check if the callback_group belongs to this executor
+    if (any_executable.callback_group && any_executable.callback_group->type() == \
+      CallbackGroupType::MutuallyExclusive)
     {
       // It should not have been taken otherwise
       assert(any_executable.callback_group->can_be_taken_from().load());
       // Set to false to indicate something is being run from this group
-      // This is reset to true either when the any_exec is executed or when the
-      // any_exec is destructued
+      // This is reset to true either when the any_executable is executed or when the
+      // any_executable is destructued
       any_executable.callback_group->can_be_taken_from().store(false);
     }
   }
@@ -652,3 +875,18 @@ Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanos
   }
   return success;
 }
+
+// Returns true iff the weak_groups_to_nodes map has node_ptr as the value in any of its entry.
+bool
+Executor::has_node(
+  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes) const
+{
+  return std::find_if(
+    weak_groups_to_nodes.begin(),
+    weak_groups_to_nodes.end(),
+    [&](const WeakCallbackGroupsToNodesMap::value_type & other) -> bool {
+      auto other_ptr = other.second.lock();
+      return other_ptr == node_ptr;
+    }) != weak_groups_to_nodes.end();
+}

rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp

@@ -18,6 +18,8 @@
 #include <memory>
 #include <stdexcept>
 #include <string>
+#include <utility>
+#include <vector>
 
 #include "rclcpp/memory_strategy.hpp"
 #include "rclcpp/executors/static_single_threaded_executor.hpp"
@@ -26,6 +28,21 @@ using rclcpp::executors::StaticExecutorEntitiesCollector;
 
 StaticExecutorEntitiesCollector::~StaticExecutorEntitiesCollector()
 {
+  // Disassocate all callback groups and thus nodes.
+  for (auto & pair : weak_groups_associated_with_executor_to_nodes_) {
+    auto group = pair.first.lock();
+    if (group) {
+      std::atomic_bool & has_executor = group->get_associated_with_executor_atomic();
+      has_executor.store(false);
+    }
+  }
+  for (auto & pair : weak_groups_to_nodes_associated_with_executor_) {
+    auto group = pair.first.lock();
+    if (group) {
+      std::atomic_bool & has_executor = group->get_associated_with_executor_atomic();
+      has_executor.store(false);
+    }
+  }
   // Disassociate all nodes
   for (auto & weak_node : weak_nodes_) {
     auto node = weak_node.lock();
@@ -34,9 +51,11 @@ StaticExecutorEntitiesCollector::~StaticExecutorEntitiesCollector()
       has_executor.store(false);
     }
   }
+  weak_groups_associated_with_executor_to_nodes_.clear();
+  weak_groups_to_nodes_associated_with_executor_.clear();
   exec_list_.clear();
   weak_nodes_.clear();
-  guard_conditions_.clear();
+  weak_nodes_to_guard_conditions_.clear();
 }
 
 void
@@ -56,8 +75,7 @@ StaticExecutorEntitiesCollector::init(
   memory_strategy_ = memory_strategy;
 
   // Add executor's guard condition
-  guard_conditions_.push_back(executor_guard_condition);
-
+  memory_strategy_->add_guard_condition(executor_guard_condition);
   // Get memory strategy and executable list. Prepare wait_set_
   execute();
 }
@@ -77,18 +95,41 @@ void
 StaticExecutorEntitiesCollector::fill_memory_strategy()
 {
   memory_strategy_->clear_handles();
-  bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_);
-
+  bool has_invalid_weak_groups_or_nodes =
+    memory_strategy_->collect_entities(weak_groups_to_nodes_associated_with_executor_);
   // Clean up any invalid nodes, if they were detected
-  if (has_invalid_weak_nodes) {
-    auto node_it = weak_nodes_.begin();
-    while (node_it != weak_nodes_.end()) {
-      if (node_it->expired()) {
-        node_it = weak_nodes_.erase(node_it);
-      } else {
-        ++node_it;
+  if (has_invalid_weak_groups_or_nodes) {
+    std::vector<rclcpp::CallbackGroup::WeakPtr> invalid_group_ptrs;
+    for (auto & pair : weak_groups_to_nodes_associated_with_executor_) {
+      auto & weak_group_ptr = pair.first;
+      auto & weak_node_ptr = pair.second;
+      if (weak_group_ptr.expired() || weak_node_ptr.expired()) {
+        invalid_group_ptrs.push_back(weak_group_ptr);
       }
     }
+    std::for_each(
+      invalid_group_ptrs.begin(), invalid_group_ptrs.end(),
+      [this](rclcpp::CallbackGroup::WeakPtr group_ptr) {
+        weak_groups_to_nodes_associated_with_executor_.erase(group_ptr);
+      });
+  }
+  has_invalid_weak_groups_or_nodes =
+    memory_strategy_->collect_entities(weak_groups_associated_with_executor_to_nodes_);
+  // Clean up any invalid nodes, if they were detected
+  if (has_invalid_weak_groups_or_nodes) {
+    std::vector<rclcpp::CallbackGroup::WeakPtr> invalid_group_ptrs;
+    for (auto & pair : weak_groups_associated_with_executor_to_nodes_) {
+      auto & weak_group_ptr = pair.first;
+      auto & weak_node_ptr = pair.second;
+      if (weak_group_ptr.expired() || weak_node_ptr.expired()) {
+        invalid_group_ptrs.push_back(weak_group_ptr);
+      }
+    }
+    std::for_each(
+      invalid_group_ptrs.begin(), invalid_group_ptrs.end(),
+      [this](rclcpp::CallbackGroup::WeakPtr group_ptr) {
+        weak_groups_associated_with_executor_to_nodes_.erase(group_ptr);
+      });
   }
 
   // Add the static executor waitable to the memory strategy
@@ -99,60 +140,59 @@ void
 StaticExecutorEntitiesCollector::fill_executable_list()
 {
   exec_list_.clear();
-
-  for (auto & weak_node : weak_nodes_) {
-    auto node = weak_node.lock();
-    if (!node) {
-      continue;
-    }
-    // Check in all the callback groups
-    for (auto & weak_group : node->get_callback_groups()) {
-      auto group = weak_group.lock();
-      if (!group || !group->can_be_taken_from().load()) {
-        continue;
-      }
-
-      group->find_timer_ptrs_if(
-        [this](const rclcpp::TimerBase::SharedPtr & timer) {
-          if (timer) {
-            exec_list_.add_timer(timer);
-          }
-          return false;
-        });
-      group->find_subscription_ptrs_if(
-        [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
-          if (subscription) {
-            exec_list_.add_subscription(subscription);
-          }
-          return false;
-        });
-      group->find_service_ptrs_if(
-        [this](const rclcpp::ServiceBase::SharedPtr & service) {
-          if (service) {
-            exec_list_.add_service(service);
-          }
-          return false;
-        });
-      group->find_client_ptrs_if(
-        [this](const rclcpp::ClientBase::SharedPtr & client) {
-          if (client) {
-            exec_list_.add_client(client);
-          }
-          return false;
-        });
-      group->find_waitable_ptrs_if(
-        [this](const rclcpp::Waitable::SharedPtr & waitable) {
-          if (waitable) {
-            exec_list_.add_waitable(waitable);
-          }
-          return false;
-        });
-    }
-  }
-
+  add_callback_groups_from_nodes_associated_to_executor();
+  fill_executable_list_from_map(weak_groups_associated_with_executor_to_nodes_);
+  fill_executable_list_from_map(weak_groups_to_nodes_associated_with_executor_);
   // Add the executor's waitable to the executable list
   exec_list_.add_waitable(shared_from_this());
 }
+void
+StaticExecutorEntitiesCollector::fill_executable_list_from_map(
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes)
+{
+  for (const auto & pair : weak_groups_to_nodes) {
+    auto group = pair.first.lock();
+    auto node = pair.second.lock();
+    if (!node || !group || !group->can_be_taken_from().load()) {
+      continue;
+    }
+    group->find_timer_ptrs_if(
+      [this](const rclcpp::TimerBase::SharedPtr & timer) {
+        if (timer) {
+          exec_list_.add_timer(timer);
+        }
+        return false;
+      });
+    group->find_subscription_ptrs_if(
+      [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
+        if (subscription) {
+          exec_list_.add_subscription(subscription);
+        }
+        return false;
+      });
+    group->find_service_ptrs_if(
+      [this](const rclcpp::ServiceBase::SharedPtr & service) {
+        if (service) {
+          exec_list_.add_service(service);
+        }
+        return false;
+      });
+    group->find_client_ptrs_if(
+      [this](const rclcpp::ClientBase::SharedPtr & client) {
+        if (client) {
+          exec_list_.add_client(client);
+        }
+        return false;
+      });
+    group->find_waitable_ptrs_if(
+      [this](const rclcpp::Waitable::SharedPtr & waitable) {
+        if (waitable) {
+          exec_list_.add_waitable(waitable);
+        }
+        return false;
+      });
+  }
+}
 
 void
 StaticExecutorEntitiesCollector::prepare_wait_set()
@@ -205,7 +245,8 @@ bool
 StaticExecutorEntitiesCollector::add_to_wait_set(rcl_wait_set_t * wait_set)
 {
   // Add waitable guard conditions (one for each registered node) into the wait set.
-  for (const auto & gc : guard_conditions_) {
+  for (const auto & pair : weak_nodes_to_guard_conditions_) {
+    auto & gc = pair.second;
     rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, gc, NULL);
     if (ret != RCL_RET_OK) {
       throw std::runtime_error("Executor waitable: couldn't add guard condition to wait set");
@@ -216,55 +257,152 @@ StaticExecutorEntitiesCollector::add_to_wait_set(rcl_wait_set_t * wait_set)
 
 size_t StaticExecutorEntitiesCollector::get_number_of_ready_guard_conditions()
 {
-  return guard_conditions_.size();
+  return weak_nodes_to_guard_conditions_.size();
 }
 
-void
+bool
 StaticExecutorEntitiesCollector::add_node(
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
 {
-  // Check to ensure node not already added
-  for (auto & weak_node : weak_nodes_) {
-    auto node = weak_node.lock();
-    if (node == node_ptr) {
-      // TODO(jacquelinekay): Use a different error here?
-      throw std::runtime_error("Cannot add node to executor, node already added.");
+  bool is_new_node = false;
+  // If the node already has an executor
+  std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
+  if (has_executor.exchange(true)) {
+    throw std::runtime_error("Node has already been added to an executor.");
+  }
+  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())
+    {
+      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);
-  guard_conditions_.push_back(node_ptr->get_notify_guard_condition());
+  return is_new_node;
+}
+
+bool
+StaticExecutorEntitiesCollector::add_callback_group(
+  rclcpp::CallbackGroup::SharedPtr group_ptr,
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
+{
+  // If the callback_group already has an executor
+  std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic();
+  if (has_executor.exchange(true)) {
+    throw std::runtime_error("Callback group has already been added to an executor.");
+  }
+  rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
+  auto insert_info = weak_groups_to_nodes.insert(
+    std::make_pair(weak_group_ptr, node_ptr));
+  bool was_inserted = insert_info.second;
+  if (!was_inserted) {
+    throw std::runtime_error("Callback group was already added to executor.");
+  }
+  bool is_new_node = !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_) &&
+    !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_);
+  if (is_new_node) {
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr);
+    weak_nodes_to_guard_conditions_[node_weak_ptr] = node_ptr->get_notify_guard_condition();
+    return true;
+  }
+  return false;
+}
+
+bool
+StaticExecutorEntitiesCollector::add_callback_group(
+  rclcpp::CallbackGroup::SharedPtr group_ptr,
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
+{
+  return add_callback_group(group_ptr, node_ptr, weak_groups_associated_with_executor_to_nodes_);
+}
+
+bool
+StaticExecutorEntitiesCollector::remove_callback_group(
+  rclcpp::CallbackGroup::SharedPtr group_ptr)
+{
+  return this->remove_callback_group_from_map(
+    group_ptr,
+    weak_groups_associated_with_executor_to_nodes_);
+}
+
+bool
+StaticExecutorEntitiesCollector::remove_callback_group_from_map(
+  rclcpp::CallbackGroup::SharedPtr group_ptr,
+  WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
+{
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr;
+  rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
+  auto iter = weak_groups_to_nodes.find(weak_group_ptr);
+  if (iter != weak_groups_to_nodes.end()) {
+    node_ptr = iter->second.lock();
+    if (node_ptr == nullptr) {
+      throw std::runtime_error("Node must not be deleted before its callback group(s).");
+    }
+    weak_groups_to_nodes.erase(iter);
+  } else {
+    throw std::runtime_error("Callback group needs to be associated with executor.");
+  }
+  // If the node was matched and removed, interrupt waiting.
+  if (!has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_) &&
+    !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_))
+  {
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr);
+    weak_nodes_to_guard_conditions_.erase(node_weak_ptr);
+    return true;
+  }
+  return false;
 }
 
 bool
 StaticExecutorEntitiesCollector::remove_node(
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
 {
+  if (!node_ptr->get_associated_with_executor_atomic().load()) {
+    return false;
+  }
+  bool node_found = false;
   auto node_it = weak_nodes_.begin();
-
   while (node_it != weak_nodes_.end()) {
     bool matched = (node_it->lock() == node_ptr);
     if (matched) {
-      // Find and remove node and its guard condition
-      auto gc_it = std::find(
-        guard_conditions_.begin(),
-        guard_conditions_.end(),
-        node_ptr->get_notify_guard_condition());
-
-      if (gc_it != guard_conditions_.end()) {
-        guard_conditions_.erase(gc_it);
-        weak_nodes_.erase(node_it);
-        return true;
-      }
-
-      throw std::runtime_error("Didn't find guard condition associated with node.");
-
-    } else {
-      ++node_it;
+      weak_nodes_.erase(node_it);
+      node_found = true;
+      break;
     }
+    ++node_it;
   }
-
-  return false;
+  if (!node_found) {
+    return false;
+  }
+  std::vector<rclcpp::CallbackGroup::SharedPtr> found_group_ptrs;
+  std::for_each(
+    weak_groups_to_nodes_associated_with_executor_.begin(),
+    weak_groups_to_nodes_associated_with_executor_.end(),
+    [&found_group_ptrs, node_ptr](std::pair<rclcpp::CallbackGroup::WeakPtr,
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> key_value_pair) {
+      auto & weak_node_ptr = key_value_pair.second;
+      auto shared_node_ptr = weak_node_ptr.lock();
+      auto group_ptr = key_value_pair.first.lock();
+      if (shared_node_ptr == node_ptr) {
+        found_group_ptrs.push_back(group_ptr);
+      }
+    });
+  std::for_each(
+    found_group_ptrs.begin(), found_group_ptrs.end(), [this]
+      (rclcpp::CallbackGroup::SharedPtr group_ptr) {
+      this->remove_callback_group_from_map(
+        group_ptr,
+        weak_groups_to_nodes_associated_with_executor_);
+    });
+  std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
+  has_executor.store(false);
+  return true;
 }
 
 bool
@@ -273,13 +411,13 @@ StaticExecutorEntitiesCollector::is_ready(rcl_wait_set_t * p_wait_set)
   // Check wait_set guard_conditions for added/removed entities to/from a node
   for (size_t i = 0; i < p_wait_set->size_of_guard_conditions; ++i) {
     if (p_wait_set->guard_conditions[i] != NULL) {
-      // Check if the guard condition triggered belongs to a node
-      auto it = std::find(
-        guard_conditions_.begin(), guard_conditions_.end(),
-        p_wait_set->guard_conditions[i]);
-
-      // If it does, we are ready to re-collect entities
-      if (it != guard_conditions_.end()) {
+      auto found_guard_condition = std::find_if(
+        weak_nodes_to_guard_conditions_.begin(), weak_nodes_to_guard_conditions_.end(),
+        [&](std::pair<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+        const rcl_guard_condition_t *> pair) -> bool {
+          return pair.second == p_wait_set->guard_conditions[i];
+        });
+      if (found_guard_condition != weak_nodes_to_guard_conditions_.end()) {
         return true;
       }
     }
@@ -287,3 +425,76 @@ StaticExecutorEntitiesCollector::is_ready(rcl_wait_set_t * p_wait_set)
   // None of the guard conditions triggered belong to a registered node
   return false;
 }
+
+// Returns true iff the weak_groups_to_nodes map has node_ptr as the value in any of its entry.
+bool
+StaticExecutorEntitiesCollector::has_node(
+  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes) const
+{
+  return std::find_if(
+    weak_groups_to_nodes.begin(),
+    weak_groups_to_nodes.end(),
+    [&](const WeakCallbackGroupsToNodesMap::value_type & other) -> bool {
+      auto other_ptr = other.second.lock();
+      return other_ptr == node_ptr;
+    }) != weak_groups_to_nodes.end();
+}
+
+void
+StaticExecutorEntitiesCollector::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)
+        {
+          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())
+          {
+            add_callback_group(
+              shared_group_ptr,
+              node,
+              weak_groups_to_nodes_associated_with_executor_);
+          }
+        });
+    }
+  }
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+StaticExecutorEntitiesCollector::get_all_callback_groups()
+{
+  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
+  for (auto const & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
+    groups.push_back(group_node_ptr.first);
+  }
+  for (auto const & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) {
+    groups.push_back(group_node_ptr.first);
+  }
+  return groups;
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+StaticExecutorEntitiesCollector::get_manually_added_callback_groups()
+{
+  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
+  for (auto const & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
+    groups.push_back(group_node_ptr.first);
+  }
+  return groups;
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+StaticExecutorEntitiesCollector::get_automatically_added_callback_groups_from_nodes()
+{
+  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
+  for (auto const & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) {
+    groups.push_back(group_node_ptr.first);
+  }
+  return groups;
+}

rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp

@@ -15,6 +15,7 @@
 #include "rclcpp/executors/static_single_threaded_executor.hpp"
 
 #include <memory>
+#include <vector>
 
 #include "rclcpp/scope_exit.hpp"
 
@@ -50,23 +51,31 @@ StaticSingleThreadedExecutor::spin()
 }
 
 void
-StaticSingleThreadedExecutor::add_node(
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
+StaticSingleThreadedExecutor::add_callback_group(
+  rclcpp::CallbackGroup::SharedPtr group_ptr,
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  bool notify)
 {
-  // If the node already has an executor
-  std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
-  if (has_executor.exchange(true)) {
-    throw std::runtime_error("Node has already been added to an executor.");
-  }
-
-  if (notify) {
+  bool is_new_node = entities_collector_->add_callback_group(group_ptr, node_ptr);
+  if (is_new_node && notify) {
     // Interrupt waiting to handle new node
     if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
       throw std::runtime_error(rcl_get_error_string().str);
     }
   }
+}
 
-  entities_collector_->add_node(node_ptr);
+void
+StaticSingleThreadedExecutor::add_node(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
+{
+  bool is_new_node = entities_collector_->add_node(node_ptr);
+  if (is_new_node && notify) {
+    // Interrupt waiting to handle new node
+    if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+      throw std::runtime_error(rcl_get_error_string().str);
+    }
+  }
 }
 
 void
@@ -75,23 +84,51 @@ StaticSingleThreadedExecutor::add_node(std::shared_ptr<rclcpp::Node> node_ptr, b
   this->add_node(node_ptr->get_node_base_interface(), notify);
 }
 
+void
+StaticSingleThreadedExecutor::remove_callback_group(
+  rclcpp::CallbackGroup::SharedPtr group_ptr, bool notify)
+{
+  bool node_removed = entities_collector_->remove_callback_group(group_ptr);
+  // If the node was matched and removed, interrupt waiting
+  if (node_removed && notify) {
+    if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+      throw std::runtime_error(rcl_get_error_string().str);
+    }
+  }
+}
+
 void
 StaticSingleThreadedExecutor::remove_node(
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
 {
   bool node_removed = entities_collector_->remove_node(node_ptr);
-
+  if (!node_removed) {
+    throw std::runtime_error("Node needs to be associated with this executor.");
+  }
+  // If the node was matched and removed, interrupt waiting
   if (notify) {
-    // If the node was matched and removed, interrupt waiting
-    if (node_removed) {
-      if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-        throw std::runtime_error(rcl_get_error_string().str);
-      }
+    if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+      throw std::runtime_error(rcl_get_error_string().str);
     }
   }
+}
 
-  std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
-  has_executor.store(false);
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+StaticSingleThreadedExecutor::get_all_callback_groups()
+{
+  return entities_collector_->get_all_callback_groups();
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+StaticSingleThreadedExecutor::get_manually_added_callback_groups()
+{
+  return entities_collector_->get_manually_added_callback_groups();
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+StaticSingleThreadedExecutor::get_automatically_added_callback_groups_from_nodes()
+{
+  return entities_collector_->get_automatically_added_callback_groups_from_nodes();
 }
 
 void

rclcpp/src/rclcpp/init_options.cpp

@@ -44,6 +44,7 @@ InitOptions::InitOptions(const InitOptions & other)
 : InitOptions(*other.get_rcl_init_options())
 {
   shutdown_on_sigint = other.shutdown_on_sigint;
+  initialize_logging_ = other.initialize_logging_;
 }
 
 bool
@@ -63,12 +64,14 @@ InitOptions &
 InitOptions::operator=(const InitOptions & other)
 {
   if (this != &other) {
-    this->finalize_init_options();
+    std::lock_guard<std::mutex> init_options_lock(init_options_mutex_);
+    this->finalize_init_options_impl();
     rcl_ret_t ret = rcl_init_options_copy(other.get_rcl_init_options(), init_options_.get());
     if (RCL_RET_OK != ret) {
       rclcpp::exceptions::throw_from_rcl_error(ret, "failed to copy rcl init options");
     }
     this->shutdown_on_sigint = other.shutdown_on_sigint;
+    this->initialize_logging_ = other.initialize_logging_;
   }
   return *this;
 }
@@ -80,6 +83,13 @@ InitOptions::~InitOptions()
 
 void
 InitOptions::finalize_init_options()
+{
+  std::lock_guard<std::mutex> init_options_lock(init_options_mutex_);
+  this->finalize_init_options_impl();
+}
+
+void
+InitOptions::finalize_init_options_impl()
 {
   if (init_options_) {
     rcl_ret_t ret = rcl_init_options_fini(init_options_.get());
@@ -99,4 +109,38 @@ InitOptions::get_rcl_init_options() const
   return init_options_.get();
 }
 
+void
+InitOptions::use_default_domain_id()
+{
+  size_t domain_id = RCL_DEFAULT_DOMAIN_ID;
+  rcl_ret_t ret = rcl_get_default_domain_id(&domain_id);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get default domain id");
+  }
+  set_domain_id(domain_id);
+}
+
+void
+InitOptions::set_domain_id(size_t domain_id)
+{
+  std::lock_guard<std::mutex> init_options_lock(init_options_mutex_);
+  rcl_ret_t ret = rcl_init_options_set_domain_id(init_options_.get(), domain_id);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to set domain id to rcl init options");
+  }
+}
+
+size_t
+InitOptions::get_domain_id() const
+{
+  std::lock_guard<std::mutex> init_options_lock(init_options_mutex_);
+  size_t domain_id;
+  rcl_ret_t ret = rcl_init_options_get_domain_id(init_options_.get(), &domain_id);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get domain id from rcl init options");
+  }
+
+  return domain_id;
+}
+
 }  // namespace rclcpp

rclcpp/src/rclcpp/logger.cpp

@@ -14,8 +14,8 @@
 
 #include <string>
 
+#include "rclcpp/exceptions.hpp"
 #include "rclcpp/logger.hpp"
-
 #include "rclcpp/logging.hpp"
 
 namespace rclcpp
@@ -44,4 +44,22 @@ get_node_logger(const rcl_node_t * node)
   return rclcpp::get_logger(logger_name);
 }
 
+void
+Logger::set_level(Level level)
+{
+  rcutils_ret_t rcutils_ret = rcutils_logging_set_logger_level(
+    get_name(),
+    static_cast<RCUTILS_LOG_SEVERITY>(level));
+  if (rcutils_ret != RCUTILS_RET_OK) {
+    if (rcutils_ret == RCUTILS_RET_INVALID_ARGUMENT) {
+      exceptions::throw_from_rcl_error(
+        RCL_RET_INVALID_ARGUMENT, "Invalid parameter",
+        rcutils_get_error_state(), rcutils_reset_error);
+    }
+    exceptions::throw_from_rcl_error(
+      RCL_RET_ERROR, "Couldn't set logger level",
+      rcutils_get_error_state(), rcutils_reset_error);
+  }
+}
+
 }  // namespace rclcpp

rclcpp/src/rclcpp/memory_strategy.cpp

@@ -20,25 +20,19 @@ using rclcpp::memory_strategy::MemoryStrategy;
 rclcpp::SubscriptionBase::SharedPtr
 MemoryStrategy::get_subscription_by_handle(
   std::shared_ptr<const rcl_subscription_t> subscriber_handle,
-  const WeakNodeList & weak_nodes)
+  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
 {
-  for (auto & weak_node : weak_nodes) {
-    auto node = weak_node.lock();
-    if (!node) {
+  for (const auto & pair : weak_groups_to_nodes) {
+    auto group = pair.first.lock();
+    if (!group) {
       continue;
     }
-    for (auto & weak_group : node->get_callback_groups()) {
-      auto group = weak_group.lock();
-      if (!group) {
-        continue;
-      }
-      auto match_subscription = group->find_subscription_ptrs_if(
-        [&subscriber_handle](const rclcpp::SubscriptionBase::SharedPtr & subscription) -> bool {
-          return subscription->get_subscription_handle() == subscriber_handle;
-        });
-      if (match_subscription) {
-        return match_subscription;
-      }
+    auto match_subscription = group->find_subscription_ptrs_if(
+      [&subscriber_handle](const rclcpp::SubscriptionBase::SharedPtr & subscription) -> bool {
+        return subscription->get_subscription_handle() == subscriber_handle;
+      });
+    if (match_subscription) {
+      return match_subscription;
     }
   }
   return nullptr;
@@ -47,25 +41,19 @@ MemoryStrategy::get_subscription_by_handle(
 rclcpp::ServiceBase::SharedPtr
 MemoryStrategy::get_service_by_handle(
   std::shared_ptr<const rcl_service_t> service_handle,
-  const WeakNodeList & weak_nodes)
+  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
 {
-  for (auto & weak_node : weak_nodes) {
-    auto node = weak_node.lock();
-    if (!node) {
+  for (const auto & pair : weak_groups_to_nodes) {
+    auto group = pair.first.lock();
+    if (!group) {
       continue;
     }
-    for (auto & weak_group : node->get_callback_groups()) {
-      auto group = weak_group.lock();
-      if (!group) {
-        continue;
-      }
-      auto service_ref = group->find_service_ptrs_if(
-        [&service_handle](const rclcpp::ServiceBase::SharedPtr & service) -> bool {
-          return service->get_service_handle() == service_handle;
-        });
-      if (service_ref) {
-        return service_ref;
-      }
+    auto service_ref = group->find_service_ptrs_if(
+      [&service_handle](const rclcpp::ServiceBase::SharedPtr & service) -> bool {
+        return service->get_service_handle() == service_handle;
+      });
+    if (service_ref) {
+      return service_ref;
     }
   }
   return nullptr;
@@ -74,25 +62,19 @@ MemoryStrategy::get_service_by_handle(
 rclcpp::ClientBase::SharedPtr
 MemoryStrategy::get_client_by_handle(
   std::shared_ptr<const rcl_client_t> client_handle,
-  const WeakNodeList & weak_nodes)
+  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
 {
-  for (auto & weak_node : weak_nodes) {
-    auto node = weak_node.lock();
-    if (!node) {
+  for (const auto & pair : weak_groups_to_nodes) {
+    auto group = pair.first.lock();
+    if (!group) {
       continue;
     }
-    for (auto & weak_group : node->get_callback_groups()) {
-      auto group = weak_group.lock();
-      if (!group) {
-        continue;
-      }
-      auto client_ref = group->find_client_ptrs_if(
-        [&client_handle](const rclcpp::ClientBase::SharedPtr & client) -> bool {
-          return client->get_client_handle() == client_handle;
-        });
-      if (client_ref) {
-        return client_ref;
-      }
+    auto client_ref = group->find_client_ptrs_if(
+      [&client_handle](const rclcpp::ClientBase::SharedPtr & client) -> bool {
+        return client->get_client_handle() == client_handle;
+      });
+    if (client_ref) {
+      return client_ref;
     }
   }
   return nullptr;
@@ -101,25 +83,19 @@ MemoryStrategy::get_client_by_handle(
 rclcpp::TimerBase::SharedPtr
 MemoryStrategy::get_timer_by_handle(
   std::shared_ptr<const rcl_timer_t> timer_handle,
-  const WeakNodeList & weak_nodes)
+  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
 {
-  for (auto & weak_node : weak_nodes) {
-    auto node = weak_node.lock();
-    if (!node) {
+  for (const auto & pair : weak_groups_to_nodes) {
+    auto group = pair.first.lock();
+    if (!group) {
       continue;
     }
-    for (auto & weak_group : node->get_callback_groups()) {
-      auto group = weak_group.lock();
-      if (!group) {
-        continue;
-      }
-      auto timer_ref = group->find_timer_ptrs_if(
-        [&timer_handle](const rclcpp::TimerBase::SharedPtr & timer) -> bool {
-          return timer->get_timer_handle() == timer_handle;
-        });
-      if (timer_ref) {
-        return timer_ref;
-      }
+    auto timer_ref = group->find_timer_ptrs_if(
+      [&timer_handle](const rclcpp::TimerBase::SharedPtr & timer) -> bool {
+        return timer->get_timer_handle() == timer_handle;
+      });
+    if (timer_ref) {
+      return timer_ref;
     }
   }
   return nullptr;
@@ -128,22 +104,17 @@ MemoryStrategy::get_timer_by_handle(
 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
 MemoryStrategy::get_node_by_group(
   rclcpp::CallbackGroup::SharedPtr group,
-  const WeakNodeList & weak_nodes)
+  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
 {
   if (!group) {
     return nullptr;
   }
-  for (auto & weak_node : weak_nodes) {
-    auto node = weak_node.lock();
-    if (!node) {
-      continue;
-    }
-    for (auto & weak_group : node->get_callback_groups()) {
-      auto callback_group = weak_group.lock();
-      if (callback_group == group) {
-        return node;
-      }
-    }
+
+  rclcpp::CallbackGroup::WeakPtr weak_group_ptr(group);
+  const auto finder = weak_groups_to_nodes.find(weak_group_ptr);
+  if (finder != weak_groups_to_nodes.end()) {
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr = finder->second.lock();
+    return node_ptr;
   }
   return nullptr;
 }
@@ -151,25 +122,20 @@ MemoryStrategy::get_node_by_group(
 rclcpp::CallbackGroup::SharedPtr
 MemoryStrategy::get_group_by_subscription(
   rclcpp::SubscriptionBase::SharedPtr subscription,
-  const WeakNodeList & weak_nodes)
+  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
 {
-  for (auto & weak_node : weak_nodes) {
-    auto node = weak_node.lock();
-    if (!node) {
+  for (const auto & pair : weak_groups_to_nodes) {
+    auto group = pair.first.lock();
+    auto node = pair.second.lock();
+    if (!group || !node) {
       continue;
     }
-    for (auto & weak_group : node->get_callback_groups()) {
-      auto group = weak_group.lock();
-      if (!group) {
-        continue;
-      }
-      auto match_sub = group->find_subscription_ptrs_if(
-        [&subscription](const rclcpp::SubscriptionBase::SharedPtr & sub) -> bool {
-          return sub == subscription;
-        });
-      if (match_sub) {
-        return group;
-      }
+    auto match_sub = group->find_subscription_ptrs_if(
+      [&subscription](const rclcpp::SubscriptionBase::SharedPtr & sub) -> bool {
+        return sub == subscription;
+      });
+    if (match_sub) {
+      return group;
     }
   }
   return nullptr;
@@ -178,25 +144,20 @@ MemoryStrategy::get_group_by_subscription(
 rclcpp::CallbackGroup::SharedPtr
 MemoryStrategy::get_group_by_service(
   rclcpp::ServiceBase::SharedPtr service,
-  const WeakNodeList & weak_nodes)
+  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
 {
-  for (auto & weak_node : weak_nodes) {
-    auto node = weak_node.lock();
-    if (!node) {
+  for (const auto & pair : weak_groups_to_nodes) {
+    auto group = pair.first.lock();
+    auto node = pair.second.lock();
+    if (!group || !node) {
       continue;
     }
-    for (auto & weak_group : node->get_callback_groups()) {
-      auto group = weak_group.lock();
-      if (!group) {
-        continue;
-      }
-      auto service_ref = group->find_service_ptrs_if(
-        [&service](const rclcpp::ServiceBase::SharedPtr & serv) -> bool {
-          return serv == service;
-        });
-      if (service_ref) {
-        return group;
-      }
+    auto service_ref = group->find_service_ptrs_if(
+      [&service](const rclcpp::ServiceBase::SharedPtr & serv) -> bool {
+        return serv == service;
+      });
+    if (service_ref) {
+      return group;
     }
   }
   return nullptr;
@@ -205,25 +166,20 @@ MemoryStrategy::get_group_by_service(
 rclcpp::CallbackGroup::SharedPtr
 MemoryStrategy::get_group_by_client(
   rclcpp::ClientBase::SharedPtr client,
-  const WeakNodeList & weak_nodes)
+  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
 {
-  for (auto & weak_node : weak_nodes) {
-    auto node = weak_node.lock();
-    if (!node) {
+  for (const auto & pair : weak_groups_to_nodes) {
+    auto group = pair.first.lock();
+    auto node = pair.second.lock();
+    if (!group || !node) {
       continue;
     }
-    for (auto & weak_group : node->get_callback_groups()) {
-      auto group = weak_group.lock();
-      if (!group) {
-        continue;
-      }
-      auto client_ref = group->find_client_ptrs_if(
-        [&client](const rclcpp::ClientBase::SharedPtr & cli) -> bool {
-          return cli == client;
-        });
-      if (client_ref) {
-        return group;
-      }
+    auto client_ref = group->find_client_ptrs_if(
+      [&client](const rclcpp::ClientBase::SharedPtr & cli) -> bool {
+        return cli == client;
+      });
+    if (client_ref) {
+      return group;
     }
   }
   return nullptr;
@@ -232,25 +188,20 @@ MemoryStrategy::get_group_by_client(
 rclcpp::CallbackGroup::SharedPtr
 MemoryStrategy::get_group_by_timer(
   rclcpp::TimerBase::SharedPtr timer,
-  const WeakNodeList & weak_nodes)
+  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
 {
-  for (auto & weak_node : weak_nodes) {
-    auto node = weak_node.lock();
-    if (!node) {
+  for (const auto & pair : weak_groups_to_nodes) {
+    auto group = pair.first.lock();
+    auto node = pair.second.lock();
+    if (!group || !node) {
       continue;
     }
-    for (auto & weak_group : node->get_callback_groups()) {
-      auto group = weak_group.lock();
-      if (!group) {
-        continue;
-      }
-      auto timer_ref = group->find_timer_ptrs_if(
-        [&timer](const rclcpp::TimerBase::SharedPtr & time) -> bool {
-          return time == timer;
-        });
-      if (timer_ref) {
-        return group;
-      }
+    auto timer_ref = group->find_timer_ptrs_if(
+      [&timer](const rclcpp::TimerBase::SharedPtr & time) -> bool {
+        return time == timer;
+      });
+    if (timer_ref) {
+      return group;
     }
   }
   return nullptr;
@@ -259,25 +210,20 @@ MemoryStrategy::get_group_by_timer(
 rclcpp::CallbackGroup::SharedPtr
 MemoryStrategy::get_group_by_waitable(
   rclcpp::Waitable::SharedPtr waitable,
-  const WeakNodeList & weak_nodes)
+  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
 {
-  for (auto & weak_node : weak_nodes) {
-    auto node = weak_node.lock();
-    if (!node) {
+  for (const auto & pair : weak_groups_to_nodes) {
+    auto group = pair.first.lock();
+    auto node = pair.second.lock();
+    if (!group || !node) {
       continue;
     }
-    for (auto & weak_group : node->get_callback_groups()) {
-      auto group = weak_group.lock();
-      if (!group) {
-        continue;
-      }
-      auto waitable_ref = group->find_waitable_ptrs_if(
-        [&waitable](const rclcpp::Waitable::SharedPtr & group_waitable) -> bool {
-          return group_waitable == waitable;
-        });
-      if (waitable_ref) {
-        return group;
-      }
+    auto waitable_ref = group->find_waitable_ptrs_if(
+      [&waitable](const rclcpp::Waitable::SharedPtr & group_waitable) -> bool {
+        return group_waitable == waitable;
+      });
+    if (waitable_ref) {
+      return group;
     }
   }
   return nullptr;

rclcpp/src/rclcpp/node.cpp

@@ -211,15 +211,11 @@ Node::get_logger() const
 }
 
 rclcpp::CallbackGroup::SharedPtr
-Node::create_callback_group(rclcpp::CallbackGroupType group_type)
+Node::create_callback_group(
+  rclcpp::CallbackGroupType group_type,
+  bool automatically_add_to_executor_with_node)
 {
-  return node_base_->create_callback_group(group_type);
-}
-
-bool
-Node::group_in_node(rclcpp::CallbackGroup::SharedPtr group)
-{
-  return node_base_->callback_group_in_node(group);
+  return node_base_->create_callback_group(group_type, automatically_add_to_executor_with_node);
 }
 
 const rclcpp::ParameterValue &

rclcpp/src/rclcpp/node_interfaces/node_base.cpp

@@ -216,11 +216,16 @@ NodeBase::get_shared_rcl_node_handle() const
 }
 
 rclcpp::CallbackGroup::SharedPtr
-NodeBase::create_callback_group(rclcpp::CallbackGroupType group_type)
+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));
+  auto group = CallbackGroup::SharedPtr(
+    new CallbackGroup(
+      group_type,
+      automatically_add_to_executor_with_node));
   callback_groups_.push_back(group);
   return group;
 }

rclcpp/src/rclcpp/node_interfaces/node_graph.cpp

@@ -414,7 +414,7 @@ NodeGraph::wait_for_graph_change(
 }
 
 size_t
-NodeGraph::count_graph_users()
+NodeGraph::count_graph_users() const
 {
   return graph_users_count_.load();
 }

rclcpp/src/rclcpp/parameter_client.cpp

@@ -99,36 +99,6 @@ AsyncParametersClient::AsyncParametersClient(
   node_services_interface->add_client(describe_parameters_base, group);
 }
 
-AsyncParametersClient::AsyncParametersClient(
-  const rclcpp::Node::SharedPtr node,
-  const std::string & remote_node_name,
-  const rmw_qos_profile_t & qos_profile,
-  rclcpp::CallbackGroup::SharedPtr group)
-: AsyncParametersClient(
-    node->get_node_base_interface(),
-    node->get_node_topics_interface(),
-    node->get_node_graph_interface(),
-    node->get_node_services_interface(),
-    remote_node_name,
-    qos_profile,
-    group)
-{}
-
-AsyncParametersClient::AsyncParametersClient(
-  rclcpp::Node * node,
-  const std::string & remote_node_name,
-  const rmw_qos_profile_t & qos_profile,
-  rclcpp::CallbackGroup::SharedPtr group)
-: AsyncParametersClient(
-    node->get_node_base_interface(),
-    node->get_node_topics_interface(),
-    node->get_node_graph_interface(),
-    node->get_node_services_interface(),
-    remote_node_name,
-    qos_profile,
-    group)
-{}
-
 std::shared_future<std::vector<rclcpp::Parameter>>
 AsyncParametersClient::get_parameters(
   const std::vector<std::string> & names,
@@ -337,78 +307,6 @@ AsyncParametersClient::wait_for_service_nanoseconds(std::chrono::nanoseconds tim
   return true;
 }
 
-SyncParametersClient::SyncParametersClient(
-  rclcpp::Node::SharedPtr node,
-  const std::string & remote_node_name,
-  const rmw_qos_profile_t & qos_profile)
-: SyncParametersClient(
-    std::make_shared<rclcpp::executors::SingleThreadedExecutor>(),
-    node,
-    remote_node_name,
-    qos_profile)
-{}
-
-SyncParametersClient::SyncParametersClient(
-  rclcpp::Executor::SharedPtr executor,
-  rclcpp::Node::SharedPtr node,
-  const std::string & remote_node_name,
-  const rmw_qos_profile_t & qos_profile)
-: SyncParametersClient(
-    executor,
-    node->get_node_base_interface(),
-    node->get_node_topics_interface(),
-    node->get_node_graph_interface(),
-    node->get_node_services_interface(),
-    remote_node_name,
-    qos_profile)
-{}
-
-SyncParametersClient::SyncParametersClient(
-  rclcpp::Node * node,
-  const std::string & remote_node_name,
-  const rmw_qos_profile_t & qos_profile)
-: SyncParametersClient(
-    std::make_shared<rclcpp::executors::SingleThreadedExecutor>(),
-    node,
-    remote_node_name,
-    qos_profile)
-{}
-
-SyncParametersClient::SyncParametersClient(
-  rclcpp::Executor::SharedPtr executor,
-  rclcpp::Node * node,
-  const std::string & remote_node_name,
-  const rmw_qos_profile_t & qos_profile)
-: SyncParametersClient(
-    executor,
-    node->get_node_base_interface(),
-    node->get_node_topics_interface(),
-    node->get_node_graph_interface(),
-    node->get_node_services_interface(),
-    remote_node_name,
-    qos_profile)
-{}
-
-SyncParametersClient::SyncParametersClient(
-  rclcpp::Executor::SharedPtr executor,
-  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
-  const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
-  const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
-  const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
-  const std::string & remote_node_name,
-  const rmw_qos_profile_t & qos_profile)
-: executor_(executor), node_base_interface_(node_base_interface)
-{
-  async_parameters_client_ =
-    std::make_shared<AsyncParametersClient>(
-    node_base_interface,
-    node_topics_interface,
-    node_graph_interface,
-    node_services_interface,
-    remote_node_name,
-    qos_profile);
-}
-
 std::vector<rclcpp::Parameter>
 SyncParametersClient::get_parameters(const std::vector<std::string> & parameter_names)
 {

rclcpp/src/rclcpp/time_source.cpp

@@ -185,11 +185,21 @@ void TimeSource::set_clock(
   const builtin_interfaces::msg::Time::SharedPtr msg, bool set_ros_time_enabled,
   std::shared_ptr<rclcpp::Clock> clock)
 {
+  std::lock_guard<std::mutex> clock_guard(clock->get_clock_mutex());
+
   // Do change
   if (!set_ros_time_enabled && clock->ros_time_is_active()) {
-    disable_ros_time(clock);
+    auto ret = rcl_disable_ros_time_override(clock->get_clock_handle());
+    if (ret != RCL_RET_OK) {
+      rclcpp::exceptions::throw_from_rcl_error(
+        ret, "Failed to disable ros_time_override_status");
+    }
   } else if (set_ros_time_enabled && !clock->ros_time_is_active()) {
-    enable_ros_time(clock);
+    auto ret = rcl_enable_ros_time_override(clock->get_clock_handle());
+    if (ret != RCL_RET_OK) {
+      rclcpp::exceptions::throw_from_rcl_error(
+        ret, "Failed to enable ros_time_override_status");
+    }
   }
 
   auto ret = rcl_set_ros_time_override(clock->get_clock_handle(), rclcpp::Time(*msg).nanoseconds());
@@ -227,7 +237,7 @@ void TimeSource::create_clock_sub()
   clock_subscription_ = rclcpp::create_subscription<rosgraph_msgs::msg::Clock>(
     node_topics_,
     "/clock",
-    rclcpp::QoS(QoSInitialization::from_rmw(rmw_qos_profile_default)),
+    rclcpp::QoS(KeepLast(1)).best_effort(),
     std::bind(&TimeSource::clock_cb, this, std::placeholders::_1)
   );
 }
@@ -273,24 +283,6 @@ void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::S
   }
 }
 
-void TimeSource::enable_ros_time(std::shared_ptr<rclcpp::Clock> clock)
-{
-  auto ret = rcl_enable_ros_time_override(clock->get_clock_handle());
-  if (ret != RCL_RET_OK) {
-    rclcpp::exceptions::throw_from_rcl_error(
-      ret, "Failed to enable ros_time_override_status");
-  }
-}
-
-void TimeSource::disable_ros_time(std::shared_ptr<rclcpp::Clock> clock)
-{
-  auto ret = rcl_disable_ros_time_override(clock->get_clock_handle());
-  if (ret != RCL_RET_OK) {
-    rclcpp::exceptions::throw_from_rcl_error(
-      ret, "Failed to enable ros_time_override_status");
-  }
-}
-
 void TimeSource::enable_ros_time()
 {
   if (ros_time_active_) {

rclcpp/test/CMakeLists.txt

@@ -17,6 +17,28 @@ rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs
   SKIP_INSTALL
 )
 
+# Increasing timeout because connext can take a long time to destroy nodes
+# TODO(brawner) remove when destroying Node for Connext is resolved. See:
+# https://github.com/ros2/rclcpp/issues/1250
+ament_add_gtest(
+  test_allocator_memory_strategy
+  rclcpp/strategies/test_allocator_memory_strategy.cpp
+  TIMEOUT 360)
+if(TARGET test_allocator_memory_strategy)
+  ament_target_dependencies(test_allocator_memory_strategy
+    "rcl"
+    "test_msgs"
+  )
+  target_link_libraries(test_allocator_memory_strategy ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_message_pool_memory_strategy rclcpp/strategies/test_message_pool_memory_strategy.cpp)
+if(TARGET test_message_pool_memory_strategy)
+  ament_target_dependencies(test_message_pool_memory_strategy
+    "rcl"
+    "test_msgs"
+  )
+  target_link_libraries(test_message_pool_memory_strategy ${PROJECT_NAME})
+endif()
 ament_add_gtest(test_any_service_callback rclcpp/test_any_service_callback.cpp)
 if(TARGET test_any_service_callback)
   ament_target_dependencies(test_any_service_callback
@@ -60,6 +82,13 @@ if(TARGET test_create_subscription)
     "test_msgs"
   )
 endif()
+ament_add_gtest(test_add_callback_groups_to_executor rclcpp/test_add_callback_groups_to_executor.cpp)
+if(TARGET test_add_callback_groups_to_executor)
+  target_link_libraries(test_add_callback_groups_to_executor ${PROJECT_NAME})
+  ament_target_dependencies(test_add_callback_groups_to_executor
+    "test_msgs"
+  )
+endif()
 ament_add_gtest(test_expand_topic_or_service_name rclcpp/test_expand_topic_or_service_name.cpp)
 if(TARGET test_expand_topic_or_service_name)
   ament_target_dependencies(test_expand_topic_or_service_name
@@ -118,6 +147,18 @@ ament_target_dependencies(test_loaned_message
 )
 target_link_libraries(test_loaned_message ${PROJECT_NAME})
 
+ament_add_gtest(test_memory_strategy rclcpp/test_memory_strategy.cpp)
+ament_target_dependencies(test_memory_strategy
+  "test_msgs"
+)
+target_link_libraries(test_memory_strategy ${PROJECT_NAME})
+
+ament_add_gtest(test_message_memory_strategy rclcpp/test_message_memory_strategy.cpp)
+ament_target_dependencies(test_message_memory_strategy
+  "test_msgs"
+)
+target_link_libraries(test_message_memory_strategy ${PROJECT_NAME})
+
 ament_add_gtest(test_node rclcpp/test_node.cpp TIMEOUT 240)
 if(TARGET test_node)
   ament_target_dependencies(test_node
@@ -219,6 +260,11 @@ if(TARGET test_node_options)
   ament_target_dependencies(test_node_options "rcl")
   target_link_libraries(test_node_options ${PROJECT_NAME})
 endif()
+ament_add_gtest(test_init_options rclcpp/test_init_options.cpp)
+if(TARGET test_init_options)
+  ament_target_dependencies(test_init_options "rcl")
+  target_link_libraries(test_init_options ${PROJECT_NAME})
+endif()
 ament_add_gtest(test_parameter_client rclcpp/test_parameter_client.cpp)
 if(TARGET test_parameter_client)
   ament_target_dependencies(test_parameter_client
@@ -327,6 +373,7 @@ if(TARGET test_service)
     "rmw"
     "rosidl_runtime_cpp"
     "rosidl_typesupport_cpp"
+    "test_msgs"
   )
   target_link_libraries(test_service ${PROJECT_NAME})
 endif()
@@ -360,6 +407,16 @@ if(TARGET test_subscription_traits)
   )
   target_link_libraries(test_subscription_traits ${PROJECT_NAME})
 endif()
+ament_add_gtest(test_type_support rclcpp/test_type_support.cpp)
+if(TARGET test_type_support)
+  ament_target_dependencies(test_type_support
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "test_msgs"
+  )
+  target_link_libraries(test_type_support ${PROJECT_NAME})
+endif()
 ament_add_gtest(test_find_weak_nodes rclcpp/test_find_weak_nodes.cpp)
 if(TARGET test_find_weak_nodes)
   ament_target_dependencies(test_find_weak_nodes
@@ -388,14 +445,6 @@ if(TARGET test_duration)
   target_link_libraries(test_duration ${PROJECT_NAME})
 endif()
 
-ament_add_gtest(test_executor rclcpp/test_executor.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_executor)
-  ament_target_dependencies(test_executor
-    "rcl")
-  target_link_libraries(test_executor ${PROJECT_NAME})
-endif()
-
 ament_add_gtest(test_logger rclcpp/test_logger.cpp)
 target_link_libraries(test_logger ${PROJECT_NAME})
 
@@ -450,6 +499,26 @@ if(TARGET test_interface_traits)
   target_link_libraries(test_interface_traits ${PROJECT_NAME})
 endif()
 
+# TODO(brawner) remove when destroying Node for Connext is resolved. See:
+# https://github.com/ros2/rclcpp/issues/1250
+ament_add_gtest(
+  test_executors
+  rclcpp/executors/test_executors.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}"
+  TIMEOUT 180)
+if(TARGET test_executors)
+  ament_target_dependencies(test_executors
+    "rcl"
+    "test_msgs")
+  target_link_libraries(test_executors ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_static_single_threaded_executor rclcpp/executors/test_static_single_threaded_executor.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_static_single_threaded_executor)
+  target_link_libraries(test_static_single_threaded_executor ${PROJECT_NAME})
+endif()
+
 ament_add_gtest(test_multi_threaded_executor rclcpp/executors/test_multi_threaded_executor.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}")
 if(TARGET test_multi_threaded_executor)
@@ -458,6 +527,15 @@ if(TARGET test_multi_threaded_executor)
   target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME})
 endif()
 
+ament_add_gtest(test_static_executor_entities_collector rclcpp/executors/test_static_executor_entities_collector.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_static_executor_entities_collector)
+  ament_target_dependencies(test_static_executor_entities_collector
+    "rcl"
+    "test_msgs")
+  target_link_libraries(test_static_executor_entities_collector ${PROJECT_NAME})
+endif()
+
 ament_add_gtest(test_guard_condition rclcpp/test_guard_condition.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}")
 if(TARGET test_guard_condition)
@@ -495,6 +573,35 @@ if(TARGET test_subscription_options)
   target_link_libraries(test_subscription_options ${PROJECT_NAME})
 endif()
 
+ament_add_gtest(test_dynamic_storage rclcpp/wait_set_policies/test_dynamic_storage.cpp)
+if(TARGET test_dynamic_storage)
+  ament_target_dependencies(test_dynamic_storage "rcl" "test_msgs")
+  target_link_libraries(test_dynamic_storage ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_storage_policy_common rclcpp/wait_set_policies/test_storage_policy_common.cpp)
+if(TARGET test_storage_policy_common)
+  ament_target_dependencies(test_storage_policy_common "rcl" "test_msgs")
+  target_link_libraries(test_storage_policy_common ${PROJECT_NAME} mimick)
+endif()
+
+ament_add_gtest(test_static_storage rclcpp/wait_set_policies/test_static_storage.cpp)
+if(TARGET test_static_storage)
+  ament_target_dependencies(test_static_storage "rcl" "test_msgs")
+  target_link_libraries(test_static_storage ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_thread_safe_synchronization rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp)
+if(TARGET test_thread_safe_synchronization)
+  ament_target_dependencies(test_thread_safe_synchronization "rcl" "test_msgs")
+  target_link_libraries(test_thread_safe_synchronization ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_rclcpp_gtest_macros utils/test_rclcpp_gtest_macros.cpp)
+if(TARGET test_rclcpp_gtest_macros)
+  target_link_libraries(test_rclcpp_gtest_macros ${PROJECT_NAME})
+endif()
+
 # Install test resources
 install(
   DIRECTORY resources

rclcpp/test/mocking_utils/patch.hpp

@@ -0,0 +1,522 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// Original file taken from:
+// https://github.com/ros2/rcutils/blob/master/test/mocking_utils/patch.hpp
+
+#ifndef MOCKING_UTILS__PATCH_HPP_
+#define MOCKING_UTILS__PATCH_HPP_
+
+#define MOCKING_UTILS_SUPPORT_VA_LIST
+#if (defined(__aarch64__) || defined(__arm__) || defined(_M_ARM) || defined(__thumb__))
+// In ARM machines, va_list does not define comparison operators
+// nor the compiler allows defining them via operator overloads.
+// Thus, Mimick argument matching code will not compile.
+#undef MOCKING_UTILS_SUPPORT_VA_LIST
+#endif
+
+#ifdef MOCKING_UTILS_SUPPORT_VA_LIST
+#include <cstdarg>
+#endif
+
+#include <functional>
+#include <string>
+#include <type_traits>
+#include <utility>
+
+#include "mimick/mimick.h"
+
+#include "rcutils/error_handling.h"
+#include "rcutils/macros.h"
+
+namespace mocking_utils
+{
+
+/// Mimick specific traits for each mocking_utils::Patch instance.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam SignatureT Type of the symbol to be patched.
+*/
+template<size_t ID, typename SignatureT>
+struct PatchTraits;
+
+/// Traits specialization for ReturnT(void) free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ */
+template<size_t ID, typename ReturnT>
+struct PatchTraits<ID, ReturnT(void)>
+{
+  mmk_mock_define(mock_type, ReturnT);
+};
+
+/// Traits specialization for void(void) free functions.
+/**
+ * Necessary for Mimick macros to adjust accordingly when the return
+ * type is `void`.
+ *
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ */
+template<size_t ID>
+struct PatchTraits<ID, void(void)>
+{
+  mmk_mock_define(mock_type, void);
+};
+
+/// Traits specialization for ReturnT(ArgT0) free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgT0 Argument type.
+ */
+template<size_t ID, typename ReturnT, typename ArgT0>
+struct PatchTraits<ID, ReturnT(ArgT0)>
+{
+  mmk_mock_define(mock_type, ReturnT, ArgT0);
+};
+
+/// Traits specialization for void(ArgT0) free functions.
+/**
+ * Necessary for Mimick macros to adjust accordingly when the return
+ * type is `void`.
+ *
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ArgT0 Argument type.
+ */
+template<size_t ID, typename ArgT0>
+struct PatchTraits<ID, void(ArgT0)>
+{
+  mmk_mock_define(mock_type, void, ArgT0);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1) free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1)>
+{
+  mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1);
+};
+
+/// Traits specialization for void(ArgT0, ArgT1) free functions.
+/**
+ * Necessary for Mimick macros to adjust accordingly when the return
+ * type is `void`.
+ *
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ArgT0, typename ArgT1>
+struct PatchTraits<ID, void(ArgT0, ArgT1)>
+{
+  mmk_mock_define(mock_type, void, ArgT0, ArgT1);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2) free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1, typename ArgT2>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2)>
+{
+  mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2);
+};
+
+/// Traits specialization for void(ArgT0, ArgT1, ArgT2) free functions.
+/**
+ * Necessary for Mimick macros to adjust accordingly when the return
+ * type is `void`.
+ *
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ArgT0, typename ArgT1, typename ArgT2>
+struct PatchTraits<ID, void(ArgT0, ArgT1, ArgT2)>
+{
+  mmk_mock_define(mock_type, void, ArgT0, ArgT1, ArgT2);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3) free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3)>
+{
+  mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3);
+};
+
+/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3) free functions.
+/**
+ * Necessary for Mimick macros to adjust accordingly when the return
+ * type is `void`.
+ *
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3>
+struct PatchTraits<ID, void(ArgT0, ArgT1, ArgT2, ArgT3)>
+{
+  mmk_mock_define(mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4)
+/// free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3, typename ArgT4>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4)>
+{
+  mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4);
+};
+
+/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4)
+/// free functions.
+/**
+ * Necessary for Mimick macros to adjust accordingly when the return
+ * type is `void`.
+ *
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3, typename ArgT4>
+struct PatchTraits<ID, void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4)>
+{
+  mmk_mock_define(mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)
+/// free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3,
+  typename ArgT4, typename ArgT5>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)>
+{
+  mmk_mock_define(
+    mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5);
+};
+
+/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)
+/// free functions.
+/**
+ * Necessary for Mimick macros to adjust accordingly when the return
+ * type is `void`.
+ *
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3,
+  typename ArgT4, typename ArgT5>
+struct PatchTraits<ID, void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)>
+{
+  mmk_mock_define(
+    mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)
+/// free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3,
+  typename ArgT4, typename ArgT5, typename ArgT6>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6)>
+{
+  mmk_mock_define(
+    mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6);
+};
+
+/// Generic trampoline to wrap generalized callables in plain functions.
+/**
+ * \tparam ID Numerical identifier of this trampoline. Ought to be unique.
+ * \tparam SignatureT Type of the symbol this trampoline replaces.
+ */
+template<size_t ID, typename SignatureT>
+struct Trampoline;
+
+/// Trampoline specialization for free functions.
+template<size_t ID, typename ReturnT, typename ... ArgTs>
+struct Trampoline<ID, ReturnT(ArgTs...)>
+{
+  static ReturnT base(ArgTs... args)
+  {
+    return target(std::forward<ArgTs>(args)...);
+  }
+
+  static std::function<ReturnT(ArgTs...)> target;
+};
+
+template<size_t ID, typename ReturnT, typename ... ArgTs>
+std::function<ReturnT(ArgTs...)>
+Trampoline<ID, ReturnT(ArgTs...)>::target;
+
+/// Setup trampoline with the given @p target.
+/**
+ * \param[in] target Callable that this trampoline will target.
+ * \return the plain base function of this trampoline.
+ *
+ * \tparam ID Numerical identifier of this trampoline. Ought to be unique.
+ * \tparam SignatureT Type of the symbol this trampoline replaces.
+ */
+template<size_t ID, typename SignatureT>
+auto prepare_trampoline(std::function<SignatureT> target)
+{
+  Trampoline<ID, SignatureT>::target = target;
+  return Trampoline<ID, SignatureT>::base;
+}
+
+/// Patch class for binary API mocking
+/**
+ * Built on top of Mimick, to enable symbol mocking on a per dynamically
+ * linked binary object basis.
+ *
+ * \tparam ID Numerical identifier for this patch. Ought to be unique.
+ * \tparam SignatureT Type of the symbol to be patched.
+ */
+template<size_t ID, typename SignatureT>
+class Patch;
+
+/// Patch specialization for ReturnT(ArgTs...) free functions.
+/**
+ * \tparam ID Numerical identifier for this patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTs Argument types.
+ */
+template<size_t ID, typename ReturnT, typename ... ArgTs>
+class Patch<ID, ReturnT(ArgTs...)>
+{
+public:
+  using mock_type = typename PatchTraits<ID, ReturnT(ArgTs...)>::mock_type;
+
+  /// Construct a patch.
+  /**
+   * \param[in] target Symbol target string, using Mimick syntax
+   *   i.e. "symbol(@scope)?", where scope may be "self" to target the current
+   *   binary, "lib:library_name" to target a given library, "file:path/to/library"
+   *   to target a given file, or "sym:other_symbol" to target the first library
+   *   that defines said symbol.
+   * \param[in] proxy An indirection to call the target function.
+   *   This indirection must ensure this call goes through the function's
+   *   trampoline, as setup by the dynamic linker.
+   * \return a mocking_utils::Patch instance.
+   */
+  explicit Patch(const std::string & target, std::function<ReturnT(ArgTs...)> proxy)
+  : target_(target), proxy_(proxy)
+  {
+  }
+
+  // Copy construction and assignment are disabled.
+  Patch(const Patch &) = delete;
+  Patch & operator=(const Patch &) = delete;
+
+  Patch(Patch && other)
+  {
+    mock_ = other.mock_;
+    other.mock_ = nullptr;
+  }
+
+  Patch & operator=(Patch && other)
+  {
+    if (mock_) {
+      mmk_reset(mock_);
+    }
+    mock_ = other.mock_;
+    other.mock_ = nullptr;
+  }
+
+  ~Patch()
+  {
+    if (mock_) {
+      mmk_reset(mock_);
+    }
+  }
+
+  /// Inject a @p replacement for the patched function.
+  Patch & then_call(std::function<ReturnT(ArgTs...)> replacement) &
+  {
+    replace_with(replacement);
+    return *this;
+  }
+
+  /// Inject a @p replacement for the patched function.
+  Patch && then_call(std::function<ReturnT(ArgTs...)> replacement) &&
+  {
+    replace_with(replacement);
+    return std::move(*this);
+  }
+
+private:
+  // Helper for template parameter pack expansion using `mmk_any`
+  // macro as pattern.
+  template<typename T>
+  T any() {return mmk_any(T);}
+
+  void replace_with(std::function<ReturnT(ArgTs...)> replacement)
+  {
+    if (mock_) {
+      throw std::logic_error("Cannot configure patch more than once");
+    }
+    auto type_erased_trampoline =
+      reinterpret_cast<mmk_fn>(prepare_trampoline<ID>(replacement));
+    auto MMK_MANGLE(mock_type, create) =
+      PatchTraits<ID, ReturnT(ArgTs...)>::MMK_MANGLE(mock_type, create);
+    mock_ = mmk_mock(target_.c_str(), mock_type);
+    mmk_when(proxy_(any<ArgTs>()...), .then_call = type_erased_trampoline);
+  }
+
+  mock_type mock_{nullptr};
+  std::string target_;
+  std::function<ReturnT(ArgTs...)> proxy_;
+};
+
+/// Make a patch for a `target` function.
+/**
+ * Useful for type deduction during \ref mocking_utils::Patch construction.
+ *
+ * \param[in] target Symbol target string, using Mimick syntax.
+ * \param[in] proxy An indirection to call the target function.
+ * \return a mocking_utils::Patch instance.
+ *
+ * \tparam ID Numerical identifier for this patch. Ought to be unique.
+ * \tparam SignatureT Type of the function to be patched.
+ *
+ * \sa mocking_utils::Patch for further reference.
+ */
+template<size_t ID, typename SignatureT>
+auto make_patch(const std::string & target, std::function<SignatureT> proxy)
+{
+  return Patch<ID, SignatureT>(target, proxy);
+}
+
+/// Define a dummy operator `op` for a given `type`.
+/**
+ * Useful to enable patching functions that take arguments whose types
+ * do not define basic comparison operators, as required by Mimick.
+*/
+#define MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(type_, op) \
+  template<typename T> \
+  typename std::enable_if<std::is_same<T, type_>::value, bool>::type \
+  operator op(const T &, const T &) { \
+    return false; \
+  }
+
+/// Get the exact \ref mocking_utils::Patch type for a given `id` and `function`.
+/**
+ * Useful to avoid ignored attribute warnings when using the \b decltype operator.
+ */
+#define MOCKING_UTILS_PATCH_TYPE(id, function) \
+  decltype(mocking_utils::make_patch<id, decltype(function)>("", nullptr))
+
+/// A transparent forwarding proxy to a given `function`.
+/**
+ * Useful to ensure a call to `function` goes through its trampoline.
+ */
+#define MOCKING_UTILS_PATCH_PROXY(function) \
+  [] (auto && ... args)->decltype(auto) { \
+    return function(std::forward<decltype(args)>(args)...); \
+  }
+
+/// Compute a Mimick symbol target string based on which `function` is to be patched
+/// in which `scope`.
+#define MOCKING_UTILS_PATCH_TARGET(scope, function) \
+  (std::string(RCUTILS_STRINGIFY(function)) + "@" + (scope))
+
+/// Prepare a mocking_utils::Patch for patching a `function` in a given `scope`
+/// but defer applying any changes.
+#define prepare_patch(scope, function) \
+  make_patch<__COUNTER__, decltype(function)>( \
+    MOCKING_UTILS_PATCH_TARGET(scope, function), MOCKING_UTILS_PATCH_PROXY(function) \
+  )
+
+/// Patch a `function` with a used-provided `replacement` in a given `scope`.
+#define patch(scope, function, replacement) \
+  prepare_patch(scope, function).then_call(replacement)
+
+/// Patch a `function` to always yield a given `return_code` in a given `scope`.
+#define patch_and_return(scope, function, return_code) \
+  patch(scope, function, [&](auto && ...) {return return_code;})
+
+/// Patch a `function` to always yield a given `return_code` in a given `scope`.
+#define patch_to_fail(scope, function, error_message, return_code) \
+  patch( \
+    scope, function, [&](auto && ...) { \
+      RCUTILS_SET_ERROR_MSG(error_message); \
+      return return_code; \
+    })
+
+/// Patch a `function` to execute normally but always yield a given `return_code`
+/// in a given `scope`.
+/**
+ * \warning On some Linux distributions (e.g. CentOS), pointers to function
+ *   reference their PLT trampolines. In such cases, it is not possible to
+ *   call `function` from within the mock.
+ */
+#define inject_on_return(scope, function, return_code) \
+  patch( \
+    scope, function, ([&, base = function](auto && ... __args) { \
+      if (base != function) { \
+        static_cast<void>(base(std::forward<decltype(__args)>(__args)...)); \
+      } else { \
+        RCUTILS_SAFE_FWRITE_TO_STDERR( \
+          "[WARNING] mocking_utils::inject_on_return() cannot forward call to " \
+          "original '" RCUTILS_STRINGIFY(function) "' function before injection\n" \
+          "    at " __FILE__ ":" RCUTILS_STRINGIFY(__LINE__) "\n"); \
+      } \
+      return return_code; \
+    }))
+
+}  // namespace mocking_utils
+
+#ifdef MOCKING_UTILS_SUPPORT_VA_LIST
+// Define dummy comparison operators for C standard va_list type
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, >)
+#endif
+
+#endif  // MOCKING_UTILS__PATCH_HPP_

rclcpp/test/rclcpp/executors/test_executors.cpp

@@ -0,0 +1,528 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/**
+ * This test checks all implementations of rclcpp::executor to check they pass they basic API
+ * tests. Anything specific to any executor in particular should go in a separate test file.
+ *
+ */
+#include <gtest/gtest.h>
+
+#include <algorithm>
+#include <chrono>
+#include <limits>
+#include <memory>
+#include <string>
+#include <thread>
+#include <utility>
+
+#include "rcl/error_handling.h"
+#include "rcl/time.h"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/duration.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "test_msgs/msg/empty.hpp"
+
+using namespace std::chrono_literals;
+
+template<typename T>
+class TestExecutors : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  void SetUp()
+  {
+    const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info();
+    std::stringstream test_name;
+    test_name << test_info->test_case_name() << "_" << test_info->name();
+    node = std::make_shared<rclcpp::Node>("node", test_name.str());
+
+    callback_count = 0;
+
+    const std::string topic_name = std::string("topic_") + test_name.str();
+    publisher = node->create_publisher<test_msgs::msg::Empty>(topic_name, rclcpp::QoS(10));
+    auto callback = [this](test_msgs::msg::Empty::SharedPtr) {this->callback_count++;};
+    subscription =
+      node->create_subscription<test_msgs::msg::Empty>(
+      topic_name, rclcpp::QoS(10), std::move(callback));
+  }
+
+  void TearDown()
+  {
+    publisher.reset();
+    subscription.reset();
+    node.reset();
+  }
+
+  rclcpp::Node::SharedPtr node;
+  rclcpp::Publisher<test_msgs::msg::Empty>::SharedPtr publisher;
+  rclcpp::Subscription<test_msgs::msg::Empty>::SharedPtr subscription;
+  int callback_count;
+};
+
+// spin_all and spin_some are not implemented correctly in StaticSingleThreadedExecutor, see:
+// https://github.com/ros2/rclcpp/issues/1219 for tracking
+template<typename T>
+class TestExecutorsStable : public TestExecutors<T> {};
+
+using ExecutorTypes =
+  ::testing::Types<
+  rclcpp::executors::SingleThreadedExecutor,
+  rclcpp::executors::MultiThreadedExecutor,
+  rclcpp::executors::StaticSingleThreadedExecutor>;
+
+class ExecutorTypeNames
+{
+public:
+  template<typename T>
+  static std::string GetName(int idx)
+  {
+    (void)idx;
+    if (std::is_same<T, rclcpp::executors::SingleThreadedExecutor>()) {
+      return "SingleThreadedExecutor";
+    }
+
+    if (std::is_same<T, rclcpp::executors::MultiThreadedExecutor>()) {
+      return "MultiThreadedExecutor";
+    }
+
+    if (std::is_same<T, rclcpp::executors::StaticSingleThreadedExecutor>()) {
+      return "StaticSingleThreadedExecutor";
+    }
+
+    return "";
+  }
+};
+
+// TYPED_TEST_CASE is deprecated as of gtest 1.9, use TYPED_TEST_SUITE when gtest dependency
+// is updated.
+TYPED_TEST_CASE(TestExecutors, ExecutorTypes, ExecutorTypeNames);
+
+// StaticSingleThreadedExecutor is not included in these tests for now, due to:
+// https://github.com/ros2/rclcpp/issues/1219
+using StandardExecutors =
+  ::testing::Types<
+  rclcpp::executors::SingleThreadedExecutor,
+  rclcpp::executors::MultiThreadedExecutor>;
+TYPED_TEST_CASE(TestExecutorsStable, StandardExecutors, ExecutorTypeNames);
+
+// Make sure that executors detach from nodes when destructing
+TYPED_TEST(TestExecutors, detachOnDestruction) {
+  using ExecutorType = TypeParam;
+  {
+    ExecutorType executor;
+    executor.add_node(this->node);
+  }
+  {
+    ExecutorType executor;
+    EXPECT_NO_THROW(executor.add_node(this->node));
+  }
+}
+
+// Make sure that the executor can automatically remove expired nodes correctly
+// Currently fails for StaticSingleThreadedExecutor so it is being skipped, see:
+// https://github.com/ros2/rclcpp/issues/1231
+TYPED_TEST(TestExecutorsStable, addTemporaryNode) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+
+  {
+    // Let node go out of scope before executor.spin()
+    auto node = std::make_shared<rclcpp::Node>("temporary_node");
+    executor.add_node(node);
+  }
+
+  // Sleep for a short time to verify executor.spin() is going, and didn't throw.
+  std::thread spinner([&]() {EXPECT_NO_THROW(executor.spin());});
+
+  std::this_thread::sleep_for(50ms);
+  executor.cancel();
+  spinner.join();
+}
+
+// Check executor throws properly if the same node is added a second time
+TYPED_TEST(TestExecutors, addNodeTwoExecutors) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor1;
+  ExecutorType executor2;
+  EXPECT_NO_THROW(executor1.add_node(this->node));
+  EXPECT_THROW(executor2.add_node(this->node), std::runtime_error);
+  executor1.remove_node(this->node, true);
+}
+
+// Check simple spin example
+TYPED_TEST(TestExecutors, spinWithTimer) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+
+  bool timer_completed = false;
+  auto timer = this->node->create_wall_timer(1ms, [&]() {timer_completed = true;});
+  executor.add_node(this->node);
+
+  std::thread spinner([&]() {executor.spin();});
+
+  auto start = std::chrono::steady_clock::now();
+  while (!timer_completed && (std::chrono::steady_clock::now() - start) < 10s) {
+    std::this_thread::sleep_for(1ms);
+  }
+
+  EXPECT_TRUE(timer_completed);
+  // Cancel needs to be called before join, so that executor.spin() returns.
+  executor.cancel();
+  spinner.join();
+  executor.remove_node(this->node, true);
+}
+
+TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+  executor.add_node(this->node);
+
+  bool timer_completed = false;
+  auto timer = this->node->create_wall_timer(1ms, [&]() {timer_completed = true;});
+
+  std::thread spinner([&]() {executor.spin();});
+  // Sleep for a short time to verify executor.spin() is going, and didn't throw.
+
+  auto start = std::chrono::steady_clock::now();
+  while (!timer_completed && (std::chrono::steady_clock::now() - start) < 10s) {
+    std::this_thread::sleep_for(1ms);
+  }
+
+  EXPECT_TRUE(timer_completed);
+  EXPECT_THROW(executor.spin(), std::runtime_error);
+
+  // Shutdown needs to be called before join, so that executor.spin() returns.
+  executor.cancel();
+  spinner.join();
+  executor.remove_node(this->node, true);
+}
+
+// Check executor exits immediately if future is complete.
+TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+  executor.add_node(this->node);
+
+  // test success of an immediately finishing future
+  std::promise<bool> promise;
+  std::future<bool> future = promise.get_future();
+  promise.set_value(true);
+
+  // spin_until_future_complete is expected to exit immediately, but would block up until its
+  // timeout if the future is not checked before spin_once_impl.
+  auto start = std::chrono::steady_clock::now();
+  auto shared_future = future.share();
+  auto ret = executor.spin_until_future_complete(shared_future, 1s);
+  executor.remove_node(this->node, true);
+  // Check it didn't reach timeout
+  EXPECT_GT(500ms, (std::chrono::steady_clock::now() - start));
+  EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret);
+}
+
+// Same test, but uses a shared future.
+TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+  executor.add_node(this->node);
+
+  // test success of an immediately finishing future
+  std::promise<bool> promise;
+  std::future<bool> future = promise.get_future();
+  promise.set_value(true);
+
+  // spin_until_future_complete is expected to exit immediately, but would block up until its
+  // timeout if the future is not checked before spin_once_impl.
+  auto shared_future = future.share();
+  auto start = std::chrono::steady_clock::now();
+  auto ret = executor.spin_until_future_complete(shared_future, 1s);
+  executor.remove_node(this->node, true);
+
+  // Check it didn't reach timeout
+  EXPECT_GT(500ms, (std::chrono::steady_clock::now() - start));
+  EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret);
+}
+
+// For a longer running future that should require several iterations of spin_once
+TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+  executor.add_node(this->node);
+
+  // This future doesn't immediately terminate, so some work gets performed.
+  std::future<void> future = std::async(
+    std::launch::async,
+    [this]() {
+      auto start = std::chrono::steady_clock::now();
+      while (this->callback_count < 1 && (std::chrono::steady_clock::now() - start) < 1s) {
+        std::this_thread::sleep_for(1ms);
+      }
+    });
+
+  bool spin_exited = false;
+
+  // Timeout set to negative for no timeout.
+  std::thread spinner([&]() {
+      auto ret = executor.spin_until_future_complete(future, -1s);
+      EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret);
+      executor.remove_node(this->node, true);
+      executor.cancel();
+      spin_exited = true;
+    });
+
+  // Do some work for longer than the future needs.
+  for (int i = 0; i < 100; ++i) {
+    this->publisher->publish(test_msgs::msg::Empty());
+    std::this_thread::sleep_for(1ms);
+    if (spin_exited) {
+      break;
+    }
+  }
+
+  // Not testing accuracy, just want to make sure that some work occurred.
+  EXPECT_LT(0, this->callback_count);
+
+  // If this fails, the test will probably crash because spinner goes out of scope while the thread
+  // is active. However, it beats letting this run until the gtest timeout.
+  ASSERT_TRUE(spin_exited);
+  executor.cancel();
+  spinner.join();
+}
+
+// Check spin_until_future_complete timeout works as expected
+TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+  executor.add_node(this->node);
+
+  bool spin_exited = false;
+
+  // Needs to run longer than spin_until_future_complete's timeout.
+  std::future<void> future = std::async(
+    std::launch::async,
+    [&spin_exited]() {
+      auto start = std::chrono::steady_clock::now();
+      while (!spin_exited && (std::chrono::steady_clock::now() - start) < 1s) {
+        std::this_thread::sleep_for(1ms);
+      }
+    });
+
+  // Short timeout
+  std::thread spinner([&]() {
+      auto ret = executor.spin_until_future_complete(future, 1ms);
+      EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret);
+      executor.remove_node(this->node, true);
+      spin_exited = true;
+    });
+
+  // Do some work for longer than timeout needs.
+  for (int i = 0; i < 100; ++i) {
+    this->publisher->publish(test_msgs::msg::Empty());
+    std::this_thread::sleep_for(1ms);
+    if (spin_exited) {
+      break;
+    }
+  }
+
+  EXPECT_TRUE(spin_exited);
+  spinner.join();
+}
+
+// Check spin_until_future_complete can be properly interrupted.
+TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+  executor.add_node(this->node);
+
+  bool spin_exited = false;
+
+  // This needs to block longer than it takes to get to the shutdown call below and for
+  // spin_until_future_complete to return
+  std::future<void> future = std::async(
+    std::launch::async,
+    [&spin_exited]() {
+      auto start = std::chrono::steady_clock::now();
+      while (!spin_exited && (std::chrono::steady_clock::now() - start) < 1s) {
+        std::this_thread::sleep_for(1ms);
+      }
+    });
+
+  // Long timeout
+  std::thread spinner([&spin_exited, &executor, &future]() {
+      auto ret = executor.spin_until_future_complete(future, 1s);
+      EXPECT_EQ(rclcpp::FutureReturnCode::INTERRUPTED, ret);
+      spin_exited = true;
+    });
+
+  // Do some minimal work
+  this->publisher->publish(test_msgs::msg::Empty());
+  std::this_thread::sleep_for(1ms);
+
+  // Force interruption
+  rclcpp::shutdown();
+
+  // Give it time to exit
+  auto start = std::chrono::steady_clock::now();
+  while (!spin_exited && (std::chrono::steady_clock::now() - start) < 1s) {
+    std::this_thread::sleep_for(1ms);
+  }
+
+  EXPECT_TRUE(spin_exited);
+  spinner.join();
+}
+
+class TestWaitable : public rclcpp::Waitable
+{
+public:
+  TestWaitable()
+  {
+    rcl_guard_condition_options_t guard_condition_options =
+      rcl_guard_condition_get_default_options();
+
+    gc_ = rcl_get_zero_initialized_guard_condition();
+    rcl_ret_t ret = rcl_guard_condition_init(
+      &gc_,
+      rclcpp::contexts::get_global_default_context()->get_rcl_context().get(),
+      guard_condition_options);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret);
+    }
+  }
+
+  bool
+  add_to_wait_set(rcl_wait_set_t * wait_set) override
+  {
+    rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &gc_, NULL);
+    if (RCL_RET_OK != ret) {
+      return false;
+    }
+    ret = rcl_trigger_guard_condition(&gc_);
+    return RCL_RET_OK == ret;
+  }
+
+  bool
+  is_ready(rcl_wait_set_t * wait_set) override
+  {
+    (void)wait_set;
+    return true;
+  }
+
+  void
+  execute() override
+  {
+    count_++;
+    std::this_thread::sleep_for(1ms);
+  }
+
+  size_t
+  get_number_of_ready_guard_conditions() override {return 1;}
+
+  size_t
+  get_count()
+  {
+    return count_;
+  }
+
+private:
+  size_t count_ = 0;
+  rcl_guard_condition_t gc_;
+};
+
+TYPED_TEST(TestExecutorsStable, spinAll) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+  auto waitable_interfaces = this->node->get_node_waitables_interface();
+  auto my_waitable = std::make_shared<TestWaitable>();
+  waitable_interfaces->add_waitable(my_waitable, nullptr);
+  executor.add_node(this->node);
+
+  // Long timeout, but should not block test if spin_all works as expected as we cancel the
+  // executor.
+  bool spin_exited = false;
+  std::thread spinner([&spin_exited, &executor, this]() {
+      executor.spin_all(1s);
+      executor.remove_node(this->node, true);
+      spin_exited = true;
+    });
+
+  // Do some work until sufficient calls to the waitable occur
+  auto start = std::chrono::steady_clock::now();
+  while (
+    my_waitable->get_count() <= 1 &&
+    !spin_exited &&
+    (std::chrono::steady_clock::now() - start < 1s))
+  {
+    this->publisher->publish(test_msgs::msg::Empty());
+    std::this_thread::sleep_for(1ms);
+  }
+
+  executor.cancel();
+  start = std::chrono::steady_clock::now();
+  while (!spin_exited && (std::chrono::steady_clock::now() - start) < 1s) {
+    std::this_thread::sleep_for(1ms);
+  }
+
+  EXPECT_LT(1u, my_waitable->get_count());
+  waitable_interfaces->remove_waitable(my_waitable, nullptr);
+  ASSERT_TRUE(spin_exited);
+  spinner.join();
+}
+
+TYPED_TEST(TestExecutorsStable, spinSome) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+  auto waitable_interfaces = this->node->get_node_waitables_interface();
+  auto my_waitable = std::make_shared<TestWaitable>();
+  waitable_interfaces->add_waitable(my_waitable, nullptr);
+  executor.add_node(this->node);
+
+  // Long timeout, doesn't block test from finishing because spin_some should exit after the
+  // first one completes.
+  bool spin_exited = false;
+  std::thread spinner([&spin_exited, &executor, this]() {
+      executor.spin_some(1s);
+      executor.remove_node(this->node, true);
+      spin_exited = true;
+    });
+
+  // Do some work until sufficient calls to the waitable occur, but keep going until either
+  // count becomes too large, spin exits, or the 1 second timeout completes.
+  auto start = std::chrono::steady_clock::now();
+  while (
+    my_waitable->get_count() <= 1 &&
+    !spin_exited &&
+    (std::chrono::steady_clock::now() - start < 1s))
+  {
+    this->publisher->publish(test_msgs::msg::Empty());
+    std::this_thread::sleep_for(1ms);
+  }
+
+  EXPECT_EQ(1u, my_waitable->get_count());
+  waitable_interfaces->remove_waitable(my_waitable, nullptr);
+  EXPECT_TRUE(spin_exited);
+  // Cancel if it hasn't exited already.
+  executor.cancel();
+
+  spinner.join();
+}

rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp

@@ -0,0 +1,295 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "test_msgs/msg/empty.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+namespace
+{
+
+struct NumberOfEntities
+{
+  size_t subscriptions = 0;
+  size_t timers = 0;
+  size_t services = 0;
+  size_t clients = 0;
+  size_t waitables = 0;
+};
+
+std::unique_ptr<NumberOfEntities> get_number_of_default_entities(rclcpp::Node::SharedPtr node)
+{
+  auto number_of_entities = std::make_unique<NumberOfEntities>();
+  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;
+      });
+  }
+
+  return number_of_entities;
+}
+
+}  // namespace
+
+class TestStaticExecutorEntitiesCollector : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+    entities_collector_ =
+      std::make_shared<rclcpp::executors::StaticExecutorEntitiesCollector>();
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+
+  rclcpp::executors::StaticExecutorEntitiesCollector::SharedPtr entities_collector_;
+};
+
+TEST_F(TestStaticExecutorEntitiesCollector, construct_destruct) {
+  EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_timers());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_services());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_clients());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_waitables());
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node) {
+  auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
+  EXPECT_NO_THROW(entities_collector_->add_node(node1->get_node_base_interface()));
+
+  // Check adding second time
+  EXPECT_THROW(entities_collector_->add_node(node1->get_node_base_interface()), std::runtime_error);
+
+  auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
+  EXPECT_FALSE(entities_collector_->remove_node(node2->get_node_base_interface()));
+  EXPECT_NO_THROW(entities_collector_->add_node(node2->get_node_base_interface()));
+
+  EXPECT_TRUE(entities_collector_->remove_node(node1->get_node_base_interface()));
+  EXPECT_FALSE(entities_collector_->remove_node(node1->get_node_base_interface()));
+  EXPECT_TRUE(entities_collector_->remove_node(node2->get_node_base_interface()));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, init_bad_arguments) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+
+  rclcpp::GuardCondition guard_condition(shared_context);
+  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
+
+  // Check memory strategy is nullptr
+  rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy = nullptr;
+  EXPECT_THROW(
+    entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition),
+    std::runtime_error);
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, add_remove_basic_node) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  const auto expected_number_of_entities = get_number_of_default_entities(node);
+  EXPECT_NE(nullptr, expected_number_of_entities);
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+
+  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
+  rclcpp::GuardCondition guard_condition(shared_context);
+  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
+
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  EXPECT_EQ(
+    expected_number_of_entities->subscriptions,
+    entities_collector_->get_number_of_subscriptions());
+  EXPECT_EQ(expected_number_of_entities->timers, entities_collector_->get_number_of_timers());
+  EXPECT_EQ(expected_number_of_entities->services, entities_collector_->get_number_of_services());
+  EXPECT_EQ(expected_number_of_entities->clients, entities_collector_->get_number_of_clients());
+  // One extra for the executor
+  EXPECT_EQ(
+    1u + expected_number_of_entities->waitables,
+    entities_collector_->get_number_of_waitables());
+
+  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_timers());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_services());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_clients());
+
+  // Still one for the executor
+  EXPECT_EQ(1u, entities_collector_->get_number_of_waitables());
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) {
+  rclcpp::Context::SharedPtr shared_context = nullptr;
+  {
+    auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
+    auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
+    auto node3 = std::make_shared<rclcpp::Node>("node3", "ns");
+    entities_collector_->add_node(node1->get_node_base_interface());
+    entities_collector_->add_node(node2->get_node_base_interface());
+    entities_collector_->add_node(node3->get_node_base_interface());
+    shared_context = node1->get_node_base_interface()->get_context();
+  }
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+
+  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
+  rclcpp::GuardCondition guard_condition(shared_context);
+  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
+
+  // Expect weak_node pointers to be cleaned up and used
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_timers());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_services());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_clients());
+
+  // Still one for the executor
+  EXPECT_EQ(1u, entities_collector_->get_number_of_waitables());
+}
+
+class TestWaitable : public rclcpp::Waitable
+{
+public:
+  bool add_to_wait_set(rcl_wait_set_t *) override {return true;}
+
+  bool is_ready(rcl_wait_set_t *) override {return true;}
+
+  void execute() override {}
+};
+
+TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  auto expected_number_of_entities = get_number_of_default_entities(node);
+  EXPECT_NE(nullptr, expected_number_of_entities);
+
+  // Create 1 of each entity type
+  auto subscription =
+    node->create_subscription<test_msgs::msg::Empty>(
+    "topic", rclcpp::QoS(10), [](test_msgs::msg::Empty::SharedPtr) {});
+  auto timer =
+    node->create_wall_timer(std::chrono::seconds(60), []() {});
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  auto waitable = std::make_shared<TestWaitable>();
+
+  // Adding a subscription with rmw_connext_cpp adds another waitable, so we need to get the
+  // current number of waitables just before adding the new waitable.
+  expected_number_of_entities->waitables = get_number_of_default_entities(node)->waitables;
+  node->get_node_waitables_interface()->add_waitable(waitable, nullptr);
+
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+
+  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
+
+  rclcpp::GuardCondition guard_condition(shared_context);
+  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
+
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+
+  EXPECT_EQ(
+    1u + expected_number_of_entities->subscriptions,
+    entities_collector_->get_number_of_subscriptions());
+  EXPECT_EQ(1u + expected_number_of_entities->timers, entities_collector_->get_number_of_timers());
+  EXPECT_EQ(
+    1u + expected_number_of_entities->services,
+    entities_collector_->get_number_of_services());
+  EXPECT_EQ(
+    1u + expected_number_of_entities->clients,
+    entities_collector_->get_number_of_clients());
+
+  // One extra for the executor
+  EXPECT_EQ(
+    2u + expected_number_of_entities->waitables,
+    entities_collector_->get_number_of_waitables());
+
+  entities_collector_->remove_node(node->get_node_base_interface());
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_timers());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_services());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_clients());
+  // Still one for the executor
+  EXPECT_EQ(1u, entities_collector_->get_number_of_waitables());
+}

rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp

@@ -0,0 +1,49 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <memory>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/executors.hpp"
+
+using namespace std::chrono_literals;
+
+class TestStaticSingleThreadedExecutor : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+};
+
+TEST_F(TestStaticSingleThreadedExecutor, check_unimplemented) {
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  executor.add_node(node);
+
+  EXPECT_THROW(executor.spin_some(), rclcpp::exceptions::UnimplementedError);
+  EXPECT_THROW(executor.spin_all(0ns), rclcpp::exceptions::UnimplementedError);
+  EXPECT_THROW(executor.spin_once(0ns), rclcpp::exceptions::UnimplementedError);
+}

rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp

@@ -23,6 +23,7 @@
 #include "rclcpp/node_interfaces/node_graph.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "test_msgs/msg/empty.hpp"
+#include "test_msgs/srv/empty.hpp"
 
 class TestNodeGraph : public ::testing::Test
 {
@@ -87,17 +88,39 @@ TEST_F(TestNodeGraph, get_service_names_and_types)
 TEST_F(TestNodeGraph, get_service_names_and_types_by_node)
 {
   auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
+  auto callback = [](
+    const test_msgs::srv::Empty::Request::SharedPtr,
+    test_msgs::srv::Empty::Response::SharedPtr) {};
+  auto service =
+    node1->create_service<test_msgs::srv::Empty>("node1_service", std::move(callback));
   auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
   const auto * node_graph =
     dynamic_cast<rclcpp::node_interfaces::NodeGraph *>(node1->get_node_graph_interface().get());
   ASSERT_NE(nullptr, node_graph);
 
+  // rcl_get_service_names_and_types_by_node() expects the node to exist, otherwise it fails
   EXPECT_THROW(
     node_graph->get_service_names_and_types_by_node("not_a_node", "not_absolute_namespace"),
     std::runtime_error);
-  auto service_names_and_types1 = node_graph->get_service_names_and_types_by_node("node1", "/ns");
-  auto service_names_and_types2 = node_graph->get_service_names_and_types_by_node("node2", "/ns");
-  EXPECT_EQ(service_names_and_types1.size(), service_names_and_types2.size());
+
+  // Check that node1_service exists for node1 but not node2. This shouldn't exercise graph
+  // discovery as node_graph belongs to node1 anyway. This is just to test the API itself.
+  auto services_of_node1 =
+    node_graph->get_service_names_and_types_by_node("node1", "/ns");
+  auto services_of_node2 =
+    node_graph->get_service_names_and_types_by_node("node2", "/ns");
+
+  auto start = std::chrono::steady_clock::now();
+  while (std::chrono::steady_clock::now() - start < std::chrono::seconds(1)) {
+    services_of_node1 = node_graph->get_service_names_and_types_by_node("node1", "/ns");
+    services_of_node2 = node_graph->get_service_names_and_types_by_node("node2", "/ns");
+    if (services_of_node1.find("/ns/node1_service") != services_of_node1.end()) {
+      break;
+    }
+  }
+
+  EXPECT_TRUE(services_of_node1.find("/ns/node1_service") != services_of_node1.end());
+  EXPECT_FALSE(services_of_node2.find("/ns/node1_service") != services_of_node2.end());
 }
 
 TEST_F(TestNodeGraph, get_node_names_and_namespaces)
@@ -170,10 +193,28 @@ TEST_F(TestNodeGraph, get_info_by_topic)
   EXPECT_EQ(rclcpp::EndpointType::Publisher, const_publisher_endpoint_info.endpoint_type());
 
   rclcpp::QoS actual_qos = publisher_endpoint_info.qos_profile();
-  EXPECT_EQ(0u, actual_qos.get_rmw_qos_profile().depth);
+  switch (actual_qos.get_rmw_qos_profile().history) {
+    case RMW_QOS_POLICY_HISTORY_KEEP_LAST:
+      EXPECT_EQ(1u, actual_qos.get_rmw_qos_profile().depth);
+      break;
+    case RMW_QOS_POLICY_HISTORY_UNKNOWN:
+      EXPECT_EQ(0u, actual_qos.get_rmw_qos_profile().depth);
+      break;
+    default:
+      ADD_FAILURE() << "unexpected history";
+  }
 
   rclcpp::QoS const_actual_qos = const_publisher_endpoint_info.qos_profile();
-  EXPECT_EQ(0u, const_actual_qos.get_rmw_qos_profile().depth);
+  switch (const_actual_qos.get_rmw_qos_profile().history) {
+    case RMW_QOS_POLICY_HISTORY_KEEP_LAST:
+      EXPECT_EQ(1u, const_actual_qos.get_rmw_qos_profile().depth);
+      break;
+    case RMW_QOS_POLICY_HISTORY_UNKNOWN:
+      EXPECT_EQ(0u, const_actual_qos.get_rmw_qos_profile().depth);
+      break;
+    default:
+      ADD_FAILURE() << "unexpected history";
+  }
 
   auto endpoint_gid = publisher_endpoint_info.endpoint_gid();
   auto const_endpoint_gid = const_publisher_endpoint_info.endpoint_gid();

rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp

@@ -0,0 +1,927 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <list>
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+#include <utility>
+
+#include "gtest/gtest.h"
+
+#include "rclcpp/scope_exit.hpp"
+#include "rclcpp/strategies/allocator_memory_strategy.hpp"
+#include "test_msgs/msg/empty.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy;
+typedef std::map<rclcpp::CallbackGroup::WeakPtr,
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+    std::owner_less<rclcpp::CallbackGroup::WeakPtr>> WeakCallbackGroupsToNodesMap;
+
+static bool test_waitable_result = false;
+
+/**
+ * Mock Waitable class, with a globally setable boolean result.
+ */
+class TestWaitable : public rclcpp::Waitable
+{
+public:
+  bool add_to_wait_set(rcl_wait_set_t *) override
+  {
+    return test_waitable_result;
+  }
+
+  bool is_ready(rcl_wait_set_t *) override
+  {
+    return test_waitable_result;
+  }
+
+  void execute() override {}
+};
+
+struct RclWaitSetSizes
+{
+  size_t size_of_subscriptions = 0;
+  size_t size_of_guard_conditions = 0;
+  size_t size_of_timers = 0;
+  size_t size_of_clients = 0;
+  size_t size_of_services = 0;
+  size_t size_of_events = 0;
+  size_t size_of_waitables = 0;
+};
+
+// For a standard rclcpp node, this should be more than enough capacity for each type.
+RclWaitSetSizes SufficientWaitSetCapacities()
+{
+  return {100, 100, 100, 100, 100, 100, 100};
+}
+
+class TestAllocatorMemoryStrategy : public ::testing::Test
+{
+public:
+  TestAllocatorMemoryStrategy()
+  : allocator_memory_strategy_(nullptr) {}
+
+  void SetUp() override
+  {
+    rclcpp::init(0, nullptr);
+    allocator_ = std::make_shared<std::allocator<void>>();
+    // Even though this is just using a basic allocator, the custom allocator constructor was
+    // not covered in general testing.
+    allocator_memory_strategy_ = std::make_shared<AllocatorMemoryStrategy<>>(allocator_);
+  }
+
+  void TearDown() override
+  {
+    rclcpp::shutdown();
+  }
+
+protected:
+  std::shared_ptr<AllocatorMemoryStrategy<>> allocator_memory_strategy()
+  {
+    return allocator_memory_strategy_;
+  }
+
+  //
+  // Convience methods, but it adds entities to vectors so the weak pointers kept by the node
+  // interfaces remain alive and valid
+  //
+
+  std::shared_ptr<rclcpp::Node> create_node_with_disabled_callback_groups(const std::string & name)
+  {
+    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) {
+        group->can_be_taken_from() = false;
+      }
+    }
+    return node;
+  }
+
+  std::shared_ptr<rclcpp::Node> create_node_with_subscription(const std::string & name)
+  {
+    auto subscription_callback = [](const test_msgs::msg::Empty::SharedPtr) {};
+    const rclcpp::QoS qos(10);
+    auto node_with_subscription = create_node_with_disabled_callback_groups(name);
+
+    rclcpp::SubscriptionOptions subscription_options;
+
+    auto callback_group =
+      node_with_subscription->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive);
+
+    subscription_options.callback_group = callback_group;
+    callback_groups_.push_back(callback_group);
+
+    auto subscription = node_with_subscription->create_subscription<
+      test_msgs::msg::Empty, decltype(subscription_callback)>(
+      "topic", qos, std::move(subscription_callback), subscription_options);
+    subscriptions_.push_back(subscription);
+
+    return node_with_subscription;
+  }
+
+  std::shared_ptr<rclcpp::Node> create_node_with_service(const std::string & name)
+  {
+    auto service_callback =
+      [](const test_msgs::srv::Empty::Request::SharedPtr,
+        test_msgs::srv::Empty::Response::SharedPtr) {};
+    auto node_with_service = create_node_with_disabled_callback_groups(name);
+
+    auto callback_group =
+      node_with_service->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive);
+
+    callback_groups_.push_back(callback_group);
+
+    services_.push_back(
+      node_with_service->create_service<test_msgs::srv::Empty>(
+        "service", std::move(service_callback), rmw_qos_profile_services_default, callback_group));
+    return node_with_service;
+  }
+
+  std::shared_ptr<rclcpp::Node> create_node_with_client(const std::string & name)
+  {
+    auto node_with_client = create_node_with_disabled_callback_groups(name);
+    auto callback_group =
+      node_with_client->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive);
+
+    callback_groups_.push_back(callback_group);
+
+    clients_.push_back(
+      node_with_client->create_client<test_msgs::srv::Empty>(
+        "service", rmw_qos_profile_services_default, callback_group));
+    return node_with_client;
+  }
+
+  std::shared_ptr<rclcpp::Node> create_node_with_timer(const std::string & name)
+  {
+    auto timer_callback = []() {};
+    auto node_with_timer = create_node_with_disabled_callback_groups(name);
+
+    auto callback_group =
+      node_with_timer->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive);
+    callback_groups_.push_back(callback_group);
+
+    timers_.push_back(
+      node_with_timer->create_wall_timer(
+        std::chrono::milliseconds(1), timer_callback, callback_group));
+    return node_with_timer;
+  }
+
+  ::testing::AssertionResult TestNumberOfEntitiesAfterCollection(
+    std::shared_ptr<rclcpp::Node> node,
+    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) {
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            weak_group_ptr,
+            node->get_node_base_interface()));
+      });
+    allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
+    EXPECT_EQ(
+      expected.size_of_subscriptions, allocator_memory_strategy()->number_of_ready_subscriptions());
+    EXPECT_EQ(
+      expected.size_of_guard_conditions, allocator_memory_strategy()->number_of_guard_conditions());
+    EXPECT_EQ(expected.size_of_timers, allocator_memory_strategy()->number_of_ready_timers());
+    EXPECT_EQ(expected.size_of_clients, allocator_memory_strategy()->number_of_ready_clients());
+    EXPECT_EQ(expected.size_of_services, allocator_memory_strategy()->number_of_ready_services());
+    EXPECT_EQ(expected.size_of_events, allocator_memory_strategy()->number_of_ready_events());
+    EXPECT_EQ(expected.size_of_waitables, allocator_memory_strategy()->number_of_waitables());
+    if (::testing::Test::HasFailure()) {
+      return ::testing::AssertionFailure() <<
+             "Expected number of entities did not match actual counts";
+    }
+    return ::testing::AssertionSuccess();
+  }
+
+  ::testing::AssertionResult TestAddHandlesToWaitSet(
+    std::shared_ptr<rclcpp::Node> node,
+    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) {
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            weak_group_ptr,
+            node->get_node_base_interface()));
+      });
+    allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
+
+    auto context = node->get_node_base_interface()->get_context();
+    rcl_context_t * rcl_context = context->get_rcl_context().get();
+    rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+    rcl_allocator_t allocator = rcl_get_default_allocator();
+    RclWaitSetSizes sufficient_capacities = SufficientWaitSetCapacities();
+
+    rcl_ret_t ret = rcl_wait_set_init(
+      &wait_set,
+      sufficient_capacities.size_of_subscriptions,
+      sufficient_capacities.size_of_guard_conditions,
+      sufficient_capacities.size_of_timers,
+      sufficient_capacities.size_of_clients,
+      sufficient_capacities.size_of_services,
+      sufficient_capacities.size_of_events,
+      rcl_context,
+      allocator);
+    if (ret != RCL_RET_OK) {
+      return ::testing::AssertionFailure() <<
+             "Calling rcl_wait_set_init() with expected sufficient capacities failed";
+    }
+
+    RCLCPP_SCOPE_EXIT(
+    {
+      EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));
+    });
+
+    if (!allocator_memory_strategy()->add_handles_to_wait_set(&wait_set)) {
+      return ::testing::AssertionFailure() <<
+             "Calling add_handles_to_wait_set() with a wait_set with expected sufficient capacities"
+             " failed";
+    }
+
+    rcl_wait_set_t wait_set_no_capacity = rcl_get_zero_initialized_wait_set();
+    ret = rcl_wait_set_init(
+      &wait_set_no_capacity,
+      insufficient_capacities.size_of_subscriptions,
+      insufficient_capacities.size_of_guard_conditions,
+      insufficient_capacities.size_of_timers,
+      insufficient_capacities.size_of_clients,
+      insufficient_capacities.size_of_services,
+      insufficient_capacities.size_of_events,
+      rcl_context,
+      allocator);
+
+    if (ret != RCL_RET_OK) {
+      return ::testing::AssertionFailure() <<
+             "Calling rcl_wait_set_init() with expected insufficient capacities failed";
+    }
+
+    RCLCPP_SCOPE_EXIT(
+    {
+      EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set_no_capacity));
+    });
+
+    if (allocator_memory_strategy()->add_handles_to_wait_set(&wait_set_no_capacity)) {
+      return ::testing::AssertionFailure() <<
+             "Calling add_handles_to_wait_set() with a wait_set with insufficient capacities"
+             " unexpectedly succeeded";
+    }
+    return ::testing::AssertionSuccess();
+  }
+
+  ::testing::AssertionResult TestGetNextEntity(
+    std::shared_ptr<rclcpp::Node> node_with_entity1,
+    std::shared_ptr<rclcpp::Node> node_with_entity2,
+    std::function<rclcpp::AnyExecutable(WeakCallbackGroupsToNodesMap)> get_next_entity_func)
+  {
+    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) {
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            weak_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) {
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            weak_group_ptr,
+            node_with_entity1->get_node_base_interface()));
+      });
+    allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
+
+    rclcpp::AnyExecutable result = get_next_entity_func(weak_groups_to_nodes);
+    if (result.node_base != node_with_entity1->get_node_base_interface()) {
+      return ::testing::AssertionFailure() <<
+             "Failed to get expected entity with specified get_next_*() function";
+    }
+
+    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) {
+        weak_groups_to_uncollected_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            weak_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) {
+        weak_groups_to_uncollected_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            weak_group_ptr,
+            node_with_entity2->get_node_base_interface()));
+      });
+    rclcpp::AnyExecutable failed_result = get_next_entity_func(weak_groups_to_uncollected_nodes);
+    if (nullptr != failed_result.node_base) {
+      return ::testing::AssertionFailure() <<
+             "A node was retrieved with the specified get_next_*() function despite"
+             " none of the nodes that were passed to it were added to the"
+             " allocator_memory_strategy. Retrieved node: " << failed_result.node_base->get_name();
+    }
+    return ::testing::AssertionSuccess();
+  }
+
+  ::testing::AssertionResult TestGetNextEntityMutuallyExclusive(
+    std::shared_ptr<rclcpp::Node> node_with_entity,
+    std::function<rclcpp::AnyExecutable(WeakCallbackGroupsToNodesMap)> get_next_entity_func)
+  {
+    auto basic_node = std::make_shared<rclcpp::Node>("basic_node", "ns");
+    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) {
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            weak_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) {
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            weak_group_ptr,
+            node_base));
+      });
+    allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
+
+    // It's important that these be set after collect_entities() otherwise collect_entities() will
+    // not do anything
+    node_base->get_default_callback_group()->can_be_taken_from() = false;
+    basic_node_base->get_default_callback_group()->can_be_taken_from() = false;
+    for (auto & callback_group : callback_groups_) {
+      callback_group->can_be_taken_from() = false;
+    }
+
+    rclcpp::AnyExecutable result = get_next_entity_func(weak_groups_to_nodes);
+
+    if (nullptr != result.node_base) {
+      return ::testing::AssertionFailure() <<
+             "A node was retrieved with the specified get_next_*() function despite"
+             " setting can_be_taken_from() to false for all nodes and callback_groups";
+    }
+
+    return ::testing::AssertionSuccess();
+  }
+
+  std::vector<std::shared_ptr<rclcpp::CallbackGroup>> callback_groups_;
+
+private:
+  std::shared_ptr<std::allocator<void>> allocator_;
+  std::shared_ptr<AllocatorMemoryStrategy<>> allocator_memory_strategy_;
+
+  // These are generally kept as weak pointers in the rclcpp::Node interfaces, so they need to be
+  // owned by this class.
+  std::vector<rclcpp::SubscriptionBase::SharedPtr> subscriptions_;
+  std::vector<rclcpp::ServiceBase::SharedPtr> services_;
+  std::vector<rclcpp::ClientBase::SharedPtr> clients_;
+  std::vector<rclcpp::TimerBase::SharedPtr> timers_;
+  std::vector<rclcpp::Waitable::SharedPtr> waitables_;
+};
+
+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) {
+      weak_groups_to_nodes.insert(
+        std::pair<rclcpp::CallbackGroup::WeakPtr,
+        rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+          weak_group_ptr,
+          basic_node->get_node_base_interface()));
+    });
+  allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
+  EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_subscriptions());
+  EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_services());
+  EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_events());
+  EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_clients());
+  EXPECT_EQ(0u, allocator_memory_strategy()->number_of_guard_conditions());
+  EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_timers());
+  EXPECT_EQ(0u, allocator_memory_strategy()->number_of_waitables());
+}
+
+TEST_F(TestAllocatorMemoryStrategy, add_remove_guard_conditions) {
+  rcl_guard_condition_t guard_condition1 = rcl_get_zero_initialized_guard_condition();
+  rcl_guard_condition_t guard_condition2 = rcl_get_zero_initialized_guard_condition();
+  rcl_guard_condition_t guard_condition3 = rcl_get_zero_initialized_guard_condition();
+
+  EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition1));
+  EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition2));
+  EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition3));
+  EXPECT_EQ(3u, allocator_memory_strategy()->number_of_guard_conditions());
+
+  // Adding a second time should not add to vector
+  EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition1));
+  EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition2));
+  EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition3));
+  EXPECT_EQ(3u, allocator_memory_strategy()->number_of_guard_conditions());
+
+  EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition1));
+  EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition2));
+  EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition3));
+  EXPECT_EQ(0u, allocator_memory_strategy()->number_of_guard_conditions());
+
+  // Removing second time should have no effect
+  EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition1));
+  EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition2));
+  EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition3));
+  EXPECT_EQ(0u, allocator_memory_strategy()->number_of_guard_conditions());
+}
+
+TEST_F(TestAllocatorMemoryStrategy, add_remove_waitables) {
+  EXPECT_THROW(allocator_memory_strategy()->add_waitable_handle(nullptr), std::runtime_error);
+  EXPECT_EQ(0u, allocator_memory_strategy()->number_of_waitables());
+
+  rclcpp::Waitable::SharedPtr waitable = std::make_shared<TestWaitable>();
+  EXPECT_NO_THROW(allocator_memory_strategy()->add_waitable_handle(waitable));
+  EXPECT_EQ(1u, allocator_memory_strategy()->number_of_waitables());
+
+  EXPECT_NO_THROW(allocator_memory_strategy()->clear_handles());
+  EXPECT_EQ(0u, allocator_memory_strategy()->number_of_waitables());
+}
+
+TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_subscription) {
+  RclWaitSetSizes expected_sizes = {};
+  expected_sizes.size_of_subscriptions = 1;
+  const std::string implementation_identifier = rmw_get_implementation_identifier();
+  if (implementation_identifier == "rmw_connext_cpp" ||
+    implementation_identifier == "rmw_cyclonedds_cpp")
+  {
+    // For connext, a subscription will also add an event and waitable
+    expected_sizes.size_of_events += 1;
+    expected_sizes.size_of_waitables += 1;
+  }
+  auto node_with_subscription = create_node_with_subscription("subscription_node");
+  EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_subscription, expected_sizes));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_service) {
+  RclWaitSetSizes expected_sizes = {};
+  expected_sizes.size_of_services = 1;
+  auto node_with_service = create_node_with_service("service_node");
+  EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_service, expected_sizes));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_client) {
+  RclWaitSetSizes expected_sizes = {};
+  expected_sizes.size_of_clients = 1;
+  auto node_with_client = create_node_with_client("client_node");
+  EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_client, expected_sizes));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_timer) {
+  RclWaitSetSizes expected_sizes = {};
+  expected_sizes.size_of_timers = 1;
+  auto node_with_timer = create_node_with_timer("timer_node");
+  EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_timer, expected_sizes));
+}
+
+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) {
+      weak_groups_to_nodes.insert(
+        std::pair<rclcpp::CallbackGroup::WeakPtr,
+        rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+          weak_group_ptr,
+          node->get_node_base_interface()));
+    });
+  allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
+  EXPECT_FALSE(allocator_memory_strategy()->add_handles_to_wait_set(nullptr));
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+}
+
+TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_subscription) {
+  auto node_with_subscription = create_node_with_subscription("subscription_node");
+  RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities();
+  insufficient_capacities.size_of_subscriptions = 0;
+  EXPECT_TRUE(TestAddHandlesToWaitSet(node_with_subscription, insufficient_capacities));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_service) {
+  auto node_with_service = create_node_with_service("service_node");
+  RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities();
+  insufficient_capacities.size_of_services = 0;
+  EXPECT_TRUE(TestAddHandlesToWaitSet(node_with_service, insufficient_capacities));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_client) {
+  auto node_with_client = create_node_with_client("client_node");
+  RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities();
+  insufficient_capacities.size_of_clients = 0;
+  EXPECT_TRUE(TestAddHandlesToWaitSet(node_with_client, insufficient_capacities));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_guard_condition) {
+  auto node = create_node_with_disabled_callback_groups("node");
+  rcl_guard_condition_t guard_condition = rcl_get_zero_initialized_guard_condition();
+  auto context = node->get_node_base_interface()->get_context();
+  rcl_context_t * rcl_context = context->get_rcl_context().get();
+  rcl_guard_condition_options_t guard_condition_options = {
+    rcl_get_default_allocator()};
+
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_guard_condition_init(&guard_condition, rcl_context, guard_condition_options));
+  RCLCPP_SCOPE_EXIT(
+  {
+    EXPECT_EQ(RCL_RET_OK, rcl_guard_condition_fini(&guard_condition));
+  });
+
+  allocator_memory_strategy()->add_guard_condition(&guard_condition);
+  RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities();
+  insufficient_capacities.size_of_guard_conditions = 0;
+  EXPECT_TRUE(TestAddHandlesToWaitSet(node, insufficient_capacities));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_timer) {
+  auto node_with_timer = create_node_with_timer("timer_node");
+  RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities();
+  insufficient_capacities.size_of_timers = 0;
+  EXPECT_TRUE(TestAddHandlesToWaitSet(node_with_timer, insufficient_capacities));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_waitable) {
+  rcl_reset_error();
+
+  rclcpp::Waitable::SharedPtr waitable = std::make_shared<TestWaitable>();
+  EXPECT_NO_THROW(allocator_memory_strategy()->add_waitable_handle(waitable));
+  EXPECT_EQ(1u, allocator_memory_strategy()->number_of_waitables());
+
+  test_waitable_result = true;
+  EXPECT_TRUE(allocator_memory_strategy()->add_handles_to_wait_set(nullptr));
+
+  test_waitable_result = false;
+  EXPECT_FALSE(allocator_memory_strategy()->add_handles_to_wait_set(nullptr));
+
+  // This calls TestWaitable's functions, so rcl errors are not set
+  EXPECT_FALSE(rcl_error_is_set());
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_subscription) {
+  auto node1 = create_node_with_subscription("node1");
+  auto node2 = create_node_with_subscription("node2");
+
+  auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) {
+      rclcpp::AnyExecutable result;
+      allocator_memory_strategy()->get_next_subscription(result, weak_groups_to_nodes);
+      return result;
+    };
+
+  EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_service) {
+  auto node1 = create_node_with_service("node1");
+  auto node2 = create_node_with_service("node2");
+
+  auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) {
+      rclcpp::AnyExecutable result;
+      allocator_memory_strategy()->get_next_service(result, weak_groups_to_nodes);
+      return result;
+    };
+
+  EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_client) {
+  auto node1 = create_node_with_client("node1");
+  auto node2 = create_node_with_client("node2");
+
+  auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) {
+      rclcpp::AnyExecutable result;
+      allocator_memory_strategy()->get_next_client(result, weak_groups_to_nodes);
+      return result;
+    };
+
+  EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_timer) {
+  auto node1 = create_node_with_timer("node1");
+  auto node2 = create_node_with_timer("node2");
+
+  auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) {
+      rclcpp::AnyExecutable result;
+      allocator_memory_strategy()->get_next_timer(result, weak_groups_to_nodes);
+      return result;
+    };
+
+  EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_waitable) {
+  auto node1 = std::make_shared<rclcpp::Node>("waitable_node", "ns");
+  auto node2 = std::make_shared<rclcpp::Node>("waitable_node2", "ns");
+  rclcpp::Waitable::SharedPtr waitable1 = std::make_shared<TestWaitable>();
+  rclcpp::Waitable::SharedPtr waitable2 = std::make_shared<TestWaitable>();
+  node1->get_node_waitables_interface()->add_waitable(waitable1, nullptr);
+  node2->get_node_waitables_interface()->add_waitable(waitable2, nullptr);
+
+  auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) {
+      rclcpp::AnyExecutable result;
+      allocator_memory_strategy()->get_next_waitable(result, weak_groups_to_nodes);
+      return result;
+    };
+
+  EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_mutually_exclusive) {
+  auto node = create_node_with_subscription("node");
+
+  auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) {
+      rclcpp::AnyExecutable result;
+      allocator_memory_strategy()->get_next_subscription(result, weak_groups_to_nodes);
+      return result;
+    };
+
+  EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_service_mutually_exclusive) {
+  auto node = create_node_with_service("node");
+
+  auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) {
+      rclcpp::AnyExecutable result;
+      allocator_memory_strategy()->get_next_service(result, weak_groups_to_nodes);
+      return result;
+    };
+
+  EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_client_mutually_exclusive) {
+  auto node = create_node_with_client("node");
+
+  auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) {
+      rclcpp::AnyExecutable result;
+      allocator_memory_strategy()->get_next_client(result, weak_groups_to_nodes);
+      return result;
+    };
+
+  EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_timer_mutually_exclusive) {
+  auto node = create_node_with_timer("node");
+
+  auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) {
+      rclcpp::AnyExecutable result;
+      allocator_memory_strategy()->get_next_timer(result, weak_groups_to_nodes);
+      return result;
+    };
+
+  EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_waitable_mutually_exclusive) {
+  auto node = std::make_shared<rclcpp::Node>("waitable_node", "ns");
+  rclcpp::Waitable::SharedPtr waitable = std::make_shared<TestWaitable>();
+  auto callback_group =
+    node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  node->get_node_waitables_interface()->add_waitable(waitable, callback_group);
+
+  auto get_next_entity =
+    [this, callback_group](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) {
+      // This callback group isn't in the base class' callback_group list, so this needs to be done
+      // before get_next_waitable() is called.
+      callback_group->can_be_taken_from() = false;
+
+      rclcpp::AnyExecutable result;
+      allocator_memory_strategy()->get_next_waitable(result, weak_groups_to_nodes);
+      return result;
+    };
+
+  EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_out_of_scope) {
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
+  auto node = create_node_with_disabled_callback_groups("node");
+  // Force subscription to go out of scope and cleanup after collecting entities.
+  {
+    rclcpp::SubscriptionOptions subscription_options;
+
+    auto callback_group =
+      node->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive);
+
+    subscription_options.callback_group = callback_group;
+
+    auto subscription_callback = [](const test_msgs::msg::Empty::SharedPtr) {};
+    const rclcpp::QoS qos(10);
+
+    auto subscription = node->create_subscription<
+      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) {
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            weak_group_ptr,
+            node->get_node_base_interface()));
+      });
+    allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
+  }
+  EXPECT_EQ(1u, allocator_memory_strategy()->number_of_ready_subscriptions());
+
+  rclcpp::AnyExecutable result;
+  allocator_memory_strategy()->get_next_subscription(result, weak_groups_to_nodes);
+  EXPECT_EQ(nullptr, result.node_base);
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_service_out_of_scope) {
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
+  auto node = create_node_with_disabled_callback_groups("node");
+  // Force service to go out of scope and cleanup after collecting entities.
+  {
+    auto callback_group =
+      node->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive);
+
+    auto service_callback =
+      [](const test_msgs::srv::Empty::Request::SharedPtr,
+        test_msgs::srv::Empty::Response::SharedPtr) {};
+    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) {
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            weak_group_ptr,
+            node->get_node_base_interface()));
+      });
+    allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
+  }
+  EXPECT_EQ(1u, allocator_memory_strategy()->number_of_ready_services());
+
+  rclcpp::AnyExecutable result;
+  allocator_memory_strategy()->get_next_service(result, weak_groups_to_nodes);
+  EXPECT_EQ(nullptr, result.node_base);
+}
+
+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) {
+      weak_groups_to_nodes.insert(
+        std::pair<rclcpp::CallbackGroup::WeakPtr,
+        rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+          weak_group_ptr,
+          node->get_node_base_interface()));
+    });
+  // Force client to go out of scope and cleanup after collecting entities.
+  {
+    auto callback_group =
+      node->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive);
+    auto client = node->create_client<test_msgs::srv::Empty>(
+      "service", rmw_qos_profile_services_default, callback_group);
+
+    weak_groups_to_nodes.insert(
+      std::pair<rclcpp::CallbackGroup::WeakPtr,
+      rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+        rclcpp::CallbackGroup::WeakPtr(callback_group),
+        node->get_node_base_interface()));
+
+    allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
+  }
+  EXPECT_EQ(1u, allocator_memory_strategy()->number_of_ready_clients());
+
+  rclcpp::AnyExecutable result;
+  allocator_memory_strategy()->get_next_client(result, weak_groups_to_nodes);
+  EXPECT_EQ(nullptr, result.node_base);
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_timer_out_of_scope) {
+  auto node = create_node_with_disabled_callback_groups("node");
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
+  // Force timer to go out of scope and cleanup after collecting entities.
+  {
+    auto callback_group =
+      node->create_callback_group(
+      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) {
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            weak_group_ptr,
+            node->get_node_base_interface()));
+      });
+    allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
+  }
+  EXPECT_EQ(1u, allocator_memory_strategy()->number_of_ready_timers());
+
+  rclcpp::AnyExecutable result;
+  allocator_memory_strategy()->get_next_timer(result, weak_groups_to_nodes);
+  EXPECT_EQ(nullptr, result.node_base);
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_waitable_out_of_scope) {
+  auto node = create_node_with_disabled_callback_groups("node");
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
+  // Force waitable to go out of scope and cleanup after collecting entities.
+  {
+    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) {
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            weak_group_ptr,
+            node->get_node_base_interface()));
+      });
+    allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
+    auto waitable = std::make_shared<TestWaitable>();
+    node->get_node_waitables_interface()->add_waitable(waitable, callback_group);
+    allocator_memory_strategy()->add_waitable_handle(waitable);
+  }
+  // Since all callback groups have been locked, except the one we added, this should only be 1
+  EXPECT_EQ(1u, allocator_memory_strategy()->number_of_waitables());
+
+  rclcpp::AnyExecutable result;
+  allocator_memory_strategy()->get_next_waitable(result, weak_groups_to_nodes);
+  EXPECT_EQ(nullptr, result.node_base);
+}

rclcpp/test/rclcpp/strategies/test_message_pool_memory_strategy.cpp

@@ -0,0 +1,73 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include "gtest/gtest.h"
+
+#include "rclcpp/strategies/message_pool_memory_strategy.hpp"
+#include "test_msgs/msg/empty.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+using rclcpp::strategies::message_pool_memory_strategy::MessagePoolMemoryStrategy;
+
+class TestMessagePoolMemoryStrategy : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    message_memory_strategy_ =
+      std::make_shared<MessagePoolMemoryStrategy<test_msgs::msg::Empty, 1>>();
+  }
+
+protected:
+  std::shared_ptr<MessagePoolMemoryStrategy<test_msgs::msg::Empty, 1>> message_memory_strategy_;
+};
+
+TEST_F(TestMessagePoolMemoryStrategy, construct_destruct) {
+  ASSERT_NE(nullptr, message_memory_strategy_);
+  EXPECT_NE(nullptr, message_memory_strategy_->message_allocator_);
+  EXPECT_NE(nullptr, message_memory_strategy_->serialized_message_allocator_);
+  EXPECT_NE(nullptr, message_memory_strategy_->buffer_allocator_);
+}
+
+TEST_F(TestMessagePoolMemoryStrategy, borrow_return) {
+  auto message = message_memory_strategy_->borrow_message();
+  ASSERT_NE(nullptr, message);
+
+  EXPECT_NO_THROW(message_memory_strategy_->return_message(message));
+}
+
+TEST_F(TestMessagePoolMemoryStrategy, borrow_too_many) {
+  auto message = message_memory_strategy_->borrow_message();
+  ASSERT_NE(nullptr, message);
+
+  // Size is 1, borrowing second time should fail
+  RCLCPP_EXPECT_THROW_EQ(
+    message_memory_strategy_->borrow_message(),
+    std::runtime_error("Tried to access message that was still in use! Abort."));
+  EXPECT_NO_THROW(message_memory_strategy_->return_message(message));
+}
+
+TEST_F(TestMessagePoolMemoryStrategy, return_unrecognized) {
+  auto message = message_memory_strategy_->borrow_message();
+  ASSERT_NE(nullptr, message);
+
+  auto unrecognized = std::make_shared<test_msgs::msg::Empty>();
+  // Unrecognized does not belong to pool
+  RCLCPP_EXPECT_THROW_EQ(
+    message_memory_strategy_->return_message(unrecognized),
+    std::runtime_error("Unrecognized message ptr in return_message."));
+  EXPECT_NO_THROW(message_memory_strategy_->return_message(message));
+}

rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp

@@ -0,0 +1,299 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <map>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "rclcpp/node.hpp"
+#include "test_msgs/msg/empty.hpp"
+#include "test_msgs/msg/empty.h"
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/executors.hpp"
+#include "rclcpp/executor.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+using namespace std::chrono_literals;
+
+template<typename T>
+class TestAddCallbackGroupsToExecutor : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+};
+
+using ExecutorTypes =
+  ::testing::Types<
+  rclcpp::executors::SingleThreadedExecutor,
+  rclcpp::executors::MultiThreadedExecutor,
+  rclcpp::executors::StaticSingleThreadedExecutor>;
+
+class ExecutorTypeNames
+{
+public:
+  template<typename T>
+  static std::string GetName(int idx)
+  {
+    (void)idx;
+    if (std::is_same<T, rclcpp::executors::SingleThreadedExecutor>()) {
+      return "SingleThreadedExecutor";
+    }
+
+    if (std::is_same<T, rclcpp::executors::MultiThreadedExecutor>()) {
+      return "MultiThreadedExecutor";
+    }
+
+    if (std::is_same<T, rclcpp::executors::StaticSingleThreadedExecutor>()) {
+      return "StaticSingleThreadedExecutor";
+    }
+
+    return "";
+  }
+};
+
+TYPED_TEST_CASE(TestAddCallbackGroupsToExecutor, ExecutorTypes, ExecutorTypeNames);
+
+/*
+ * Test adding callback groups.
+ */
+TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups) {
+  using ExecutorType = TypeParam;
+  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+  auto timer_callback = []() {};
+  rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer(
+    2s, timer_callback, cb_grp);
+  ExecutorType executor;
+  executor.add_callback_group(cb_grp, node->get_node_base_interface());
+  ASSERT_EQ(executor.get_all_callback_groups().size(), 1u);
+  ASSERT_EQ(executor.get_manually_added_callback_groups().size(), 1u);
+  ASSERT_EQ(executor.get_automatically_added_callback_groups_from_nodes().size(), 0u);
+
+  const rclcpp::QoS qos(10);
+  auto options = rclcpp::SubscriptionOptions();
+  auto callback = [](const test_msgs::msg::Empty::SharedPtr) {};
+  rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  options.callback_group = cb_grp2;
+  auto subscription =
+    node->create_subscription<test_msgs::msg::Empty>("topic_name", qos, callback, options);
+  executor.add_callback_group(cb_grp2, node->get_node_base_interface());
+  ASSERT_EQ(executor.get_all_callback_groups().size(), 2u);
+  ASSERT_EQ(executor.get_manually_added_callback_groups().size(), 2u);
+  ASSERT_EQ(executor.get_automatically_added_callback_groups_from_nodes().size(), 0u);
+
+  executor.add_node(node);
+  ASSERT_EQ(executor.get_manually_added_callback_groups().size(), 2u);
+  ASSERT_EQ(executor.get_automatically_added_callback_groups_from_nodes().size(), 1u);
+
+  executor.remove_node(node);
+  ASSERT_EQ(executor.get_manually_added_callback_groups().size(), 2u);
+  ASSERT_EQ(executor.get_automatically_added_callback_groups_from_nodes().size(), 0u);
+
+  executor.remove_callback_group(cb_grp);
+  ASSERT_EQ(executor.get_manually_added_callback_groups().size(), 1u);
+  ASSERT_EQ(executor.get_automatically_added_callback_groups_from_nodes().size(), 0u);
+
+  executor.remove_callback_group(cb_grp2);
+  ASSERT_EQ(executor.get_manually_added_callback_groups().size(), 0u);
+  ASSERT_EQ(executor.get_automatically_added_callback_groups_from_nodes().size(), 0u);
+}
+
+/*
+ * Test removing callback groups.
+ */
+TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_groups) {
+  using ExecutorType = TypeParam;
+  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+  auto timer_callback = []() {};
+  rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer(
+    2s, timer_callback, cb_grp);
+  ExecutorType executor;
+  executor.add_callback_group(cb_grp, node->get_node_base_interface());
+  const rclcpp::QoS qos(10);
+  auto options = rclcpp::SubscriptionOptions();
+  auto callback = [](const test_msgs::msg::Empty::SharedPtr) {};
+  rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  options.callback_group = cb_grp2;
+  auto subscription =
+    node->create_subscription<test_msgs::msg::Empty>("topic_name", qos, callback, options);
+  executor.add_callback_group(cb_grp2, node->get_node_base_interface());
+
+  executor.remove_callback_group(cb_grp);
+  ASSERT_EQ(executor.get_all_callback_groups().size(), 1u);
+  executor.remove_callback_group(cb_grp2);
+  ASSERT_EQ(executor.get_all_callback_groups().size(), 0u);
+}
+
+/*
+ * Test adding duplicate callback groups to executor.
+ */
+TYPED_TEST(TestAddCallbackGroupsToExecutor, add_duplicate_callback_groups)
+{
+  rclcpp::executors::MultiThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+  auto timer_callback = []() {};
+  rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer(
+    2s, timer_callback, cb_grp);
+  executor.add_callback_group(cb_grp, node->get_node_base_interface());
+  EXPECT_THROW(
+    executor.add_callback_group(cb_grp, node->get_node_base_interface()),
+    std::exception);
+}
+
+/*
+ * Test adding callback group after node is added to executor.
+ */
+TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_to_executor)
+{
+  rclcpp::executors::MultiThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+  executor.add_node(node->get_node_base_interface());
+  ASSERT_EQ(executor.get_all_callback_groups().size(), 1u);
+  std::atomic_int timer_count {0};
+  auto timer_callback = [&executor, &timer_count]() {
+      if (timer_count > 0) {
+        ASSERT_EQ(executor.get_all_callback_groups().size(), 3u);
+        executor.cancel();
+      }
+      timer_count++;
+    };
+  rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer(
+    2s, timer_callback, cb_grp);
+  rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive, false);
+  auto timer2_callback = []() {};
+  rclcpp::TimerBase::SharedPtr timer2_ = node->create_wall_timer(
+    2s, timer2_callback, cb_grp2);
+  rclcpp::CallbackGroup::SharedPtr cb_grp3 = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive, true);
+  auto timer3_callback = []() {};
+  rclcpp::TimerBase::SharedPtr timer3_ = node->create_wall_timer(
+    2s, timer3_callback, cb_grp3);
+  executor.spin();
+}
+
+/*
+ * Test adding unallowable callback group.
+ */
+TYPED_TEST(TestAddCallbackGroupsToExecutor, add_unallowable_callback_groups)
+{
+  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+  auto timer_callback = []() {};
+  rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive, false);
+  rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer(
+    2s, timer_callback, cb_grp);
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_callback_group(cb_grp, node->get_node_base_interface());
+  ASSERT_EQ(executor.get_all_callback_groups().size(), 1u);
+
+  const rclcpp::QoS qos(10);
+  auto options = rclcpp::SubscriptionOptions();
+  auto callback = [](const test_msgs::msg::Empty::SharedPtr) {};
+  rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive, false);
+  options.callback_group = cb_grp2;
+  auto subscription =
+    node->create_subscription<test_msgs::msg::Empty>("topic_name", qos, callback, options);
+  executor.add_callback_group(cb_grp2, node->get_node_base_interface());
+  ASSERT_EQ(executor.get_all_callback_groups().size(), 2u);
+
+  auto timer2_callback = []() {};
+  rclcpp::CallbackGroup::SharedPtr cb_grp3 = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive, false);
+  rclcpp::TimerBase::SharedPtr timer2_ = node->create_wall_timer(
+    2s, timer2_callback, cb_grp3);
+  executor.add_node(node->get_node_base_interface());
+  ASSERT_EQ(executor.get_all_callback_groups().size(), 3u);
+}
+
+/*
+ * Test callback groups from one node to many executors.
+ */
+TYPED_TEST(TestAddCallbackGroupsToExecutor, one_node_many_callback_groups_many_executors)
+{
+  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+  auto timer_callback = []() {};
+  rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive, false);
+  rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer(
+    2s, timer_callback, cb_grp);
+  rclcpp::executors::MultiThreadedExecutor timer_executor;
+  rclcpp::executors::MultiThreadedExecutor sub_executor;
+  timer_executor.add_callback_group(cb_grp, node->get_node_base_interface());
+  const rclcpp::QoS qos(10);
+  auto options = rclcpp::SubscriptionOptions();
+  auto callback = [](const test_msgs::msg::Empty::SharedPtr) {};
+  rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive, false);
+  options.callback_group = cb_grp2;
+  auto subscription =
+    node->create_subscription<test_msgs::msg::Empty>("topic_name", qos, callback, options);
+  sub_executor.add_callback_group(cb_grp2, node->get_node_base_interface());
+  ASSERT_EQ(sub_executor.get_all_callback_groups().size(), 1u);
+  ASSERT_EQ(timer_executor.get_all_callback_groups().size(), 1u);
+  auto timer2_callback = []() {};
+  rclcpp::CallbackGroup::SharedPtr cb_grp3 = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive, false);
+  rclcpp::TimerBase::SharedPtr timer2 = node->create_wall_timer(
+    2s, timer2_callback, cb_grp3);
+  sub_executor.add_node(node);
+  ASSERT_EQ(sub_executor.get_all_callback_groups().size(), 2u);
+  timer_executor.add_callback_group(cb_grp3, node->get_node_base_interface());
+  ASSERT_EQ(timer_executor.get_all_callback_groups().size(), 2u);
+}
+
+/*
+ * Test removing callback group from executor that its not associated with.
+ */
+TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_group)
+{
+  rclcpp::executors::MultiThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+  auto timer_callback = []() {};
+  rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer(
+    2s, timer_callback, cb_grp);
+  EXPECT_THROW(
+    executor.remove_callback_group(cb_grp),
+    std::exception);
+  executor.add_callback_group(cb_grp, node->get_node_base_interface());
+  EXPECT_NO_THROW(executor.remove_callback_group(cb_grp));
+  EXPECT_THROW(
+    executor.remove_callback_group(cb_grp),
+    std::exception);
+}

rclcpp/test/rclcpp/test_duration.cpp

@@ -41,6 +41,7 @@ TEST_F(TestDuration, operators) {
   EXPECT_TRUE(old <= young);
   EXPECT_TRUE(young >= old);
   EXPECT_FALSE(young == old);
+  EXPECT_TRUE(young != old);
 
   rclcpp::Duration add = old + young;
   EXPECT_EQ(add.nanoseconds(), old.nanoseconds() + young.nanoseconds());

rclcpp/test/rclcpp/test_executor.cpp

@@ -1,199 +0,0 @@
-// Copyright 2017 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 <algorithm>
-#include <chrono>
-#include <limits>
-#include <memory>
-#include <string>
-
-#include "rcl/error_handling.h"
-#include "rcl/time.h"
-#include "rclcpp/clock.hpp"
-#include "rclcpp/duration.hpp"
-#include "rclcpp/rclcpp.hpp"
-
-using namespace std::chrono_literals;
-
-class TestExecutors : public ::testing::Test
-{
-protected:
-  static void SetUpTestCase()
-  {
-    rclcpp::init(0, nullptr);
-  }
-
-  void SetUp()
-  {
-    node = std::make_shared<rclcpp::Node>("my_node");
-  }
-
-  void TearDown()
-  {
-    node.reset();
-  }
-
-  rclcpp::Node::SharedPtr node;
-};
-
-// Make sure that executors detach from nodes when destructing
-TEST_F(TestExecutors, detachOnDestruction) {
-  {
-    rclcpp::executors::SingleThreadedExecutor executor;
-    executor.add_node(node);
-  }
-  {
-    rclcpp::executors::SingleThreadedExecutor executor;
-    EXPECT_NO_THROW(executor.add_node(node));
-  }
-}
-
-// Make sure that the executor can automatically remove expired nodes correctly
-TEST_F(TestExecutors, addTemporaryNode) {
-  rclcpp::executors::SingleThreadedExecutor executor;
-  executor.add_node(std::make_shared<rclcpp::Node>("temporary_node"));
-  EXPECT_NO_THROW(executor.spin_some());
-}
-
-// Make sure that the spin_until_future_complete works correctly with std::future
-TEST_F(TestExecutors, testSpinUntilFutureComplete) {
-  rclcpp::executors::SingleThreadedExecutor executor;
-  std::future<void> future;
-  rclcpp::FutureReturnCode ret;
-
-  // test success
-  future = std::async(
-    []() {
-      return;
-    });
-  ret = executor.spin_until_future_complete(future, 1s);
-  EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret);
-
-  // test timeout
-  future = std::async(
-    []() {
-      std::this_thread::sleep_for(1s);
-      return;
-    });
-  ret = executor.spin_until_future_complete(future, 100ms);
-  EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret);
-}
-
-// Make sure that the spin_until_future_complete works correctly with std::shared_future
-TEST_F(TestExecutors, testSpinUntilFutureCompleteSharedFuture) {
-  rclcpp::executors::SingleThreadedExecutor executor;
-  std::future<void> future;
-  rclcpp::FutureReturnCode ret;
-
-  // test success
-  future = std::async(
-    []() {
-      return;
-    });
-  ret = executor.spin_until_future_complete(future.share(), 1s);
-  EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret);
-
-  // test timeout
-  future = std::async(
-    []() {
-      std::this_thread::sleep_for(1s);
-      return;
-    });
-  ret = executor.spin_until_future_complete(future.share(), 100ms);
-  EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret);
-}
-
-class TestWaitable : public rclcpp::Waitable
-{
-public:
-  TestWaitable()
-  {
-    rcl_guard_condition_options_t guard_condition_options =
-      rcl_guard_condition_get_default_options();
-
-    gc_ = rcl_get_zero_initialized_guard_condition();
-    rcl_ret_t ret = rcl_guard_condition_init(
-      &gc_,
-      rclcpp::contexts::get_global_default_context()->get_rcl_context().get(),
-      guard_condition_options);
-    if (RCL_RET_OK != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret);
-    }
-  }
-
-  bool
-  add_to_wait_set(rcl_wait_set_t * wait_set) override
-  {
-    rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &gc_, NULL);
-    if (RCL_RET_OK != ret) {
-      return false;
-    }
-    ret = rcl_trigger_guard_condition(&gc_);
-    return RCL_RET_OK == ret;
-  }
-
-  bool
-  is_ready(rcl_wait_set_t * wait_set) override
-  {
-    (void)wait_set;
-    return true;
-  }
-
-  void
-  execute() override
-  {
-    count_++;
-    std::this_thread::sleep_for(100ms);
-  }
-
-  size_t
-  get_number_of_ready_guard_conditions() override {return 1;}
-
-  size_t
-  get_count()
-  {
-    return count_;
-  }
-
-private:
-  size_t count_ = 0;
-  rcl_guard_condition_t gc_;
-};
-
-TEST_F(TestExecutors, testSpinAllvsSpinSome) {
-  {
-    rclcpp::executors::SingleThreadedExecutor executor;
-    auto waitable_interfaces = node->get_node_waitables_interface();
-    auto my_waitable = std::make_shared<TestWaitable>();
-    waitable_interfaces->add_waitable(my_waitable, nullptr);
-    executor.add_node(node);
-    executor.spin_all(1s);
-    executor.remove_node(node);
-    EXPECT_GT(my_waitable->get_count(), 1u);
-    waitable_interfaces->remove_waitable(my_waitable, nullptr);
-  }
-  {
-    rclcpp::executors::SingleThreadedExecutor executor;
-    auto waitable_interfaces = node->get_node_waitables_interface();
-    auto my_waitable = std::make_shared<TestWaitable>();
-    waitable_interfaces->add_waitable(my_waitable, nullptr);
-    executor.add_node(node);
-    executor.spin_some(1s);
-    executor.remove_node(node);
-    EXPECT_EQ(my_waitable->get_count(), 1u);
-    waitable_interfaces->remove_waitable(my_waitable, nullptr);
-  }
-}

rclcpp/test/rclcpp/test_find_weak_nodes.cpp

@@ -36,22 +36,24 @@ TEST_F(TestFindWeakNodes, allocator_strategy_with_weak_nodes) {
     rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>();
   auto existing_node = rclcpp::Node::make_shared("existing_node");
   auto dead_node = rclcpp::Node::make_shared("dead_node");
-  rclcpp::memory_strategy::MemoryStrategy::WeakNodeList weak_nodes;
-  weak_nodes.push_back(existing_node->get_node_base_interface());
-  weak_nodes.push_back(dead_node->get_node_base_interface());
+  auto existing_group = existing_node->get_node_base_interface()->get_default_callback_group();
+  auto dead_group = dead_node->get_node_base_interface()->get_default_callback_group();
+  rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
+  weak_groups_to_nodes[existing_group] = existing_node->get_node_base_interface();
+  weak_groups_to_nodes[dead_group] = dead_node->get_node_base_interface();
 
   // AND
   // Delete dead_node, creating a dangling pointer in weak_nodes
   dead_node.reset();
-  ASSERT_FALSE(weak_nodes.front().expired());
-  ASSERT_TRUE(weak_nodes.back().expired());
+  ASSERT_FALSE(weak_groups_to_nodes[existing_group].expired());
+  ASSERT_TRUE(weak_groups_to_nodes[dead_group].expired());
 
   // WHEN
-  bool has_invalid_weak_nodes = memory_strategy->collect_entities(weak_nodes);
+  bool has_invalid_weak_groups_or_nodes = memory_strategy->collect_entities(weak_groups_to_nodes);
 
   // THEN
   // The result of finding dangling node pointers should be true
-  ASSERT_TRUE(has_invalid_weak_nodes);
+  ASSERT_TRUE(has_invalid_weak_groups_or_nodes);
 
   // Prevent memory leak due to the order of destruction
   memory_strategy->clear_handles();
@@ -64,18 +66,20 @@ TEST_F(TestFindWeakNodes, allocator_strategy_no_weak_nodes) {
     rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>();
   auto existing_node1 = rclcpp::Node::make_shared("existing_node1");
   auto existing_node2 = rclcpp::Node::make_shared("existing_node2");
-  rclcpp::memory_strategy::MemoryStrategy::WeakNodeList weak_nodes;
-  weak_nodes.push_back(existing_node1->get_node_base_interface());
-  weak_nodes.push_back(existing_node2->get_node_base_interface());
-  ASSERT_FALSE(weak_nodes.front().expired());
-  ASSERT_FALSE(weak_nodes.back().expired());
+  auto existing_group1 = existing_node1->get_node_base_interface()->get_default_callback_group();
+  auto existing_group2 = existing_node2->get_node_base_interface()->get_default_callback_group();
+  rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
+  weak_groups_to_nodes[existing_group1] = existing_node1->get_node_base_interface();
+  weak_groups_to_nodes[existing_group2] = existing_node2->get_node_base_interface();
+  ASSERT_FALSE(weak_groups_to_nodes[existing_group1].expired());
+  ASSERT_FALSE(weak_groups_to_nodes[existing_group2].expired());
 
   // WHEN
-  bool has_invalid_weak_nodes = memory_strategy->collect_entities(weak_nodes);
+  bool has_invalid_weak_groups_or_nodes = memory_strategy->collect_entities(weak_groups_to_nodes);
 
   // THEN
   // The result of finding dangling node pointers should be false
-  ASSERT_FALSE(has_invalid_weak_nodes);
+  ASSERT_FALSE(has_invalid_weak_groups_or_nodes);
 
   // Prevent memory leak due to the order of destruction
   memory_strategy->clear_handles();

rclcpp/test/rclcpp/test_init_options.cpp

@@ -0,0 +1,77 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <vector>
+
+#include "rcl/allocator.h"
+#include "rcl/domain_id.h"
+
+#include "rclcpp/init_options.hpp"
+
+
+TEST(TestInitOptions, test_construction) {
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto options = rclcpp::InitOptions(allocator);
+  const rcl_init_options_t * rcl_options = options.get_rcl_init_options();
+  ASSERT_TRUE(rcl_options != nullptr);
+  ASSERT_TRUE(rcl_options->impl != nullptr);
+
+  {
+    auto options_copy = rclcpp::InitOptions(options);
+    const rcl_init_options_t * rcl_options_copy = options_copy.get_rcl_init_options();
+    ASSERT_TRUE(rcl_options_copy != nullptr);
+    ASSERT_TRUE(rcl_options_copy->impl != nullptr);
+  }
+
+  {
+    auto options_copy = options;
+    const rcl_init_options_t * rcl_options_copy = options_copy.get_rcl_init_options();
+    ASSERT_TRUE(rcl_options_copy != nullptr);
+    ASSERT_TRUE(rcl_options_copy->impl != nullptr);
+  }
+}
+
+TEST(TestInitOptions, test_initialize_logging) {
+  {
+    auto options = rclcpp::InitOptions();
+    EXPECT_TRUE(options.auto_initialize_logging());
+  }
+
+  {
+    auto options = rclcpp::InitOptions().auto_initialize_logging(true);
+    EXPECT_TRUE(options.auto_initialize_logging());
+  }
+
+  {
+    auto options = rclcpp::InitOptions().auto_initialize_logging(false);
+    EXPECT_FALSE(options.auto_initialize_logging());
+  }
+}
+
+TEST(TestInitOptions, test_domain_id) {
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto options = rclcpp::InitOptions(allocator);
+  size_t domain_id = RCL_DEFAULT_DOMAIN_ID;
+  EXPECT_EQ(RCL_RET_OK, rcl_get_default_domain_id(&domain_id));
+
+  options.use_default_domain_id();
+  EXPECT_EQ(domain_id, options.get_domain_id());
+  options.set_domain_id(42);
+  EXPECT_EQ((size_t)42, options.get_domain_id());
+  options.use_default_domain_id();
+  EXPECT_EQ(domain_id, options.get_domain_id());
+}

rclcpp/test/rclcpp/test_logger.cpp

@@ -44,6 +44,114 @@ TEST(TestLogger, get_node_logger) {
   EXPECT_STREQ(logger.get_name(), "ns.my_node");
 
   logger = rclcpp::get_node_logger(nullptr);
+  rcl_reset_error();
   EXPECT_STREQ(logger.get_name(), "rclcpp");
   rclcpp::shutdown();
 }
+
+struct LogEvent
+{
+  bool console_output_handler_called;
+  std::string message;
+};
+LogEvent g_last_log_event;
+
+TEST(TestLogger, set_level) {
+  ASSERT_EQ(RCUTILS_RET_OK, rcutils_logging_initialize());
+
+  rclcpp::Logger logger = rclcpp::get_logger("test_logger");
+  EXPECT_THROW(
+  {
+    logger.set_level(static_cast<rclcpp::Logger::Level>(99999));
+  }, rclcpp::exceptions::RCLInvalidArgument);
+
+  auto rcutils_logging_console_output_handler = [](
+    const rcutils_log_location_t *,
+    int, const char *, rcutils_time_point_value_t,
+    const char * format, va_list * args) -> void
+    {
+      g_last_log_event.console_output_handler_called = true;
+      char buffer[1024];
+      vsnprintf(buffer, sizeof(buffer), format, *args);
+      g_last_log_event.message = buffer;
+    };
+
+  rcutils_logging_output_handler_t previous_output_handler = rcutils_logging_get_output_handler();
+  rcutils_logging_set_output_handler(rcutils_logging_console_output_handler);
+
+  // default
+  RCLCPP_DEBUG(logger, std::string("message %s"), "debug");
+  EXPECT_FALSE(g_last_log_event.console_output_handler_called);
+  RCLCPP_INFO(logger, std::string("message %s"), "info");
+  EXPECT_TRUE(g_last_log_event.console_output_handler_called);
+  EXPECT_EQ("message info", g_last_log_event.message);
+
+  // unset
+  g_last_log_event.console_output_handler_called = false;
+  logger.set_level(rclcpp::Logger::Level::Unset);
+  RCLCPP_DEBUG(logger, std::string("message %s"), "debug");
+  EXPECT_FALSE(g_last_log_event.console_output_handler_called);
+  RCLCPP_INFO(logger, std::string("message %s"), "info");
+  EXPECT_TRUE(g_last_log_event.console_output_handler_called);
+  EXPECT_EQ("message info", g_last_log_event.message);
+
+  // debug
+  g_last_log_event.console_output_handler_called = false;
+  logger.set_level(rclcpp::Logger::Level::Debug);
+  RCLCPP_DEBUG(logger, std::string("message %s"), "debug");
+  EXPECT_TRUE(g_last_log_event.console_output_handler_called);
+  EXPECT_EQ("message debug", g_last_log_event.message);
+  RCLCPP_INFO(logger, std::string("message %s"), "info");
+  EXPECT_EQ("message info", g_last_log_event.message);
+
+  // info
+  g_last_log_event.console_output_handler_called = false;
+  logger.set_level(rclcpp::Logger::Level::Info);
+  RCLCPP_DEBUG(logger, std::string("message %s"), "debug");
+  EXPECT_FALSE(g_last_log_event.console_output_handler_called);
+  RCLCPP_INFO(logger, std::string("message %s"), "info");
+  EXPECT_TRUE(g_last_log_event.console_output_handler_called);
+  EXPECT_EQ("message info", g_last_log_event.message);
+
+  // warn
+  g_last_log_event.console_output_handler_called = false;
+  logger.set_level(rclcpp::Logger::Level::Warn);
+  RCLCPP_DEBUG(logger, std::string("message %s"), "debug");
+  EXPECT_FALSE(g_last_log_event.console_output_handler_called);
+  RCLCPP_INFO(logger, std::string("message %s"), "info");
+  EXPECT_FALSE(g_last_log_event.console_output_handler_called);
+  RCLCPP_WARN(logger, std::string("message %s"), "warn");
+  EXPECT_TRUE(g_last_log_event.console_output_handler_called);
+  EXPECT_EQ("message warn", g_last_log_event.message);
+
+  // error
+  g_last_log_event.console_output_handler_called = false;
+  logger.set_level(rclcpp::Logger::Level::Error);
+  RCLCPP_DEBUG(logger, std::string("message %s"), "debug");
+  EXPECT_FALSE(g_last_log_event.console_output_handler_called);
+  RCLCPP_INFO(logger, std::string("message %s"), "info");
+  EXPECT_FALSE(g_last_log_event.console_output_handler_called);
+  RCLCPP_WARN(logger, std::string("message %s"), "warn");
+  EXPECT_FALSE(g_last_log_event.console_output_handler_called);
+  RCLCPP_ERROR(logger, std::string("message %s"), "error");
+  EXPECT_TRUE(g_last_log_event.console_output_handler_called);
+  EXPECT_EQ("message error", g_last_log_event.message);
+
+  // fatal
+  g_last_log_event.console_output_handler_called = false;
+  logger.set_level(rclcpp::Logger::Level::Fatal);
+  RCLCPP_DEBUG(logger, std::string("message %s"), "debug");
+  EXPECT_FALSE(g_last_log_event.console_output_handler_called);
+  RCLCPP_INFO(logger, std::string("message %s"), "info");
+  EXPECT_FALSE(g_last_log_event.console_output_handler_called);
+  RCLCPP_WARN(logger, std::string("message %s"), "warn");
+  EXPECT_FALSE(g_last_log_event.console_output_handler_called);
+  RCLCPP_ERROR(logger, std::string("message %s"), "error");
+  EXPECT_FALSE(g_last_log_event.console_output_handler_called);
+  RCLCPP_FATAL(logger, std::string("message %s"), "fatal");
+  EXPECT_TRUE(g_last_log_event.console_output_handler_called);
+  EXPECT_EQ("message fatal", g_last_log_event.message);
+
+  rcutils_logging_set_output_handler(previous_output_handler);
+  EXPECT_EQ(RCUTILS_RET_OK, rcutils_logging_shutdown());
+}

rclcpp/test/rclcpp/test_memory_strategy.cpp

@@ -0,0 +1,531 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <list>
+#include <map>
+#include <memory>
+#include <utility>
+
+#include "rclcpp/strategies/allocator_memory_strategy.hpp"
+#include "rclcpp/memory_strategy.hpp"
+#include "test_msgs/msg/empty.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+using rclcpp::memory_strategy::MemoryStrategy;
+typedef std::map<rclcpp::CallbackGroup::WeakPtr,
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+    std::owner_less<rclcpp::CallbackGroup::WeakPtr>> WeakCallbackGroupsToNodesMap;
+
+/**
+ * Mock Waitable class
+ */
+class TestWaitable : public rclcpp::Waitable
+{
+public:
+  bool add_to_wait_set(rcl_wait_set_t *) override {return true;}
+  bool is_ready(rcl_wait_set_t *) override {return true;}
+  void execute() override {}
+};
+
+class TestMemoryStrategy : public ::testing::Test
+{
+public:
+  TestMemoryStrategy()
+  : memory_strategy_(nullptr) {}
+
+  void SetUp() override
+  {
+    rclcpp::init(0, nullptr);
+
+    // This doesn't test AllocatorMemoryStrategy directly, so we cast to the base class.
+    // AllocatorMemoryStrategy is more commonly used than MessagePoolMemoryStrategy
+    // so we use this derived class for these tests.
+    memory_strategy_ =
+      std::make_shared<
+      rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>();
+  }
+
+  void TearDown() override
+  {
+    rclcpp::shutdown();
+  }
+
+protected:
+  std::shared_ptr<MemoryStrategy> memory_strategy()
+  {
+    return memory_strategy_;
+  }
+
+private:
+  std::shared_ptr<MemoryStrategy> memory_strategy_;
+};
+
+TEST_F(TestMemoryStrategy, construct_destruct) {
+  EXPECT_NE(nullptr, memory_strategy());
+}
+
+TEST_F(TestMemoryStrategy, get_subscription_by_handle) {
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
+  std::shared_ptr<const rcl_subscription_t> subscription_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) {
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            weak_group_ptr,
+            node->get_node_base_interface()));
+      });
+    memory_strategy()->collect_entities(weak_groups_to_nodes);
+    {
+      auto callback_group =
+        node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+      auto subscription_callback = [](const test_msgs::msg::Empty::SharedPtr) {};
+      const rclcpp::QoS qos(10);
+
+      {
+        auto subscription = node->create_subscription<
+          test_msgs::msg::Empty, decltype(subscription_callback)>(
+          "topic", qos, std::move(subscription_callback));
+
+        subscription_handle = subscription->get_subscription_handle();
+
+        EXPECT_EQ(
+          subscription,
+          memory_strategy()->get_subscription_by_handle(subscription_handle, weak_groups_to_nodes));
+      }  // subscription goes out of scope
+      EXPECT_EQ(
+        nullptr,
+        memory_strategy()->get_subscription_by_handle(subscription_handle, weak_groups_to_nodes));
+    }  // callback_group goes out of scope
+    EXPECT_EQ(
+      nullptr,
+      memory_strategy()->get_subscription_by_handle(subscription_handle, weak_groups_to_nodes));
+  }  // Node goes out of scope
+  EXPECT_EQ(
+    nullptr,
+    memory_strategy()->get_subscription_by_handle(subscription_handle, weak_groups_to_nodes));
+}
+
+TEST_F(TestMemoryStrategy, get_service_by_handle) {
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
+  std::shared_ptr<const rcl_service_t> service_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) {
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            weak_group_ptr,
+            node->get_node_base_interface()));
+      });
+    memory_strategy()->collect_entities(weak_groups_to_nodes);
+    {
+      auto callback_group =
+        node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+      auto service_callback =
+        [](const test_msgs::srv::Empty::Request::SharedPtr,
+          test_msgs::srv::Empty::Response::SharedPtr) {};
+      const rclcpp::QoS qos(10);
+      weak_groups_to_nodes.insert(
+        std::pair<rclcpp::CallbackGroup::WeakPtr,
+        rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+          rclcpp::CallbackGroup::WeakPtr(callback_group),
+          node->get_node_base_interface()));
+      {
+        auto service = node->create_service<test_msgs::srv::Empty>(
+          "service", std::move(service_callback),
+          rmw_qos_profile_services_default, callback_group);
+
+        service_handle = service->get_service_handle();
+
+        EXPECT_EQ(
+          service,
+          memory_strategy()->get_service_by_handle(service_handle, weak_groups_to_nodes));
+      }  // service goes out of scope
+      EXPECT_EQ(
+        nullptr,
+        memory_strategy()->get_service_by_handle(service_handle, weak_groups_to_nodes));
+    }  // callback_group goes out of scope
+    EXPECT_EQ(
+      nullptr,
+      memory_strategy()->get_service_by_handle(service_handle, weak_groups_to_nodes));
+  }  // Node goes out of scope
+  EXPECT_EQ(
+    nullptr,
+    memory_strategy()->get_service_by_handle(service_handle, weak_groups_to_nodes));
+}
+
+TEST_F(TestMemoryStrategy, get_client_by_handle) {
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
+  std::shared_ptr<const rcl_client_t> client_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) {
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            weak_group_ptr,
+            node->get_node_base_interface()));
+      });
+    memory_strategy()->collect_entities(weak_groups_to_nodes);
+    {
+      auto callback_group =
+        node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+      {
+        auto client = node->create_client<test_msgs::srv::Empty>(
+          "service", rmw_qos_profile_services_default, callback_group);
+
+        client_handle = client->get_client_handle();
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            rclcpp::CallbackGroup::WeakPtr(callback_group),
+            node->get_node_base_interface()));
+
+        EXPECT_EQ(
+          client,
+          memory_strategy()->get_client_by_handle(client_handle, weak_groups_to_nodes));
+      }  // client goes out of scope
+      EXPECT_EQ(
+        nullptr,
+        memory_strategy()->get_client_by_handle(client_handle, weak_groups_to_nodes));
+    }  // callback_group goes out of scope
+    EXPECT_EQ(
+      nullptr,
+      memory_strategy()->get_client_by_handle(client_handle, weak_groups_to_nodes));
+  }  // Node goes out of scope
+  EXPECT_EQ(
+    nullptr,
+    memory_strategy()->get_client_by_handle(client_handle, weak_groups_to_nodes));
+}
+
+TEST_F(TestMemoryStrategy, get_timer_by_handle) {
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
+  std::shared_ptr<const rcl_timer_t> timer_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) {
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            weak_group_ptr,
+            node->get_node_base_interface()));
+      });
+    memory_strategy()->collect_entities(weak_groups_to_nodes);
+    {
+      auto callback_group =
+        node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+      {
+        auto timer_callback = []() {};
+        auto timer = node->create_wall_timer(
+          std::chrono::milliseconds(1), timer_callback, callback_group);
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            rclcpp::CallbackGroup::WeakPtr(callback_group),
+            node->get_node_base_interface()));
+
+        timer_handle = timer->get_timer_handle();
+
+        EXPECT_EQ(
+          timer,
+          memory_strategy()->get_timer_by_handle(timer_handle, weak_groups_to_nodes));
+      }  // timer goes out of scope
+      EXPECT_EQ(
+        nullptr,
+        memory_strategy()->get_timer_by_handle(timer_handle, weak_groups_to_nodes));
+    }  // callback_group goes out of scope
+    EXPECT_EQ(
+      nullptr,
+      memory_strategy()->get_timer_by_handle(timer_handle, weak_groups_to_nodes));
+  }  // Node goes out of scope
+  EXPECT_EQ(
+    nullptr,
+    memory_strategy()->get_timer_by_handle(timer_handle, weak_groups_to_nodes));
+}
+
+TEST_F(TestMemoryStrategy, get_node_by_group) {
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
+  rclcpp::CallbackGroup::SharedPtr callback_group = nullptr;
+  {
+    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) {
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            weak_group_ptr,
+            node_handle));
+      });
+    memory_strategy()->collect_entities(weak_groups_to_nodes);
+    EXPECT_EQ(
+      nullptr,
+      memory_strategy()->get_node_by_group(nullptr, weak_groups_to_nodes));
+
+    callback_group =
+      node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+    weak_groups_to_nodes.insert(
+      std::pair<rclcpp::CallbackGroup::WeakPtr,
+      rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+        rclcpp::CallbackGroup::WeakPtr(callback_group),
+        node->get_node_base_interface()));
+    EXPECT_EQ(
+      node_handle,
+      memory_strategy()->get_node_by_group(callback_group, weak_groups_to_nodes));
+  }  // Node goes out of scope
+  EXPECT_EQ(
+    nullptr,
+    memory_strategy()->get_node_by_group(callback_group, weak_groups_to_nodes));
+}
+
+TEST_F(TestMemoryStrategy, get_group_by_subscription) {
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
+  rclcpp::SubscriptionBase::SharedPtr subscription = nullptr;
+  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) {
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            weak_group_ptr,
+            node->get_node_base_interface()));
+      });
+    memory_strategy()->collect_entities(weak_groups_to_nodes);
+    {
+      // This group is just used to test that a callback group that is held as a weak pointer
+      // by node, doesn't confuse get_group_by_subscription() when it goes out of scope
+      auto non_persistant_group =
+        node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+
+      callback_group =
+        node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+      auto subscription_callback = [](const test_msgs::msg::Empty::SharedPtr) {};
+      const rclcpp::QoS qos(10);
+
+      rclcpp::SubscriptionOptions subscription_options;
+
+      // This callback group is held as a shared_ptr in subscription_options, which means it
+      // stays alive as long as subscription does.
+      subscription_options.callback_group = callback_group;
+
+      subscription = node->create_subscription<
+        test_msgs::msg::Empty, decltype(subscription_callback)>(
+        "topic", qos, std::move(subscription_callback), subscription_options);
+      weak_groups_to_nodes.insert(
+        std::pair<rclcpp::CallbackGroup::WeakPtr,
+        rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+          rclcpp::CallbackGroup::WeakPtr(callback_group),
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr(node->get_node_base_interface())));
+      EXPECT_EQ(
+        callback_group,
+        memory_strategy()->get_group_by_subscription(subscription, weak_groups_to_nodes));
+    }  // callback_group goes out of scope
+    EXPECT_EQ(
+      callback_group,
+      memory_strategy()->get_group_by_subscription(subscription, weak_groups_to_nodes));
+  }  // Node goes out of scope
+  EXPECT_EQ(
+    nullptr,
+    memory_strategy()->get_group_by_subscription(subscription, weak_groups_to_nodes));
+}
+
+TEST_F(TestMemoryStrategy, get_group_by_service) {
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
+  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) {
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            weak_group_ptr,
+            node->get_node_base_interface()));
+      });
+    memory_strategy()->collect_entities(weak_groups_to_nodes);
+    {
+      auto callback_group =
+        node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+      auto service_callback =
+        [](const test_msgs::srv::Empty::Request::SharedPtr,
+          test_msgs::srv::Empty::Response::SharedPtr) {};
+      const rclcpp::QoS qos(10);
+
+      service = node->create_service<test_msgs::srv::Empty>(
+        "service", std::move(service_callback),
+        rmw_qos_profile_services_default, callback_group);
+      weak_groups_to_nodes.insert(
+        std::pair<rclcpp::CallbackGroup::WeakPtr,
+        rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+          rclcpp::CallbackGroup::WeakPtr(callback_group),
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr(node->get_node_base_interface())));
+      EXPECT_EQ(
+        callback_group,
+        memory_strategy()->get_group_by_service(service, weak_groups_to_nodes));
+    }  // callback_group goes out of scope
+    EXPECT_EQ(
+      nullptr,
+      memory_strategy()->get_group_by_service(service, weak_groups_to_nodes));
+  }  // Node goes out of scope
+  EXPECT_EQ(
+    nullptr,
+    memory_strategy()->get_group_by_service(service, weak_groups_to_nodes));
+}
+
+TEST_F(TestMemoryStrategy, get_group_by_client) {
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
+  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) {
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            weak_group_ptr,
+            node->get_node_base_interface()));
+      });
+    memory_strategy()->collect_entities(weak_groups_to_nodes);
+    {
+      auto callback_group =
+        node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+
+      client = node->create_client<test_msgs::srv::Empty>(
+        "service", rmw_qos_profile_services_default, callback_group);
+      weak_groups_to_nodes.insert(
+        std::pair<rclcpp::CallbackGroup::WeakPtr,
+        rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+          rclcpp::CallbackGroup::WeakPtr(callback_group),
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr(node->get_node_base_interface())));
+      EXPECT_EQ(
+        callback_group,
+        memory_strategy()->get_group_by_client(client, weak_groups_to_nodes));
+    }  // callback_group goes out of scope
+    EXPECT_EQ(
+      nullptr,
+      memory_strategy()->get_group_by_client(client, weak_groups_to_nodes));
+  }  // Node goes out of scope
+  EXPECT_EQ(
+    nullptr,
+    memory_strategy()->get_group_by_client(client, weak_groups_to_nodes));
+}
+
+TEST_F(TestMemoryStrategy, get_group_by_timer) {
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
+  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) {
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            weak_group_ptr,
+            node->get_node_base_interface()));
+      });
+    memory_strategy()->collect_entities(weak_groups_to_nodes);
+    {
+      auto callback_group =
+        node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+      auto timer_callback = []() {};
+      timer = node->create_wall_timer(
+        std::chrono::milliseconds(1), timer_callback, callback_group);
+      weak_groups_to_nodes.insert(
+        std::pair<rclcpp::CallbackGroup::WeakPtr,
+        rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+          rclcpp::CallbackGroup::WeakPtr(callback_group),
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr(node->get_node_base_interface())));
+      EXPECT_EQ(
+        callback_group,
+        memory_strategy()->get_group_by_timer(timer, weak_groups_to_nodes));
+    }  // callback_group goes out of scope
+    EXPECT_EQ(
+      nullptr,
+      memory_strategy()->get_group_by_timer(timer, weak_groups_to_nodes));
+  }  // Node goes out of scope
+  EXPECT_EQ(
+    nullptr,
+    memory_strategy()->get_group_by_timer(timer, weak_groups_to_nodes));
+}
+
+TEST_F(TestMemoryStrategy, get_group_by_waitable) {
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
+  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) {
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            weak_group_ptr,
+            node->get_node_base_interface()));
+      });
+    memory_strategy()->collect_entities(weak_groups_to_nodes);
+    {
+      waitable = std::make_shared<TestWaitable>();
+      auto callback_group =
+        node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+      node->get_node_waitables_interface()->add_waitable(waitable, callback_group);
+      weak_groups_to_nodes.insert(
+        std::pair<rclcpp::CallbackGroup::WeakPtr,
+        rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+          rclcpp::CallbackGroup::WeakPtr(callback_group),
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr(node->get_node_base_interface())));
+      EXPECT_EQ(
+        callback_group,
+        memory_strategy()->get_group_by_waitable(waitable, weak_groups_to_nodes));
+    }  // callback_group goes out of scope
+    EXPECT_EQ(
+      nullptr,
+      memory_strategy()->get_group_by_waitable(waitable, weak_groups_to_nodes));
+  }  // Node goes out of scope
+  EXPECT_EQ(
+    nullptr,
+    memory_strategy()->get_group_by_waitable(waitable, weak_groups_to_nodes));
+}

rclcpp/test/rclcpp/test_message_memory_strategy.cpp

@@ -0,0 +1,58 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+
+#include "rclcpp/message_memory_strategy.hpp"
+#include "test_msgs/msg/empty.hpp"
+
+TEST(TestMemoryStrategies, construct_destruct) {
+  rclcpp::message_memory_strategy::MessageMemoryStrategy<test_msgs::msg::Empty> memory_strategy1;
+
+  EXPECT_NE(nullptr, memory_strategy1.message_allocator_);
+  EXPECT_NE(nullptr, memory_strategy1.serialized_message_allocator_);
+  EXPECT_NE(nullptr, memory_strategy1.buffer_allocator_);
+
+  auto allocator = std::make_shared<std::allocator<void>>();
+  rclcpp::message_memory_strategy::MessageMemoryStrategy<test_msgs::msg::Empty> memory_strategy2(
+    allocator);
+
+  EXPECT_NE(nullptr, memory_strategy2.message_allocator_);
+  EXPECT_NE(nullptr, memory_strategy2.serialized_message_allocator_);
+  EXPECT_NE(nullptr, memory_strategy2.buffer_allocator_);
+}
+
+TEST(TestMemoryStrategies, standard_allocation) {
+  auto memory_strategy =
+    rclcpp::message_memory_strategy::MessageMemoryStrategy<
+    test_msgs::msg::Empty>::create_default();
+  ASSERT_NE(nullptr, memory_strategy);
+
+  auto borrowed_message = memory_strategy->borrow_message();
+  ASSERT_NE(nullptr, borrowed_message);
+  EXPECT_NO_THROW(memory_strategy->return_message(borrowed_message));
+
+  auto serialized_message = memory_strategy->borrow_serialized_message();
+  ASSERT_NE(nullptr, serialized_message);
+  EXPECT_EQ(0u, serialized_message->capacity());
+  EXPECT_NO_THROW(memory_strategy->return_serialized_message(serialized_message));
+
+  memory_strategy->set_default_buffer_capacity(42);
+  serialized_message = memory_strategy->borrow_serialized_message();
+  ASSERT_NE(nullptr, serialized_message);
+  EXPECT_EQ(42u, serialized_message->capacity());
+  EXPECT_NO_THROW(memory_strategy->return_serialized_message(serialized_message));
+}

rclcpp/test/rclcpp/test_publisher.cpp

@@ -203,3 +203,59 @@ TEST_F(TestPublisherSub, construction_and_destruction) {
     }, rclcpp::exceptions::InvalidTopicNameError);
   }
 }
+
+// Auxiliary class used to test getter for const PublisherBase
+const rosidl_message_type_support_t EmptyTypeSupport()
+{
+  return *rosidl_typesupport_cpp::get_message_type_support_handle<test_msgs::msg::Empty>();
+}
+
+const rcl_publisher_options_t PublisherOptions()
+{
+  return rclcpp::PublisherOptionsWithAllocator<std::allocator<void>>().template
+         to_rcl_publisher_options<test_msgs::msg::Empty>(rclcpp::QoS(10));
+}
+
+class TestPublisherBase : public rclcpp::PublisherBase
+{
+public:
+  explicit TestPublisherBase(rclcpp::Node * node)
+  : rclcpp::PublisherBase(
+      node->get_node_base_interface().get(), "topic", EmptyTypeSupport(), PublisherOptions()) {}
+};
+
+/*
+   Testing some publisher getters
+ */
+TEST_F(TestPublisher, basic_getters) {
+  initialize();
+  using test_msgs::msg::Empty;
+  {
+    using rclcpp::QoS;
+    using rclcpp::KeepLast;
+    const size_t qos_depth_size = 10u;
+    auto publisher = node->create_publisher<Empty>("topic", QoS(KeepLast(qos_depth_size)));
+
+    size_t publisher_queue_size = publisher->get_queue_size();
+    EXPECT_EQ(qos_depth_size, publisher_queue_size);
+
+    const rmw_gid_t & publisher_rmw_gid = publisher->get_gid();
+    EXPECT_NE(nullptr, publisher_rmw_gid.implementation_identifier);
+
+    std::shared_ptr<rcl_publisher_t> publisher_handle = publisher->get_publisher_handle();
+    EXPECT_NE(nullptr, publisher_handle);
+
+    EXPECT_TRUE(publisher->assert_liveliness());
+  }
+  {
+    const TestPublisherBase publisher = TestPublisherBase(node.get());
+    std::shared_ptr<const rcl_publisher_t> publisher_handle = publisher.get_publisher_handle();
+    EXPECT_NE(nullptr, publisher_handle);
+
+    const rmw_gid_t & publisher_rmw_gid = publisher.get_gid();
+    EXPECT_NE(nullptr, publisher_rmw_gid.implementation_identifier);
+
+    // Test == operator of publisher with rmw_gid_t
+    EXPECT_EQ(publisher, publisher_rmw_gid);
+  }
+}

rclcpp/test/rclcpp/test_serialized_message.cpp

@@ -140,6 +140,20 @@ TEST(TestSerializedMessage, release) {
   EXPECT_EQ(RCL_RET_OK, rmw_serialized_message_fini(&released_handle));
 }
 
+TEST(TestSerializedMessage, reserve) {
+  rclcpp::SerializedMessage serialized_msg(13);
+  EXPECT_EQ(13u, serialized_msg.capacity());
+
+  // Resize using reserve method
+  serialized_msg.reserve(15);
+  EXPECT_EQ(15u, serialized_msg.capacity());
+
+  // Use invalid value throws exception
+  EXPECT_THROW(
+    {serialized_msg.reserve(-1);},
+    rclcpp::exceptions::RCLBadAlloc);
+}
+
 TEST(TestSerializedMessage, serialization) {
   using MessageT = test_msgs::msg::BasicTypes;
 
@@ -159,6 +173,85 @@ TEST(TestSerializedMessage, serialization) {
   }
 }
 
+TEST(TestSerializedMessage, assignment_operators) {
+  const std::string content = "Hello World";
+  const auto content_size = content.size() + 1;  // accounting for null terminator
+  auto default_allocator = rcl_get_default_allocator();
+  auto rcl_serialized_msg = rmw_get_zero_initialized_serialized_message();
+  auto ret = rmw_serialized_message_init(&rcl_serialized_msg, 13, &default_allocator);
+  ASSERT_EQ(RCL_RET_OK, ret);
+
+  // manually copy some content
+  std::memcpy(rcl_serialized_msg.buffer, content.c_str(), content_size);
+  rcl_serialized_msg.buffer_length = content_size;
+  EXPECT_EQ(13u, rcl_serialized_msg.buffer_capacity);
+  rclcpp::SerializedMessage serialized_message_to_assign(rcl_serialized_msg);
+  EXPECT_EQ(13u, serialized_message_to_assign.capacity());
+  EXPECT_EQ(content_size, serialized_message_to_assign.size());
+
+  // Test copy assignment with = operator, on another rclcpp::SerializedMessage
+  rclcpp::SerializedMessage serialized_msg_copy(2);
+  EXPECT_EQ(2u, serialized_msg_copy.capacity());
+  EXPECT_EQ(0u, serialized_msg_copy.size());
+  serialized_msg_copy = serialized_message_to_assign;
+  EXPECT_EQ(13u, serialized_msg_copy.capacity());
+  EXPECT_EQ(content_size, serialized_msg_copy.size());
+
+  // Test copy assignment with = operator, with a rcl_serialized_message_t
+  rclcpp::SerializedMessage serialized_msg_copy_rcl(2);
+  EXPECT_EQ(2u, serialized_msg_copy_rcl.capacity());
+  EXPECT_EQ(0u, serialized_msg_copy_rcl.size());
+  serialized_msg_copy_rcl = rcl_serialized_msg;
+  EXPECT_EQ(13u, serialized_msg_copy_rcl.capacity());
+  EXPECT_EQ(content_size, serialized_msg_copy_rcl.size());
+
+  // Test move assignment with = operator
+  rclcpp::SerializedMessage serialized_msg_move(2);
+  EXPECT_EQ(2u, serialized_msg_move.capacity());
+  EXPECT_EQ(0u, serialized_msg_move.size());
+  serialized_msg_move = std::move(serialized_message_to_assign);
+  EXPECT_EQ(13u, serialized_msg_move.capacity());
+  EXPECT_EQ(content_size, serialized_msg_move.size());
+  EXPECT_EQ(0u, serialized_message_to_assign.capacity());
+  EXPECT_EQ(0u, serialized_message_to_assign.size());
+
+  // Test move assignment with = operator, with a rcl_serialized_message_t
+  rclcpp::SerializedMessage serialized_msg_move_rcl(2);
+  EXPECT_EQ(2u, serialized_msg_move_rcl.capacity());
+  EXPECT_EQ(0u, serialized_msg_move_rcl.size());
+  serialized_msg_move_rcl = std::move(rcl_serialized_msg);
+  EXPECT_EQ(13u, serialized_msg_move_rcl.capacity());
+  EXPECT_EQ(content_size, serialized_msg_move_rcl.size());
+  EXPECT_EQ(0u, rcl_serialized_msg.buffer_capacity);
+
+  // Error because it was moved
+  EXPECT_EQ(RCUTILS_RET_INVALID_ARGUMENT, rmw_serialized_message_fini(&rcl_serialized_msg));
+}
+
+TEST(TestSerializedMessage, failed_init_throws) {
+  rclcpp::SerializedMessage serialized_msg(13);
+  EXPECT_EQ(13u, serialized_msg.capacity());
+
+  // Constructor with invalid size throws exception
+  EXPECT_THROW(
+    {rclcpp::SerializedMessage serialized_msg_fail(-1);},
+    rclcpp::exceptions::RCLBadAlloc);
+
+  // Constructor copy with rmw_serialized bad msg throws
+  auto default_allocator = rcl_get_default_allocator();
+  auto rcl_serialized_msg = rmw_get_zero_initialized_serialized_message();
+  auto ret = rmw_serialized_message_init(&rcl_serialized_msg, 13, &default_allocator);
+  ASSERT_EQ(RCL_RET_OK, ret);
+  EXPECT_EQ(13u, rcl_serialized_msg.buffer_capacity);
+  rcl_serialized_msg.buffer_capacity = -1;
+  EXPECT_THROW(
+    {rclcpp::SerializedMessage serialized_msg_fail_2(rcl_serialized_msg);},
+    rclcpp::exceptions::RCLBadAlloc);
+
+  rcl_serialized_msg.buffer_capacity = 13;
+  EXPECT_EQ(RCL_RET_OK, rmw_serialized_message_fini(&rcl_serialized_msg));
+}
+
 void serialize_default_ros_msg()
 {
   using MessageT = test_msgs::msg::BasicTypes;

rclcpp/test/rclcpp/test_service.cpp

@@ -21,6 +21,8 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include "rcl_interfaces/srv/list_parameters.hpp"
+#include "test_msgs/srv/empty.hpp"
+#include "test_msgs/srv/empty.h"
 
 class TestService : public ::testing::Test
 {
@@ -105,3 +107,44 @@ TEST_F(TestServiceSub, construction_and_destruction) {
     }, rclcpp::exceptions::InvalidServiceNameError);
   }
 }
+
+/* Testing basic getters */
+TEST_F(TestService, basic_public_getters) {
+  using rcl_interfaces::srv::ListParameters;
+  auto callback =
+    [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) {
+    };
+  auto service = node->create_service<ListParameters>("service", callback);
+  EXPECT_STREQ(service->get_service_name(), "/ns/service");
+  std::shared_ptr<rcl_service_t> service_handle = service->get_service_handle();
+  EXPECT_NE(nullptr, service_handle);
+
+  {
+    // Create a extern defined const service
+    auto node_handle_int = rclcpp::Node::make_shared("base_node");
+    rcl_service_t service_handle = rcl_get_zero_initialized_service();
+    rcl_service_options_t service_options = rcl_service_get_default_options();
+    const rosidl_service_type_support_t * ts =
+      rosidl_typesupport_cpp::get_service_type_support_handle<test_msgs::srv::Empty>();
+    rcl_ret_t ret = rcl_service_init(
+      &service_handle,
+      node_handle_int->get_node_base_interface()->get_rcl_node_handle(),
+      ts, "base_node_service", &service_options);
+    if (ret != RCL_RET_OK) {
+      FAIL();
+      return;
+    }
+    rclcpp::AnyServiceCallback<test_msgs::srv::Empty> cb;
+    const rclcpp::Service<test_msgs::srv::Empty> base(
+      node_handle_int->get_node_base_interface()->get_shared_rcl_node_handle(),
+      &service_handle, cb);
+    // Use get_service_handle specific to const service
+    std::shared_ptr<const rcl_service_t> const_service_handle = base.get_service_handle();
+    EXPECT_NE(nullptr, const_service_handle);
+
+    EXPECT_EQ(
+      RCL_RET_OK, rcl_service_fini(
+        &service_handle,
+        node_handle_int->get_node_base_interface()->get_rcl_node_handle()));
+  }
+}

rclcpp/test/rclcpp/test_type_support.cpp

@@ -0,0 +1,199 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/type_support_decl.hpp"
+#include "test_msgs/msg/empty.hpp"
+
+class TestTypeSupport : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    if (!rclcpp::ok()) {
+      rclcpp::init(0, nullptr);
+    }
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  ::testing::AssertionResult test_type_support_init_fini(const rosidl_service_type_support_t * ts)
+  {
+    rcl_service_t service_handle = rcl_get_zero_initialized_service();
+    rcl_service_options_t service_options = rcl_service_get_default_options();
+    auto node_handle = node->get_node_base_interface()->get_rcl_node_handle();
+    rcl_ret_t ret = rcl_service_init(
+      &service_handle, node_handle, ts, "base_node_service", &service_options);
+    if (ret != RCL_RET_OK) {
+      return ::testing::AssertionFailure() <<
+             "Failed rcl_service_init with error string: " << rcl_get_error_string().str;
+    }
+    ret = rcl_service_fini(&service_handle, node_handle);
+    if (ret != RCL_RET_OK) {
+      return ::testing::AssertionFailure() <<
+             "Failed rcl_service_fini with error string: " << rcl_get_error_string().str;
+    }
+    return ::testing::AssertionSuccess();
+  }
+
+protected:
+  rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+};
+
+const rcl_publisher_options_t PublisherOptions()
+{
+  return rclcpp::PublisherOptionsWithAllocator<std::allocator<void>>().template
+         to_rcl_publisher_options<test_msgs::msg::Empty>(rclcpp::QoS(10));
+}
+
+// Auxiliary classes used to test rosidl_message_type_support_t getters
+// defined in type_support.hpp
+const rosidl_message_type_support_t * ts_parameter_event =
+  rclcpp::type_support::get_parameter_event_msg_type_support();
+
+class TestTSParameterEvent : public rclcpp::PublisherBase
+{
+public:
+  explicit TestTSParameterEvent(rclcpp::Node * node)
+  : rclcpp::PublisherBase(
+      node->get_node_base_interface().get(),
+      "topicTSParameterEvent",
+      *ts_parameter_event,
+      PublisherOptions()) {}
+};
+
+const rosidl_message_type_support_t * ts_set_parameter_result =
+  rclcpp::type_support::get_set_parameters_result_msg_type_support();
+
+class TestTSSetParameterResult : public rclcpp::PublisherBase
+{
+public:
+  explicit TestTSSetParameterResult(rclcpp::Node * node)
+  : rclcpp::PublisherBase(
+      node->get_node_base_interface().get(),
+      "topicTSSetParameterResult",
+      *ts_set_parameter_result,
+      PublisherOptions()) {}
+};
+
+const rosidl_message_type_support_t * ts_parameter_descriptor =
+  rclcpp::type_support::get_parameter_descriptor_msg_type_support();
+
+class TestTSParameterDescriptor : public rclcpp::PublisherBase
+{
+public:
+  explicit TestTSParameterDescriptor(rclcpp::Node * node)
+  : rclcpp::PublisherBase(
+      node->get_node_base_interface().get(),
+      "topicTSParameterDescriptor",
+      *ts_parameter_descriptor,
+      PublisherOptions()) {}
+};
+
+const rosidl_message_type_support_t * ts_list_parameter_result =
+  rclcpp::type_support::get_list_parameters_result_msg_type_support();
+
+class TestTSListParametersResult : public rclcpp::PublisherBase
+{
+public:
+  explicit TestTSListParametersResult(rclcpp::Node * node)
+  : rclcpp::PublisherBase(
+      node->get_node_base_interface().get(),
+      "topicTSListParametersResult",
+      *ts_list_parameter_result,
+      PublisherOptions()) {}
+};
+
+/*
+   Test that the publisher is created properly for different msg typesupport
+ */
+TEST_F(TestTypeSupport, basic_getters) {
+  {
+    auto publisher = TestTSParameterEvent(node.get());
+    std::shared_ptr<const rcl_publisher_t> publisher_handle = publisher.get_publisher_handle();
+    EXPECT_NE(nullptr, publisher_handle);
+  }
+  {
+    auto publisher = TestTSSetParameterResult(node.get());
+    std::shared_ptr<const rcl_publisher_t> publisher_handle = publisher.get_publisher_handle();
+    EXPECT_NE(nullptr, publisher_handle);
+  }
+  {
+    auto publisher = TestTSParameterDescriptor(node.get());
+    std::shared_ptr<const rcl_publisher_t> publisher_handle = publisher.get_publisher_handle();
+    EXPECT_NE(nullptr, publisher_handle);
+  }
+  {
+    auto publisher = TestTSListParametersResult(node.get());
+    std::shared_ptr<const rcl_publisher_t> publisher_handle = publisher.get_publisher_handle();
+    EXPECT_NE(nullptr, publisher_handle);
+  }
+}
+
+/* Testing type support getters */
+TEST_F(TestTypeSupport, test_service_ts_get_params_srv) {
+  const rosidl_service_type_support_t * ts =
+    rclcpp::type_support::get_get_parameters_srv_type_support();
+  ASSERT_NE(nullptr, ts);
+  EXPECT_TRUE(test_type_support_init_fini(ts));
+}
+
+TEST_F(TestTypeSupport, test_service_ts_get_params_srv_type) {
+  const rosidl_service_type_support_t * ts =
+    rclcpp::type_support::get_get_parameters_srv_type_support();
+  ASSERT_NE(nullptr, ts);
+  EXPECT_TRUE(test_type_support_init_fini(ts));
+}
+
+TEST_F(TestTypeSupport, test_service_ts_get_parameters_types_srv) {
+  const rosidl_service_type_support_t * ts =
+    rclcpp::type_support::get_get_parameter_types_srv_type_support();
+  ASSERT_NE(nullptr, ts);
+  EXPECT_TRUE(test_type_support_init_fini(ts));
+}
+
+TEST_F(TestTypeSupport, test_service_ts_set_params_srv) {
+  const rosidl_service_type_support_t * ts =
+    rclcpp::type_support::get_set_parameters_srv_type_support();
+  ASSERT_NE(nullptr, ts);
+  EXPECT_TRUE(test_type_support_init_fini(ts));
+}
+
+TEST_F(TestTypeSupport, test_service_ts_list_params_srv) {
+  const rosidl_service_type_support_t * ts =
+    rclcpp::type_support::get_list_parameters_srv_type_support();
+  ASSERT_NE(nullptr, ts);
+  EXPECT_TRUE(test_type_support_init_fini(ts));
+}
+
+TEST_F(TestTypeSupport, test_service_ts_describe_params_srv) {
+  const rosidl_service_type_support_t * ts =
+    rclcpp::type_support::get_describe_parameters_srv_type_support();
+  ASSERT_NE(nullptr, ts);
+  EXPECT_TRUE(test_type_support_init_fini(ts));
+}
+
+TEST_F(TestTypeSupport, test_service_ts_set_params_atomically_srv) {
+  const rosidl_service_type_support_t * ts =
+    rclcpp::type_support::get_set_parameters_atomically_srv_type_support();
+  ASSERT_NE(nullptr, ts);
+  EXPECT_TRUE(test_type_support_init_fini(ts));
+}

rclcpp/test/rclcpp/test_wait_set.cpp

@@ -15,12 +15,15 @@
 #include <gtest/gtest.h>
 
 #include <memory>
+#include <utility>
 #include <vector>
 
 #include "rcl_interfaces/srv/list_parameters.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "test_msgs/msg/basic_types.hpp"
 
+#include "../utils/rclcpp_gtest_macros.hpp"
+
 class TestWaitSet : public ::testing::Test
 {
 protected:
@@ -262,3 +265,39 @@ TEST_F(TestWaitSet, add_guard_condition_to_two_different_wait_set) {
     }, std::runtime_error);
   }
 }
+
+/*
+ * Get wait_set from result.
+ */
+TEST_F(TestWaitSet, get_result_from_wait_result) {
+  rclcpp::WaitSet wait_set;
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  wait_set.add_guard_condition(guard_condition);
+  guard_condition->trigger();
+
+  rclcpp::WaitResult<rclcpp::WaitSet> result = wait_set.wait();
+  ASSERT_EQ(rclcpp::WaitResultKind::Ready, result.kind());
+  EXPECT_EQ(&wait_set, &result.get_wait_set());
+
+  const rclcpp::WaitResult<rclcpp::WaitSet> const_result(std::move(result));
+  ASSERT_EQ(rclcpp::WaitResultKind::Ready, const_result.kind());
+  EXPECT_EQ(&wait_set, &const_result.get_wait_set());
+}
+
+TEST_F(TestWaitSet, get_result_from_wait_result_not_ready_error) {
+  rclcpp::WaitSet wait_set;
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  wait_set.add_guard_condition(guard_condition);
+
+  rclcpp::WaitResult<rclcpp::WaitSet> result = wait_set.wait(std::chrono::milliseconds(10));
+  ASSERT_EQ(rclcpp::WaitResultKind::Timeout, result.kind());
+  RCLCPP_EXPECT_THROW_EQ(
+    result.get_wait_set(),
+    std::runtime_error("cannot access wait set when the result was not ready"));
+
+  const rclcpp::WaitResult<rclcpp::WaitSet> const_result(std::move(result));
+  ASSERT_EQ(rclcpp::WaitResultKind::Timeout, const_result.kind());
+  RCLCPP_EXPECT_THROW_EQ(
+    const_result.get_wait_set(),
+    std::runtime_error("cannot access wait set when the result was not ready"));
+}

rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp

@@ -18,6 +18,7 @@
 #include <chrono>
 #include <iostream>
 #include <memory>
+#include <random>
 #include <set>
 #include <stdexcept>
 #include <string>
@@ -49,8 +50,13 @@ constexpr const char kTestSubNodeName[]{"test_sub_stats_node"};
 constexpr const char kTestSubStatsTopic[]{"/test_sub_stats_topic"};
 constexpr const char kTestSubStatsEmptyTopic[]{"/test_sub_stats_empty_topic"};
 constexpr const char kTestTopicStatisticsTopic[]{"/test_topic_statistics_topic"};
+constexpr const char kMessageAgeSourceLabel[]{"message_age"};
+constexpr const char kMessagePeriodSourceLabel[]{"message_period"};
 constexpr const uint64_t kNoSamples{0};
 constexpr const std::chrono::seconds kTestDuration{10};
+constexpr const uint64_t kNumExpectedMessages{8};
+constexpr const uint64_t kNumExpectedMessageAgeMessages{kNumExpectedMessages / 2};
+constexpr const uint64_t kNumExpectedMessagePeriodMessages{kNumExpectedMessages / 2};
 }  // namespace
 
 using rclcpp::msg::MessageWithHeader;
@@ -61,6 +67,10 @@ using statistics_msgs::msg::StatisticDataPoint;
 using statistics_msgs::msg::StatisticDataType;
 using libstatistics_collector::moving_average_statistics::StatisticData;
 
+/**
+ * Wrapper class to test and expose parts of the SubscriptionTopicStatistics<T> class.
+ * \tparam CallbackMessageT
+ */
 template<typename CallbackMessageT>
 class TestSubscriptionTopicStatistics : public SubscriptionTopicStatistics<CallbackMessageT>
 {
@@ -128,6 +138,7 @@ public:
       publish_period, [this]() {
         this->publish_message();
       });
+    uniform_dist_ = std::uniform_int_distribution<uint32_t>{1000000, 100000000};
   }
 
   virtual ~MessageWithHeaderPublisher() = default;
@@ -135,14 +146,19 @@ public:
 private:
   void publish_message()
   {
+    std::random_device rd;
+    std::mt19937 gen{rd()};
+    uint32_t d = uniform_dist_(gen);
     auto msg = MessageWithHeader{};
-    // Subtract 1 sec from current time so the received message age is always > 0
-    msg.header.stamp = this->now() - rclcpp::Duration{1, 0};
+    // Subtract ~1 second (add some noise for a non-zero standard deviation)
+    // so the received message age calculation is always > 0
+    msg.header.stamp = this->now() - rclcpp::Duration{1, d};
     publisher_->publish(msg);
   }
 
   rclcpp::Publisher<MessageWithHeader>::SharedPtr publisher_;
   rclcpp::TimerBase::SharedPtr publish_timer_;
+  std::uniform_int_distribution<uint32_t> uniform_dist_;
 };
 
 /**
@@ -220,6 +236,66 @@ protected:
   }
 };
 
+/**
+ * Check if a received statistics message is empty (no data was observed)
+ * \param message_to_check
+ */
+void check_if_statistics_message_is_empty(const MetricsMessage & message_to_check)
+{
+  for (const auto & stats_point : message_to_check.statistics) {
+    const auto type = stats_point.data_type;
+    switch (type) {
+      case StatisticDataType::STATISTICS_DATA_TYPE_SAMPLE_COUNT:
+        EXPECT_EQ(0, stats_point.data) << "unexpected sample count" << stats_point.data;
+        break;
+      case StatisticDataType::STATISTICS_DATA_TYPE_AVERAGE:
+      case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM:
+      case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM:
+      case StatisticDataType::STATISTICS_DATA_TYPE_STDDEV:
+        EXPECT_TRUE(std::isnan(stats_point.data)) << "unexpected value" << stats_point.data <<
+          " for type:" << type;
+        break;
+      default:
+        FAIL() << "received unknown statistics type: " << std::dec <<
+          static_cast<unsigned int>(type);
+    }
+  }
+}
+
+/**
+ * Check if a received statistics message observed data and contains some calculation
+ * \param message_to_check
+ */
+void check_if_statistic_message_is_populated(const MetricsMessage & message_to_check)
+{
+  for (const auto & stats_point : message_to_check.statistics) {
+    const auto type = stats_point.data_type;
+    switch (type) {
+      case StatisticDataType::STATISTICS_DATA_TYPE_SAMPLE_COUNT:
+        EXPECT_LT(0, stats_point.data) << "unexpected sample count " << stats_point.data;
+        break;
+      case StatisticDataType::STATISTICS_DATA_TYPE_AVERAGE:
+        EXPECT_LT(0, stats_point.data) << "unexpected avg " << stats_point.data;
+        break;
+      case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM:
+        EXPECT_LT(0, stats_point.data) << "unexpected mi n" << stats_point.data;
+        break;
+      case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM:
+        EXPECT_LT(0, stats_point.data) << "unexpected max " << stats_point.data;
+        break;
+      case StatisticDataType::STATISTICS_DATA_TYPE_STDDEV:
+        EXPECT_LT(0, stats_point.data) << "unexpected stddev " << stats_point.data;
+        break;
+      default:
+        FAIL() << "received unknown statistics type: " << std::dec <<
+          static_cast<unsigned int>(type);
+    }
+  }
+}
+
+/**
+ * Test an invalid argument is thrown for a bad input publish period.
+ */
 TEST(TestSubscriptionTopicStatistics, test_invalid_publish_period)
 {
   rclcpp::init(0 /* argc */, nullptr /* argv */);
@@ -244,6 +320,10 @@ TEST(TestSubscriptionTopicStatistics, test_invalid_publish_period)
   rclcpp::shutdown();
 }
 
+/**
+ * Test that we can manually construct the subscription topic statistics utility class
+ * without any errors and defaults to empty measurements.
+ */
 TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction)
 {
   auto empty_subscriber = std::make_shared<EmptySubscriber>(
@@ -271,6 +351,10 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction)
   }
 }
 
+/**
+ * Publish messages that do not have a header timestamp, test that all statistics messages
+ * were received, and verify the statistics message contents.
+ */
 TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no_header)
 {
   // Create an empty publisher
@@ -284,7 +368,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no
   auto statistics_listener = std::make_shared<rclcpp::topic_statistics::MetricsMessageSubscriber>(
     "test_receive_single_empty_stats_message_listener",
     "/statistics",
-    2);
+    kNumExpectedMessages);
 
   auto empty_subscriber = std::make_shared<EmptySubscriber>(
     kTestSubNodeName,
@@ -301,49 +385,37 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no
     kTestDuration);
 
   // Compare message counts, sample count should be the same as published and received count
-  EXPECT_EQ(2, statistics_listener->GetNumberOfMessagesReceived());
+  EXPECT_EQ(kNumExpectedMessages, statistics_listener->GetNumberOfMessagesReceived());
 
-  // Check the received message and the data types
+  // Check the received message total count
   const auto received_messages = statistics_listener->GetReceivedMessages();
+  EXPECT_EQ(kNumExpectedMessages, received_messages.size());
 
-  EXPECT_EQ(2u, received_messages.size());
+  // check the type of statistics that were received and their counts
+  uint64_t message_age_count{0};
+  uint64_t message_period_count{0};
 
   std::set<std::string> received_metrics;
   for (const auto & msg : received_messages) {
-    received_metrics.insert(msg.metrics_source);
+    if (msg.metrics_source == "message_age") {
+      message_age_count++;
+    }
+    if (msg.metrics_source == "message_period") {
+      message_period_count++;
+    }
   }
-  EXPECT_TRUE(received_metrics.find("message_age") != received_metrics.end());
-  EXPECT_TRUE(received_metrics.find("message_period") != received_metrics.end());
+  EXPECT_EQ(kNumExpectedMessageAgeMessages, message_age_count);
+  EXPECT_EQ(kNumExpectedMessagePeriodMessages, message_period_count);
 
   // Check the collected statistics for message period.
   // Message age statistics will not be calculated because Empty messages
-  // don't have a `header` with timestamp.
+  // don't have a `header` with timestamp. This means that we expect to receive a `message_age`
+  // and `message_period` message for each empty message published.
   for (const auto & msg : received_messages) {
-    if (msg.metrics_source != "message_period") {
-      continue;
-    }
-    for (const auto & stats_point : msg.statistics) {
-      const auto type = stats_point.data_type;
-      switch (type) {
-        case StatisticDataType::STATISTICS_DATA_TYPE_SAMPLE_COUNT:
-          EXPECT_LT(0, stats_point.data) << "unexpected sample count";
-          break;
-        case StatisticDataType::STATISTICS_DATA_TYPE_AVERAGE:
-          EXPECT_LT(0, stats_point.data) << "unexpected avg";
-          break;
-        case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM:
-          EXPECT_LT(0, stats_point.data) << "unexpected min";
-          break;
-        case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM:
-          EXPECT_LT(0, stats_point.data) << "unexpected max";
-          break;
-        case StatisticDataType::STATISTICS_DATA_TYPE_STDDEV:
-          EXPECT_LT(0, stats_point.data) << "unexpected stddev";
-          break;
-        default:
-          FAIL() << "received unknown statistics type: " << std::dec <<
-            static_cast<unsigned int>(type);
-      }
+    if (msg.metrics_source == kMessageAgeSourceLabel) {
+      check_if_statistics_message_is_empty(msg);
+    } else if (msg.metrics_source == kMessagePeriodSourceLabel) {
+      check_if_statistic_message_is_populated(msg);
     }
   }
 }
@@ -361,7 +433,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi
   auto statistics_listener = std::make_shared<rclcpp::topic_statistics::MetricsMessageSubscriber>(
     "test_receive_stats_for_message_with_header",
     "/statistics",
-    2);
+    kNumExpectedMessages);
 
   auto msg_with_header_subscriber = std::make_shared<MessageWithHeaderSubscriber>(
     kTestSubNodeName,
@@ -378,44 +450,29 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi
     kTestDuration);
 
   // Compare message counts, sample count should be the same as published and received count
-  EXPECT_EQ(2, statistics_listener->GetNumberOfMessagesReceived());
+  EXPECT_EQ(kNumExpectedMessages, statistics_listener->GetNumberOfMessagesReceived());
 
-  // Check the received message and the data types
+  // Check the received message total count
   const auto received_messages = statistics_listener->GetReceivedMessages();
+  EXPECT_EQ(kNumExpectedMessages, received_messages.size());
 
-  EXPECT_EQ(2u, received_messages.size());
+  // check the type of statistics that were received and their counts
+  uint64_t message_age_count{0};
+  uint64_t message_period_count{0};
 
   std::set<std::string> received_metrics;
   for (const auto & msg : received_messages) {
-    received_metrics.insert(msg.metrics_source);
-  }
-  EXPECT_TRUE(received_metrics.find("message_age") != received_metrics.end());
-  EXPECT_TRUE(received_metrics.find("message_period") != received_metrics.end());
-
-  // Check the collected statistics for message period.
-  for (const auto & msg : received_messages) {
-    for (const auto & stats_point : msg.statistics) {
-      const auto type = stats_point.data_type;
-      switch (type) {
-        case StatisticDataType::STATISTICS_DATA_TYPE_SAMPLE_COUNT:
-          EXPECT_LT(0, stats_point.data) << "unexpected sample count";
-          break;
-        case StatisticDataType::STATISTICS_DATA_TYPE_AVERAGE:
-          EXPECT_LT(0, stats_point.data) << "unexpected avg";
-          break;
-        case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM:
-          EXPECT_LT(0, stats_point.data) << "unexpected min";
-          break;
-        case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM:
-          EXPECT_LT(0, stats_point.data) << "unexpected max";
-          break;
-        case StatisticDataType::STATISTICS_DATA_TYPE_STDDEV:
-          EXPECT_LE(0, stats_point.data) << "unexpected stddev";
-          break;
-        default:
-          FAIL() << "received unknown statistics type: " << std::dec <<
-            static_cast<unsigned int>(type);
-      }
+    if (msg.metrics_source == kMessageAgeSourceLabel) {
+      message_age_count++;
+    }
+    if (msg.metrics_source == kMessagePeriodSourceLabel) {
+      message_period_count++;
     }
   }
+  EXPECT_EQ(kNumExpectedMessageAgeMessages, message_age_count);
+  EXPECT_EQ(kNumExpectedMessagePeriodMessages, message_period_count);
+
+  for (const auto & msg : received_messages) {
+    check_if_statistic_message_is_populated(msg);
+  }
 }

rclcpp/test/rclcpp/topic_statistics/test_topic_stats_utils.hpp

@@ -90,7 +90,7 @@ public:
   MetricsMessageSubscriber(
     const std::string & name,
     const std::string & topic_name,
-    const int number_of_messages_to_receive = 2)
+    const uint64_t number_of_messages_to_receive = 2)
   : rclcpp::Node(name),
     number_of_messages_to_receive_(number_of_messages_to_receive)
   {
@@ -118,7 +118,7 @@ public:
    * Return the number of messages received by this subscriber.
    * \return the number of messages received by the subscriber callback
    */
-  int GetNumberOfMessagesReceived() const
+  uint64_t GetNumberOfMessagesReceived() const
   {
     return num_messages_received_;
   }
@@ -142,8 +142,8 @@ private:
   std::vector<MetricsMessage> received_messages_;
   rclcpp::Subscription<MetricsMessage>::SharedPtr subscription_;
   mutable std::mutex mutex_;
-  std::atomic<int> num_messages_received_{0};
-  const int number_of_messages_to_receive_;
+  std::atomic<uint64_t> num_messages_received_{0};
+  const uint64_t number_of_messages_to_receive_;
 };
 
 }  // namespace topic_statistics

rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp

@@ -0,0 +1,299 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/wait_set.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+#include "test_msgs/msg/empty.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+class TestDynamicStorage : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("node", "ns");
+  }
+
+  std::shared_ptr<rclcpp::Node> node;
+};
+
+class TestWaitable : public rclcpp::Waitable
+{
+public:
+  TestWaitable()
+  : is_ready_(false) {}
+
+  bool add_to_wait_set(rcl_wait_set_t *) override {return true;}
+
+  bool is_ready(rcl_wait_set_t *) override {return is_ready_;}
+
+  void execute() override {}
+
+  void set_is_ready(bool value) {is_ready_ = value;}
+
+private:
+  bool is_ready_;
+};
+
+TEST_F(TestDynamicStorage, default_construct_destruct) {
+  rclcpp::WaitSet wait_set;
+  EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set.get_rcl_wait_set()));
+  EXPECT_EQ(rclcpp::WaitResultKind::Empty, wait_set.wait().kind());
+}
+
+TEST_F(TestDynamicStorage, iterables_construct_destruct) {
+  auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+    "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {});
+  auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {});
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  auto waitable = std::make_shared<TestWaitable>();
+  auto subscriptions =
+    std::vector<rclcpp::WaitSet::SubscriptionEntry>{{subscription}};
+  auto guard_conditions =
+    std::vector<rclcpp::GuardCondition::SharedPtr>{guard_condition};
+  auto timers =
+    std::vector<rclcpp::TimerBase::SharedPtr>{timer};
+  auto clients =
+    std::vector<rclcpp::ClientBase::SharedPtr>{client};
+  auto services =
+    std::vector<rclcpp::ServiceBase::SharedPtr>{service};
+  auto waitables =
+    std::vector<rclcpp::WaitSet::WaitableEntry>{{waitable}};
+  rclcpp::WaitSet wait_set(subscriptions, guard_conditions, timers, clients, services, waitables);
+
+  EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set.get_rcl_wait_set()));
+}
+
+TEST_F(TestDynamicStorage, add_remove_dynamically) {
+  rclcpp::WaitSet wait_set;
+
+  // Adds more coverage
+  rclcpp::SubscriptionOptionsWithAllocator<std::allocator<void>> options;
+  options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable;
+
+  auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+    "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}, options);
+
+  rclcpp::SubscriptionWaitSetMask mask{true, true, true};
+  wait_set.add_subscription(subscription, mask);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_subscription(subscription, mask),
+    std::runtime_error("subscription already associated with a wait set"));
+  wait_set.remove_subscription(subscription, mask);
+
+  // This is long, so it can stick around and be removed
+  auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {});
+  wait_set.add_timer(timer);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_timer(timer),
+    std::runtime_error("timer already in use by another wait set"));
+  wait_set.remove_timer(timer);
+
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  wait_set.add_guard_condition(guard_condition);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_guard_condition(guard_condition),
+    std::runtime_error("guard condition already in use by another wait set"));
+  wait_set.remove_guard_condition(guard_condition);
+
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+  wait_set.add_service(service);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_service(service),
+    std::runtime_error("service already in use by another wait set"));
+  wait_set.remove_service(service);
+
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  wait_set.add_client(client);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_client(client),
+    std::runtime_error("client already in use by another wait set"));
+  wait_set.remove_client(client);
+
+  auto waitable = std::make_shared<TestWaitable>();
+  wait_set.add_waitable(waitable);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_waitable(waitable),
+    std::runtime_error("waitable already in use by another wait set"));
+
+  wait_set.remove_waitable(waitable);
+  wait_set.prune_deleted_entities();
+  EXPECT_EQ(rclcpp::WaitResultKind::Empty, wait_set.wait().kind());
+}
+
+TEST_F(TestDynamicStorage, add_remove_nullptr) {
+  rclcpp::WaitSet wait_set;
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_subscription(nullptr), std::invalid_argument("subscription is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_subscription(nullptr), std::invalid_argument("subscription is nullptr"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_guard_condition(nullptr), std::invalid_argument("guard_condition is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_guard_condition(nullptr), std::invalid_argument("guard_condition is nullptr"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_timer(nullptr), std::invalid_argument("timer is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_timer(nullptr), std::invalid_argument("timer is nullptr"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_client(nullptr), std::invalid_argument("client is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_client(nullptr), std::invalid_argument("client is nullptr"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_service(nullptr), std::invalid_argument("service is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_service(nullptr), std::invalid_argument("service is nullptr"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_waitable(nullptr), std::invalid_argument("waitable is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_waitable(nullptr), std::invalid_argument("waitable is nullptr"));
+}
+
+TEST_F(TestDynamicStorage, add_remove_out_of_scope) {
+  rclcpp::WaitSet wait_set;
+
+  {
+    auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+      "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {});
+    wait_set.add_subscription(subscription);
+
+    // This is short, so if it's not cleaned up, it will trigger wait and it won't timeout
+    auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {});
+    wait_set.add_timer(timer);
+
+    auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+    wait_set.add_guard_condition(guard_condition);
+
+    auto service =
+      node->create_service<test_msgs::srv::Empty>(
+      "service",
+      [](
+        const test_msgs::srv::Empty::Request::SharedPtr,
+        test_msgs::srv::Empty::Response::SharedPtr) {});
+    wait_set.add_service(service);
+
+    auto client = node->create_client<test_msgs::srv::Empty>("service");
+    wait_set.add_client(client);
+
+    auto waitable = std::make_shared<TestWaitable>();
+    wait_set.add_waitable(waitable);
+  }
+
+  EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_set.wait(std::chrono::milliseconds(10)).kind());
+}
+
+TEST_F(TestDynamicStorage, wait_subscription) {
+  rclcpp::WaitSet wait_set;
+
+  // Not added to wait_set, just used for publishing to the topic
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>("topic", 10);
+
+  auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+    "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {});
+  wait_set.add_subscription(subscription);
+
+  {
+    auto wait_result = wait_set.wait(std::chrono::milliseconds(10));
+    EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind());
+  }
+
+  publisher->publish(test_msgs::msg::Empty());
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(-1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind());
+  }
+}
+
+TEST_F(TestDynamicStorage, wait_timer) {
+  rclcpp::WaitSet wait_set;
+
+  auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {});
+  wait_set.add_timer(timer);
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(-1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind());
+  }
+}
+
+TEST_F(TestDynamicStorage, wait_client_service) {
+  rclcpp::WaitSet wait_set;
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  wait_set.add_guard_condition(guard_condition);
+
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+  wait_set.add_service(service);
+
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  wait_set.add_client(client);
+  {
+    auto wait_result = wait_set.wait(std::chrono::milliseconds(10));
+    EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind());
+  }
+  client->async_send_request(std::make_shared<test_msgs::srv::Empty::Request>());
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(-1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind());
+  }
+}
+
+TEST_F(TestDynamicStorage, wait_waitable) {
+  rclcpp::WaitSet wait_set;
+  auto waitable = std::make_shared<TestWaitable>();
+  wait_set.add_waitable(waitable);
+  {
+    // This waitable doesn't add itself to the rcl_wait_set_t, so Empty is to be expected
+    auto wait_result = wait_set.wait(std::chrono::seconds(-1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Empty, wait_result.kind());
+  }
+}

rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp

@@ -0,0 +1,202 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/wait_set.hpp"
+#include "rclcpp/wait_set_policies/static_storage.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+#include "test_msgs/msg/empty.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+class TestStaticStorage : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("node", "ns");
+  }
+
+  std::shared_ptr<rclcpp::Node> node;
+};
+
+class TestWaitable : public rclcpp::Waitable
+{
+public:
+  TestWaitable()
+  : is_ready_(false) {}
+  bool add_to_wait_set(rcl_wait_set_t *) override {return true;}
+
+  bool is_ready(rcl_wait_set_t *) override {return is_ready_;}
+
+  void execute() override {}
+
+  void set_is_ready(bool value) {is_ready_ = value;}
+
+private:
+  bool is_ready_;
+};
+
+TEST_F(TestStaticStorage, iterables_construct_destruct) {
+  auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+    "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {});
+  // This is long, so it can stick around and be removed
+  auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {});
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  auto waitable = std::make_shared<TestWaitable>();
+  rclcpp::StaticWaitSet<1, 1, 1, 1, 1, 1> wait_set(
+    {{{subscription}}}, {guard_condition}, {timer}, {client}, {service}, {{{waitable}}});
+
+  EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set.get_rcl_wait_set()));
+}
+
+// Because these StaticWaitSet's have templated sizes larger than the input arguments passed
+// to the constructor, their shared-pointer contents will be default constructed to null. This
+// test just checks the appropriate exception is thrown.
+// std::shared_ptr<StaticWaitSet>::reset() is not required for these exceptions, it just
+// disables the unused return value warning of std::make_shared
+TEST_F(TestStaticStorage, fixed_storage_needs_pruning) {
+  {
+    using StaticWaitSet = rclcpp::StaticWaitSet<1, 0, 0, 0, 0, 0>;
+    RCLCPP_EXPECT_THROW_EQ(
+      std::make_shared<StaticWaitSet>().reset(),
+      std::runtime_error("unexpected condition, fixed storage policy needs pruning"));
+  }
+  {
+    using StaticWaitSet = rclcpp::StaticWaitSet<0, 1, 0, 0, 0, 0>;
+    RCLCPP_EXPECT_THROW_EQ(
+      std::make_shared<StaticWaitSet>().reset(),
+      std::runtime_error("unexpected condition, fixed storage policy needs pruning"));
+  }
+  {
+    using StaticWaitSet = rclcpp::StaticWaitSet<0, 0, 1, 0, 0, 0>;
+    RCLCPP_EXPECT_THROW_EQ(
+      std::make_shared<StaticWaitSet>().reset(),
+      std::runtime_error("unexpected condition, fixed storage policy needs pruning"));
+  }
+  {
+    using StaticWaitSet = rclcpp::StaticWaitSet<0, 0, 0, 1, 0, 0>;
+    RCLCPP_EXPECT_THROW_EQ(
+      std::make_shared<StaticWaitSet>().reset(),
+      std::runtime_error("unexpected condition, fixed storage policy needs pruning"));
+  }
+  {
+    using StaticWaitSet = rclcpp::StaticWaitSet<0, 0, 0, 0, 1, 0>;
+    RCLCPP_EXPECT_THROW_EQ(
+      std::make_shared<StaticWaitSet>().reset(),
+      std::runtime_error("unexpected condition, fixed storage policy needs pruning"));
+  }
+  {
+    using StaticWaitSet = rclcpp::StaticWaitSet<0, 0, 0, 0, 0, 1>;
+    RCLCPP_EXPECT_THROW_EQ(
+      std::make_shared<StaticWaitSet>().reset(),
+      std::runtime_error("unexpected condition, fixed storage policy needs pruning"));
+  }
+}
+
+TEST_F(TestStaticStorage, wait_subscription) {
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>("topic", 10);
+  auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+    "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {});
+  rclcpp::SubscriptionWaitSetMask mask{true, true, true};
+  rclcpp::StaticWaitSet<1, 0, 0, 0, 0, 0> wait_set({{{subscription, mask}}});
+
+  {
+    auto wait_result = wait_set.wait(std::chrono::milliseconds(10));
+    EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind());
+  }
+
+  publisher->publish(test_msgs::msg::Empty());
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(-1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind());
+  }
+}
+
+TEST_F(TestStaticStorage, wait_timer) {
+  auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {});
+  rclcpp::StaticWaitSet<0, 0, 1, 0, 0, 0> wait_set({}, {}, {timer});
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(-1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind());
+  }
+}
+
+TEST_F(TestStaticStorage, wait_guard_condition) {
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  rclcpp::StaticWaitSet<0, 1, 0, 0, 0, 0> wait_set({}, {guard_condition});
+
+  {
+    auto wait_result = wait_set.wait(std::chrono::milliseconds(10));
+    EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind());
+  }
+
+  guard_condition->trigger();
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(-1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind());
+  }
+}
+
+TEST_F(TestStaticStorage, wait_client_service) {
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  rclcpp::StaticWaitSet<0, 0, 0, 1, 1, 0> wait_set({}, {}, {}, {client}, {service});
+  {
+    auto wait_result = wait_set.wait(std::chrono::milliseconds(10));
+    EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind());
+  }
+  client->async_send_request(std::make_shared<test_msgs::srv::Empty::Request>());
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(-1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind());
+  }
+}
+
+TEST_F(TestStaticStorage, wait_waitable) {
+  auto waitable = std::make_shared<TestWaitable>();
+  rclcpp::StaticWaitSet<0, 0, 0, 0, 0, 1> wait_set({}, {}, {}, {}, {}, {{{waitable}}});
+  {
+    // This waitable doesn't add itself to the rcl_wait_set_t, so Empty is to be expected
+    auto wait_result = wait_set.wait(std::chrono::seconds(-1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Empty, wait_result.kind());
+  }
+}

rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp

@@ -0,0 +1,170 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/wait_set.hpp"
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+#include "test_msgs/msg/empty.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+class TestStoragePolicyCommon : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("node", "ns");
+  }
+
+  std::shared_ptr<rclcpp::Node> node;
+};
+
+class TestWaitable : public rclcpp::Waitable
+{
+public:
+  TestWaitable()
+  : is_ready_(false), add_to_wait_set_(false) {}
+  bool add_to_wait_set(rcl_wait_set_t *) override {return add_to_wait_set_;}
+
+  bool is_ready(rcl_wait_set_t *) override {return is_ready_;}
+
+  void execute() override {}
+
+  void set_is_ready(bool value) {is_ready_ = value;}
+
+  void set_add_to_wait_set(bool value) {add_to_wait_set_ = value;}
+
+private:
+  bool is_ready_;
+  bool add_to_wait_set_;
+};
+
+TEST_F(TestStoragePolicyCommon, rcl_wait_set_fini_error) {
+  auto wait_set = std::make_shared<rclcpp::WaitSet>();
+  auto mock = mocking_utils::inject_on_return(
+    "lib:rclcpp", rcl_wait_set_fini, RCL_RET_ERROR);
+  EXPECT_NO_THROW(wait_set.reset());
+}
+
+TEST_F(TestStoragePolicyCommon, rcl_wait_set_resize_error) {
+  rclcpp::WaitSet wait_set;
+
+  auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+    "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {});
+  rclcpp::SubscriptionWaitSetMask mask{true, true, true};
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait_set_resize, RCL_RET_ERROR);
+  wait_set.add_subscription(subscription, mask);
+  EXPECT_THROW(
+    wait_set.wait(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestStoragePolicyCommon, rcl_wait_set_clear_error) {
+  rclcpp::WaitSet wait_set;
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR);
+  EXPECT_THROW(
+    wait_set.wait(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestStoragePolicyCommon, rcl_wait_set_add_subscription_error) {
+  rclcpp::WaitSet wait_set;
+  auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+    "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {});
+  rclcpp::SubscriptionWaitSetMask mask{true, true, true};
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait_set_add_subscription, RCL_RET_ERROR);
+  wait_set.add_subscription(subscription, mask);
+  EXPECT_THROW(
+    wait_set.wait(),
+    rclcpp::exceptions::RCLError);
+}
+TEST_F(TestStoragePolicyCommon, rcl_wait_set_add_guard_condition_error) {
+  rclcpp::WaitSet wait_set;
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait_set_add_guard_condition, RCL_RET_ERROR);
+  wait_set.add_guard_condition(guard_condition);
+  EXPECT_THROW(
+    wait_set.wait(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestStoragePolicyCommon, rcl_wait_set_add_timer_error) {
+  rclcpp::WaitSet wait_set;
+  auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {});
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait_set_add_timer, RCL_RET_ERROR);
+  wait_set.add_timer(timer);
+  EXPECT_THROW(
+    wait_set.wait(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestStoragePolicyCommon, rcl_wait_set_add_service_error) {
+  rclcpp::WaitSet wait_set;
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait_set_add_service, RCL_RET_ERROR);
+  wait_set.add_service(service);
+  EXPECT_THROW(
+    wait_set.wait(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestStoragePolicyCommon, rcl_wait_set_add_client_error) {
+  rclcpp::WaitSet wait_set;
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait_set_add_client, RCL_RET_ERROR);
+  wait_set.add_client(client);
+  EXPECT_THROW(
+    wait_set.wait(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestStoragePolicyCommon, add_waitable_error) {
+  rclcpp::WaitSet wait_set;
+  auto waitable = std::make_shared<TestWaitable>();
+  waitable->set_add_to_wait_set(false);
+  wait_set.add_waitable(waitable);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.wait(),
+    std::runtime_error("waitable unexpectedly failed to be added to wait set"));
+}

rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp

@@ -0,0 +1,303 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/wait_set.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+#include "test_msgs/msg/empty.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+class TestThreadSafeStorage : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("node", "ns");
+  }
+
+  std::shared_ptr<rclcpp::Node> node;
+};
+
+class TestWaitable : public rclcpp::Waitable
+{
+public:
+  TestWaitable()
+  : is_ready_(false) {}
+  bool add_to_wait_set(rcl_wait_set_t *) override {return true;}
+
+  bool is_ready(rcl_wait_set_t *) override {return is_ready_;}
+
+  void execute() override {}
+
+  void set_is_ready(bool value) {is_ready_ = value;}
+
+private:
+  bool is_ready_;
+};
+
+TEST_F(TestThreadSafeStorage, default_construct_destruct) {
+  rclcpp::ThreadSafeWaitSet wait_set;
+  EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set.get_rcl_wait_set()));
+
+  // Expected behavior of thread-safe is to timeout here
+  EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_set.wait(std::chrono::milliseconds(10)).kind());
+}
+
+TEST_F(TestThreadSafeStorage, iterables_construct_destruct) {
+  auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+    "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {});
+  // This is long, so it can stick around
+  auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {});
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  auto waitable = std::make_shared<TestWaitable>();
+  auto subscriptions =
+    std::vector<rclcpp::ThreadSafeWaitSet::SubscriptionEntry>{{subscription}};
+  auto guard_conditions =
+    std::vector<rclcpp::GuardCondition::SharedPtr>{guard_condition};
+  auto timers =
+    std::vector<rclcpp::TimerBase::SharedPtr>{timer};
+  auto clients =
+    std::vector<rclcpp::ClientBase::SharedPtr>{client};
+  auto services =
+    std::vector<rclcpp::ServiceBase::SharedPtr>{service};
+  auto waitables =
+    std::vector<rclcpp::ThreadSafeWaitSet::WaitableEntry>{{waitable}};
+  rclcpp::ThreadSafeWaitSet wait_set(
+    subscriptions, guard_conditions, timers, clients, services, waitables);
+
+  EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set.get_rcl_wait_set()));
+}
+
+TEST_F(TestThreadSafeStorage, add_remove_dynamically) {
+  rclcpp::ThreadSafeWaitSet wait_set;
+
+  // Adds more coverage
+  rclcpp::SubscriptionOptionsWithAllocator<std::allocator<void>> options;
+  options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable;
+
+  auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+    "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}, options);
+
+  rclcpp::SubscriptionWaitSetMask mask{true, true, true};
+  wait_set.add_subscription(subscription, mask);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_subscription(subscription, mask),
+    std::runtime_error("subscription already associated with a wait set"));
+  wait_set.remove_subscription(subscription, mask);
+
+  // This is long, so it can stick around and be removed
+  auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {});
+  wait_set.add_timer(timer);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_timer(timer),
+    std::runtime_error("timer already in use by another wait set"));
+  wait_set.remove_timer(timer);
+
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  wait_set.add_guard_condition(guard_condition);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_guard_condition(guard_condition),
+    std::runtime_error("guard condition already in use by another wait set"));
+  wait_set.remove_guard_condition(guard_condition);
+
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+  wait_set.add_service(service);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_service(service),
+    std::runtime_error("service already in use by another wait set"));
+  wait_set.remove_service(service);
+
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  wait_set.add_client(client);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_client(client),
+    std::runtime_error("client already in use by another wait set"));
+  wait_set.remove_client(client);
+
+  auto waitable = std::make_shared<TestWaitable>();
+  wait_set.add_waitable(waitable);
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_waitable(waitable),
+    std::runtime_error("waitable already in use by another wait set"));
+  wait_set.remove_waitable(waitable);
+  wait_set.prune_deleted_entities();
+
+  // Expected behavior of thread-safe is to timeout here
+  EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_set.wait(std::chrono::milliseconds(10)).kind());
+}
+
+TEST_F(TestThreadSafeStorage, add_remove_nullptr) {
+  rclcpp::ThreadSafeWaitSet wait_set;
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_subscription(nullptr), std::invalid_argument("subscription is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_subscription(nullptr), std::invalid_argument("subscription is nullptr"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_guard_condition(nullptr), std::invalid_argument("guard_condition is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_guard_condition(nullptr), std::invalid_argument("guard_condition is nullptr"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_timer(nullptr), std::invalid_argument("timer is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_timer(nullptr), std::invalid_argument("timer is nullptr"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_client(nullptr), std::invalid_argument("client is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_client(nullptr), std::invalid_argument("client is nullptr"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_service(nullptr), std::invalid_argument("service is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_service(nullptr), std::invalid_argument("service is nullptr"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.add_waitable(nullptr), std::invalid_argument("waitable is nullptr"));
+  RCLCPP_EXPECT_THROW_EQ(
+    wait_set.remove_waitable(nullptr), std::invalid_argument("waitable is nullptr"));
+}
+
+TEST_F(TestThreadSafeStorage, add_remove_out_of_scope) {
+  rclcpp::ThreadSafeWaitSet wait_set;
+
+  {
+    auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+      "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {});
+    wait_set.add_subscription(subscription);
+
+    // This is short, so if it's not cleaned up, it will trigger wait
+    auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {});
+    wait_set.add_timer(timer);
+
+    auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+    wait_set.add_guard_condition(guard_condition);
+
+    auto service =
+      node->create_service<test_msgs::srv::Empty>(
+      "service",
+      [](
+        const test_msgs::srv::Empty::Request::SharedPtr,
+        test_msgs::srv::Empty::Response::SharedPtr) {});
+    wait_set.add_service(service);
+
+    auto client = node->create_client<test_msgs::srv::Empty>("service");
+    wait_set.add_client(client);
+
+    auto waitable = std::make_shared<TestWaitable>();
+    wait_set.add_waitable(waitable);
+  }
+
+  EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_set.wait(std::chrono::milliseconds(10)).kind());
+}
+
+TEST_F(TestThreadSafeStorage, wait_subscription) {
+  rclcpp::ThreadSafeWaitSet wait_set;
+
+  // Not added to wait_set, just used for publishing to the topic
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>("topic", 10);
+
+  auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+    "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {});
+  wait_set.add_subscription(subscription);
+
+  {
+    auto wait_result = wait_set.wait(std::chrono::milliseconds(10));
+    EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind());
+  }
+
+  publisher->publish(test_msgs::msg::Empty());
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind());
+  }
+}
+
+TEST_F(TestThreadSafeStorage, wait_timer) {
+  rclcpp::ThreadSafeWaitSet wait_set;
+
+  auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {});
+  wait_set.add_timer(timer);
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind());
+  }
+}
+
+TEST_F(TestThreadSafeStorage, wait_client_service) {
+  rclcpp::ThreadSafeWaitSet wait_set;
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  wait_set.add_guard_condition(guard_condition);
+
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+  wait_set.add_service(service);
+
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  wait_set.add_client(client);
+  {
+    auto wait_result = wait_set.wait(std::chrono::milliseconds(10));
+    EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind());
+  }
+  client->async_send_request(std::make_shared<test_msgs::srv::Empty::Request>());
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(1));
+    EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind());
+  }
+}
+
+TEST_F(TestThreadSafeStorage, wait_waitable) {
+  rclcpp::ThreadSafeWaitSet wait_set;
+  auto waitable = std::make_shared<TestWaitable>();
+  wait_set.add_waitable(waitable);
+  {
+    // This waitable doesn't add itself to the rcl_wait_set_t, so Timeout is to be expected
+    auto wait_result = wait_set.wait(std::chrono::milliseconds(10));
+    EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind());
+  }
+}

rclcpp/test/utils/rclcpp_gtest_macros.hpp

@@ -0,0 +1,195 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef UTILS__RCLCPP_GTEST_MACROS_HPP_
+#define UTILS__RCLCPP_GTEST_MACROS_HPP_
+
+#include <gtest/gtest.h>
+
+#include <cstring>
+#include <type_traits>
+
+#include "rclcpp/exceptions/exceptions.hpp"
+
+namespace rclcpp
+{
+namespace testing
+{
+namespace details
+{
+
+/**
+ * \brief Check if two thrown objects are equals.
+ *
+ * For generic thrown objects, probably is unlikely to be used. This type must
+ * overload the == and << operators.
+ */
+template<typename T,
+  typename = typename std::enable_if_t<
+    !std::is_convertible<T, std::exception>::value>>
+::testing::AssertionResult AreThrowableContentsEqual(
+  const T & expected, const T & actual, const char * expected_exception_expression,
+  const char * throwing_expression)
+{
+  if (expected == actual) {
+    return ::testing::AssertionSuccess() <<
+           "'\nThe value of the non-standard throwable thrown by the expression\n'" <<
+           throwing_expression << "'\n\nmatches the value of the expected thrown object\n'" <<
+           expected_exception_expression << "'\n\t(" << expected << " == " << actual << ")\n";
+  }
+
+  return ::testing::AssertionFailure() <<
+         "\nThe value of the non-standard throwable thrown by the expression\n'" <<
+         throwing_expression << "'\n\ndoes not match the value of the expected thrown object\n'" <<
+         expected_exception_expression << "'\n\t(" << expected << " != " << actual << ")\n";
+}
+
+/**
+ * \brief Check if two std::exceptions are equal according to their message.
+ *
+ * If the exception type also derives from rclcpp::Exception, then the next overload is called
+ * instead
+ */
+template<typename T,
+  typename = typename std::enable_if_t<
+    !std::is_convertible<T, rclcpp::exceptions::RCLErrorBase>::value>>
+::testing::AssertionResult AreThrowableContentsEqual(
+  const std::exception & expected, const std::exception & actual,
+  const char * expected_exception_expression,
+  const char * throwing_expression)
+{
+  if (std::strcmp(expected.what(), actual.what()) == 0) {
+    return ::testing::AssertionSuccess() <<
+           "'\nThe contents of the std::exception thrown by the expression\n'" <<
+           throwing_expression << "':\n\te.what(): '" << actual.what() <<
+           "'\n\nmatch the contents of the expected std::exception\n'" <<
+           expected_exception_expression << "'\n\te.what(): '" << expected.what() << "'\n";
+  }
+
+  return ::testing::AssertionFailure() <<
+         "\nThe contents of the std::exception thrown by the expression\n'" <<
+         throwing_expression << "':\n\te.what(): '" << actual.what() <<
+         "'\n\ndo not match the contents of the expected std::exception\n'" <<
+         expected_exception_expression << "'\n\te.what(): '" << expected.what() << "'\n";
+}
+
+/**
+ * \brief Check if two exceptions that derive from rclcpp::RCLErrorBase are equal.
+ *
+ * This checks equality based on their return and message. It does not check the formatted
+ * message, which is what is reported by std::exception::what() for RCLErrors.
+ */
+template<typename T,
+  typename = typename std::enable_if_t<
+    std::is_convertible<T, rclcpp::exceptions::RCLErrorBase>::value>>
+::testing::AssertionResult AreThrowableContentsEqual(
+  const rclcpp::exceptions::RCLErrorBase & expected,
+  const rclcpp::exceptions::RCLErrorBase & actual,
+  const char * expected_exception_expression,
+  const char * throwing_expression)
+{
+  if ((expected.ret == actual.ret) && (expected.message == actual.message)) {
+    return ::testing::AssertionSuccess() <<
+           "'\nThe contents of the RCLError thrown by the expression\n'" << throwing_expression <<
+           "':\n\trcl_ret_t: " << actual.ret << "\n\tmessage: '" << actual.message <<
+           "'\n\nmatch the contents of the expected RCLError\n'" <<
+           expected_exception_expression << "'\n\trcl_ret_t: " << expected.ret <<
+           "\n\tmessage: '" << expected.message << "'\n";
+  }
+
+  return ::testing::AssertionFailure() <<
+         "'\nThe contents of the RCLError thrown by the expression\n'" << throwing_expression <<
+         "':\n\trcl_ret_t: " << actual.ret << "\n\tmessage: '" << actual.message <<
+         "'\n\ndo not match the contents of the expected RCLError\n'" <<
+         expected_exception_expression << "'\n\trcl_ret_t: " << expected.ret << "\n\tmessage: '" <<
+         expected.message << "'\n";
+}
+
+}  // namespace details
+}  // namespace testing
+}  // namespace rclcpp
+
+/**
+ * \def CHECK_THROW_EQ_IMPL
+ * \brief Implemented check if statement throws expected exception. don't use directly, use
+ * RCLCPP_EXPECT_THROW_EQ or RCLCPP_ASSERT_THROW_EQ instead.
+ */
+#define CHECK_THROW_EQ_IMPL(throwing_statement, expected_exception, assertion_result) \
+  do { \
+    using ExceptionT = decltype(expected_exception); \
+    try { \
+      throwing_statement; \
+      assertion_result = ::testing::AssertionFailure() << \
+        "\nExpected the expression:\n\t'" #throwing_statement "'\nto throw: \n\t'" << \
+        #expected_exception "'\nbut it did not throw.\n"; \
+    } catch (const ExceptionT & e) { \
+      assertion_result = \
+        rclcpp::testing::details::AreThrowableContentsEqual<ExceptionT>( \
+        expected_exception, e, #expected_exception, #throwing_statement); \
+    } catch (const std::exception & e) { \
+      assertion_result = ::testing::AssertionFailure() << \
+        "\nExpected the expression:\n\t'" #throwing_statement "'\nto throw: \n\t'" << \
+        #expected_exception "'\nbut it threw:\n\tType: " << typeid(e).name() << \
+        "\n\te.what(): '" << e.what() << "'\n"; \
+    } catch (...) { \
+      assertion_result = ::testing::AssertionFailure() << \
+        "\nExpected the expression:\n\t'" #throwing_statement "'\nto throw: \n\t'" << \
+        #expected_exception "'\nbut it threw an unrecognized throwable type.\n"; \
+    } \
+  } while (0)
+
+/**
+ * \def RCLCPP_EXPECT_THROW_EQ
+ * \brief Check if a statement throws the expected exception type and that the exceptions matches
+ *   the expected exception.
+ *
+ * Like other gtest EXPECT_ macros, this doesn't halt a test and return on failure. Instead it
+ * just adds a failure to the current test.
+ *
+ * See test_gtest_macros.cpp for examples
+ */
+#define RCLCPP_EXPECT_THROW_EQ(throwing_statement, expected_exception) \
+  do { \
+    ::testing::AssertionResult \
+    is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable = \
+      ::testing::AssertionSuccess(); \
+    CHECK_THROW_EQ_IMPL( \
+      throwing_statement, \
+      expected_exception, \
+      is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable); \
+    EXPECT_TRUE(is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable); \
+  } while (0)
+
+/**
+ * \def RCLCPP_ASSERT_THROW_EQ
+ * \brief Assert that a statement throws the expected exception type and that the exceptions
+ * matches the expected exception.
+ *
+ * See test_gtest_macros.cpp for examples
+ *
+ * Like other gtest ASSERT_ macros, this will halt the test on failure and return.
+ */
+#define RCLCPP_ASSERT_THROW_EQ(throwing_statement, expected_exception) \
+  do { \
+    ::testing::AssertionResult \
+    is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable = \
+      ::testing::AssertionSuccess(); \
+    CHECK_THROW_EQ_IMPL( \
+      throwing_statement, \
+      expected_exception, \
+      is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable); \
+    ASSERT_TRUE(is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable); \
+  } while (0)
+
+#endif  // UTILS__RCLCPP_GTEST_MACROS_HPP_

rclcpp/test/utils/test_rclcpp_gtest_macros.cpp

@@ -0,0 +1,204 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <cstring>
+#include <exception>
+
+#include <string>
+
+#include "./rclcpp_gtest_macros.hpp"
+
+#include "rcl/rcl.h"
+#include "rclcpp/rclcpp.hpp"
+
+struct NonStandardThrowable
+{
+  bool operator==(const NonStandardThrowable &) const
+  {
+    return true;
+  }
+};
+
+std::ostream & operator<<(std::ostream & os, const NonStandardThrowable &)
+{
+  os << "NonStandardThrowable";
+  return os;
+}
+
+TEST(TestGtestMacros, standard_exceptions) {
+  RCLCPP_EXPECT_THROW_EQ(
+    throw std::runtime_error("some runtime error"),
+    std::runtime_error("some runtime error"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    throw std::invalid_argument("some invalid argument error"),
+    std::invalid_argument("some invalid argument error"));
+
+  RCLCPP_ASSERT_THROW_EQ(
+    throw std::runtime_error("some runtime error"),
+    std::runtime_error("some runtime error"));
+
+  RCLCPP_ASSERT_THROW_EQ(
+    throw std::invalid_argument("some invalid argument error"),
+    std::invalid_argument("some invalid argument error"));
+}
+
+TEST(TestGtestMacros, standard_exceptions_not_equals) {
+  ::testing::AssertionResult result = ::testing::AssertionSuccess();
+  CHECK_THROW_EQ_IMPL(
+    throw std::runtime_error("some runtime error"),
+    std::range_error("some runtime error"),
+    result);
+  EXPECT_FALSE(result);
+
+  CHECK_THROW_EQ_IMPL(
+    throw std::invalid_argument("some invalid argument error"),
+    std::invalid_argument("some different invalid argument error"),
+    result);
+  EXPECT_FALSE(result);
+}
+
+TEST(TestGTestMacros, non_standard_types) {
+  RCLCPP_EXPECT_THROW_EQ(throw 0, 0);
+
+  RCLCPP_EXPECT_THROW_EQ(throw 42, 42);
+
+  RCLCPP_EXPECT_THROW_EQ(throw std::string("some string"), std::string("some string"));
+
+  RCLCPP_EXPECT_THROW_EQ(throw NonStandardThrowable(), NonStandardThrowable());
+
+  RCLCPP_ASSERT_THROW_EQ(throw 0, 0);
+
+  RCLCPP_ASSERT_THROW_EQ(throw 42, 42);
+
+  RCLCPP_ASSERT_THROW_EQ(throw std::string("some string"), std::string("some string"));
+
+  RCLCPP_ASSERT_THROW_EQ(throw NonStandardThrowable(), NonStandardThrowable());
+}
+
+TEST(TestGTestMacros, non_standard_types_not_equals) {
+  ::testing::AssertionResult result = ::testing::AssertionSuccess();
+
+  CHECK_THROW_EQ_IMPL(throw 0, 1, result);
+  EXPECT_FALSE(result);
+  result = ::testing::AssertionSuccess();
+
+  CHECK_THROW_EQ_IMPL(throw -42, 42, result);
+  EXPECT_FALSE(result);
+  result = ::testing::AssertionSuccess();
+
+  CHECK_THROW_EQ_IMPL(throw std::string("some string"), std::string("some other string"), result);
+  EXPECT_FALSE(result);
+}
+
+TEST(TestGTestMacros, rclcpp_exceptions) {
+  rcutils_error_state_t rcl_error_state = {"this is some error message", __FILE__, __LINE__};
+  {
+    auto expected =
+      rclcpp::exceptions::RCLError(RCL_RET_ERROR, &rcl_error_state, "exception_prefix");
+    auto actual =
+      rclcpp::exceptions::RCLError(RCL_RET_ERROR, &rcl_error_state, "exception_prefix");
+    RCLCPP_EXPECT_THROW_EQ(throw actual, expected);
+    RCLCPP_ASSERT_THROW_EQ(throw actual, expected);
+  }
+  {
+    auto expected =
+      rclcpp::exceptions::RCLBadAlloc(RCL_RET_BAD_ALLOC, &rcl_error_state);
+    auto actual =
+      rclcpp::exceptions::RCLBadAlloc(RCL_RET_BAD_ALLOC, &rcl_error_state);
+    RCLCPP_EXPECT_THROW_EQ(throw actual, expected);
+  }
+  {
+    // Prefixes are not checked
+    auto expected =
+      rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, &rcl_error_state, "exception_prefix");
+    auto actual =
+      rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, &rcl_error_state, "different_prefix");
+    RCLCPP_EXPECT_THROW_EQ(throw actual, expected);
+    RCLCPP_ASSERT_THROW_EQ(throw actual, expected);
+  }
+  {
+    // File names are not checked
+    rcutils_error_state_t different_error_state = rcl_error_state;
+    std::snprintf(
+      different_error_state.file, RCUTILS_ERROR_STATE_FILE_MAX_LENGTH, "different_file.cpp");
+    auto expected =
+      rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, &rcl_error_state, "exception_prefix");
+    auto actual =
+      rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, &different_error_state, "exception_prefix");
+    RCLCPP_EXPECT_THROW_EQ(throw actual, expected);
+    RCLCPP_ASSERT_THROW_EQ(throw actual, expected);
+  }
+  {
+    // Line numbers are not checked
+    rcutils_error_state_t different_error_state = rcl_error_state;
+    different_error_state.line_number += 42;
+    auto expected =
+      rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, &rcl_error_state, "exception_prefix");
+    auto actual =
+      rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, &different_error_state, "exception_prefix");
+    RCLCPP_EXPECT_THROW_EQ(throw actual, expected);
+    RCLCPP_ASSERT_THROW_EQ(throw actual, expected);
+  }
+}
+
+TEST(TestGTestMacros, rclcpp_exceptions_not_equal) {
+  rcutils_error_state_t rcl_error_state = {"this is some error message", __FILE__, __LINE__};
+  {
+    // Check different return errors
+    ::testing::AssertionResult result = ::testing::AssertionSuccess();
+    auto expected =
+      rclcpp::exceptions::RCLError(RCL_RET_ERROR, &rcl_error_state, "exception_prefix");
+
+    auto actual =
+      rclcpp::exceptions::RCLError(RCL_RET_BAD_ALLOC, &rcl_error_state, "exception_prefix");
+    CHECK_THROW_EQ_IMPL(throw actual, expected, result);
+    EXPECT_FALSE(result);
+  }
+  {
+    // Check different error messages
+    rcutils_error_state_t different_error_state = rcl_error_state;
+    std::snprintf(
+      different_error_state.message,
+      RCUTILS_ERROR_STATE_MESSAGE_MAX_LENGTH,
+      "this is a different error message");
+    ::testing::AssertionResult result = ::testing::AssertionSuccess();
+    auto expected =
+      rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, &rcl_error_state, "exception_prefix");
+    auto actual =
+      rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, &different_error_state, "exception_prefix");
+    CHECK_THROW_EQ_IMPL(throw actual, expected, result);
+    EXPECT_FALSE(result);
+  }
+  {
+    // Check different exception types
+    ::testing::AssertionResult result = ::testing::AssertionSuccess();
+    auto expected =
+      rclcpp::exceptions::RCLError(RCL_RET_ERROR, &rcl_error_state, "exception_prefix");
+    auto actual =
+      rclcpp::exceptions::RCLInvalidArgument(RCL_RET_ERROR, &rcl_error_state, "exception_prefix");
+    CHECK_THROW_EQ_IMPL(throw actual, expected, result);
+    EXPECT_FALSE(result);
+  }
+}

rclcpp_action/CHANGELOG.rst

@@ -3,6 +3,18 @@ Changelog for package rclcpp_action
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+5.0.0 (2020-09-18)
+------------------
+* Pass goal handle to goal response callback instead of a future (`#1311 <https://github.com/ros2/rclcpp/issues/1311>`_)
+* Remove deprecated client goal handle method for getting result (`#1309 <https://github.com/ros2/rclcpp/issues/1309>`_)
+* Increase test timeout necessary for Connext (`#1256 <https://github.com/ros2/rclcpp/issues/1256>`_)
+* Contributors: Dirk Thomas, Jacob Perron
+
+4.0.0 (2020-07-09)
+------------------
+* Bump to QD to level 3 and fixed links (`#1158 <https://github.com/ros2/rclcpp/issues/1158>`_)
+* Contributors: Alejandro Hernández Cordero
+
 3.0.0 (2020-06-18)
 ------------------
 * Add rcl_action_client_options when creating action client. (`#1133 <https://github.com/ros2/rclcpp/issues/1133>`_)

rclcpp_action/CMakeLists.txt

@@ -74,7 +74,7 @@ if(BUILD_TESTING)
   set(ament_cmake_cppcheck_ADDITIONAL_INCLUDE_DIRS ${rclcpp_INCLUDE_DIRS})
   ament_lint_auto_find_test_dependencies()
 
-  ament_add_gtest(test_client test/test_client.cpp TIMEOUT 120)
+  ament_add_gtest(test_client test/test_client.cpp TIMEOUT 180)
   if(TARGET test_client)
     ament_target_dependencies(test_client
       "test_msgs"

rclcpp_action/include/rclcpp_action/client.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP_ACTION__CLIENT_HPP_
 #define RCLCPP_ACTION__CLIENT_HPP_
 
+#include <rclcpp/exceptions.hpp>
 #include <rclcpp/macros.hpp>
 #include <rclcpp/node_interfaces/node_base_interface.hpp>
 #include <rclcpp/node_interfaces/node_logging_interface.hpp>
@@ -37,6 +38,7 @@
 #include <utility>
 
 #include "rclcpp_action/client_goal_handle.hpp"
+#include "rclcpp_action/exceptions.hpp"
 #include "rclcpp_action/types.hpp"
 #include "rclcpp_action/visibility_control.hpp"
 
@@ -261,8 +263,7 @@ public:
   using Feedback = typename ActionT::Feedback;
   using GoalHandle = ClientGoalHandle<ActionT>;
   using WrappedResult = typename GoalHandle::WrappedResult;
-  using GoalResponseCallback =
-    std::function<void (std::shared_future<typename GoalHandle::SharedPtr>)>;
+  using GoalResponseCallback = std::function<void (typename GoalHandle::SharedPtr)>;
   using FeedbackCallback = typename GoalHandle::FeedbackCallback;
   using ResultCallback = typename GoalHandle::ResultCallback;
   using CancelRequest = typename ActionT::Impl::CancelGoalService::Request;
@@ -284,12 +285,9 @@ public:
 
     /// Function called when the goal is accepted or rejected.
     /**
-     * Takes a single argument that is a future to a goal handle shared pointer.
+     * Takes a single argument that is a goal handle shared pointer.
      * If the goal is accepted, then the pointer points to a valid goal handle.
      * If the goal is rejected, then pointer has the value `nullptr`.
-     * If an error occurs while waiting for the goal response an exception will be thrown
-     * when calling `future::get()`.
-     * Possible exceptions include `rclcpp::RCLError` and `rclcpp::RCLBadAlloc`.
      */
     GoalResponseCallback goal_response_callback;
 
@@ -360,7 +358,7 @@ public:
         if (!goal_response->accepted) {
           promise->set_value(nullptr);
           if (options.goal_response_callback) {
-            options.goal_response_callback(future);
+            options.goal_response_callback(nullptr);
           }
           return;
         }
@@ -376,16 +374,11 @@ public:
         }
         promise->set_value(goal_handle);
         if (options.goal_response_callback) {
-          options.goal_response_callback(future);
+          options.goal_response_callback(goal_handle);
         }
 
         if (options.result_callback) {
-          try {
-            this->make_result_aware(goal_handle);
-          } catch (...) {
-            promise->set_exception(std::current_exception());
-            return;
-          }
+          this->make_result_aware(goal_handle);
         }
       });
 
@@ -414,7 +407,7 @@ public:
   /// Asynchronously get the result for an active goal.
   /**
    * \throws exceptions::UnknownGoalHandleError If the goal unknown or already reached a terminal
-   *   state.
+   *   state, or if there was an error requesting the result.
    * \param[in] goal_handle The goal handle for which to get the result.
    * \param[in] result_callback Optional callback that is called when the result is received.
    * \return A future that is set to the goal result when the goal is finished.
@@ -428,6 +421,11 @@ public:
     if (goal_handles_.count(goal_handle->get_goal_id()) == 0) {
       throw exceptions::UnknownGoalHandleError();
     }
+    if (goal_handle->is_invalidated()) {
+      // This case can happen if there was a failure to send the result request
+      // during the goal response callback
+      throw goal_handle->invalidate_exception_;
+    }
     if (result_callback) {
       // This will override any previously registered callback
       goal_handle->set_result_callback(result_callback);
@@ -518,7 +516,7 @@ public:
     while (it != goal_handles_.end()) {
       typename GoalHandle::SharedPtr goal_handle = it->second.lock();
       if (goal_handle) {
-        goal_handle->invalidate();
+        goal_handle->invalidate(exceptions::UnawareGoalHandleError());
       }
       it = goal_handles_.erase(it);
     }
@@ -632,22 +630,27 @@ private:
     using GoalResultRequest = typename ActionT::Impl::GetResultService::Request;
     auto goal_result_request = std::make_shared<GoalResultRequest>();
     goal_result_request->goal_id.uuid = goal_handle->get_goal_id();
-    this->send_result_request(
-      std::static_pointer_cast<void>(goal_result_request),
-      [goal_handle, this](std::shared_ptr<void> response) mutable
-      {
-        // Wrap the response in a struct with the fields a user cares about
-        WrappedResult wrapped_result;
-        using GoalResultResponse = typename ActionT::Impl::GetResultService::Response;
-        auto result_response = std::static_pointer_cast<GoalResultResponse>(response);
-        wrapped_result.result = std::make_shared<typename ActionT::Result>();
-        *wrapped_result.result = result_response->result;
-        wrapped_result.goal_id = goal_handle->get_goal_id();
-        wrapped_result.code = static_cast<ResultCode>(result_response->status);
-        goal_handle->set_result(wrapped_result);
-        std::lock_guard<std::mutex> lock(goal_handles_mutex_);
-        goal_handles_.erase(goal_handle->get_goal_id());
-      });
+    try {
+      this->send_result_request(
+        std::static_pointer_cast<void>(goal_result_request),
+        [goal_handle, this](std::shared_ptr<void> response) mutable
+        {
+          // Wrap the response in a struct with the fields a user cares about
+          WrappedResult wrapped_result;
+          using GoalResultResponse = typename ActionT::Impl::GetResultService::Response;
+          auto result_response = std::static_pointer_cast<GoalResultResponse>(response);
+          wrapped_result.result = std::make_shared<typename ActionT::Result>();
+          *wrapped_result.result = result_response->result;
+          wrapped_result.goal_id = goal_handle->get_goal_id();
+          wrapped_result.code = static_cast<ResultCode>(result_response->status);
+          goal_handle->set_result(wrapped_result);
+          std::lock_guard<std::mutex> lock(goal_handles_mutex_);
+          goal_handles_.erase(goal_handle->get_goal_id());
+        });
+    } catch (rclcpp::exceptions::RCLError & ex) {
+      // This will cause an exception when the user tries to access the result
+      goal_handle->invalidate(exceptions::UnawareGoalHandleError(ex.message));
+    }
   }
 
   /// \internal

rclcpp_action/include/rclcpp_action/client_goal_handle.hpp

@@ -26,6 +26,7 @@
 #include <memory>
 #include <mutex>
 
+#include "rclcpp_action/exceptions.hpp"
 #include "rclcpp_action/types.hpp"
 #include "rclcpp_action/visibility_control.hpp"
 
@@ -89,22 +90,6 @@ public:
   rclcpp::Time
   get_goal_stamp() const;
 
-  /// Get a future to the goal result.
-  /**
-   * \deprecated Use rclcpp_action::Client::async_get_result() instead.
-   *
-   * This method should not be called if the `ignore_result` flag was set when
-   * sending the original goal request (see Client::async_send_goal).
-   *
-   * `is_result_aware()` can be used to check if it is safe to call this method.
-   *
-   * \throws exceptions::UnawareGoalHandleError If the the goal handle is unaware of the result.
-   * \return A future to the result.
-   */
-  [[deprecated("use rclcpp_action::Client::async_get_result() instead")]]
-  std::shared_future<WrappedResult>
-  async_result();
-
   /// Get the goal status code.
   int8_t
   get_status();
@@ -161,10 +146,15 @@ private:
   set_result(const WrappedResult & wrapped_result);
 
   void
-  invalidate();
+  invalidate(const exceptions::UnawareGoalHandleError & ex);
+
+  bool
+  is_invalidated() const;
 
   GoalInfo info_;
 
+  std::exception_ptr invalidate_exception_{nullptr};
+
   bool is_result_aware_{false};
   std::promise<WrappedResult> result_promise_;
   std::shared_future<WrappedResult> result_future_;

rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp

@@ -55,13 +55,6 @@ ClientGoalHandle<ActionT>::get_goal_stamp() const
   return info_.stamp;
 }
 
-template<typename ActionT>
-std::shared_future<typename ClientGoalHandle<ActionT>::WrappedResult>
-ClientGoalHandle<ActionT>::async_result()
-{
-  return this->async_get_result();
-}
-
 template<typename ActionT>
 std::shared_future<typename ClientGoalHandle<ActionT>::WrappedResult>
 ClientGoalHandle<ActionT>::async_get_result()
@@ -145,11 +138,24 @@ ClientGoalHandle<ActionT>::set_result_awareness(bool awareness)
 
 template<typename ActionT>
 void
-ClientGoalHandle<ActionT>::invalidate()
+ClientGoalHandle<ActionT>::invalidate(const exceptions::UnawareGoalHandleError & ex)
 {
   std::lock_guard<std::mutex> guard(handle_mutex_);
+  // Guard against multiple calls
+  if (is_invalidated()) {
+    return;
+  }
+  is_result_aware_ = false;
+  invalidate_exception_ = std::make_exception_ptr(ex);
   status_ = GoalStatus::STATUS_UNKNOWN;
-  result_promise_.set_exception(std::make_exception_ptr(exceptions::UnawareGoalHandleError()));
+  result_promise_.set_exception(invalidate_exception_);
+}
+
+template<typename ActionT>
+bool
+ClientGoalHandle<ActionT>::is_invalidated() const
+{
+  return invalidate_exception_ != nullptr;
 }
 
 template<typename ActionT>

rclcpp_action/include/rclcpp_action/exceptions.hpp

@@ -16,6 +16,7 @@
 #define RCLCPP_ACTION__EXCEPTIONS_HPP_
 
 #include <stdexcept>
+#include <string>
 
 namespace rclcpp_action
 {
@@ -33,8 +34,9 @@ public:
 class UnawareGoalHandleError : public std::runtime_error
 {
 public:
-  UnawareGoalHandleError()
-  : std::runtime_error("Goal handle is not tracking the goal result.")
+  UnawareGoalHandleError(
+    const std::string & message = "Goal handle is not tracking the goal result.")
+  : std::runtime_error(message)
   {
   }
 };

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>3.0.0</version>
+  <version>5.0.0</version>
   <description>Adds action APIs for C++.</description>
   <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
   <license>Apache License 2.0</license>

rclcpp_action/test/test_client.cpp

@@ -435,9 +435,8 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait
   auto send_goal_ops = rclcpp_action::Client<ActionType>::SendGoalOptions();
   send_goal_ops.goal_response_callback =
     [&goal_response_received]
-      (std::shared_future<typename ActionGoalHandle::SharedPtr> future) mutable
+      (typename ActionGoalHandle::SharedPtr goal_handle) mutable
     {
-      auto goal_handle = future.get();
       if (goal_handle) {
         goal_response_received = true;
       }

rclcpp_components/CHANGELOG.rst

@@ -2,6 +2,15 @@
 Changelog for package rclcpp_components
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+5.0.0 (2020-09-18)
+------------------
+
+4.0.0 (2020-07-09)
+------------------
+* Bump to QD to level 3 and fixed links (`#1158 <https://github.com/ros2/rclcpp/issues/1158>`_)
+* Include original exception in ComponentManagerException (`#1157 <https://github.com/ros2/rclcpp/issues/1157>`_)
+* Contributors: Alejandro Hernández Cordero, Martijn Buijs, Tomoya Fujita
+
 3.0.0 (2020-06-18)
 ------------------
 

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>3.0.0</version>
+  <version>5.0.0</version>
   <description>Package containing tools for dynamically loadable components</description>
   <maintainer email="michael@openrobotics.org">Michael Carroll</maintainer>
   <license>Apache License 2.0</license>

rclcpp_lifecycle/CHANGELOG.rst

@@ -3,6 +3,23 @@ Changelog for package rclcpp_lifecycle
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+5.0.0 (2020-09-18)
+------------------
+* Increase test coverage of rclcpp_lifecycle to 96% (`#1298 <https://github.com/ros2/rclcpp/issues/1298>`_)
+* Log error instead of throwing exception in Transition and State reset(), mark no except (`#1297 <https://github.com/ros2/rclcpp/issues/1297>`_)
+* Remove unused private function (rclcpp::Node and rclcpp_lifecycle::Node) (`#1294 <https://github.com/ros2/rclcpp/issues/1294>`_)
+* Remove rmw-dependent unit-test checks (`#1293 <https://github.com/ros2/rclcpp/issues/1293>`_)
+* Added missing tests for rclcpp lifecycle (`#1240 <https://github.com/ros2/rclcpp/issues/1240>`_)
+* Warn about unused result of add_on_set_parameters_callback (`#1238 <https://github.com/ros2/rclcpp/issues/1238>`_)
+* Contributors: Alejandro Hernández Cordero, Jacob Perron, Stephen Brawner
+
+4.0.0 (2020-07-09)
+------------------
+* Remove deprecated set_on_parameters_set_callback function (`#1199 <https://github.com/ros2/rclcpp/issues/1199>`_)
+* Bump to QD to level 3 and fixed links (`#1158 <https://github.com/ros2/rclcpp/issues/1158>`_)
+* Fix race in test_lifecycle_service_client (`#1204 <https://github.com/ros2/rclcpp/issues/1204>`_)
+* Contributors: Alejandro Hernández Cordero, Claire Wang, Dirk Thomas
+
 3.0.0 (2020-06-18)
 ------------------
 * Fix doxygen warnings (`#1163 <https://github.com/ros2/rclcpp/issues/1163>`_)

rclcpp_lifecycle/CMakeLists.txt

@@ -56,16 +56,27 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_lifecycle_node
       "rcl_lifecycle"
       "rclcpp"
+      "rcutils"
     )
-    target_link_libraries(test_lifecycle_node ${PROJECT_NAME})
+    target_link_libraries(test_lifecycle_node ${PROJECT_NAME} mimick)
+  endif()
+  ament_add_gtest(test_lifecycle_publisher test/test_lifecycle_publisher.cpp)
+  if(TARGET test_lifecycle_publisher)
+    ament_target_dependencies(test_lifecycle_publisher
+      "rcl_lifecycle"
+      "rclcpp"
+      "test_msgs"
+    )
+    target_link_libraries(test_lifecycle_publisher ${PROJECT_NAME})
   endif()
   ament_add_gtest(test_lifecycle_service_client test/test_lifecycle_service_client.cpp)
   if(TARGET test_lifecycle_service_client)
     ament_target_dependencies(test_lifecycle_service_client
       "rcl_lifecycle"
       "rclcpp"
+      "rcutils"
     )
-    target_link_libraries(test_lifecycle_service_client ${PROJECT_NAME})
+    target_link_libraries(test_lifecycle_service_client ${PROJECT_NAME} mimick)
   endif()
   ament_add_gtest(test_state_machine_info test/test_state_machine_info.cpp)
   if(TARGET test_state_machine_info)
@@ -104,8 +115,12 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_transition_wrapper
       "rcl_lifecycle"
       "rclcpp"
+      "rcutils"
+    )
+    target_link_libraries(test_transition_wrapper ${PROJECT_NAME} mimick)
+    target_compile_definitions(test_transition_wrapper
+      PUBLIC RCUTILS_ENABLE_FAULT_INJECTION
     )
-    target_link_libraries(test_transition_wrapper ${PROJECT_NAME})
   endif()
 endif()
 

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp

@@ -42,6 +42,8 @@
 #include <utility>
 #include <vector>
 
+#include "rcutils/macros.h"
+
 #include "rcl/error_handling.h"
 #include "rcl/node.h"
 
@@ -456,6 +458,7 @@ public:
    * \sa rclcpp::Node::add_on_set_parameters_callback
    */
   RCLCPP_LIFECYCLE_PUBLIC
+  RCUTILS_WARN_UNUSED
   rclcpp_lifecycle::LifecycleNode::OnSetParametersCallbackHandle::SharedPtr
   add_on_set_parameters_callback(
     rclcpp_lifecycle::LifecycleNode::OnParametersSetCallbackType callback);
@@ -893,10 +896,6 @@ protected:
 private:
   RCLCPP_DISABLE_COPY(LifecycleNode)
 
-  RCLCPP_LIFECYCLE_PUBLIC
-  bool
-  group_in_node(rclcpp::CallbackGroup::SharedPtr group);
-
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
   rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;

rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp

@@ -87,7 +87,7 @@ public:
 protected:
   RCLCPP_LIFECYCLE_PUBLIC
   void
-  reset();
+  reset() noexcept;
 
   rcutils_allocator_t allocator_;
 

rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp

@@ -119,7 +119,7 @@ public:
 protected:
   RCLCPP_LIFECYCLE_PUBLIC
   void
-  reset();
+  reset() noexcept;
 
   rcutils_allocator_t allocator_;
 

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>3.0.0</version>
+  <version>5.0.0</version>
   <description>Package containing a prototype for lifecycle implementation</description>
   <maintainer email="karsten@osrfoundation.org">Karsten Knese</maintainer>
   <license>Apache License 2.0</license>
@@ -24,6 +24,9 @@
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>mimick_vendor</test_depend>
+  <test_depend>rcutils</test_depend>
+  <test_depend>test_msgs</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

rclcpp_lifecycle/src/lifecycle_node.cpp

@@ -180,12 +180,6 @@ LifecycleNode::set_parameter(const rclcpp::Parameter & parameter)
   return this->set_parameters_atomically({parameter});
 }
 
-bool
-LifecycleNode::group_in_node(rclcpp::CallbackGroup::SharedPtr group)
-{
-  return node_base_->callback_group_in_node(group);
-}
-
 std::vector<rcl_interfaces::msg::SetParametersResult>
 LifecycleNode::set_parameters(
   const std::vector<rclcpp::Parameter> & parameters)

rclcpp_lifecycle/src/state.cpp

@@ -21,6 +21,7 @@
 #include "rcl_lifecycle/rcl_lifecycle.h"
 
 #include "rclcpp/exceptions.hpp"
+#include "rclcpp/logging.hpp"
 
 #include "rcutils/allocator.h"
 
@@ -143,7 +144,7 @@ State::label() const
 }
 
 void
-State::reset()
+State::reset() noexcept
 {
   if (!owns_rcl_state_handle_) {
     state_handle_ = nullptr;
@@ -157,7 +158,9 @@ State::reset()
   allocator_.deallocate(state_handle_, allocator_.state);
   state_handle_ = nullptr;
   if (ret != RCL_RET_OK) {
-    rclcpp::exceptions::throw_from_rcl_error(ret);
+    RCLCPP_ERROR(
+      rclcpp::get_logger("rclcpp_lifecycle"),
+      "rcl_lifecycle_transition_fini did not complete successfully, leaking memory");
   }
 }
 

rclcpp_lifecycle/src/transition.cpp

@@ -21,6 +21,7 @@
 #include "rcl_lifecycle/rcl_lifecycle.h"
 
 #include "rclcpp/exceptions.hpp"
+#include "rclcpp/logging.hpp"
 
 #include "rcutils/allocator.h"
 
@@ -246,7 +247,7 @@ Transition::goal_state() const
 }
 
 void
-Transition::reset()
+Transition::reset() noexcept
 {
   // can't free anything which is not owned
   if (!owns_rcl_transition_handle_) {
@@ -261,7 +262,9 @@ Transition::reset()
   allocator_.deallocate(transition_handle_, allocator_.state);
   transition_handle_ = nullptr;
   if (ret != RCL_RET_OK) {
-    rclcpp::exceptions::throw_from_rcl_error(ret);
+    RCLCPP_ERROR(
+      rclcpp::get_logger("rclcpp_lifecycle"),
+      "rcl_lifecycle_transition_fini did not complete successfully, leaking memory");
   }
 }
 }  // namespace rclcpp_lifecycle

rclcpp_lifecycle/test/mocking_utils/patch.hpp

@@ -0,0 +1,527 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// Original file taken from:
+// https://github.com/ros2/rcutils/blob/master/test/mocking_utils/patch.hpp
+
+#ifndef MOCKING_UTILS__PATCH_HPP_
+#define MOCKING_UTILS__PATCH_HPP_
+
+#define MOCKING_UTILS_SUPPORT_VA_LIST
+#if (defined(__aarch64__) || defined(__arm__) || defined(_M_ARM) || defined(__thumb__))
+// In ARM machines, va_list does not define comparison operators
+// nor the compiler allows defining them via operator overloads.
+// Thus, Mimick argument matching code will not compile.
+#undef MOCKING_UTILS_SUPPORT_VA_LIST
+#endif
+
+#ifdef MOCKING_UTILS_SUPPORT_VA_LIST
+#include <cstdarg>
+#endif
+
+#include <functional>
+#include <string>
+#include <type_traits>
+#include <utility>
+
+#include "mimick/mimick.h"
+
+#include "rcutils/error_handling.h"
+#include "rcutils/macros.h"
+
+namespace mocking_utils
+{
+
+/// Mimick specific traits for each mocking_utils::Patch instance.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam SignatureT Type of the symbol to be patched.
+*/
+template<size_t ID, typename SignatureT>
+struct PatchTraits;
+
+/// Traits specialization for ReturnT(void) free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ */
+template<size_t ID, typename ReturnT>
+struct PatchTraits<ID, ReturnT(void)>
+{
+  mmk_mock_define(mock_type, ReturnT);
+};
+
+/// Traits specialization for void(void) free functions.
+/**
+ * Necessary for Mimick macros to adjust accordingly when the return
+ * type is `void`.
+ *
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ */
+template<size_t ID>
+struct PatchTraits<ID, void(void)>
+{
+  mmk_mock_define(mock_type, void);
+};
+
+/// Traits specialization for ReturnT(ArgT0) free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgT0 Argument type.
+ */
+template<size_t ID, typename ReturnT, typename ArgT0>
+struct PatchTraits<ID, ReturnT(ArgT0)>
+{
+  mmk_mock_define(mock_type, ReturnT, ArgT0);
+};
+
+/// Traits specialization for void(ArgT0) free functions.
+/**
+ * Necessary for Mimick macros to adjust accordingly when the return
+ * type is `void`.
+ *
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ArgT0 Argument type.
+ */
+template<size_t ID, typename ArgT0>
+struct PatchTraits<ID, void(ArgT0)>
+{
+  mmk_mock_define(mock_type, void, ArgT0);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1) free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1)>
+{
+  mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1);
+};
+
+/// Traits specialization for void(ArgT0, ArgT1) free functions.
+/**
+ * Necessary for Mimick macros to adjust accordingly when the return
+ * type is `void`.
+ *
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ArgT0, typename ArgT1>
+struct PatchTraits<ID, void(ArgT0, ArgT1)>
+{
+  mmk_mock_define(mock_type, void, ArgT0, ArgT1);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2) free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1, typename ArgT2>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2)>
+{
+  mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2);
+};
+
+/// Traits specialization for void(ArgT0, ArgT1, ArgT2) free functions.
+/**
+ * Necessary for Mimick macros to adjust accordingly when the return
+ * type is `void`.
+ *
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ArgT0, typename ArgT1, typename ArgT2>
+struct PatchTraits<ID, void(ArgT0, ArgT1, ArgT2)>
+{
+  mmk_mock_define(mock_type, void, ArgT0, ArgT1, ArgT2);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3) free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3)>
+{
+  mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3);
+};
+
+/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3) free functions.
+/**
+ * Necessary for Mimick macros to adjust accordingly when the return
+ * type is `void`.
+ *
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3>
+struct PatchTraits<ID, void(ArgT0, ArgT1, ArgT2, ArgT3)>
+{
+  mmk_mock_define(mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4)
+/// free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3, typename ArgT4>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4)>
+{
+  mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4);
+};
+
+/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4)
+/// free functions.
+/**
+ * Necessary for Mimick macros to adjust accordingly when the return
+ * type is `void`.
+ *
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3, typename ArgT4>
+struct PatchTraits<ID, void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4)>
+{
+  mmk_mock_define(mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)
+/// free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3,
+  typename ArgT4, typename ArgT5>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)>
+{
+  mmk_mock_define(
+    mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5);
+};
+
+/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7)
+/// free functions.
+/**
+ * Necessary for Mimick macros to adjust accordingly when the return
+ * type is `void`.
+ *
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3,
+  typename ArgT4, typename ArgT5,
+  typename ArgT6, typename ArgT7,
+  typename ArgT8, typename ArgT9>
+struct PatchTraits<ID, ReturnT(
+    ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8, ArgT9)>
+{
+  mmk_mock_define(
+    mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8, ArgT9);
+};
+
+/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)
+/// free functions.
+/**
+ * Necessary for Mimick macros to adjust accordingly when the return
+ * type is `void`.
+ *
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3,
+  typename ArgT4, typename ArgT5>
+struct PatchTraits<ID, void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)>
+{
+  mmk_mock_define(
+    mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5);
+};
+
+/// Generic trampoline to wrap generalized callables in plain functions.
+/**
+ * \tparam ID Numerical identifier of this trampoline. Ought to be unique.
+ * \tparam SignatureT Type of the symbol this trampoline replaces.
+ */
+template<size_t ID, typename SignatureT>
+struct Trampoline;
+
+/// Trampoline specialization for free functions.
+template<size_t ID, typename ReturnT, typename ... ArgTs>
+struct Trampoline<ID, ReturnT(ArgTs...)>
+{
+  static ReturnT base(ArgTs... args)
+  {
+    return target(std::forward<ArgTs>(args)...);
+  }
+
+  static std::function<ReturnT(ArgTs...)> target;
+};
+
+template<size_t ID, typename ReturnT, typename ... ArgTs>
+std::function<ReturnT(ArgTs...)>
+Trampoline<ID, ReturnT(ArgTs...)>::target;
+
+/// Setup trampoline with the given @p target.
+/**
+ * \param[in] target Callable that this trampoline will target.
+ * \return the plain base function of this trampoline.
+ *
+ * \tparam ID Numerical identifier of this trampoline. Ought to be unique.
+ * \tparam SignatureT Type of the symbol this trampoline replaces.
+ */
+template<size_t ID, typename SignatureT>
+auto prepare_trampoline(std::function<SignatureT> target)
+{
+  Trampoline<ID, SignatureT>::target = target;
+  return Trampoline<ID, SignatureT>::base;
+}
+
+/// Patch class for binary API mocking
+/**
+ * Built on top of Mimick, to enable symbol mocking on a per dynamically
+ * linked binary object basis.
+ *
+ * \tparam ID Numerical identifier for this patch. Ought to be unique.
+ * \tparam SignatureT Type of the symbol to be patched.
+ */
+template<size_t ID, typename SignatureT>
+class Patch;
+
+/// Patch specialization for ReturnT(ArgTs...) free functions.
+/**
+ * \tparam ID Numerical identifier for this patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTs Argument types.
+ */
+template<size_t ID, typename ReturnT, typename ... ArgTs>
+class Patch<ID, ReturnT(ArgTs...)>
+{
+public:
+  using mock_type = typename PatchTraits<ID, ReturnT(ArgTs...)>::mock_type;
+
+  /// Construct a patch.
+  /**
+   * \param[in] target Symbol target string, using Mimick syntax
+   *   i.e. "symbol(@scope)?", where scope may be "self" to target the current
+   *   binary, "lib:library_name" to target a given library, "file:path/to/library"
+   *   to target a given file, or "sym:other_symbol" to target the first library
+   *   that defines said symbol.
+   * \param[in] proxy An indirection to call the target function.
+   *   This indirection must ensure this call goes through the function's
+   *   trampoline, as setup by the dynamic linker.
+   * \return a mocking_utils::Patch instance.
+   */
+  explicit Patch(const std::string & target, std::function<ReturnT(ArgTs...)> proxy)
+  : target_(target), proxy_(proxy)
+  {
+  }
+
+  // Copy construction and assignment are disabled.
+  Patch(const Patch &) = delete;
+  Patch & operator=(const Patch &) = delete;
+
+  Patch(Patch && other)
+  {
+    mock_ = other.mock_;
+    other.mock_ = nullptr;
+  }
+
+  Patch & operator=(Patch && other)
+  {
+    if (mock_) {
+      mmk_reset(mock_);
+    }
+    mock_ = other.mock_;
+    other.mock_ = nullptr;
+  }
+
+  ~Patch()
+  {
+    if (mock_) {
+      mmk_reset(mock_);
+    }
+  }
+
+  /// Inject a @p replacement for the patched function.
+  Patch & then_call(std::function<ReturnT(ArgTs...)> replacement) &
+  {
+    replace_with(replacement);
+    return *this;
+  }
+
+  /// Inject a @p replacement for the patched function.
+  Patch && then_call(std::function<ReturnT(ArgTs...)> replacement) &&
+  {
+    replace_with(replacement);
+    return std::move(*this);
+  }
+
+private:
+  // Helper for template parameter pack expansion using `mmk_any`
+  // macro as pattern.
+  template<typename T>
+  T any() {return mmk_any(T);}
+
+  void replace_with(std::function<ReturnT(ArgTs...)> replacement)
+  {
+    if (mock_) {
+      throw std::logic_error("Cannot configure patch more than once");
+    }
+    auto type_erased_trampoline =
+      reinterpret_cast<mmk_fn>(prepare_trampoline<ID>(replacement));
+    auto MMK_MANGLE(mock_type, create) =
+      PatchTraits<ID, ReturnT(ArgTs...)>::MMK_MANGLE(mock_type, create);
+    mock_ = mmk_mock(target_.c_str(), mock_type);
+    mmk_when(proxy_(any<ArgTs>()...), .then_call = type_erased_trampoline);
+  }
+
+  mock_type mock_{nullptr};
+  std::string target_;
+  std::function<ReturnT(ArgTs...)> proxy_;
+};
+
+/// Make a patch for a `target` function.
+/**
+ * Useful for type deduction during \ref mocking_utils::Patch construction.
+ *
+ * \param[in] target Symbol target string, using Mimick syntax.
+ * \param[in] proxy An indirection to call the target function.
+ * \return a mocking_utils::Patch instance.
+ *
+ * \tparam ID Numerical identifier for this patch. Ought to be unique.
+ * \tparam SignatureT Type of the function to be patched.
+ *
+ * \sa mocking_utils::Patch for further reference.
+ */
+template<size_t ID, typename SignatureT>
+auto make_patch(const std::string & target, std::function<SignatureT> proxy)
+{
+  return Patch<ID, SignatureT>(target, proxy);
+}
+
+/// Define a dummy operator `op` for a given `type`.
+/**
+ * Useful to enable patching functions that take arguments whose types
+ * do not define basic comparison operators, as required by Mimick.
+*/
+#define MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(type_, op) \
+  template<typename T> \
+  typename std::enable_if<std::is_same<T, type_>::value, bool>::type \
+  operator op(const T &, const T &) { \
+    return false; \
+  }
+
+/// Get the exact \ref mocking_utils::Patch type for a given `id` and `function`.
+/**
+ * Useful to avoid ignored attribute warnings when using the \b decltype operator.
+ */
+#define MOCKING_UTILS_PATCH_TYPE(id, function) \
+  decltype(mocking_utils::make_patch<id, decltype(function)>("", nullptr))
+
+/// A transparent forwarding proxy to a given `function`.
+/**
+ * Useful to ensure a call to `function` goes through its trampoline.
+ */
+#define MOCKING_UTILS_PATCH_PROXY(function) \
+  [] (auto && ... args)->decltype(auto) { \
+    return function(std::forward<decltype(args)>(args)...); \
+  }
+
+/// Compute a Mimick symbol target string based on which `function` is to be patched
+/// in which `scope`.
+#define MOCKING_UTILS_PATCH_TARGET(scope, function) \
+  (std::string(RCUTILS_STRINGIFY(function)) + "@" + (scope))
+
+/// Prepare a mocking_utils::Patch for patching a `function` in a given `scope`
+/// but defer applying any changes.
+#define prepare_patch(scope, function) \
+  make_patch<__COUNTER__, decltype(function)>( \
+    MOCKING_UTILS_PATCH_TARGET(scope, function), MOCKING_UTILS_PATCH_PROXY(function) \
+  )
+
+/// Patch a `function` with a used-provided `replacement` in a given `scope`.
+#define patch(scope, function, replacement) \
+  prepare_patch(scope, function).then_call(replacement)
+
+/// Patch a `function` to always yield a given `return_code` in a given `scope`.
+#define patch_and_return(scope, function, return_code) \
+  patch(scope, function, [&](auto && ...) {return return_code;})
+
+/// Patch a `function` to always yield a given `return_code` in a given `scope`.
+#define patch_to_fail(scope, function, error_message, return_code) \
+  patch( \
+    scope, function, [&](auto && ...) { \
+      RCUTILS_SET_ERROR_MSG(error_message); \
+      return return_code; \
+    })
+
+/// Patch a `function` to execute normally but always yield a given `return_code`
+/// in a given `scope`.
+/**
+ * \warning On some Linux distributions (e.g. CentOS), pointers to function
+ *   reference their PLT trampolines. In such cases, it is not possible to
+ *   call `function` from within the mock.
+ */
+#define inject_on_return(scope, function, return_code) \
+  patch( \
+    scope, function, ([&, base = function](auto && ... __args) { \
+      if (base != function) { \
+        static_cast<void>(base(std::forward<decltype(__args)>(__args)...)); \
+      } else { \
+        RCUTILS_SAFE_FWRITE_TO_STDERR( \
+          "[WARNING] mocking_utils::inject_on_return() cannot forward call to " \
+          "original '" RCUTILS_STRINGIFY(function) "' function before injection\n" \
+          "    at " __FILE__ ":" RCUTILS_STRINGIFY(__LINE__) "\n"); \
+      } \
+      return return_code; \
+    }))
+
+}  // namespace mocking_utils
+
+#ifdef MOCKING_UTILS_SUPPORT_VA_LIST
+// Define dummy comparison operators for C standard va_list type
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, >)
+#endif
+
+#endif  // MOCKING_UTILS__PATCH_HPP_

rclcpp_lifecycle/test/test_lifecycle_node.cpp

@@ -24,9 +24,13 @@
 #include "lifecycle_msgs/msg/state.hpp"
 #include "lifecycle_msgs/msg/transition.hpp"
 
+#include "rcl_lifecycle/rcl_lifecycle.h"
+
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 
+#include "./mocking_utils/patch.hpp"
+
 using lifecycle_msgs::msg::State;
 using lifecycle_msgs::msg::Transition;
 
@@ -37,13 +41,17 @@ protected:
   {
     rclcpp::init(0, nullptr);
   }
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
 };
 
 class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode
 {
 public:
-  explicit EmptyLifecycleNode(std::string node_name)
-  : rclcpp_lifecycle::LifecycleNode(std::move(node_name))
+  explicit EmptyLifecycleNode(const std::string & node_name)
+  : rclcpp_lifecycle::LifecycleNode(node_name)
   {}
 };
 
@@ -142,6 +150,22 @@ TEST_F(TestDefaultStateMachine, empty_initializer) {
   EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id());
 }
 
+TEST_F(TestDefaultStateMachine, empty_initializer_rcl_errors) {
+  {
+    auto patch = mocking_utils::patch_and_return(
+      "lib:rclcpp_lifecycle", rcl_lifecycle_state_machine_init, RCL_RET_ERROR);
+    EXPECT_THROW(
+      std::make_shared<EmptyLifecycleNode>("testnode").reset(),
+      std::runtime_error);
+  }
+  {
+    auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
+    auto patch = mocking_utils::inject_on_return(
+      "lib:rclcpp_lifecycle", rcl_lifecycle_state_machine_fini, RCL_RET_ERROR);
+    EXPECT_NO_THROW(test_node.reset());
+  }
+}
+
 TEST_F(TestDefaultStateMachine, trigger_transition) {
   auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
 
@@ -163,6 +187,35 @@ TEST_F(TestDefaultStateMachine, trigger_transition) {
       rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)).id());
 }
 
+TEST_F(TestDefaultStateMachine, trigger_transition_rcl_errors) {
+  auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp_lifecycle", rcl_lifecycle_state_machine_is_initialized, RCL_RET_ERROR);
+    EXPECT_EQ(
+      State::PRIMARY_STATE_UNCONFIGURED,
+      test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id());
+  }
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp_lifecycle", rcl_lifecycle_trigger_transition_by_id, RCL_RET_ERROR);
+    EXPECT_EQ(
+      State::PRIMARY_STATE_UNCONFIGURED,
+      test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id());
+  }
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp_lifecycle", rcl_lifecycle_trigger_transition_by_label, RCL_RET_ERROR);
+    EXPECT_EQ(
+      State::TRANSITION_STATE_CONFIGURING,
+      test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id());
+  }
+}
+
 TEST_F(TestDefaultStateMachine, trigger_transition_with_error_code) {
   auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
   auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
@@ -381,6 +434,8 @@ TEST_F(TestDefaultStateMachine, check_parameters) {
   auto descriptors = test_node->describe_parameters(parameter_names);
   EXPECT_EQ(descriptors.size(), parameter_names.size());
 
+  // This actually throws inside NodeParameters::describe_parameters(), so it's not currently
+  // possible to cover this method 100%.
   EXPECT_THROW(
     test_node->describe_parameter("not_a_real_parameter"),
     rclcpp::exceptions::ParameterNotDeclaredException);
@@ -437,6 +492,12 @@ TEST_F(TestDefaultStateMachine, check_parameters) {
   test_node->set_parameters({int_parameter});
   EXPECT_EQ(parameters_set, 2u);
 
+  test_node->remove_on_set_parameters_callback(callback_handle.get());
+  rclcpp::Parameter bool_parameter2(bool_name, rclcpp::ParameterValue(true));
+  EXPECT_TRUE(test_node->set_parameter(bool_parameter2).successful);
+  EXPECT_EQ(parameters_set, 2u);
+
+
   // List parameters
   list_result = test_node->list_parameters({}, 0u);
   EXPECT_EQ(list_result.names.size(), 3u);
@@ -489,46 +550,112 @@ TEST_F(TestDefaultStateMachine, test_getters) {
   EXPECT_NE(nullptr, const_cast<const EmptyLifecycleNode *>(test_node.get())->get_clock());
 }
 
-TEST_F(TestDefaultStateMachine, test_graph) {
+TEST_F(TestDefaultStateMachine, test_graph_topics) {
   auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
   auto names = test_node->get_node_names();
-  EXPECT_EQ(names.size(), 1u);
-  EXPECT_STREQ(names[0].c_str(), "/testnode");
 
-  // parameter_events, rosout, /testnode/transition_event
+  ASSERT_NE(names.end(), std::find(names.begin(), names.end(), std::string("/testnode")));
+
+  // Other topics may exist for an rclcpp::Node, but just checking the lifecycle one exists
   auto topic_names_and_types = test_node->get_topic_names_and_types();
-  EXPECT_EQ(topic_names_and_types.size(), 3u);
+  ASSERT_NE(
+    topic_names_and_types.end(),
+    topic_names_and_types.find(std::string("/testnode/transition_event")));
   EXPECT_STREQ(
     topic_names_and_types["/testnode/transition_event"][0].c_str(),
     "lifecycle_msgs/msg/TransitionEvent");
 
-  auto service_names_and_types = test_node->get_service_names_and_types();
-  EXPECT_EQ(service_names_and_types.size(), 11u);
-  // These are specific to lifecycle nodes, other services are provided by rclcpp::Node
-  EXPECT_STREQ(
-    service_names_and_types["/testnode/change_state"][0].c_str(),
-    "lifecycle_msgs/srv/ChangeState");
-  EXPECT_STREQ(
-    service_names_and_types["/testnode/get_available_states"][0].c_str(),
-    "lifecycle_msgs/srv/GetAvailableStates");
-  EXPECT_STREQ(
-    service_names_and_types["/testnode/get_available_transitions"][0].c_str(),
-    "lifecycle_msgs/srv/GetAvailableTransitions");
-  EXPECT_STREQ(
-    service_names_and_types["/testnode/get_state"][0].c_str(),
-    "lifecycle_msgs/srv/GetState");
-  EXPECT_STREQ(
-    service_names_and_types["/testnode/get_transition_graph"][0].c_str(),
-    "lifecycle_msgs/srv/GetAvailableTransitions");
-
   EXPECT_EQ(1u, test_node->count_publishers("/testnode/transition_event"));
   EXPECT_EQ(0u, test_node->count_subscribers("/testnode/transition_event"));
 
   auto publishers_info = test_node->get_publishers_info_by_topic("/testnode/transition_event");
-  EXPECT_EQ(publishers_info.size(), 1u);
+  EXPECT_EQ(1u, publishers_info.size());
   auto subscriptions_info =
     test_node->get_subscriptions_info_by_topic("/testnode/transition_event");
-  EXPECT_EQ(subscriptions_info.size(), 0u);
+  EXPECT_EQ(0u, subscriptions_info.size());
+}
+
+TEST_F(TestDefaultStateMachine, test_graph_services) {
+  auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
+
+  auto service_names_and_types = test_node->get_service_names_and_types();
+  // These are specific to lifecycle nodes, other services are provided by rclcpp::Node
+  ASSERT_NE(
+    service_names_and_types.end(),
+    service_names_and_types.find(std::string("/testnode/change_state")));
+  EXPECT_STREQ(
+    service_names_and_types["/testnode/change_state"][0].c_str(),
+    "lifecycle_msgs/srv/ChangeState");
+
+  ASSERT_NE(
+    service_names_and_types.end(),
+    service_names_and_types.find(std::string("/testnode/get_available_states")));
+  EXPECT_STREQ(
+    service_names_and_types["/testnode/get_available_states"][0].c_str(),
+    "lifecycle_msgs/srv/GetAvailableStates");
+
+  ASSERT_NE(
+    service_names_and_types.end(),
+    service_names_and_types.find(std::string("/testnode/get_available_transitions")));
+  EXPECT_STREQ(
+    service_names_and_types["/testnode/get_available_transitions"][0].c_str(),
+    "lifecycle_msgs/srv/GetAvailableTransitions");
+
+  ASSERT_NE(
+    service_names_and_types.end(),
+    service_names_and_types.find(std::string("/testnode/get_state")));
+  EXPECT_STREQ(
+    service_names_and_types["/testnode/get_state"][0].c_str(),
+    "lifecycle_msgs/srv/GetState");
+
+  ASSERT_NE(
+    service_names_and_types.end(),
+    service_names_and_types.find(std::string("/testnode/get_transition_graph")));
+  EXPECT_STREQ(
+    service_names_and_types["/testnode/get_transition_graph"][0].c_str(),
+    "lifecycle_msgs/srv/GetAvailableTransitions");
+}
+
+TEST_F(TestDefaultStateMachine, test_graph_services_by_node) {
+  auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
+
+  auto service_names_and_types_by_node =
+    test_node->get_service_names_and_types_by_node("testnode", "");
+  // These are specific to lifecycle nodes, other services are provided by rclcpp::Node
+  ASSERT_NE(
+    service_names_and_types_by_node.end(),
+    service_names_and_types_by_node.find(std::string("/testnode/change_state")));
+  EXPECT_STREQ(
+    service_names_and_types_by_node["/testnode/change_state"][0].c_str(),
+    "lifecycle_msgs/srv/ChangeState");
+
+  ASSERT_NE(
+    service_names_and_types_by_node.end(),
+    service_names_and_types_by_node.find(std::string("/testnode/get_available_states")));
+  EXPECT_STREQ(
+    service_names_and_types_by_node["/testnode/get_available_states"][0].c_str(),
+    "lifecycle_msgs/srv/GetAvailableStates");
+
+  ASSERT_NE(
+    service_names_and_types_by_node.end(),
+    service_names_and_types_by_node.find(std::string("/testnode/get_available_transitions")));
+  EXPECT_STREQ(
+    service_names_and_types_by_node["/testnode/get_available_transitions"][0].c_str(),
+    "lifecycle_msgs/srv/GetAvailableTransitions");
+
+  ASSERT_NE(
+    service_names_and_types_by_node.end(),
+    service_names_and_types_by_node.find(std::string("/testnode/get_state")));
+  EXPECT_STREQ(
+    service_names_and_types_by_node["/testnode/get_state"][0].c_str(),
+    "lifecycle_msgs/srv/GetState");
+
+  ASSERT_NE(
+    service_names_and_types_by_node.end(),
+    service_names_and_types_by_node.find(std::string("/testnode/get_transition_graph")));
+  EXPECT_STREQ(
+    service_names_and_types_by_node["/testnode/get_transition_graph"][0].c_str(),
+    "lifecycle_msgs/srv/GetAvailableTransitions");
 }
 
 TEST_F(TestDefaultStateMachine, test_callback_groups) {
@@ -544,3 +671,16 @@ TEST_F(TestDefaultStateMachine, test_callback_groups) {
   EXPECT_EQ(groups.size(), 2u);
   EXPECT_EQ(groups[1].lock().get(), group.get());
 }
+
+TEST_F(TestDefaultStateMachine, wait_for_graph_change)
+{
+  auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
+  EXPECT_THROW(
+    test_node->wait_for_graph_change(nullptr, std::chrono::milliseconds(1)),
+    rclcpp::exceptions::InvalidEventError);
+
+  auto event = std::make_shared<rclcpp::Event>();
+  EXPECT_THROW(
+    test_node->wait_for_graph_change(event, std::chrono::milliseconds(0)),
+    rclcpp::exceptions::EventNotRegisteredError);
+}

rclcpp_lifecycle/test/test_lifecycle_publisher.cpp

@@ -0,0 +1,104 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <string>
+#include <utility>
+
+#include "test_msgs/msg/empty.hpp"
+
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
+
+class TestDefaultStateMachine : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+};
+
+class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode
+{
+public:
+  explicit EmptyLifecycleNode(const std::string & node_name)
+  : rclcpp_lifecycle::LifecycleNode(node_name)
+  {
+    rclcpp::PublisherOptionsWithAllocator<std::allocator<void>> options;
+    publisher_ =
+      std::make_shared<rclcpp_lifecycle::LifecyclePublisher<test_msgs::msg::Empty>>(
+      get_node_base_interface().get(), std::string("topic"), rclcpp::QoS(10), options);
+    add_publisher_handle(publisher_);
+
+    // For coverage this is being added here
+    auto timer = create_wall_timer(std::chrono::seconds(1), []() {});
+    add_timer_handle(timer);
+  }
+
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<test_msgs::msg::Empty>> publisher()
+  {
+    return publisher_;
+  }
+
+private:
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<test_msgs::msg::Empty>> publisher_;
+};
+
+class TestLifecyclePublisher : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+    node_ = std::make_shared<EmptyLifecycleNode>("node");
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+
+protected:
+  std::shared_ptr<EmptyLifecycleNode> node_;
+};
+
+TEST_F(TestLifecyclePublisher, publish) {
+  node_->publisher()->on_deactivate();
+  EXPECT_FALSE(node_->publisher()->is_activated());
+  {
+    auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
+    EXPECT_NO_THROW(node_->publisher()->publish(*msg_ptr));
+  }
+  {
+    auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
+    EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr)));
+  }
+  node_->publisher()->on_activate();
+  EXPECT_TRUE(node_->publisher()->is_activated());
+  {
+    auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
+    EXPECT_NO_THROW(node_->publisher()->publish(*msg_ptr));
+  }
+  {
+    auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
+    EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr)));
+  }
+}

rclcpp_lifecycle/test/test_lifecycle_service_client.cpp

@@ -33,9 +33,12 @@
 #include "lifecycle_msgs/srv/get_available_transitions.hpp"
 #include "lifecycle_msgs/srv/get_state.hpp"
 
+#include "rclcpp/node_interfaces/node_graph.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 
+#include "./mocking_utils/patch.hpp"
+
 using namespace std::chrono_literals;
 
 constexpr char const * lifecycle_node_name = "lc_talker";
@@ -355,3 +358,104 @@ TEST_F(TestLifecycleServiceClient, lifecycle_transitions) {
   transitions = lifecycle_client()->get_available_transitions();
   EXPECT_EQ(transitions.size(), 0u);
 }
+
+TEST_F(TestLifecycleServiceClient, get_service_names_and_types_by_node)
+{
+  auto node1 = std::make_shared<LifecycleServiceClient>("client1");
+  auto node2 = std::make_shared<LifecycleServiceClient>("client2");
+
+  auto node_graph = node1->get_node_graph_interface();
+  ASSERT_NE(nullptr, node_graph);
+
+  EXPECT_THROW(
+    node_graph->get_service_names_and_types_by_node("not_a_node", "not_absolute_namespace"),
+    std::runtime_error);
+  auto service_names_and_types1 = node_graph->get_service_names_and_types_by_node("client1", "/");
+  auto service_names_and_types2 = node_graph->get_service_names_and_types_by_node("client2", "/");
+  EXPECT_EQ(service_names_and_types1.size(), service_names_and_types2.size());
+}
+
+TEST_F(TestLifecycleServiceClient, declare_parameter_with_no_initial_values)
+{
+  auto node1 = std::make_shared<LifecycleServiceClient>("client1");
+
+  auto on_set_parameters =
+    [](const std::vector<rclcpp::Parameter> &) {
+      rcl_interfaces::msg::SetParametersResult result;
+      result.successful = true;
+      return result;
+    };
+
+  auto handler = node1->add_on_set_parameters_callback(on_set_parameters);
+  RCLCPP_SCOPE_EXIT({node1->remove_on_set_parameters_callback(handler.get());});    // always reset
+}
+
+TEST_F(TestLifecycleServiceClient, wait_for_graph_change)
+{
+  auto node = std::make_shared<LifecycleServiceClient>("client_wait_for_graph_change");
+  auto node_graph = node->get_node_graph_interface();
+  ASSERT_NE(nullptr, node_graph);
+
+  EXPECT_NO_THROW(node_graph->notify_graph_change());
+  EXPECT_THROW(
+    node_graph->wait_for_graph_change(nullptr, std::chrono::milliseconds(1)),
+    rclcpp::exceptions::InvalidEventError);
+
+  auto event = std::make_shared<rclcpp::Event>();
+  EXPECT_THROW(
+    node_graph->wait_for_graph_change(event, std::chrono::milliseconds(0)),
+    rclcpp::exceptions::EventNotRegisteredError);
+}
+
+class TestLifecycleServiceClientRCLErrors : public ::testing::Test
+{
+protected:
+  void SetUp() override
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void TearDown() override
+  {
+    rclcpp::shutdown();
+  }
+};
+
+TEST_F(TestLifecycleServiceClientRCLErrors, call_services_rcl_errors) {
+  auto lifecycle_node = std::make_shared<EmptyLifecycleNode>();
+  auto lifecycle_client = std::make_shared<LifecycleServiceClient>("client_with_errors");
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp_lifecycle", rcl_lifecycle_state_machine_is_initialized, RCL_RET_ERROR);
+
+  // on_change_state
+  lifecycle_client->change_state(
+    lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+  rclcpp::spin_some(lifecycle_client);
+  EXPECT_THROW(
+    rclcpp::spin_some(lifecycle_node->get_node_base_interface()), std::runtime_error);
+
+  // on_get_state
+  lifecycle_client->get_state();
+  rclcpp::spin_some(lifecycle_client);
+  EXPECT_THROW(
+    rclcpp::spin_some(lifecycle_node->get_node_base_interface()), std::runtime_error);
+
+  // on_get_avilable_states
+  lifecycle_client->get_available_states();
+  rclcpp::spin_some(lifecycle_client);
+  EXPECT_THROW(
+    rclcpp::spin_some(lifecycle_node->get_node_base_interface()), std::runtime_error);
+
+  // on_get_available_transitions
+  lifecycle_client->get_available_transitions();
+  rclcpp::spin_some(lifecycle_client);
+  EXPECT_THROW(
+    rclcpp::spin_some(lifecycle_node->get_node_base_interface()), std::runtime_error);
+
+  // on_get_transition_graph
+  lifecycle_client->get_transition_graph();
+  rclcpp::spin_some(lifecycle_client);
+  EXPECT_THROW(
+    rclcpp::spin_some(lifecycle_node->get_node_base_interface()), std::runtime_error);
+}

rclcpp_lifecycle/test/test_transition_wrapper.cpp

@@ -18,8 +18,12 @@
 #include <utility>
 #include <vector>
 
+#include "rcutils/testing/fault_injection.h"
+
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 
+#include "./mocking_utils/patch.hpp"
+
 class TestTransitionWrapper : public ::testing::Test
 {
 protected:
@@ -31,8 +35,11 @@ protected:
 class TransitionDerived : public rclcpp_lifecycle::Transition
 {
 public:
+  TransitionDerived(const uint8_t id, const std::string & label)
+  : Transition(id, label) {}
+
   TransitionDerived(
-    uint8_t id, const std::string & label,
+    const uint8_t id, const std::string & label,
     rclcpp_lifecycle::State && start, rclcpp_lifecycle::State && goal)
   : Transition(id, label, std::move(start), std::move(goal)) {}
   void expose_reset()
@@ -125,4 +132,80 @@ TEST_F(TestTransitionWrapper, exceptions) {
   EXPECT_THROW(a->goal_state(), std::runtime_error);
   EXPECT_THROW(a->id(), std::runtime_error);
   EXPECT_THROW(a->label(), std::runtime_error);
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp_lifecycle", rcl_lifecycle_transition_init, RCL_RET_ERROR);
+
+    EXPECT_THROW(
+      std::make_shared<TransitionDerived>(1, "one").reset(),
+      std::runtime_error);
+
+    rclcpp_lifecycle::State state1(1, "start_state");
+    rclcpp_lifecycle::State state2(2, "goal_state");
+    EXPECT_THROW(
+      std::make_shared<TransitionDerived>(
+        2, "two", std::move(start_state), std::move(goal_state)).reset(),
+      std::runtime_error);
+  }
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp_lifecycle", rcl_lifecycle_transition_fini, RCL_RET_ERROR);
+    auto transition1 = std::make_shared<TransitionDerived>(1, "one");
+    EXPECT_NO_THROW(transition1->expose_reset());
+
+    rclcpp_lifecycle::State state1(1, "start_state");
+    rclcpp_lifecycle::State state2(2, "goal_state");
+    auto transition2 =
+      std::make_shared<TransitionDerived>(2, "two", std::move(start_state), std::move(goal_state));
+    EXPECT_NO_THROW(transition2->expose_reset());
+  }
+
+  RCUTILS_FAULT_INJECTION_TEST(
+  {
+    std::shared_ptr<TransitionDerived> transition = nullptr;
+    try {
+      transition = std::make_shared<TransitionDerived>(1, "one");
+    } catch (...) {
+    }
+    if (nullptr != transition) {
+      EXPECT_NO_THROW(transition->expose_reset());
+    }
+  });
+
+  RCUTILS_FAULT_INJECTION_TEST(
+  {
+    std::shared_ptr<TransitionDerived> transition = nullptr;
+    try {
+      {
+        // These will fail due to failed allocations
+        rclcpp_lifecycle::State state1(1, "start_state");
+        rclcpp_lifecycle::State state2(2, "goal_state");
+
+        // Failed allocations and failed rcl init functions
+        transition = std::make_shared<TransitionDerived>(
+          2, "two", std::move(state1), std::move(state2));
+      }
+    } catch (...) {
+    }
+
+    if (nullptr != transition) {
+      EXPECT_NO_THROW(transition->expose_reset());
+    }
+  });
+
+  RCUTILS_FAULT_INJECTION_TEST(
+  {
+    try {
+      // These will fail due to failed allocations
+      rclcpp_lifecycle::State state1(1, "start_state");
+      rclcpp_lifecycle::State state2(2, "goal_state");
+
+      // Failed allocations and failed rcl init functions
+      auto a = std::make_shared<TransitionDerived>(2, "two", std::move(state1), std::move(state2));
+      auto b = std::make_shared<TransitionDerived>(3, "three");
+      *b = *a;
+    } catch (...) {
+    }
+  });
 }