5.1.0

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>

rclcpp/CHANGELOG.rst

@@ -2,6 +2,136 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+5.1.0 (2020-11-02)
+------------------
+* Deprecate Duration(rcl_duration_value_t) in favor of static Duration::from_nanoseconds(rcl_duration_value_t) (`#1432 <https://github.com/ros2/rclcpp/issues/1432>`_)
+* Avoid parsing arguments twice in `rclcpp::init_and_remove_ros_arguments` (`#1415 <https://github.com/ros2/rclcpp/issues/1415>`_)
+* Add service and client benchmarks (`#1425 <https://github.com/ros2/rclcpp/issues/1425>`_)
+* Set CMakeLists to only use default rmw for benchmarks (`#1427 <https://github.com/ros2/rclcpp/issues/1427>`_)
+* Update tracetools' QL in rclcpp's QD (`#1428 <https://github.com/ros2/rclcpp/issues/1428>`_)
+* Add missing locking to the rclcpp_action::ServerBase. (`#1421 <https://github.com/ros2/rclcpp/issues/1421>`_)
+* Initial benchmark tests for rclcpp::init/shutdown create/destroy node (`#1411 <https://github.com/ros2/rclcpp/issues/1411>`_)
+* Refactor test CMakeLists in prep for benchmarks (`#1422 <https://github.com/ros2/rclcpp/issues/1422>`_)
+* Add methods in topic and service interface to resolve a name (`#1410 <https://github.com/ros2/rclcpp/issues/1410>`_)
+* Update deprecated gtest macros (`#1370 <https://github.com/ros2/rclcpp/issues/1370>`_)
+* Clear members for StaticExecutorEntitiesCollector to avoid shared_ptr dependency (`#1303 <https://github.com/ros2/rclcpp/issues/1303>`_)
+* Increase test timeouts of slow running tests with rmw_connext_cpp (`#1400 <https://github.com/ros2/rclcpp/issues/1400>`_)
+* Avoid self dependency that not destoryed (`#1301 <https://github.com/ros2/rclcpp/issues/1301>`_)
+* Update maintainers (`#1384 <https://github.com/ros2/rclcpp/issues/1384>`_)
+* Add clock qos to node options (`#1375 <https://github.com/ros2/rclcpp/issues/1375>`_)
+* Fix NodeOptions copy constructor (`#1376 <https://github.com/ros2/rclcpp/issues/1376>`_)
+* Make sure to clean the external client/service handle. (`#1296 <https://github.com/ros2/rclcpp/issues/1296>`_)
+* Increase coverage of WaitSetTemplate (`#1368 <https://github.com/ros2/rclcpp/issues/1368>`_)
+* Increase coverage of guard_condition.cpp to 100% (`#1369 <https://github.com/ros2/rclcpp/issues/1369>`_)
+* Add coverage statement (`#1367 <https://github.com/ros2/rclcpp/issues/1367>`_)
+* Tests for LoanedMessage with mocked loaned message publisher (`#1366 <https://github.com/ros2/rclcpp/issues/1366>`_)
+* Add unit tests for qos and qos_event files (`#1352 <https://github.com/ros2/rclcpp/issues/1352>`_)
+* Finish coverage of publisher API (`#1365 <https://github.com/ros2/rclcpp/issues/1365>`_)
+* Finish API coverage on executors. (`#1364 <https://github.com/ros2/rclcpp/issues/1364>`_)
+* Add test for ParameterService (`#1355 <https://github.com/ros2/rclcpp/issues/1355>`_)
+* Add time API coverage tests (`#1347 <https://github.com/ros2/rclcpp/issues/1347>`_)
+* Add timer coverage tests (`#1363 <https://github.com/ros2/rclcpp/issues/1363>`_)
+* Add in additional tests for parameter_client.cpp coverage.
+* Minor fixes to the parameter_service.cpp file.
+* reset rcl_context shared_ptr after calling rcl_init sucessfully (`#1357 <https://github.com/ros2/rclcpp/issues/1357>`_)
+* Improved test publisher - zero qos history depth value exception (`#1360 <https://github.com/ros2/rclcpp/issues/1360>`_)
+* Covered resolve_use_intra_process (`#1359 <https://github.com/ros2/rclcpp/issues/1359>`_)
+* Improve test_subscription_options (`#1358 <https://github.com/ros2/rclcpp/issues/1358>`_)
+* Add in more tests for init_options coverage. (`#1353 <https://github.com/ros2/rclcpp/issues/1353>`_)
+* Test the remaining node public API (`#1342 <https://github.com/ros2/rclcpp/issues/1342>`_)
+* Complete coverage of Parameter and ParameterValue API (`#1344 <https://github.com/ros2/rclcpp/issues/1344>`_)
+* Add in more tests for the utilities. (`#1349 <https://github.com/ros2/rclcpp/issues/1349>`_)
+* Add in two more tests for expand_topic_or_service_name. (`#1350 <https://github.com/ros2/rclcpp/issues/1350>`_)
+* Add tests for node_options API (`#1343 <https://github.com/ros2/rclcpp/issues/1343>`_)
+* Add in more coverage for expand_topic_or_service_name. (`#1346 <https://github.com/ros2/rclcpp/issues/1346>`_)
+* Test exception in spin_until_future_complete. (`#1345 <https://github.com/ros2/rclcpp/issues/1345>`_)
+* Add coverage tests graph_listener (`#1330 <https://github.com/ros2/rclcpp/issues/1330>`_)
+* Add in unit tests for the Executor class.
+* Allow mimick patching of methods with up to 9 arguments.
+* Improve the error messages in the Executor class.
+* Add coverage for client API (`#1329 <https://github.com/ros2/rclcpp/issues/1329>`_)
+* Increase service coverage (`#1332 <https://github.com/ros2/rclcpp/issues/1332>`_)
+* Make more of the static entity collector API private.
+* Const-ify more of the static executor.
+* Add more tests for the static single threaded executor.
+* Many more tests for the static_executor_entities_collector.
+* Get one more line of code coverage in memory_strategy.cpp
+* Bugfix when adding callback group.
+* Fix typos in comments.
+* Remove deprecated executor::FutureReturnCode APIs. (`#1327 <https://github.com/ros2/rclcpp/issues/1327>`_)
+* Increase coverage of publisher/subscription API (`#1325 <https://github.com/ros2/rclcpp/issues/1325>`_)
+* Not finalize guard condition while destructing SubscriptionIntraProcess (`#1307 <https://github.com/ros2/rclcpp/issues/1307>`_)
+* Expose qos setting for /rosout (`#1247 <https://github.com/ros2/rclcpp/issues/1247>`_)
+* Add coverage for missing API (except executors) (`#1326 <https://github.com/ros2/rclcpp/issues/1326>`_)
+* Include topic name in QoS mismatch warning messages (`#1286 <https://github.com/ros2/rclcpp/issues/1286>`_)
+* Add coverage tests context functions (`#1321 <https://github.com/ros2/rclcpp/issues/1321>`_)
+* Increase coverage of node_interfaces, including with mocking rcl errors (`#1322 <https://github.com/ros2/rclcpp/issues/1322>`_)
+* Contributors: Ada-King, Alejandro Hernández Cordero, Audrow Nash, Barry Xu, Chen Lihui, Chris Lalancette, Christophe Bedard, Ivan Santiago Paunovic, Jorge Perez, Morgan Quigley, brawner
+
+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/QUALITY_DECLARATION.md

@@ -123,6 +123,8 @@ Changes are required to make a best effort to keep or increase coverage before b
 
 Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastSuccessfulBuild/cobertura/src_ros2_rclcpp_rclcpp_src_rclcpp/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://index.ros.org/doc/ros2/Contributing/ROS-2-On-boarding-Guide/#note-on-coverage-runs).
 
+`rclcpp` has a line coverage `>= 95%`, which is calculated over all directories within `rclcpp` with the exception of the `experimental` directory.
+
 ### Performance [4.iv]
 
 It is not yet defined if this package requires performance testing and how addresses this topic.
@@ -195,7 +197,7 @@ It is **Quality Level 3**, see its [Quality Declaration document](https://github
 
 The `tracetools` package provides utilities for instrumenting the code in `rclcpp` so that it may be traced for debugging and performance analysis.
 
-It is **Quality Level 2**, see its [Quality Declaration document](https://gitlab.com/micro-ROS/ros_tracing/ros2_tracing/-/blob/master/tracetools/QUALITY_DECLARATION.md).
+It is **Quality Level 1**, see its [Quality Declaration document](https://gitlab.com/ros-tracing/ros2_tracing/-/blob/master/tracetools/QUALITY_DECLARATION.md).
 
 ### Direct Runtime non-ROS Dependency [5.iii]
 

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
+   *
+   * Additionally, 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/create_subscription.hpp

@@ -117,8 +117,14 @@ create_subscription(
       rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>
       >(node_topics->get_node_base_interface()->get_name(), publisher);
 
-    auto sub_call_back = [subscription_topic_stats]() {
-        subscription_topic_stats->publish_message();
+    std::weak_ptr<
+      rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>
+    > weak_subscription_topic_stats(subscription_topic_stats);
+    auto sub_call_back = [weak_subscription_topic_stats]() {
+        auto subscription_topic_stats = weak_subscription_topic_stats.lock();
+        if (subscription_topic_stats) {
+          subscription_topic_stats->publish_message();
+        }
       };
 
     auto node_timer_interface = node_topics->get_node_timers_interface();

rclcpp/include/rclcpp/duration.hpp

@@ -38,10 +38,14 @@ public:
    */
   Duration(int32_t seconds, uint32_t nanoseconds);
 
-  // This constructor matches any numeric value - ints or floats.
+  /// Construct duration from the specified nanoseconds.
+  [[deprecated(
+    "Use Duration::from_nanoseconds instead or std::chrono_literals. For example:"
+    "rclcpp::Duration::from_nanoseconds(int64_variable);"
+    "rclcpp::Duration(0ns);")]]
   explicit Duration(rcl_duration_value_t nanoseconds);
 
-  // This constructor matches std::chrono::nanoseconds.
+  /// Construct duration from the specified std::chrono::nanoseconds.
   explicit Duration(std::chrono::nanoseconds nanoseconds);
 
   // This constructor matches any std::chrono value other than nanoseconds
@@ -77,6 +81,9 @@ public:
   bool
   operator==(const rclcpp::Duration & rhs) const;
 
+  bool
+  operator!=(const rclcpp::Duration & rhs) const;
+
   bool
   operator<(const rclcpp::Duration & rhs) const;
 
@@ -126,6 +133,10 @@ public:
   static Duration
   from_seconds(double seconds);
 
+  /// Create a duration object from an integer number representing nanoseconds
+  static Duration
+  from_nanoseconds(rcl_duration_value_t nanoseconds);
+
   /// Convert Duration into a std::chrono::Duration.
   template<class DurationT>
   DurationT
@@ -140,6 +151,8 @@ public:
 
 private:
   rcl_duration_t rcl_duration_;
+
+  Duration() = default;
 };
 
 }  // namespace rclcpp

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
@@ -58,34 +64,23 @@ public:
   void
   init(
     rcl_wait_set_t * p_wait_set,
-    rclcpp::memory_strategy::MemoryStrategy::SharedPtr & memory_strategy,
+    rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy,
     rcl_guard_condition_t * executor_guard_condition);
 
+  /// Finalize StaticExecutorEntitiesCollector to clear resources
+  RCLCPP_PUBLIC
+  void
+  fini();
+
   RCLCPP_PUBLIC
   void
   execute() override;
 
-  RCLCPP_PUBLIC
-  void
-  fill_memory_strategy();
-
-  RCLCPP_PUBLIC
-  void
-  fill_executable_list();
-
-  /// Function to reallocate space for entities in the wait set.
-  /**
-   * \throws std::runtime_error if wait set couldn't be cleared or resized.
-   */
-  RCLCPP_PUBLIC
-  void
-  prepare_wait_set();
-
   /// Function to add_handles_to_wait_set and wait for work and
   /**
    * 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 +97,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 +156,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
@@ -216,12 +272,55 @@ public:
   get_waitable(size_t i) {return exec_list_.waitable[i];}
 
 private:
-  /// Nodes guard conditions which trigger this waitable
-  std::list<const rcl_guard_condition_t *> guard_conditions_;
+  /// Function to reallocate space for entities in the wait set.
+  /**
+   * \throws std::runtime_error if wait set couldn't be cleared or resized.
+   */
+  void
+  prepare_wait_set();
+
+  void
+  fill_executable_list();
+
+  void
+  fill_memory_strategy();
+
+  /// 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,
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const;
+
+  /// 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()
+   */
+  void
+  add_callback_groups_from_nodes_associated_to_executor();
+
+  void
+  fill_executable_list_from_map(const 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,8 +192,8 @@ public:
     }
     std::chrono::nanoseconds timeout_left = timeout_ns;
 
-    entities_collector_ = std::make_shared<StaticExecutorEntitiesCollector>();
     entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_);
+    RCLCPP_SCOPE_EXIT(entities_collector_->fini());
 
     while (rclcpp::ok(this->context_)) {
       // Do one set of work.
@@ -191,6 +221,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/intra_process_manager.hpp

@@ -207,7 +207,7 @@ public:
       sub_ids.take_shared_subscriptions.size() <= 1)
     {
       // There is at maximum 1 buffer that does not require ownership.
-      // So we this case is equivalent to all the buffers requiring ownership
+      // So this case is equivalent to all the buffers requiring ownership
 
       // Merge the two vector of ids into a unique one
       std::vector<uint64_t> concatenated_vector(sub_ids.take_shared_subscriptions);

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.
@@ -102,6 +102,16 @@ public:
 #endif
   }
 
+  ~SubscriptionIntraProcess()
+  {
+    if (rcl_guard_condition_fini(&gc_) != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "Failed to destroy guard condition: %s",
+        rcutils_get_error_string().str);
+    }
+  }
+
   bool
   is_ready(rcl_wait_set_t * wait_set)
   {

rclcpp/include/rclcpp/future_return_code.hpp

@@ -42,20 +42,6 @@ RCLCPP_PUBLIC
 std::string
 to_string(const FutureReturnCode & future_return_code);
 
-namespace executor
-{
-
-using FutureReturnCode [[deprecated("use rclcpp::FutureReturnCode instead")]] = FutureReturnCode;
-
-[[deprecated("use rclcpp::to_string(const rclcpp::FutureReturnCode &) instead")]]
-inline
-std::string
-to_string(const rclcpp::FutureReturnCode & future_return_code)
-{
-  return rclcpp::to_string(future_return_code);
-}
-
-}  // namespace executor
 }  // namespace rclcpp
 
 #endif  // RCLCPP__FUTURE_RETURN_CODE_HPP_

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
@@ -116,6 +118,10 @@ public:
   bool
   get_enable_topic_statistics_default() const override;
 
+  std::string
+  resolve_topic_or_service_name(
+    const std::string & name, bool is_service, bool only_expand = false) const override;
+
 private:
   RCLCPP_DISABLE_COPY(NodeBase)
 

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
@@ -161,6 +163,13 @@ public:
   virtual
   bool
   get_enable_topic_statistics_default() const = 0;
+
+  /// Expand and remap a given topic or service name.
+  RCLCPP_PUBLIC
+  virtual
+  std::string
+  resolve_topic_or_service_name(
+    const std::string & name, bool is_service, bool only_expand = false) const = 0;
 };
 
 }  // namespace node_interfaces

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/node_interfaces/node_services.hpp

@@ -15,6 +15,8 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_SERVICES_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_SERVICES_HPP_
 
+#include <string>
+
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/client.hpp"
 #include "rclcpp/macros.hpp"
@@ -53,6 +55,10 @@ public:
     rclcpp::ServiceBase::SharedPtr service_base_ptr,
     rclcpp::CallbackGroup::SharedPtr group) override;
 
+  RCLCPP_PUBLIC
+  std::string
+  resolve_service_name(const std::string & name, bool only_expand = false) const override;
+
 private:
   RCLCPP_DISABLE_COPY(NodeServices)
 

rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp

@@ -15,6 +15,8 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_
 
+#include <string>
+
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/client.hpp"
 #include "rclcpp/macros.hpp"
@@ -49,6 +51,12 @@ public:
   add_service(
     rclcpp::ServiceBase::SharedPtr service_base_ptr,
     rclcpp::CallbackGroup::SharedPtr group) = 0;
+
+  /// Get the remapped and expanded service name given a input name.
+  RCLCPP_PUBLIC
+  virtual
+  std::string
+  resolve_service_name(const std::string & name, bool only_expand = false) const = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp

@@ -24,6 +24,7 @@
 #include "rclcpp/node_interfaces/node_services_interface.hpp"
 #include "rclcpp/node_interfaces/node_time_source_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/time_source.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -46,7 +47,8 @@ public:
     rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
     rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
     rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
-    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters
+    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters,
+    const rclcpp::QoS & qos = rclcpp::RosoutQoS()
   );
 
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/node_interfaces/node_topics.hpp

@@ -81,6 +81,10 @@ public:
   rclcpp::node_interfaces::NodeTimersInterface *
   get_node_timers_interface() const override;
 
+  RCLCPP_PUBLIC
+  std::string
+  resolve_topic_name(const std::string & name, bool only_expand = false) const override;
+
 private:
   RCLCPP_DISABLE_COPY(NodeTopics)
 

rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp

@@ -86,6 +86,12 @@ public:
   virtual
   rclcpp::node_interfaces::NodeTimersInterface *
   get_node_timers_interface() const = 0;
+
+  /// Get a remapped and expanded topic name given an input name.
+  RCLCPP_PUBLIC
+  virtual
+  std::string
+  resolve_topic_name(const std::string & name, bool only_expand = false) const = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_options.hpp

@@ -46,6 +46,8 @@ public:
    *   - enable_topic_statistics = false
    *   - start_parameter_services = true
    *   - start_parameter_event_publisher = true
+   *   - clock_qos = rclcpp::ClockQoS()
+   *   - rosout_qos = rclcpp::RosoutQoS()
    *   - parameter_event_qos = rclcpp::ParameterEventQoS
    *     - with history setting and depth from rmw_qos_profile_parameter_events
    *   - parameter_event_publisher_options = rclcpp::PublisherOptionsBase
@@ -243,6 +245,19 @@ public:
   NodeOptions &
   start_parameter_event_publisher(bool start_parameter_event_publisher);
 
+  /// Return a reference to the clock QoS.
+  RCLCPP_PUBLIC
+  const rclcpp::QoS &
+  clock_qos() const;
+
+  /// Set the clock QoS.
+  /**
+   * The QoS settings to be used for the publisher on /clock topic, if enabled.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  clock_qos(const rclcpp::QoS & clock_qos);
+
   /// Return a reference to the parameter_event_qos QoS.
   RCLCPP_PUBLIC
   const rclcpp::QoS &
@@ -256,6 +271,19 @@ public:
   NodeOptions &
   parameter_event_qos(const rclcpp::QoS & parameter_event_qos);
 
+  /// Return a reference to the rosout QoS.
+  RCLCPP_PUBLIC
+  const rclcpp::QoS &
+  rosout_qos() const;
+
+  /// Set the rosout QoS.
+  /**
+   * The QoS settings to be used for the publisher on /rosout topic, if enabled.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  rosout_qos(const rclcpp::QoS & rosout_qos);
+
   /// Return a reference to the parameter_event_publisher_options.
   RCLCPP_PUBLIC
   const rclcpp::PublisherOptionsBase &
@@ -354,10 +382,14 @@ private:
 
   bool start_parameter_event_publisher_ {true};
 
+  rclcpp::QoS clock_qos_ = rclcpp::ClockQoS();
+
   rclcpp::QoS parameter_event_qos_ = rclcpp::ParameterEventsQoS(
     rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)
   );
 
+  rclcpp::QoS rosout_qos_ = rclcpp::RosoutQoS();
+
   rclcpp::PublisherOptionsBase parameter_event_publisher_options_ = rclcpp::PublisherOptionsBase();
 
   bool allow_undeclared_parameters_ {false};

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

@@ -19,6 +19,7 @@
 
 #include "rclcpp/duration.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rcl/logging_rosout.h"
 #include "rmw/incompatible_qos_events_statuses.h"
 #include "rmw/qos_profiles.h"
 #include "rmw/types.h"
@@ -26,6 +27,7 @@
 namespace rclcpp
 {
 
+RCLCPP_PUBLIC
 std::string qos_policy_name_from_kind(rmw_qos_policy_kind_t policy_kind);
 
 /// QoS initialization values, cannot be created directly, use KeepAll or KeepLast instead.
@@ -161,6 +163,26 @@ bool operator==(const QoS & left, const QoS & right);
 RCLCPP_PUBLIC
 bool operator!=(const QoS & left, const QoS & right);
 
+/**
+ * Clock QoS class
+ *    - History: Keep last,
+ *    - Depth: 1,
+ *    - Reliability: Best effort,
+ *    - Durability: Volatile,
+ *    - Deadline: Default,
+ *    - Lifespan: Default,
+ *    - Liveliness: System default,
+ *    - Liveliness lease duration: default,
+ *    - avoid ros namespace conventions: false
+ */
+class RCLCPP_PUBLIC ClockQoS : public QoS
+{
+public:
+  explicit
+  ClockQoS(
+    const QoSInitialization & qos_initialization = KeepLast(1));
+};
+
 /**
  * Sensor Data QoS class
  *    - History: Keep last,
@@ -249,6 +271,28 @@ public:
   ));
 };
 
+/**
+ * Rosout QoS class
+ *    - History: Keep last,
+ *    - Depth: 1000,
+ *    - Reliability: Reliable,
+ *    - Durability: TRANSIENT_LOCAL,
+ *    - Deadline: Default,
+ *    - Lifespan: {10, 0},
+ *    - Liveliness: System default,
+ *    - Liveliness lease duration: default,
+ *    - Avoid ros namespace conventions: false
+ */
+class RCLCPP_PUBLIC RosoutQoS : public QoS
+{
+public:
+  explicit
+  RosoutQoS(
+    const QoSInitialization & rosout_qos_initialization = (
+      QoSInitialization::from_rmw(rcl_qos_profile_rosout_default)
+  ));
+};
+
 /**
  * System defaults QoS class
  *    - History: System default,

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

@@ -218,6 +218,7 @@ private:
 /**
  * \throws std::overflow_error if addition leads to overflow
  */
+RCLCPP_PUBLIC
 Time
 operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs);
 

rclcpp/include/rclcpp/time_source.hpp

@@ -40,16 +40,19 @@ public:
    * The node will be attached to the time source.
    *
    * \param node std::shared pointer to a initialized node
+   * \param qos QoS that will be used when creating a `/clock` subscription.
    */
   RCLCPP_PUBLIC
-  explicit TimeSource(rclcpp::Node::SharedPtr node);
+  explicit TimeSource(rclcpp::Node::SharedPtr node, const rclcpp::QoS & qos = rclcpp::ClockQoS());
 
   /// Empty constructor
   /**
    * An Empty TimeSource class
+   *
+   * \param qos QoS that will be used when creating a `/clock` subscription.
    */
   RCLCPP_PUBLIC
-  TimeSource();
+  explicit TimeSource(const rclcpp::QoS & qos = rclcpp::ClockQoS());
 
   /// Attack node to the time source.
   /**
@@ -114,11 +117,14 @@ private:
   // Store (and update on node attach) logger for logging.
   Logger logger_;
 
+  // QoS of the clock subscription.
+  rclcpp::QoS qos_;
+
   // The subscription for the clock callback
   using MessageT = rosgraph_msgs::msg::Clock;
   using Alloc = std::allocator<void>;
   using SubscriptionT = rclcpp::Subscription<MessageT, Alloc>;
-  std::shared_ptr<SubscriptionT> clock_subscription_;
+  std::shared_ptr<SubscriptionT> clock_subscription_{nullptr};
   std::mutex clock_sub_lock_;
 
   // The clock callback itself
@@ -148,8 +154,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,
@@ -157,7 +161,7 @@ private:
 
   // Local storage of validity of ROS time
   // This is needed when new clocks are added.
-  bool ros_time_active_;
+  bool ros_time_active_{false};
   // Last set message to be passed to newly registered clocks
   rosgraph_msgs::msg::Clock::SharedPtr last_msg_set_;
 
@@ -166,7 +170,7 @@ private:
   // A vector to store references to associated clocks.
   std::vector<rclcpp::Clock::SharedPtr> associated_clocks_;
   // A handler for the use_sim_time parameter callback.
-  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr sim_time_cb_handler_ = nullptr;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr sim_time_cb_handler_{nullptr};
 };
 
 }  // namespace rclcpp

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,10 +2,13 @@
 <?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.1.0</version>
   <description>The ROS client library in C++.</description>
-  <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
+  <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
+  <maintainer email="mabel@openrobotics.org">Mabel Zhang</maintainer>
+  <maintainer email="william@openrobotics.org">William Woodall</maintainer>
   <license>Apache License 2.0</license>
+  <author email="dthomas@openrobotics.org">Dirk Thomas</author>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
@@ -35,6 +38,8 @@
   <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>performance_test_fixture</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

@@ -199,13 +199,17 @@ Context::init(
     throw rclcpp::ContextAlreadyInitialized();
   }
   this->clean_up();
-  rcl_context_.reset(new rcl_context_t, __delete_context);
-  *rcl_context_.get() = rcl_get_zero_initialized_context();
-  rcl_ret_t ret = rcl_init(argc, argv, init_options.get_rcl_init_options(), rcl_context_.get());
+  rcl_context_t * context = new rcl_context_t;
+  if (!context) {
+    throw std::runtime_error("failed to allocate memory for rcl context");
+  }
+  *context = rcl_get_zero_initialized_context();
+  rcl_ret_t ret = rcl_init(argc, argv, init_options.get_rcl_init_options(), context);
   if (RCL_RET_OK != ret) {
-    rcl_context_.reset();
+    delete context;
     rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl");
   }
+  rcl_context_.reset(context, __delete_context);
 
   if (init_options.auto_initialize_logging()) {
     logging_mutex_ = get_global_logging_mutex();
@@ -275,6 +279,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

@@ -37,7 +37,7 @@ Duration::Duration(int32_t seconds, uint32_t nanoseconds)
   rcl_duration_.nanoseconds += nanoseconds;
 }
 
-Duration::Duration(int64_t nanoseconds)
+Duration::Duration(rcl_duration_value_t nanoseconds)
 {
   rcl_duration_.nanoseconds = nanoseconds;
 }
@@ -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
 {
@@ -142,7 +148,7 @@ Duration::operator+(const rclcpp::Duration & rhs) const
     this->rcl_duration_.nanoseconds,
     rhs.rcl_duration_.nanoseconds,
     std::numeric_limits<rcl_duration_value_t>::max());
-  return Duration(
+  return Duration::from_nanoseconds(
     rcl_duration_.nanoseconds + rhs.rcl_duration_.nanoseconds);
 }
 
@@ -171,7 +177,7 @@ Duration::operator-(const rclcpp::Duration & rhs) const
     rhs.rcl_duration_.nanoseconds,
     std::numeric_limits<rcl_duration_value_t>::max());
 
-  return Duration(
+  return Duration::from_nanoseconds(
     rcl_duration_.nanoseconds - rhs.rcl_duration_.nanoseconds);
 }
 
@@ -202,7 +208,7 @@ Duration::operator*(double scale) const
     scale,
     std::numeric_limits<rcl_duration_value_t>::max());
   long double scale_ld = static_cast<long double>(scale);
-  return Duration(
+  return Duration::from_nanoseconds(
     static_cast<rcl_duration_value_t>(
       static_cast<long double>(rcl_duration_.nanoseconds) * scale_ld));
 }
@@ -243,7 +249,17 @@ Duration::to_rmw_time() const
 Duration
 Duration::from_seconds(double seconds)
 {
-  return Duration(static_cast<int64_t>(RCL_S_TO_NS(seconds)));
+  Duration ret;
+  ret.rcl_duration_.nanoseconds = static_cast<int64_t>(RCL_S_TO_NS(seconds));
+  return ret;
+}
+
+Duration
+Duration::from_nanoseconds(rcl_duration_value_t nanoseconds)
+{
+  Duration ret;
+  ret.rcl_duration_.nanoseconds = nanoseconds;
+  return ret;
 }
 
 }  // namespace rclcpp

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"
@@ -76,25 +79,39 @@ Executor::Executor(const rclcpp::ExecutorOptions & options)
         "failed to destroy guard condition: %s", rcl_get_error_string().str);
       rcl_reset_error();
     }
-    throw std::runtime_error("Failed to create wait set in Executor constructor");
+    throw_from_rcl_error(ret, "Failed to create wait set in Executor constructor");
   }
 }
 
 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();
+  // Disassociate 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);
     }
   }
+  // Disassociate 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,117 @@ 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
+      rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
+      if (ret != RCL_RET_OK) {
+        throw_from_rcl_error(ret, "Failed to trigger guard condition on callback group add");
+      }
+    }
+    // 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 +251,68 @@ 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) {
+      rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
+      if (ret != RCL_RET_OK) {
+        throw_from_rcl_error(ret, "Failed to trigger guard condition on callback group remove");
+      }
+    }
+    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 +324,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
@@ -288,8 +472,9 @@ void
 Executor::cancel()
 {
   spinning.store(false);
-  if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-    throw std::runtime_error(rcl_get_error_string().str);
+  rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
+  if (ret != RCL_RET_OK) {
+    throw_from_rcl_error(ret, "Failed to trigger guard condition in cancel");
   }
 }
 
@@ -327,8 +512,9 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
   any_exec.callback_group->can_be_taken_from().store(true);
   // Wake the wait, because it may need to be recalculated or work that
   // was previously blocked is now available.
-  if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-    throw std::runtime_error(rcl_get_error_string().str);
+  rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
+  if (ret != RCL_RET_OK) {
+    throw_from_rcl_error(ret, "Failed to trigger guard condition from execute_any_executable");
   }
 }
 
@@ -473,45 +659,67 @@ 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");
+    rcl_ret_t ret = rcl_wait_set_clear(&wait_set_);
+    if (ret != RCL_RET_OK) {
+      throw_from_rcl_error(ret, "Couldn't clear wait set");
     }
 
     // The size of waitables are accounted for in size of the other entities
-    rcl_ret_t ret = rcl_wait_set_resize(
+    ret = rcl_wait_set_resize(
       &wait_set_, memory_strategy_->number_of_ready_subscriptions(),
       memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(),
       memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(),
       memory_strategy_->number_of_ready_events());
     if (RCL_RET_OK != ret) {
-      throw std::runtime_error(
-              std::string("Couldn't resize the wait set : ") + rcl_get_error_string().str);
+      throw_from_rcl_error(ret, "Couldn't resize the wait set");
     }
 
     if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) {
       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 +737,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 +756,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 +879,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,23 +28,40 @@ using rclcpp::executors::StaticExecutorEntitiesCollector;
 
 StaticExecutorEntitiesCollector::~StaticExecutorEntitiesCollector()
 {
+  // Disassociate all callback groups and thus nodes.
+  for (const 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 (const 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_) {
+  for (const auto & weak_node : weak_nodes_) {
     auto node = weak_node.lock();
     if (node) {
       std::atomic_bool & has_executor = node->get_associated_with_executor_atomic();
       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
 StaticExecutorEntitiesCollector::init(
   rcl_wait_set_t * p_wait_set,
-  rclcpp::memory_strategy::MemoryStrategy::SharedPtr & memory_strategy,
+  rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy,
   rcl_guard_condition_t * executor_guard_condition)
 {
   // Empty initialize executable list
@@ -56,12 +75,18 @@ 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();
 }
 
+void
+StaticExecutorEntitiesCollector::fini()
+{
+  memory_strategy_->clear_handles();
+  exec_list_.clear();
+}
+
 void
 StaticExecutorEntitiesCollector::execute()
 {
@@ -77,18 +102,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 (const 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 (const auto & pair : weak_groups_associated_with_executor_to_nodes_) {
+      auto & weak_group_ptr = pair.first;
+      const 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 +147,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(
+  const 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()
@@ -171,14 +218,14 @@ StaticExecutorEntitiesCollector::prepare_wait_set()
 
   if (RCL_RET_OK != ret) {
     throw std::runtime_error(
-            std::string("Couldn't resize the wait set : ") + rcl_get_error_string().str);
+            std::string("Couldn't resize the wait set: ") + rcl_get_error_string().str);
   }
 }
 
 void
 StaticExecutorEntitiesCollector::refresh_wait_set(std::chrono::nanoseconds timeout)
 {
-  // clear wait set (memeset to '0' all wait_set_ entities
+  // clear wait set (memset to '0' all wait_set_ entities
   // but keeps the wait_set_ number of entities)
   if (rcl_wait_set_clear(p_wait_set_) != RCL_RET_OK) {
     throw std::runtime_error("Couldn't clear wait set");
@@ -205,7 +252,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 +264,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 (const auto & weak_group : node_ptr->get_callback_groups()) {
+    auto group_ptr = weak_group.lock();
+    if (group_ptr != nullptr && !group_ptr->get_associated_with_executor_atomic().load() &&
+      group_ptr->automatically_add_to_executor_with_node())
+    {
+      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.");
+  }
+  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_);
+  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.");
+  }
+  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 +418,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 +432,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,
+  const 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 (const auto & weak_node : weak_nodes_) {
+    auto node = weak_node.lock();
+    if (node) {
+      auto group_ptrs = node->get_callback_groups();
+      std::for_each(
+        group_ptrs.begin(), group_ptrs.end(),
+        [this, node](rclcpp::CallbackGroup::WeakPtr group_ptr)
+        {
+          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 (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
+    groups.push_back(group_node_ptr.first);
+  }
+  for (const auto & 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 (const auto & 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 (const auto & 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"
 
@@ -41,6 +42,7 @@ StaticSingleThreadedExecutor::spin()
   // Set memory_strategy_ and exec_list_ based on weak_nodes_
   // Prepare wait_set_ based on memory_strategy_
   entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
 
   while (rclcpp::ok(this->context_) && spinning.load()) {
     // Refresh wait set and wait for work
@@ -50,23 +52,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 +85,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

@@ -26,7 +26,7 @@ InitOptions::InitOptions(rcl_allocator_t allocator)
   *init_options_ = rcl_get_zero_initialized_init_options();
   rcl_ret_t ret = rcl_init_options_init(init_options_.get(), allocator);
   if (RCL_RET_OK != ret) {
-    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialized rcl init options");
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl init options");
   }
 }
 
@@ -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

@@ -138,7 +138,8 @@ Node::Node(
       node_services_,
       node_logging_,
       node_clock_,
-      node_parameters_
+      node_parameters_,
+      options.clock_qos()
     )),
   node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())),
   node_options_(options),
@@ -211,15 +212,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;
 }
@@ -283,3 +288,24 @@ NodeBase::get_enable_topic_statistics_default() const
 {
   return enable_topic_statistics_default_;
 }
+
+std::string
+NodeBase::resolve_topic_or_service_name(
+  const std::string & name, bool is_service, bool only_expand) const
+{
+  char * output_cstr = NULL;
+  auto allocator = rcl_get_default_allocator();
+  rcl_ret_t ret = rcl_node_resolve_name(
+    node_handle_.get(),
+    name.c_str(),
+    allocator,
+    is_service,
+    only_expand,
+    &output_cstr);
+  if (RCL_RET_OK != ret) {
+    throw_from_rcl_error(ret, "failed to resolve name", rcl_get_error_state());
+  }
+  std::string output{output_cstr};
+  allocator.deallocate(output_cstr, allocator.state);
+  return output;
+}

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/node_interfaces/node_services.cpp

@@ -78,3 +78,9 @@ NodeServices::add_client(
     }
   }
 }
+
+std::string
+NodeServices::resolve_service_name(const std::string & name, bool only_expand) const
+{
+  return node_base_->resolve_topic_or_service_name(name, true, only_expand);
+}

rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp

@@ -26,14 +26,16 @@ NodeTimeSource::NodeTimeSource(
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
   rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
   rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
-  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters)
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters,
+  const rclcpp::QoS & qos)
 : node_base_(node_base),
   node_topics_(node_topics),
   node_graph_(node_graph),
   node_services_(node_services),
   node_logging_(node_logging),
   node_clock_(node_clock),
-  node_parameters_(node_parameters)
+  node_parameters_(node_parameters),
+  time_source_(qos)
 {
   time_source_.attachNode(
     node_base_,

rclcpp/src/rclcpp/node_interfaces/node_topics.cpp

@@ -129,3 +129,9 @@ NodeTopics::get_node_timers_interface() const
 {
   return node_timers_;
 }
+
+std::string
+NodeTopics::resolve_topic_name(const std::string & name, bool only_expand) const
+{
+  return node_base_->resolve_topic_or_service_name(name, false, only_expand);
+}

rclcpp/src/rclcpp/node_options.cpp

@@ -67,6 +67,7 @@ NodeOptions &
 NodeOptions::operator=(const NodeOptions & other)
 {
   if (this != &other) {
+    this->node_options_.reset();
     this->context_ = other.context_;
     this->arguments_ = other.arguments_;
     this->parameter_overrides_ = other.parameter_overrides_;
@@ -75,11 +76,15 @@ NodeOptions::operator=(const NodeOptions & other)
     this->use_intra_process_comms_ = other.use_intra_process_comms_;
     this->enable_topic_statistics_ = other.enable_topic_statistics_;
     this->start_parameter_services_ = other.start_parameter_services_;
-    this->allocator_ = other.allocator_;
+    this->start_parameter_event_publisher_ = other.start_parameter_event_publisher_;
+    this->clock_qos_ = other.clock_qos_;
+    this->parameter_event_qos_ = other.parameter_event_qos_;
+    this->rosout_qos_ = other.rosout_qos_;
+    this->parameter_event_publisher_options_ = other.parameter_event_publisher_options_;
     this->allow_undeclared_parameters_ = other.allow_undeclared_parameters_;
     this->automatically_declare_parameters_from_overrides_ =
       other.automatically_declare_parameters_from_overrides_;
-    this->node_options_.reset();
+    this->allocator_ = other.allocator_;
   }
   return *this;
 }
@@ -94,6 +99,7 @@ NodeOptions::get_rcl_node_options() const
     node_options_->allocator = this->allocator_;
     node_options_->use_global_arguments = this->use_global_arguments_;
     node_options_->enable_rosout = this->enable_rosout_;
+    node_options_->rosout_qos = this->rosout_qos_.get_rmw_qos_profile();
 
     int c_argc = 0;
     std::unique_ptr<const char *[]> c_argv;
@@ -253,6 +259,19 @@ NodeOptions::start_parameter_event_publisher(bool start_parameter_event_publishe
   return *this;
 }
 
+const rclcpp::QoS &
+NodeOptions::clock_qos() const
+{
+  return this->clock_qos_;
+}
+
+NodeOptions &
+NodeOptions::clock_qos(const rclcpp::QoS & clock_qos)
+{
+  this->clock_qos_ = clock_qos;
+  return *this;
+}
+
 const rclcpp::QoS &
 NodeOptions::parameter_event_qos() const
 {
@@ -266,6 +285,20 @@ NodeOptions::parameter_event_qos(const rclcpp::QoS & parameter_event_qos)
   return *this;
 }
 
+const rclcpp::QoS &
+NodeOptions::rosout_qos() const
+{
+  return this->rosout_qos_;
+}
+
+NodeOptions &
+NodeOptions::rosout_qos(const rclcpp::QoS & rosout_qos)
+{
+  this->node_options_.reset();
+  this->rosout_qos_ = rosout_qos;
+  return *this;
+}
+
 const rclcpp::PublisherOptionsBase &
 NodeOptions::parameter_event_publisher_options() const
 {

rclcpp/src/rclcpp/parameter_client.cpp

@@ -15,7 +15,12 @@
 #include "rclcpp/parameter_client.hpp"
 
 #include <algorithm>
+#include <chrono>
+#include <functional>
+#include <future>
+#include <iterator>
 #include <memory>
+#include <stdexcept>
 #include <string>
 #include <vector>
 
@@ -99,36 +104,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 +312,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/parameter_service.cpp

@@ -119,7 +119,7 @@ ParameterService::ParameterService(
         RCLCPP_WARN(
           rclcpp::get_logger("rclcpp"), "Failed to set parameters atomically: %s", ex.what());
         response->result.successful = false;
-        response->result.reason = "One or more parameters wer not declared before setting";
+        response->result.reason = "One or more parameters were not declared before setting";
       }
     },
     qos_profile, nullptr);

rclcpp/src/rclcpp/publisher_base.cpp

@@ -262,7 +262,9 @@ PublisherBase::default_incompatible_qos_callback(
   std::string policy_name = qos_policy_name_from_kind(event.last_policy_kind);
   RCLCPP_WARN(
     rclcpp::get_logger(rcl_node_get_logger_name(rcl_node_handle_.get())),
-    "New subscription discovered on this topic, requesting incompatible QoS. "
+    "New subscription discovered on topic '%s', requesting incompatible QoS. "
     "No messages will be sent to it. "
-    "Last incompatible policy: %s", policy_name.c_str());
+    "Last incompatible policy: %s",
+    get_topic_name(),
+    policy_name.c_str());
 }

rclcpp/src/rclcpp/qos.cpp

@@ -235,6 +235,12 @@ bool operator!=(const QoS & left, const QoS & right)
   return !(left == right);
 }
 
+ClockQoS::ClockQoS(const QoSInitialization & qos_initialization)
+// Using `rmw_qos_profile_sensor_data` intentionally.
+// It's best effort and `qos_initialization` is overriding the depth to 1.
+: QoS(qos_initialization, rmw_qos_profile_sensor_data)
+{}
+
 SensorDataQoS::SensorDataQoS(const QoSInitialization & qos_initialization)
 : QoS(qos_initialization, rmw_qos_profile_sensor_data)
 {}
@@ -251,6 +257,10 @@ ParameterEventsQoS::ParameterEventsQoS(const QoSInitialization & qos_initializat
 : QoS(qos_initialization, rmw_qos_profile_parameter_events)
 {}
 
+RosoutQoS::RosoutQoS(const QoSInitialization & rosout_initialization)
+: QoS(rosout_initialization, rcl_qos_profile_rosout_default)
+{}
+
 SystemDefaultsQoS::SystemDefaultsQoS(const QoSInitialization & qos_initialization)
 : QoS(qos_initialization, rmw_qos_profile_system_default)
 {}

rclcpp/src/rclcpp/subscription_base.cpp

@@ -245,9 +245,11 @@ SubscriptionBase::default_incompatible_qos_callback(
   std::string policy_name = qos_policy_name_from_kind(event.last_policy_kind);
   RCLCPP_WARN(
     rclcpp::get_logger(rcl_node_get_logger_name(node_handle_.get())),
-    "New publisher discovered on this topic, offering incompatible QoS. "
+    "New publisher discovered on topic '%s', offering incompatible QoS. "
     "No messages will be sent to it. "
-    "Last incompatible policy: %s", policy_name.c_str());
+    "Last incompatible policy: %s",
+    get_topic_name(),
+    policy_name.c_str());
 }
 
 bool

rclcpp/src/rclcpp/time.cpp

@@ -199,7 +199,7 @@ Time::operator-(const rclcpp::Time & rhs) const
     throw std::underflow_error("time subtraction leads to int64_t underflow");
   }
 
-  return Duration(rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds);
+  return Duration::from_nanoseconds(rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds);
 }
 
 Time

rclcpp/src/rclcpp/time_source.cpp

@@ -33,17 +33,16 @@
 namespace rclcpp
 {
 
-TimeSource::TimeSource(std::shared_ptr<rclcpp::Node> node)
+TimeSource::TimeSource(std::shared_ptr<rclcpp::Node> node, const rclcpp::QoS & qos)
 : logger_(rclcpp::get_logger("rclcpp")),
-  clock_subscription_(nullptr),
-  ros_time_active_(false)
+  qos_(qos)
 {
   this->attachNode(node);
 }
 
-TimeSource::TimeSource()
+TimeSource::TimeSource(const rclcpp::QoS & qos)
 : logger_(rclcpp::get_logger("rclcpp")),
-  ros_time_active_(false)
+  qos_(qos)
 {
 }
 
@@ -185,11 +184,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 +236,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 +282,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/src/rclcpp/timer.cpp

@@ -76,8 +76,9 @@ TimerBase::~TimerBase()
 void
 TimerBase::cancel()
 {
-  if (rcl_timer_cancel(timer_handle_.get()) != RCL_RET_OK) {
-    throw std::runtime_error(std::string("Couldn't cancel timer: ") + rcl_get_error_string().str);
+  rcl_ret_t ret = rcl_timer_cancel(timer_handle_.get());
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't cancel timer");
   }
 }
 
@@ -95,8 +96,9 @@ TimerBase::is_canceled()
 void
 TimerBase::reset()
 {
-  if (rcl_timer_reset(timer_handle_.get()) != RCL_RET_OK) {
-    throw std::runtime_error(std::string("Couldn't reset timer: ") + rcl_get_error_string().str);
+  rcl_ret_t ret = rcl_timer_reset(timer_handle_.get());
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't reset timer");
   }
 }
 
@@ -104,8 +106,9 @@ bool
 TimerBase::is_ready()
 {
   bool ready = false;
-  if (rcl_timer_is_ready(timer_handle_.get(), &ready) != RCL_RET_OK) {
-    throw std::runtime_error(std::string("Failed to check timer: ") + rcl_get_error_string().str);
+  rcl_ret_t ret = rcl_timer_is_ready(timer_handle_.get(), &ready);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to check timer");
   }
   return ready;
 }
@@ -114,14 +117,10 @@ std::chrono::nanoseconds
 TimerBase::time_until_trigger()
 {
   int64_t time_until_next_call = 0;
-  if (
-    rcl_timer_get_time_until_next_call(
-      timer_handle_.get(),
-      &time_until_next_call) != RCL_RET_OK)
-  {
-    throw std::runtime_error(
-            std::string(
-              "Timer could not get time until next call: ") + rcl_get_error_string().str);
+  rcl_ret_t ret = rcl_timer_get_time_until_next_call(
+    timer_handle_.get(), &time_until_next_call);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Timer could not get time until next call");
   }
   return std::chrono::nanoseconds(time_until_next_call);
 }

rclcpp/src/rclcpp/utilities.cpp

@@ -14,11 +14,14 @@
 
 #include "rclcpp/utilities.hpp"
 
+#include <chrono>
+#include <functional>
 #include <string>
 #include <vector>
 
 #include "./signal_handler.hpp"
 #include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/detail/utilities.hpp"
 #include "rclcpp/exceptions.hpp"
 
 #include "rcl/error_handling.h"
@@ -54,6 +57,47 @@ uninstall_signal_handlers()
   return SignalHandler::get_global_signal_handler().uninstall();
 }
 
+static
+std::vector<std::string>
+_remove_ros_arguments(
+  char const * const argv[],
+  const rcl_arguments_t * args,
+  rcl_allocator_t alloc)
+{
+  rcl_ret_t ret;
+  int nonros_argc = 0;
+  const char ** nonros_argv = NULL;
+
+  ret = rcl_remove_ros_arguments(
+    argv,
+    args,
+    alloc,
+    &nonros_argc,
+    &nonros_argv);
+
+  if (RCL_RET_OK != ret || nonros_argc < 0) {
+    // Not using throw_from_rcl_error, because we may need to append deallocation failures.
+    exceptions::RCLError exc(ret, rcl_get_error_state(), "");
+    rcl_reset_error();
+    if (NULL != nonros_argv) {
+      alloc.deallocate(nonros_argv, alloc.state);
+    }
+    throw exc;
+  }
+
+  std::vector<std::string> return_arguments(static_cast<size_t>(nonros_argc));
+
+  for (size_t ii = 0; ii < static_cast<size_t>(nonros_argc); ++ii) {
+    return_arguments[ii] = std::string(nonros_argv[ii]);
+  }
+
+  if (NULL != nonros_argv) {
+    alloc.deallocate(nonros_argv, alloc.state);
+  }
+
+  return return_arguments;
+}
+
 std::vector<std::string>
 init_and_remove_ros_arguments(
   int argc,
@@ -61,7 +105,10 @@ init_and_remove_ros_arguments(
   const InitOptions & init_options)
 {
   init(argc, argv, init_options);
-  return remove_ros_arguments(argc, argv);
+
+  using rclcpp::contexts::get_global_default_context;
+  auto rcl_context = get_global_default_context()->get_rcl_context();
+  return _remove_ros_arguments(argv, &(rcl_context->global_arguments), rcl_get_default_allocator());
 }
 
 std::vector<std::string>
@@ -77,40 +124,17 @@ remove_ros_arguments(int argc, char const * const argv[])
     exceptions::throw_from_rcl_error(ret, "failed to parse arguments");
   }
 
-  int nonros_argc = 0;
-  const char ** nonros_argv = NULL;
-
-  ret = rcl_remove_ros_arguments(
-    argv,
-    &parsed_args,
-    alloc,
-    &nonros_argc,
-    &nonros_argv);
-
-  if (RCL_RET_OK != ret || nonros_argc < 0) {
-    // Not using throw_from_rcl_error, because we may need to append deallocation failures.
-    exceptions::RCLErrorBase base_exc(ret, rcl_get_error_state());
-    rcl_reset_error();
-    if (NULL != nonros_argv) {
-      alloc.deallocate(nonros_argv, alloc.state);
-    }
+  std::vector<std::string> return_arguments;
+  try {
+    return_arguments = _remove_ros_arguments(argv, &parsed_args, alloc);
+  } catch (exceptions::RCLError & exc) {
     if (RCL_RET_OK != rcl_arguments_fini(&parsed_args)) {
-      base_exc.formatted_message += std::string(
+      exc.formatted_message += std::string(
         ", failed also to cleanup parsed arguments, leaking memory: ") +
         rcl_get_error_string().str;
       rcl_reset_error();
     }
-    throw exceptions::RCLError(base_exc, "");
-  }
-
-  std::vector<std::string> return_arguments(static_cast<size_t>(nonros_argc));
-
-  for (size_t ii = 0; ii < static_cast<size_t>(nonros_argc); ++ii) {
-    return_arguments[ii] = std::string(nonros_argv[ii]);
-  }
-
-  if (NULL != nonros_argv) {
-    alloc.deallocate(nonros_argv, alloc.state);
+    throw exc;
   }
 
   ret = rcl_arguments_fini(&parsed_args);

rclcpp/test/CMakeLists.txt

@@ -1,498 +1,15 @@
-find_package(ament_cmake_gtest REQUIRED)
-
-find_package(rmw_implementation_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
 find_package(test_msgs REQUIRED)
 
 include(cmake/rclcpp_add_build_failure_test.cmake)
 
-add_definitions(-DTEST_RESOURCES_DIRECTORY="${CMAKE_CURRENT_BINARY_DIR}/resources")
+add_subdirectory(benchmark)
+add_subdirectory(rclcpp)
 
-rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs
-  msg/Header.msg
-  msg/MessageWithHeader.msg
-  DEPENDENCIES builtin_interfaces
-  LIBRARY_NAME ${PROJECT_NAME}
-  SKIP_INSTALL
-)
-
-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
-    "test_msgs"
-  )
-  target_link_libraries(test_any_service_callback ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_any_subscription_callback rclcpp/test_any_subscription_callback.cpp)
-if(TARGET test_any_subscription_callback)
-  ament_target_dependencies(test_any_subscription_callback
-    "test_msgs"
-  )
-  target_link_libraries(test_any_subscription_callback ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_client rclcpp/test_client.cpp)
-if(TARGET test_client)
-  ament_target_dependencies(test_client
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_client ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_create_timer rclcpp/test_create_timer.cpp)
-if(TARGET test_create_timer)
-  ament_target_dependencies(test_create_timer
-    "rcl_interfaces"
-    "rmw"
-    "rcl"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_create_timer ${PROJECT_NAME})
-  target_include_directories(test_create_timer PRIVATE rclcpp/)
-endif()
-ament_add_gtest(test_create_subscription rclcpp/test_create_subscription.cpp)
-if(TARGET test_create_subscription)
-  target_link_libraries(test_create_subscription ${PROJECT_NAME})
-  ament_target_dependencies(test_create_subscription
-    "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
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_function_traits rclcpp/test_function_traits.cpp)
-if(TARGET test_function_traits)
-  target_include_directories(test_function_traits PUBLIC ../include)
-  ament_target_dependencies(test_function_traits
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-endif()
-ament_add_gmock(test_intra_process_manager rclcpp/test_intra_process_manager.cpp)
-if(TARGET test_intra_process_manager)
-  ament_target_dependencies(test_intra_process_manager
-    "rcl"
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_intra_process_manager ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_ring_buffer_implementation rclcpp/test_ring_buffer_implementation.cpp)
-if(TARGET test_ring_buffer_implementation)
-  ament_target_dependencies(test_ring_buffer_implementation
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_ring_buffer_implementation ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_intra_process_buffer rclcpp/test_intra_process_buffer.cpp)
-if(TARGET test_intra_process_buffer)
-  ament_target_dependencies(test_intra_process_buffer
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_intra_process_buffer ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_loaned_message rclcpp/test_loaned_message.cpp)
-ament_target_dependencies(test_loaned_message
-  "test_msgs"
-)
-target_link_libraries(test_loaned_message ${PROJECT_NAME})
-
-ament_add_gtest(test_node rclcpp/test_node.cpp TIMEOUT 240)
-if(TARGET test_node)
-  ament_target_dependencies(test_node
-    "rcl_interfaces"
-    "rcpputils"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "test_msgs"
-  )
-  target_link_libraries(test_node ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_node_interfaces__get_node_interfaces
-  rclcpp/node_interfaces/test_get_node_interfaces.cpp)
-if(TARGET test_node_interfaces__get_node_interfaces)
-  target_link_libraries(test_node_interfaces__get_node_interfaces ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_node_interfaces__node_base
-  rclcpp/node_interfaces/test_node_base.cpp)
-if(TARGET test_node_interfaces__node_base)
-  target_link_libraries(test_node_interfaces__node_base ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_node_interfaces__node_clock
-  rclcpp/node_interfaces/test_node_clock.cpp)
-if(TARGET test_node_interfaces__node_clock)
-  target_link_libraries(test_node_interfaces__node_clock ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_node_interfaces__node_graph
-  rclcpp/node_interfaces/test_node_graph.cpp)
-if(TARGET test_node_interfaces__node_graph)
-  ament_target_dependencies(
-    test_node_interfaces__node_graph
-    "test_msgs")
-  target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_node_interfaces__node_parameters
-  rclcpp/node_interfaces/test_node_parameters.cpp)
-if(TARGET test_node_interfaces__node_parameters)
-  target_link_libraries(test_node_interfaces__node_parameters ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_node_interfaces__node_services
-  rclcpp/node_interfaces/test_node_services.cpp)
-if(TARGET test_node_interfaces__node_services)
-  target_link_libraries(test_node_interfaces__node_services ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_node_interfaces__node_timers
-  rclcpp/node_interfaces/test_node_timers.cpp)
-if(TARGET test_node_interfaces__node_timers)
-  target_link_libraries(test_node_interfaces__node_timers ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_node_interfaces__node_topics
-  rclcpp/node_interfaces/test_node_topics.cpp)
-if(TARGET test_node_interfaces__node_topics)
-  ament_target_dependencies(
-    test_node_interfaces__node_topics
-    "test_msgs")
-  target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_node_interfaces__node_waitables
-  rclcpp/node_interfaces/test_node_waitables.cpp)
-if(TARGET test_node_interfaces__node_waitables)
-  target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME})
-endif()
-
-# TODO(wjwwood): reenable these build failure tests when I can get Jenkins to ignore their output
-# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_rclcpp_node
-#   rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp)
-# target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node
-#   ${PROJECT_NAME})
-
-# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
-#   rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp)
-# target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
-#   ${PROJECT_NAME})
-
-# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_wrapped_node
-#   rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp)
-# target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node
-#   ${PROJECT_NAME})
-
-# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_wrapped_node
-#   rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp)
-# target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
-#   ${PROJECT_NAME})
-
-ament_add_gtest(test_node_global_args rclcpp/test_node_global_args.cpp)
-if(TARGET test_node_global_args)
-  ament_target_dependencies(test_node_global_args
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_node_global_args ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_node_options rclcpp/test_node_options.cpp)
-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_parameter_client rclcpp/test_parameter_client.cpp)
-if(TARGET test_parameter_client)
-  ament_target_dependencies(test_parameter_client
-    "rcl_interfaces"
-  )
-  target_link_libraries(test_parameter_client ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_parameter_events_filter rclcpp/test_parameter_events_filter.cpp)
-if(TARGET test_parameter_events_filter)
-  ament_target_dependencies(test_parameter_events_filter
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_parameter_events_filter ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_parameter rclcpp/test_parameter.cpp)
-if(TARGET test_parameter)
-  ament_target_dependencies(test_parameter
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_parameter ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_parameter_map rclcpp/test_parameter_map.cpp)
-if(TARGET test_parameter_map)
-  target_link_libraries(test_parameter_map ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_publisher rclcpp/test_publisher.cpp)
-if(TARGET test_publisher)
-  ament_target_dependencies(test_publisher
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "test_msgs"
-  )
-  target_link_libraries(test_publisher ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_publisher_subscription_count_api rclcpp/test_publisher_subscription_count_api.cpp)
-if(TARGET test_publisher_subscription_count_api)
-  ament_target_dependencies(test_publisher_subscription_count_api
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "test_msgs"
-  )
-  target_link_libraries(test_publisher_subscription_count_api ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_qos rclcpp/test_qos.cpp)
-if(TARGET test_qos)
-  ament_target_dependencies(test_qos
-    "rmw"
-  )
-  target_link_libraries(test_qos
-    ${PROJECT_NAME}
-  )
-endif()
-ament_add_gtest(test_qos_event rclcpp/test_qos_event.cpp)
-if(TARGET test_qos_event)
-  ament_target_dependencies(test_qos_event
-    "rmw"
-    "test_msgs"
-  )
-  target_link_libraries(test_qos_event
-    ${PROJECT_NAME}
-  )
-endif()
-ament_add_gtest(test_rate rclcpp/test_rate.cpp)
-if(TARGET test_rate)
-  ament_target_dependencies(test_rate
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_rate
-    ${PROJECT_NAME}
-  )
-endif()
-ament_add_gtest(test_serialized_message_allocator rclcpp/test_serialized_message_allocator.cpp)
-if(TARGET test_serialized_message_allocator)
-  ament_target_dependencies(test_serialized_message_allocator
-    test_msgs
-  )
-  target_link_libraries(test_serialized_message_allocator
-    ${PROJECT_NAME}
-  )
-endif()
-ament_add_gtest(test_serialized_message rclcpp/test_serialized_message.cpp)
-if(TARGET test_serialized_message)
-  ament_target_dependencies(test_serialized_message
-    test_msgs
-  )
-  target_link_libraries(test_serialized_message
-    ${PROJECT_NAME}
-  )
-endif()
-ament_add_gtest(test_service rclcpp/test_service.cpp)
-if(TARGET test_service)
-  ament_target_dependencies(test_service
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_service ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_subscription rclcpp/test_subscription.cpp)
-if(TARGET test_subscription)
-  ament_target_dependencies(test_subscription
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "test_msgs"
-  )
-  target_link_libraries(test_subscription ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_subscription_publisher_count_api rclcpp/test_subscription_publisher_count_api.cpp)
-if(TARGET test_subscription_publisher_count_api)
-  ament_target_dependencies(test_subscription_publisher_count_api
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "test_msgs"
-  )
-  target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_subscription_traits rclcpp/test_subscription_traits.cpp)
-if(TARGET test_subscription_traits)
-  ament_target_dependencies(test_subscription_traits
-    "rcl"
-    "test_msgs"
-  )
-  target_link_libraries(test_subscription_traits ${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
-    "rcl"
-  )
-  target_link_libraries(test_find_weak_nodes ${PROJECT_NAME})
-endif()
-
-set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}")
-if(WIN32)
-  set(append_library_dirs "${append_library_dirs}/$<CONFIG>")
-endif()
-
-ament_add_gtest(test_externally_defined_services rclcpp/test_externally_defined_services.cpp)
-ament_target_dependencies(test_externally_defined_services
-  "rcl"
-  "test_msgs"
-)
-target_link_libraries(test_externally_defined_services ${PROJECT_NAME})
-
-ament_add_gtest(test_duration rclcpp/test_duration.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_duration)
-  ament_target_dependencies(test_duration
-    "rcl")
-  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})
-
-ament_add_gmock(test_logging rclcpp/test_logging.cpp)
-target_link_libraries(test_logging ${PROJECT_NAME})
-
-ament_add_gtest(test_time rclcpp/test_time.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_time)
-  ament_target_dependencies(test_time
-    "rcl")
-  target_link_libraries(test_time ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_timer rclcpp/test_timer.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_timer)
-  ament_target_dependencies(test_timer
-    "rcl")
-  target_link_libraries(test_timer ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_time_source rclcpp/test_time_source.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_time_source)
-  ament_target_dependencies(test_time_source
-    "rcl")
-  target_link_libraries(test_time_source ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_utilities rclcpp/test_utilities.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_utilities)
-  ament_target_dependencies(test_utilities
-    "rcl")
-  target_link_libraries(test_utilities ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_init rclcpp/test_init.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_init)
-  ament_target_dependencies(test_init
-    "rcl")
-  target_link_libraries(test_init ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_interface_traits rclcpp/test_interface_traits.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_interface_traits)
-  ament_target_dependencies(test_interface_traits
-    "rcl")
-  target_link_libraries(test_interface_traits ${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)
-  ament_target_dependencies(test_multi_threaded_executor
-    "rcl")
-  target_link_libraries(test_multi_threaded_executor ${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)
-  target_link_libraries(test_guard_condition ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_wait_set rclcpp/test_wait_set.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_wait_set)
-  ament_target_dependencies(test_wait_set "test_msgs")
-  target_link_libraries(test_wait_set ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_subscription_topic_statistics rclcpp/topic_statistics/test_subscription_topic_statistics.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}"
-)
-if(TARGET test_subscription_topic_statistics)
-  ament_target_dependencies(test_subscription_topic_statistics
-    "builtin_interfaces"
-    "libstatistics_collector"
-    "rcl_interfaces"
-    "rcutils"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "statistics_msgs"
-    "test_msgs")
-  rosidl_target_interfaces(test_subscription_topic_statistics ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp")
-  target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_subscription_options rclcpp/test_subscription_options.cpp)
-if(TARGET test_subscription_options)
-  ament_target_dependencies(test_subscription_options "rcl")
-  target_link_libraries(test_subscription_options ${PROJECT_NAME})
+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

rclcpp/test/benchmark/CMakeLists.txt

@@ -0,0 +1,27 @@
+find_package(performance_test_fixture REQUIRED)
+
+# These benchmarks are only being created and run for the default RMW
+# implementation. We are looking to test the performance of the ROS 2 code, not
+# the underlying middleware.
+
+add_performance_test(benchmark_client benchmark_client.cpp)
+if(TARGET benchmark_client)
+  target_link_libraries(benchmark_client ${PROJECT_NAME})
+  ament_target_dependencies(benchmark_client test_msgs rcl_interfaces)
+endif()
+
+add_performance_test(benchmark_init_shutdown benchmark_init_shutdown.cpp)
+if(TARGET benchmark_init_shutdown)
+  target_link_libraries(benchmark_init_shutdown ${PROJECT_NAME})
+endif()
+
+add_performance_test(benchmark_node benchmark_node.cpp)
+if(TARGET benchmark_node)
+  target_link_libraries(benchmark_node ${PROJECT_NAME})
+endif()
+
+add_performance_test(benchmark_service benchmark_service.cpp)
+if(TARGET benchmark_service)
+  target_link_libraries(benchmark_service ${PROJECT_NAME})
+  ament_target_dependencies(benchmark_service test_msgs rcl_interfaces)
+endif()

rclcpp/test/benchmark/benchmark_client.cpp

@@ -0,0 +1,153 @@
+// 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 <string>
+
+#include "performance_test_fixture/performance_test_fixture.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+using performance_test_fixture::PerformanceTest;
+
+constexpr char empty_service_name[] = "empty_service";
+
+class ClientPerformanceTest : public PerformanceTest
+{
+public:
+  explicit ClientPerformanceTest(rclcpp::NodeOptions = rclcpp::NodeOptions()) {}
+  void SetUp(benchmark::State & state)
+  {
+    rclcpp::init(0, nullptr);
+    node = std::make_unique<rclcpp::Node>("node", "ns");
+
+    auto empty_service_callback =
+      [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {};
+    empty_service =
+      node->create_service<test_msgs::srv::Empty>(empty_service_name, empty_service_callback);
+
+    performance_test_fixture::PerformanceTest::SetUp(state);
+  }
+
+  void TearDown(benchmark::State & state)
+  {
+    performance_test_fixture::PerformanceTest::TearDown(state);
+    empty_service.reset();
+    node.reset();
+    rclcpp::shutdown();
+  }
+
+protected:
+  std::unique_ptr<rclcpp::Node> node;
+  std::shared_ptr<rclcpp::Service<test_msgs::srv::Empty>> empty_service;
+};
+
+BENCHMARK_F(ClientPerformanceTest, construct_client_no_service)(benchmark::State & state) {
+  // Prime cache
+  auto outer_client = node->create_client<test_msgs::srv::Empty>("not_an_existing_service");
+  outer_client.reset();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    auto client = node->create_client<test_msgs::srv::Empty>("not_an_existing_service");
+    benchmark::DoNotOptimize(client);
+    benchmark::ClobberMemory();
+
+    state.PauseTiming();
+    client.reset();
+    state.ResumeTiming();
+  }
+}
+
+BENCHMARK_F(ClientPerformanceTest, construct_client_empty_srv)(benchmark::State & state) {
+  // Prime cache
+  auto outer_client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
+  outer_client.reset();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    auto client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
+    benchmark::DoNotOptimize(client);
+    benchmark::ClobberMemory();
+
+    state.PauseTiming();
+    client.reset();
+    state.ResumeTiming();
+  }
+}
+
+BENCHMARK_F(ClientPerformanceTest, destroy_client_empty_srv)(benchmark::State & state) {
+  // Prime cache
+  auto outer_client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
+  outer_client.reset();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    state.PauseTiming();
+    auto client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
+    state.ResumeTiming();
+    benchmark::DoNotOptimize(client);
+    benchmark::ClobberMemory();
+
+    client.reset();
+  }
+}
+
+BENCHMARK_F(ClientPerformanceTest, wait_for_service)(benchmark::State & state) {
+  int count = 0;
+  for (auto _ : state) {
+    state.PauseTiming();
+    const std::string service_name = std::string("service_") + std::to_string(count++);
+    // Create client before service so it has to 'discover' the service after construction
+    auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+    auto callback =
+      [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {};
+    auto service =
+      node->create_service<test_msgs::srv::Empty>(service_name, callback);
+    state.ResumeTiming();
+
+    client->wait_for_service(std::chrono::seconds(1));
+    benchmark::ClobberMemory();
+  }
+}
+
+BENCHMARK_F(ClientPerformanceTest, async_send_request_only)(benchmark::State & state) {
+  auto client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
+  auto shared_request = std::make_shared<test_msgs::srv::Empty::Request>();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    auto future = client->async_send_request(shared_request);
+    benchmark::DoNotOptimize(future);
+    benchmark::ClobberMemory();
+  }
+}
+
+BENCHMARK_F(ClientPerformanceTest, async_send_request_and_response)(benchmark::State & state) {
+  auto client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
+  auto shared_request = std::make_shared<test_msgs::srv::Empty::Request>();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    auto future = client->async_send_request(shared_request);
+    rclcpp::spin_until_future_complete(
+      node->get_node_base_interface(), future, std::chrono::seconds(1));
+    benchmark::DoNotOptimize(future);
+    benchmark::ClobberMemory();
+  }
+}

rclcpp/test/benchmark/benchmark_init_shutdown.cpp

@@ -0,0 +1,53 @@
+// 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 "performance_test_fixture/performance_test_fixture.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+
+using performance_test_fixture::PerformanceTest;
+
+BENCHMARK_F(PerformanceTest, rclcpp_init)(benchmark::State & state)
+{
+  // Warmup and prime caches
+  rclcpp::init(0, nullptr);
+  rclcpp::shutdown();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    rclcpp::init(0, nullptr);
+
+    state.PauseTiming();
+    rclcpp::shutdown();
+    state.ResumeTiming();
+    benchmark::ClobberMemory();
+  }
+}
+
+BENCHMARK_F(PerformanceTest, rclcpp_shutdown)(benchmark::State & state)
+{
+  // Warmup and prime caches
+  rclcpp::init(0, nullptr);
+  rclcpp::shutdown();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    state.PauseTiming();
+    rclcpp::init(0, nullptr);
+    state.ResumeTiming();
+
+    rclcpp::shutdown();
+    benchmark::ClobberMemory();
+  }
+}

rclcpp/test/benchmark/benchmark_node.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 <memory>
+#include <string>
+
+#include "performance_test_fixture/performance_test_fixture.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+using performance_test_fixture::PerformanceTest;
+
+class NodePerformanceTest : public PerformanceTest
+{
+public:
+  void SetUp(benchmark::State & state)
+  {
+    rclcpp::init(0, nullptr);
+    performance_test_fixture::PerformanceTest::SetUp(state);
+  }
+
+  void TearDown(benchmark::State & state)
+  {
+    performance_test_fixture::PerformanceTest::TearDown(state);
+    rclcpp::shutdown();
+  }
+};
+
+BENCHMARK_F(NodePerformanceTest, create_node)(benchmark::State & state)
+{
+  // Warmup and prime caches
+  auto outer_node = std::make_shared<rclcpp::Node>("node");
+  outer_node.reset();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    // Using pointer to separate construction and destruction in timing
+    auto node = std::make_shared<rclcpp::Node>("node");
+    benchmark::DoNotOptimize(node);
+    benchmark::ClobberMemory();
+
+    // Ensure destruction of node is not counted toward timing
+    state.PauseTiming();
+    node.reset();
+    state.ResumeTiming();
+  }
+}
+
+BENCHMARK_F(NodePerformanceTest, destroy_node)(benchmark::State & state)
+{
+  // Warmup and prime caches
+  auto outer_node = std::make_shared<rclcpp::Node>("node");
+  outer_node.reset();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    // Using pointer to separate construction and destruction in timing
+    state.PauseTiming();
+    auto node = std::make_shared<rclcpp::Node>("node");
+    state.ResumeTiming();
+
+    benchmark::DoNotOptimize(node);
+    benchmark::ClobberMemory();
+
+    node.reset();
+  }
+}

rclcpp/test/benchmark/benchmark_service.cpp

@@ -0,0 +1,141 @@
+// 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 "performance_test_fixture/performance_test_fixture.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+using performance_test_fixture::PerformanceTest;
+
+constexpr char empty_service_name[] = "empty_service";
+
+class ServicePerformanceTest : public PerformanceTest
+{
+public:
+  ServicePerformanceTest()
+  : callback_count(0) {}
+
+  void SetUp(benchmark::State & state)
+  {
+    rclcpp::init(0, nullptr);
+    node = std::make_unique<rclcpp::Node>("node", "ns");
+    empty_client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
+    callback_count = 0;
+
+    performance_test_fixture::PerformanceTest::SetUp(state);
+  }
+
+  void TearDown(benchmark::State & state)
+  {
+    performance_test_fixture::PerformanceTest::TearDown(state);
+
+    empty_client.reset();
+    node.reset();
+    rclcpp::shutdown();
+  }
+
+  void ServiceCallback(
+    const test_msgs::srv::Empty::Request::SharedPtr,
+    test_msgs::srv::Empty::Response::SharedPtr)
+  {
+    callback_count++;
+  }
+
+protected:
+  std::unique_ptr<rclcpp::Node> node;
+  std::shared_ptr<rclcpp::Client<test_msgs::srv::Empty>> empty_client;
+  int callback_count;
+};
+
+BENCHMARK_F(ServicePerformanceTest, construct_service_no_client)(benchmark::State & state) {
+  auto callback = std::bind(
+    &ServicePerformanceTest::ServiceCallback,
+    this, std::placeholders::_1, std::placeholders::_2);
+
+  auto outer_service = node->create_service<test_msgs::srv::Empty>("not_a_service", callback);
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    auto service = node->create_service<test_msgs::srv::Empty>("not_a_service", callback);
+    benchmark::DoNotOptimize(service);
+    benchmark::ClobberMemory();
+
+    state.PauseTiming();
+    service.reset();
+    state.ResumeTiming();
+  }
+}
+
+BENCHMARK_F(ServicePerformanceTest, construct_service_empty_srv)(benchmark::State & state) {
+  auto callback = std::bind(
+    &ServicePerformanceTest::ServiceCallback,
+    this, std::placeholders::_1, std::placeholders::_2);
+  auto outer_service = node->create_service<test_msgs::srv::Empty>(empty_service_name, callback);
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    auto service = node->create_service<test_msgs::srv::Empty>(empty_service_name, callback);
+    benchmark::DoNotOptimize(service);
+    benchmark::ClobberMemory();
+
+    state.PauseTiming();
+    service.reset();
+    state.ResumeTiming();
+  }
+}
+
+BENCHMARK_F(ServicePerformanceTest, destroy_service_empty_srv)(benchmark::State & state) {
+  auto callback = std::bind(
+    &ServicePerformanceTest::ServiceCallback,
+    this, std::placeholders::_1, std::placeholders::_2);
+  auto outer_service = node->create_service<test_msgs::srv::Empty>(empty_service_name, callback);
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    state.PauseTiming();
+    auto service = node->create_service<test_msgs::srv::Empty>(empty_service_name, callback);
+    state.ResumeTiming();
+    benchmark::DoNotOptimize(service);
+    benchmark::ClobberMemory();
+
+    service.reset();
+  }
+}
+
+BENCHMARK_F(ServicePerformanceTest, async_send_response)(benchmark::State & state) {
+  auto callback = std::bind(
+    &ServicePerformanceTest::ServiceCallback,
+    this, std::placeholders::_1, std::placeholders::_2);
+  auto service = node->create_service<test_msgs::srv::Empty>(empty_service_name, callback);
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    state.PauseTiming();
+    // Clear executor queue
+    rclcpp::spin_some(node->get_node_base_interface());
+
+    auto request = std::make_shared<test_msgs::srv::Empty::Request>();
+    auto future = empty_client->async_send_request(request);
+    state.ResumeTiming();
+    benchmark::DoNotOptimize(service);
+    benchmark::ClobberMemory();
+
+    rclcpp::spin_until_future_complete(node->get_node_base_interface(), future);
+  }
+  if (callback_count == 0) {
+    state.SkipWithError("Service callback was not called");
+  }
+}

rclcpp/test/mocking_utils/patch.hpp

@@ -0,0 +1,578 @@
+// 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, ArgT6)
+/// 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);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7)
+/// 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, typename ArgT7>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7)>
+{
+  mmk_mock_define(
+    mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8)
+/// 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, typename ArgT7, typename ArgT8>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8)>
+{
+  mmk_mock_define(
+    mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7,
+/// ArgT8, ArgT9) 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, 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);
+};
+
+/// 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/CMakeLists.txt

@@ -0,0 +1,653 @@
+find_package(ament_cmake_gtest REQUIRED)
+
+find_package(rmw_implementation_cmake REQUIRED)
+
+add_definitions(-DTEST_RESOURCES_DIRECTORY="${CMAKE_CURRENT_BINARY_DIR}/../resources")
+
+rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs
+  ../msg/Header.msg
+  ../msg/MessageWithHeader.msg
+  DEPENDENCIES builtin_interfaces
+  LIBRARY_NAME ${PROJECT_NAME}
+  SKIP_INSTALL
+)
+
+ament_add_gtest(
+  test_allocator_common
+  allocator/test_allocator_common.cpp)
+if(TARGET test_allocator_common)
+  target_link_libraries(test_allocator_common ${PROJECT_NAME})
+endif()
+ament_add_gtest(
+  test_allocator_deleter
+  allocator/test_allocator_deleter.cpp)
+if(TARGET test_allocator_deleter)
+  target_link_libraries(test_allocator_deleter ${PROJECT_NAME})
+endif()
+ament_add_gtest(
+  test_exceptions
+  exceptions/test_exceptions.cpp)
+if(TARGET test_exceptions)
+  target_link_libraries(test_exceptions ${PROJECT_NAME} mimick)
+endif()
+
+# 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
+  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 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 test_any_service_callback.cpp)
+if(TARGET test_any_service_callback)
+  ament_target_dependencies(test_any_service_callback
+    "test_msgs"
+  )
+  target_link_libraries(test_any_service_callback ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_any_subscription_callback test_any_subscription_callback.cpp)
+if(TARGET test_any_subscription_callback)
+  ament_target_dependencies(test_any_subscription_callback
+    "test_msgs"
+  )
+  target_link_libraries(test_any_subscription_callback ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_client test_client.cpp)
+if(TARGET test_client)
+  ament_target_dependencies(test_client
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "test_msgs"
+  )
+  target_link_libraries(test_client ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_create_timer test_create_timer.cpp)
+if(TARGET test_create_timer)
+  ament_target_dependencies(test_create_timer
+    "rcl_interfaces"
+    "rmw"
+    "rcl"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+  target_link_libraries(test_create_timer ${PROJECT_NAME})
+  target_include_directories(test_create_timer PRIVATE ./)
+endif()
+ament_add_gtest(test_create_subscription test_create_subscription.cpp)
+if(TARGET test_create_subscription)
+  target_link_libraries(test_create_subscription ${PROJECT_NAME})
+  ament_target_dependencies(test_create_subscription
+    "test_msgs"
+  )
+endif()
+ament_add_gtest(test_add_callback_groups_to_executor
+  test_add_callback_groups_to_executor.cpp
+  TIMEOUT 120)
+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 test_expand_topic_or_service_name.cpp)
+if(TARGET test_expand_topic_or_service_name)
+  ament_target_dependencies(test_expand_topic_or_service_name
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+  target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_function_traits test_function_traits.cpp)
+if(TARGET test_function_traits)
+  target_include_directories(test_function_traits PUBLIC ../../include)
+  ament_target_dependencies(test_function_traits
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+endif()
+ament_add_gtest(
+  test_future_return_code
+  test_future_return_code.cpp)
+if(TARGET test_future_return_code)
+  target_link_libraries(test_future_return_code ${PROJECT_NAME})
+endif()
+ament_add_gmock(test_intra_process_manager test_intra_process_manager.cpp)
+if(TARGET test_intra_process_manager)
+  ament_target_dependencies(test_intra_process_manager
+    "rcl"
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+  target_link_libraries(test_intra_process_manager ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_ring_buffer_implementation test_ring_buffer_implementation.cpp)
+if(TARGET test_ring_buffer_implementation)
+  ament_target_dependencies(test_ring_buffer_implementation
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+  target_link_libraries(test_ring_buffer_implementation ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_intra_process_buffer test_intra_process_buffer.cpp)
+if(TARGET test_intra_process_buffer)
+  ament_target_dependencies(test_intra_process_buffer
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+  target_link_libraries(test_intra_process_buffer ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_loaned_message test_loaned_message.cpp)
+ament_target_dependencies(test_loaned_message
+  "test_msgs"
+)
+target_link_libraries(test_loaned_message ${PROJECT_NAME} mimick)
+
+ament_add_gtest(test_memory_strategy 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 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 test_node.cpp TIMEOUT 240)
+if(TARGET test_node)
+  ament_target_dependencies(test_node
+    "rcl_interfaces"
+    "rcpputils"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "test_msgs"
+  )
+  target_link_libraries(test_node ${PROJECT_NAME} mimick)
+endif()
+
+ament_add_gtest(test_node_interfaces__get_node_interfaces
+  node_interfaces/test_get_node_interfaces.cpp)
+if(TARGET test_node_interfaces__get_node_interfaces)
+  target_link_libraries(test_node_interfaces__get_node_interfaces ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_node_interfaces__node_base
+  node_interfaces/test_node_base.cpp)
+if(TARGET test_node_interfaces__node_base)
+  target_link_libraries(test_node_interfaces__node_base ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_node_interfaces__node_clock
+  node_interfaces/test_node_clock.cpp)
+if(TARGET test_node_interfaces__node_clock)
+  target_link_libraries(test_node_interfaces__node_clock ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_node_interfaces__node_graph
+  node_interfaces/test_node_graph.cpp
+  TIMEOUT 120)
+if(TARGET test_node_interfaces__node_graph)
+  ament_target_dependencies(
+    test_node_interfaces__node_graph
+    "test_msgs")
+  target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_node_interfaces__node_parameters
+  node_interfaces/test_node_parameters.cpp)
+if(TARGET test_node_interfaces__node_parameters)
+  target_link_libraries(test_node_interfaces__node_parameters ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_node_interfaces__node_services
+  node_interfaces/test_node_services.cpp)
+if(TARGET test_node_interfaces__node_services)
+  target_link_libraries(test_node_interfaces__node_services ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_node_interfaces__node_timers
+  node_interfaces/test_node_timers.cpp)
+if(TARGET test_node_interfaces__node_timers)
+  target_link_libraries(test_node_interfaces__node_timers ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_node_interfaces__node_topics
+  node_interfaces/test_node_topics.cpp)
+if(TARGET test_node_interfaces__node_topics)
+  ament_target_dependencies(
+    test_node_interfaces__node_topics
+    "test_msgs")
+  target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_node_interfaces__node_waitables
+  node_interfaces/test_node_waitables.cpp)
+if(TARGET test_node_interfaces__node_waitables)
+  target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME} mimick)
+endif()
+
+# TODO(wjwwood): reenable these build failure tests when I can get Jenkins to ignore their output
+# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_rclcpp_node
+#   node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp)
+# target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node
+#   ${PROJECT_NAME})
+
+# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
+#   node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp)
+# target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
+#   ${PROJECT_NAME})
+
+# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_wrapped_node
+#   node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp)
+# target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node
+#   ${PROJECT_NAME})
+
+# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_wrapped_node
+#   node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp)
+# target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
+#   ${PROJECT_NAME})
+
+ament_add_gtest(test_node_global_args test_node_global_args.cpp)
+if(TARGET test_node_global_args)
+  ament_target_dependencies(test_node_global_args
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+  target_link_libraries(test_node_global_args ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_node_options test_node_options.cpp)
+if(TARGET test_node_options)
+  ament_target_dependencies(test_node_options "rcl")
+  target_link_libraries(test_node_options ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_init_options test_init_options.cpp)
+if(TARGET test_init_options)
+  ament_target_dependencies(test_init_options "rcl")
+  target_link_libraries(test_init_options ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_parameter_client test_parameter_client.cpp)
+if(TARGET test_parameter_client)
+  ament_target_dependencies(test_parameter_client
+    "rcl_interfaces"
+  )
+  target_link_libraries(test_parameter_client ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_parameter_service test_parameter_service.cpp)
+if(TARGET test_parameter_service)
+  ament_target_dependencies(test_parameter_service
+    "rcl_interfaces"
+  )
+  target_link_libraries(test_parameter_service ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_parameter_events_filter test_parameter_events_filter.cpp)
+if(TARGET test_parameter_events_filter)
+  ament_target_dependencies(test_parameter_events_filter
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+  target_link_libraries(test_parameter_events_filter ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_parameter test_parameter.cpp)
+if(TARGET test_parameter)
+  ament_target_dependencies(test_parameter
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+  target_link_libraries(test_parameter ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_parameter_map test_parameter_map.cpp)
+if(TARGET test_parameter_map)
+  target_link_libraries(test_parameter_map ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_publisher test_publisher.cpp TIMEOUT 120)
+if(TARGET test_publisher)
+  ament_target_dependencies(test_publisher
+    "rcl"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "test_msgs"
+  )
+  target_link_libraries(test_publisher ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_publisher_subscription_count_api test_publisher_subscription_count_api.cpp)
+if(TARGET test_publisher_subscription_count_api)
+  ament_target_dependencies(test_publisher_subscription_count_api
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "test_msgs"
+  )
+  target_link_libraries(test_publisher_subscription_count_api ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_qos test_qos.cpp)
+if(TARGET test_qos)
+  ament_target_dependencies(test_qos
+    "rmw"
+  )
+  target_link_libraries(test_qos
+    ${PROJECT_NAME}
+  )
+endif()
+ament_add_gtest(test_qos_event test_qos_event.cpp)
+if(TARGET test_qos_event)
+  ament_target_dependencies(test_qos_event
+    "rmw"
+    "test_msgs"
+  )
+  target_link_libraries(test_qos_event
+    ${PROJECT_NAME}
+    mimick
+  )
+endif()
+ament_add_gtest(test_rate test_rate.cpp)
+if(TARGET test_rate)
+  ament_target_dependencies(test_rate
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+  target_link_libraries(test_rate
+    ${PROJECT_NAME}
+  )
+endif()
+ament_add_gtest(test_serialized_message_allocator test_serialized_message_allocator.cpp)
+if(TARGET test_serialized_message_allocator)
+  ament_target_dependencies(test_serialized_message_allocator
+    test_msgs
+  )
+  target_link_libraries(test_serialized_message_allocator
+    ${PROJECT_NAME}
+  )
+endif()
+ament_add_gtest(test_serialized_message test_serialized_message.cpp)
+if(TARGET test_serialized_message)
+  ament_target_dependencies(test_serialized_message
+    test_msgs
+  )
+  target_link_libraries(test_serialized_message
+    ${PROJECT_NAME}
+  )
+endif()
+ament_add_gtest(test_service test_service.cpp)
+if(TARGET test_service)
+  ament_target_dependencies(test_service
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "test_msgs"
+  )
+  target_link_libraries(test_service ${PROJECT_NAME} mimick)
+endif()
+# Creating and destroying nodes is slow with Connext, so this needs larger timeout.
+ament_add_gtest(test_subscription test_subscription.cpp TIMEOUT 120)
+if(TARGET test_subscription)
+  ament_target_dependencies(test_subscription
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "test_msgs"
+  )
+  target_link_libraries(test_subscription ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_subscription_publisher_count_api test_subscription_publisher_count_api.cpp)
+if(TARGET test_subscription_publisher_count_api)
+  ament_target_dependencies(test_subscription_publisher_count_api
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "test_msgs"
+  )
+  target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_subscription_traits test_subscription_traits.cpp)
+if(TARGET test_subscription_traits)
+  ament_target_dependencies(test_subscription_traits
+    "rcl"
+    "test_msgs"
+  )
+  target_link_libraries(test_subscription_traits ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_type_support 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 test_find_weak_nodes.cpp)
+if(TARGET test_find_weak_nodes)
+  ament_target_dependencies(test_find_weak_nodes
+    "rcl"
+  )
+  target_link_libraries(test_find_weak_nodes ${PROJECT_NAME})
+endif()
+
+set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}")
+if(WIN32)
+  set(append_library_dirs "${append_library_dirs}/$<CONFIG>")
+endif()
+
+ament_add_gtest(test_externally_defined_services test_externally_defined_services.cpp)
+ament_target_dependencies(test_externally_defined_services
+  "rcl"
+  "test_msgs"
+)
+target_link_libraries(test_externally_defined_services ${PROJECT_NAME})
+
+ament_add_gtest(test_duration test_duration.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_duration)
+  ament_target_dependencies(test_duration
+    "rcl")
+  target_link_libraries(test_duration ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_logger test_logger.cpp)
+target_link_libraries(test_logger ${PROJECT_NAME})
+
+ament_add_gmock(test_logging test_logging.cpp)
+target_link_libraries(test_logging ${PROJECT_NAME})
+
+ament_add_gtest(test_time test_time.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_time)
+  ament_target_dependencies(test_time
+    "rcl")
+  target_link_libraries(test_time ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_timer test_timer.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_timer)
+  ament_target_dependencies(test_timer
+    "rcl")
+  target_link_libraries(test_timer ${PROJECT_NAME} mimick)
+endif()
+
+ament_add_gtest(test_time_source test_time_source.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_time_source)
+  ament_target_dependencies(test_time_source
+    "rcl")
+  target_link_libraries(test_time_source ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_utilities test_utilities.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_utilities)
+  ament_target_dependencies(test_utilities
+    "rcl")
+  target_link_libraries(test_utilities ${PROJECT_NAME} mimick)
+endif()
+
+ament_add_gtest(test_init test_init.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_init)
+  ament_target_dependencies(test_init
+    "rcl")
+  target_link_libraries(test_init ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_interface_traits test_interface_traits.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_interface_traits)
+  ament_target_dependencies(test_interface_traits
+    "rcl")
+  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
+  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 executors/test_static_single_threaded_executor.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_static_single_threaded_executor)
+  ament_target_dependencies(test_static_single_threaded_executor
+    "test_msgs")
+  target_link_libraries(test_static_single_threaded_executor ${PROJECT_NAME} mimick)
+endif()
+
+ament_add_gtest(test_multi_threaded_executor executors/test_multi_threaded_executor.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_multi_threaded_executor)
+  ament_target_dependencies(test_multi_threaded_executor
+    "rcl")
+  target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_static_executor_entities_collector executors/test_static_executor_entities_collector.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120)
+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} mimick)
+endif()
+
+ament_add_gtest(test_guard_condition test_guard_condition.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_guard_condition)
+  target_link_libraries(test_guard_condition ${PROJECT_NAME} mimick)
+endif()
+
+ament_add_gtest(test_wait_set test_wait_set.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_wait_set)
+  ament_target_dependencies(test_wait_set "test_msgs")
+  target_link_libraries(test_wait_set ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_subscription_topic_statistics topic_statistics/test_subscription_topic_statistics.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}"
+)
+if(TARGET test_subscription_topic_statistics)
+  ament_target_dependencies(test_subscription_topic_statistics
+    "builtin_interfaces"
+    "libstatistics_collector"
+    "rcl_interfaces"
+    "rcutils"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "statistics_msgs"
+    "test_msgs")
+  rosidl_target_interfaces(test_subscription_topic_statistics ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp")
+  target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_subscription_options test_subscription_options.cpp)
+if(TARGET test_subscription_options)
+  ament_target_dependencies(test_subscription_options "rcl")
+  target_link_libraries(test_subscription_options ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_dynamic_storage 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 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 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 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_rosout_qos test_rosout_qos.cpp)
+if(TARGET test_rosout_qos)
+  ament_target_dependencies(test_rosout_qos "rcl")
+  target_link_libraries(test_rosout_qos ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_executor test_executor.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}"
+  TIMEOUT 120)
+if(TARGET test_executor)
+  ament_target_dependencies(test_executor "rcl")
+  target_link_libraries(test_executor ${PROJECT_NAME} mimick)
+endif()
+
+ament_add_gtest(test_graph_listener test_graph_listener.cpp)
+if(TARGET test_graph_listener)
+  target_link_libraries(test_graph_listener ${PROJECT_NAME} mimick)
+endif()

rclcpp/test/rclcpp/allocator/test_allocator_common.cpp

@@ -0,0 +1,67 @@
+// 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/allocator/allocator_common.hpp"
+
+TEST(TestAllocatorCommon, retyped_allocate) {
+  std::allocator<int> allocator;
+  void * untyped_allocator = &allocator;
+  void * allocated_mem =
+    rclcpp::allocator::retyped_allocate<std::allocator<char>>(1u, untyped_allocator);
+  ASSERT_NE(nullptr, allocated_mem);
+
+  auto code = [&untyped_allocator, allocated_mem]() {
+      rclcpp::allocator::retyped_deallocate<int, std::allocator<int>>(
+        allocated_mem, untyped_allocator);
+    };
+  EXPECT_NO_THROW(code());
+
+  allocated_mem = allocator.allocate(1);
+  ASSERT_NE(nullptr, allocated_mem);
+  void * reallocated_mem =
+    rclcpp::allocator::retyped_reallocate<int, std::allocator<int>>(
+    allocated_mem, 2u, untyped_allocator);
+  ASSERT_NE(nullptr, reallocated_mem);
+
+  auto code2 = [&untyped_allocator, reallocated_mem]() {
+      rclcpp::allocator::retyped_deallocate<int, std::allocator<int>>(
+        reallocated_mem, untyped_allocator);
+    };
+  EXPECT_NO_THROW(code2());
+}
+
+TEST(TestAllocatorCommon, get_rcl_allocator) {
+  std::allocator<int> allocator;
+  auto rcl_allocator = rclcpp::allocator::get_rcl_allocator<int>(allocator);
+  EXPECT_NE(nullptr, rcl_allocator.allocate);
+  EXPECT_NE(nullptr, rcl_allocator.deallocate);
+  EXPECT_NE(nullptr, rcl_allocator.reallocate);
+  EXPECT_NE(nullptr, rcl_allocator.zero_allocate);
+  // Not testing state as that may or may not be null depending on platform
+}
+
+TEST(TestAllocatorCommon, get_void_rcl_allocator) {
+  std::allocator<void> allocator;
+  auto rcl_allocator =
+    rclcpp::allocator::get_rcl_allocator<void, std::allocator<void>>(allocator);
+  EXPECT_NE(nullptr, rcl_allocator.allocate);
+  EXPECT_NE(nullptr, rcl_allocator.deallocate);
+  EXPECT_NE(nullptr, rcl_allocator.reallocate);
+  EXPECT_NE(nullptr, rcl_allocator.zero_allocate);
+  // Not testing state as that may or may not be null depending on platform
+}

rclcpp/test/rclcpp/allocator/test_allocator_deleter.cpp

@@ -0,0 +1,101 @@
+// 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/allocator/allocator_deleter.hpp"
+
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+TEST(TestAllocatorDeleter, construct_destruct) {
+  std::allocator<int> allocator;
+
+  rclcpp::allocator::AllocatorDeleter<std::allocator<int>> deleter;
+  EXPECT_EQ(nullptr, deleter.get_allocator());
+  deleter.set_allocator(&allocator);
+  EXPECT_EQ(&allocator, deleter.get_allocator());
+
+  rclcpp::allocator::AllocatorDeleter<std::allocator<int>> deleter2(&allocator);
+  EXPECT_EQ(&allocator, deleter2.get_allocator());
+
+  rclcpp::allocator::AllocatorDeleter<std::allocator<int>> deleter3(deleter2);
+  EXPECT_EQ(&allocator, deleter3.get_allocator());
+}
+
+TEST(TestAllocatorDeleter, delete) {
+  std::allocator<int> allocator;
+  int * some_mem = allocator.allocate(1u);
+  ASSERT_NE(nullptr, some_mem);
+
+  rclcpp::allocator::AllocatorDeleter<std::allocator<int>> deleter(&allocator);
+  EXPECT_NO_THROW(deleter(some_mem));
+}
+
+TEST(TestAllocatorDeleter, set_allocator_for_deleter_AllocatorDeleter) {
+  using AllocatorT = std::allocator<int>;
+  using DeleterT = rclcpp::allocator::AllocatorDeleter<AllocatorT>;
+  AllocatorT allocator;
+  DeleterT deleter(&allocator);
+
+  std::allocator<int> allocator2;
+  rclcpp::allocator::set_allocator_for_deleter<AllocatorT, AllocatorT>(&deleter, &allocator2);
+  EXPECT_EQ(&allocator2, deleter.get_allocator());
+
+  auto throwing_statement = [&allocator]() {
+      DeleterT * null_del_ptr = nullptr;
+      rclcpp::allocator::set_allocator_for_deleter<AllocatorT, AllocatorT>(
+        null_del_ptr, &allocator);
+    };
+  RCLCPP_EXPECT_THROW_EQ(
+    throwing_statement(),
+    std::invalid_argument("Argument was NULL to set_allocator_for_deleter"));
+
+  auto throwing_statement2 = [&deleter]() {
+      AllocatorT * null_alloc_ptr = nullptr;
+      rclcpp::allocator::set_allocator_for_deleter<AllocatorT, AllocatorT>(
+        &deleter, null_alloc_ptr);
+    };
+
+  RCLCPP_EXPECT_THROW_EQ(
+    throwing_statement2(),
+    std::invalid_argument("Argument was NULL to set_allocator_for_deleter"));
+}
+
+TEST(TestAllocatorDeleter, set_allocator_for_deleter_std_default_delete) {
+  using AllocatorT = std::allocator<int>;
+  using DeleterT = std::default_delete<int>;
+  auto not_throwing_statement = []() {
+      AllocatorT allocator;
+      DeleterT deleter;
+      rclcpp::allocator::set_allocator_for_deleter<int, int>(&deleter, &allocator);
+    };
+  EXPECT_NO_THROW(not_throwing_statement());
+}
+
+TEST(TestAllocatorDeleter, set_allocator_for_deleter_unexpected_template) {
+  class SomeAllocatorClass {};
+  class SomeDeleterClass {};
+  using AllocatorT = SomeAllocatorClass;
+  using DeleterT = SomeDeleterClass;
+  auto throwing_statement = []() {
+      DeleterT deleter;
+      AllocatorT allocator;
+      rclcpp::allocator::set_allocator_for_deleter<AllocatorT, int, DeleterT>(&deleter, &allocator);
+    };
+  RCLCPP_EXPECT_THROW_EQ(
+    throwing_statement(),
+    std::runtime_error("Reached unexpected template specialization"));
+}

rclcpp/test/rclcpp/exceptions/test_exceptions.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 "rclcpp/exceptions/exceptions.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+TEST(TestExceptions, throw_from_rcl_error) {
+  EXPECT_THROW(
+    rclcpp::exceptions::throw_from_rcl_error(RCL_RET_BAD_ALLOC, ""),
+    rclcpp::exceptions::RCLBadAlloc);
+
+  EXPECT_THROW(
+    rclcpp::exceptions::throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, ""),
+    rclcpp::exceptions::RCLInvalidArgument);
+
+  EXPECT_THROW(
+    rclcpp::exceptions::throw_from_rcl_error(RCL_RET_INVALID_ROS_ARGS, ""),
+    rclcpp::exceptions::RCLInvalidROSArgsError);
+
+  EXPECT_THROW(
+    rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, ""),
+    rclcpp::exceptions::RCLError);
+
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::exceptions::throw_from_rcl_error(RCL_RET_OK, ""),
+    std::invalid_argument("ret is RCL_RET_OK"));
+
+  {
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_get_error_state, nullptr);
+    RCLCPP_EXPECT_THROW_EQ(
+      rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, ""),
+      std::runtime_error("rcl error state is not set"));
+  }
+}
+
+TEST(TestExceptions, basic_constructors) {
+  EXPECT_STREQ("node is invalid", rclcpp::exceptions::InvalidNodeError().what());
+  rcl_error_state_t error_state{"error", __FILE__, __LINE__};
+  EXPECT_STREQ(
+    "prefix: error not set",
+    rclcpp::exceptions::RCLInvalidROSArgsError(RCL_RET_ERROR, &error_state, "prefix: ").what());
+}

rclcpp/test/rclcpp/executors/test_executors.cpp

@@ -0,0 +1,605 @@
+// 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_SUITE is deprecated as of gtest 1.9, use TYPED_TEST_SUITE when gtest dependency
+// is updated.
+TYPED_TEST_SUITE(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_SUITE(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);
+    }
+  }
+
+  ~TestWaitable()
+  {
+    rcl_ret_t ret = rcl_guard_condition_fini(&gc_);
+    if (RCL_RET_OK != ret) {
+      fprintf(stderr, "failed to call rcl_guard_condition_fini\n");
+    }
+  }
+
+  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();
+}
+
+// Check spin_node_until_future_complete with node base pointer
+TYPED_TEST(TestExecutorsStable, testSpinNodeUntilFutureCompleteNodeBasePtr) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+
+  std::promise<bool> promise;
+  std::future<bool> future = promise.get_future();
+  promise.set_value(true);
+
+  auto shared_future = future.share();
+  auto ret = rclcpp::executors::spin_node_until_future_complete(
+    executor, this->node->get_node_base_interface(), shared_future, 1s);
+  EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret);
+}
+
+// Check spin_node_until_future_complete with node pointer
+TYPED_TEST(TestExecutorsStable, testSpinNodeUntilFutureCompleteNodePtr) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+
+  std::promise<bool> promise;
+  std::future<bool> future = promise.get_future();
+  promise.set_value(true);
+
+  auto shared_future = future.share();
+  auto ret = rclcpp::executors::spin_node_until_future_complete(
+    executor, this->node, shared_future, 1s);
+  EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret);
+}
+
+// Check spin_until_future_complete with node base pointer (instantiates its own executor)
+TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) {
+  rclcpp::init(0, nullptr);
+
+  {
+    auto node = std::make_shared<rclcpp::Node>("node");
+
+    std::promise<bool> promise;
+    std::future<bool> future = promise.get_future();
+    promise.set_value(true);
+
+    auto shared_future = future.share();
+    auto ret = rclcpp::spin_until_future_complete(
+      node->get_node_base_interface(), shared_future, 1s);
+    EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret);
+  }
+
+  rclcpp::shutdown();
+}
+
+// Check spin_until_future_complete with node pointer (instantiates its own executor)
+TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr) {
+  rclcpp::init(0, nullptr);
+
+  {
+    auto node = std::make_shared<rclcpp::Node>("node");
+
+    std::promise<bool> promise;
+    std::future<bool> future = promise.get_future();
+    promise.set_value(true);
+
+    auto shared_future = future.share();
+    auto ret = rclcpp::spin_until_future_complete(node, shared_future, 1s);
+    EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret);
+  }
+
+  rclcpp::shutdown();
+}

rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp

@@ -0,0 +1,622 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <memory>
+#include <stdexcept>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "test_msgs/msg/empty.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+namespace
+{
+
+struct NumberOfEntities
+{
+  size_t subscriptions = 0;
+  size_t timers = 0;
+  size_t services = 0;
+  size_t clients = 0;
+  size_t waitables = 0;
+};
+
+std::unique_ptr<NumberOfEntities> get_number_of_default_entities(rclcpp::Node::SharedPtr node)
+{
+  auto number_of_entities = std::make_unique<NumberOfEntities>();
+  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
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->add_node(node1->get_node_base_interface()),
+    std::runtime_error("Node has already been added to an executor."));
+
+  auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
+  EXPECT_FALSE(entities_collector_->remove_node(node2->get_node_base_interface()));
+  EXPECT_NO_THROW(entities_collector_->add_node(node2->get_node_base_interface()));
+
+  EXPECT_TRUE(entities_collector_->remove_node(node1->get_node_base_interface()));
+  EXPECT_FALSE(entities_collector_->remove_node(node1->get_node_base_interface()));
+  EXPECT_TRUE(entities_collector_->remove_node(node2->get_node_base_interface()));
+
+  auto node3 = std::make_shared<rclcpp::Node>("node3", "ns");
+  node3->get_node_base_interface()->get_associated_with_executor_atomic().exchange(true);
+  EXPECT_FALSE(entities_collector_->remove_node(node3->get_node_base_interface()));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, init_bad_arguments) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  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);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+  EXPECT_EQ(
+    expected_number_of_entities->subscriptions,
+    entities_collector_->get_number_of_subscriptions());
+  EXPECT_EQ(expected_number_of_entities->timers, entities_collector_->get_number_of_timers());
+  EXPECT_EQ(expected_number_of_entities->services, entities_collector_->get_number_of_services());
+  EXPECT_EQ(expected_number_of_entities->clients, entities_collector_->get_number_of_clients());
+  // One extra for the executor
+  EXPECT_EQ(
+    1u + expected_number_of_entities->waitables,
+    entities_collector_->get_number_of_waitables());
+
+  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
+  entities_collector_->init(&wait_set, memory_strategy, &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);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_timers());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_services());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_clients());
+
+  // Still one for the executor
+  EXPECT_EQ(1u, entities_collector_->get_number_of_waitables());
+}
+
+class TestWaitable : public rclcpp::Waitable
+{
+public:
+  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);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+
+  EXPECT_EQ(
+    1u + expected_number_of_entities->subscriptions,
+    entities_collector_->get_number_of_subscriptions());
+  EXPECT_EQ(1u + expected_number_of_entities->timers, entities_collector_->get_number_of_timers());
+  EXPECT_EQ(
+    1u + expected_number_of_entities->services,
+    entities_collector_->get_number_of_services());
+  EXPECT_EQ(
+    1u + expected_number_of_entities->clients,
+    entities_collector_->get_number_of_clients());
+
+  // One extra for the executor
+  EXPECT_EQ(
+    2u + expected_number_of_entities->waitables,
+    entities_collector_->get_number_of_waitables());
+
+  entities_collector_->remove_node(node->get_node_base_interface());
+  entities_collector_->init(&wait_set, memory_strategy, &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_callback_group) {
+  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
+  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, add_callback_group_after_add_node) {
+  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_node(node->get_node_base_interface());
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->add_callback_group(cb_group, node->get_node_base_interface()),
+    std::runtime_error("Callback group has already been added to an executor."));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, add_callback_group_twice) {
+  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
+  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
+  cb_group->get_associated_with_executor_atomic().exchange(false);
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->add_callback_group(cb_group, node->get_node_base_interface()),
+    std::runtime_error("Callback group was already added to executor."));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_clear_error) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  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);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+
+  {
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      entities_collector_->execute(),
+      std::runtime_error("Couldn't clear wait set"));
+  }
+
+  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_resize_error) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  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);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+
+  {
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_resize, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      entities_collector_->execute(),
+      std::runtime_error("Couldn't resize the wait set: error not set"));
+  }
+
+  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_not_initialized) {
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->refresh_wait_set(std::chrono::nanoseconds(1000)),
+    std::runtime_error("Couldn't clear wait set"));
+  rcl_reset_error();
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_rcl_wait_failed) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  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);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+
+  {
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      entities_collector_->refresh_wait_set(std::chrono::nanoseconds(1000)),
+      std::runtime_error("rcl_wait() failed: error not set"));
+  }
+
+  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_add_handles_to_wait_set_failed) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+
+  // Create 1 of each entity type
+  auto subscription =
+    node->create_subscription<test_msgs::msg::Empty>(
+    "topic", rclcpp::QoS(10), [](test_msgs::msg::Empty::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>();
+
+  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);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_wait_set_add_subscription,
+      RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      entities_collector_->refresh_wait_set(std::chrono::nanoseconds(1000)),
+      std::runtime_error("Couldn't fill wait set"));
+  }
+
+  entities_collector_->remove_node(node->get_node_base_interface());
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, add_to_wait_set_nullptr) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  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);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->add_to_wait_set(nullptr),
+    std::runtime_error("Executor waitable: couldn't add guard condition to wait set"));
+  rcl_reset_error();
+
+  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, fill_memory_strategy_invalid_group) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  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();
+
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
+  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 2u);
+
+  cb_group.reset();
+
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
+
+  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, remove_callback_group_after_node) {
+  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
+  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
+
+  node.reset();
+
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->remove_callback_group(cb_group),
+    std::runtime_error("Node must not be deleted before its callback group(s)."));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, remove_callback_group_twice) {
+  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
+  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
+
+  entities_collector_->remove_callback_group(cb_group);
+
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->remove_callback_group(cb_group),
+    std::runtime_error("Callback group needs to be associated with executor."));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, remove_node_opposite_order) {
+  auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
+  EXPECT_NO_THROW(entities_collector_->add_node(node1->get_node_base_interface()));
+
+  auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
+  EXPECT_NO_THROW(entities_collector_->add_node(node2->get_node_base_interface()));
+
+  EXPECT_TRUE(entities_collector_->remove_node(node2->get_node_base_interface()));
+}
+
+TEST_F(
+  TestStaticExecutorEntitiesCollector,
+  add_callback_groups_from_nodes_associated_to_executor_add) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  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);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+
+  cb_group->get_associated_with_executor_atomic().exchange(false);
+  entities_collector_->execute();
+
+  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
+}

rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp

@@ -0,0 +1,152 @@
+// 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 <future>
+#include <memory>
+#include <stdexcept>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/executors.hpp"
+
+#include "test_msgs/srv/empty.hpp"
+
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.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);
+}
+
+TEST_F(TestStaticSingleThreadedExecutor, add_callback_group_trigger_guard_failed) {
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      executor.add_callback_group(cb_group, node->get_node_base_interface(), true),
+      std::runtime_error("error not set"));
+  }
+}
+
+TEST_F(TestStaticSingleThreadedExecutor, add_node_trigger_guard_failed) {
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      executor.add_node(node),
+      std::runtime_error("error not set"));
+  }
+}
+
+TEST_F(TestStaticSingleThreadedExecutor, remove_callback_group_trigger_guard_failed) {
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  executor.add_callback_group(cb_group, node->get_node_base_interface(), true);
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      executor.remove_callback_group(cb_group, true),
+      std::runtime_error("error not set"));
+  }
+}
+
+TEST_F(TestStaticSingleThreadedExecutor, remove_node_failed) {
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      executor.remove_node(node, true),
+      std::runtime_error("Node needs to be associated with this executor."));
+  }
+}
+
+TEST_F(TestStaticSingleThreadedExecutor, remove_node_trigger_guard_failed) {
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+
+  executor.add_node(node);
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      executor.remove_node(node, true),
+      std::runtime_error("error not set"));
+  }
+}
+
+TEST_F(TestStaticSingleThreadedExecutor, execute_service) {
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  executor.add_node(node);
+
+  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");
+
+  std::promise<void> promise;
+  std::future<void> future = promise.get_future();
+  EXPECT_EQ(
+    rclcpp::FutureReturnCode::TIMEOUT,
+    executor.spin_until_future_complete(future, std::chrono::milliseconds(1)));
+
+  executor.remove_node(node);
+  executor.spin_until_future_complete(future, std::chrono::milliseconds(1));
+}

rclcpp/test/rclcpp/node_interfaces/test_node_base.cpp

@@ -21,6 +21,11 @@
 #include "rclcpp/node.hpp"
 #include "rclcpp/node_interfaces/node_base.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "rmw/validate_namespace.h"
+#include "rmw/validate_node_name.h"
+
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
 
 class TestNodeBase : public ::testing::Test
 {
@@ -36,6 +41,12 @@ public:
   }
 };
 
+// Required for mocking_utils below
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, >)
+
 TEST_F(TestNodeBase, construct_from_node)
 {
   std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
@@ -58,3 +69,125 @@ TEST_F(TestNodeBase, construct_from_node)
   EXPECT_NE(nullptr, const_node_base->get_rcl_node_handle());
   EXPECT_NE(nullptr, const_node_base->get_shared_rcl_node_handle());
 }
+
+TEST_F(TestNodeBase, construct_destruct_rcl_guard_condition_init_error) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_guard_condition_init, RCL_RET_ERROR);
+  EXPECT_THROW(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_error) {
+  auto mock_node_init = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_node_init, RCL_RET_ERROR);
+
+  // This function is called only if rcl_node_init fails, so both mocked functions are required
+  // This just logs an error, so behavior shouldn't change
+  auto mock_guard_condition_fini = mocking_utils::inject_on_return(
+    "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR);
+
+  EXPECT_THROW(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_node_name) {
+  auto mock_node_init = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAME);
+
+  // `rmw_validate_node_name` is only called if `rcl_node_init` returns INVALID_NAME
+  auto mock_validate_node_name = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_node_name, RMW_RET_ERROR);
+
+  EXPECT_THROW(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_node_name_invalid_argument) {
+  auto mock_node_init = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAME);
+
+  // `rmw_validate_node_name` is only called if `rcl_node_init` returns INVALID_NAME
+  auto mock_validate_node_name = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_node_name, RMW_RET_INVALID_ARGUMENT);
+
+  EXPECT_THROW(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    rclcpp::exceptions::RCLInvalidArgument);
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_node_name_valid_rmw_node_name) {
+  auto mock_node_init = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAME);
+
+  // `rmw_validate_node_name` is only called if `rcl_node_init` returns INVALID_NAME
+  auto mock = mocking_utils::patch(
+    "lib:rclcpp", rmw_validate_node_name, [](const char *, int * validation_result, size_t *)
+    {
+      *validation_result = RMW_NODE_NAME_VALID;
+      return RMW_RET_OK;
+    });
+
+  RCLCPP_EXPECT_THROW_EQ(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    std::runtime_error("valid rmw node name but invalid rcl node name"));
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_namespace) {
+  auto mock_node_init = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAMESPACE);
+
+  // `rmw_validate_namespace` is only called if `rcl_node_init` returns INVALID_NAMESPACE
+  auto mock_validate_namespace = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_namespace, RMW_RET_ERROR);
+
+  EXPECT_THROW(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_namespace_rmw_invalid_argument) {
+  auto mock_node_init = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAMESPACE);
+
+  // `rmw_validate_namespace` is only called if `rcl_node_init` returns INVALID_NAMESPACE
+  auto mock_validate_namespace = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_namespace, RMW_RET_INVALID_ARGUMENT);
+
+  EXPECT_THROW(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    rclcpp::exceptions::RCLInvalidArgument);
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_namespace_valid_rmw_namespace) {
+  auto mock_node_init = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAMESPACE);
+
+  // `rmw_validate_namespace` is only called if `rcl_node_init` returns INVALID_NAMESPACE
+  auto mock = mocking_utils::patch(
+    "lib:rclcpp", rmw_validate_namespace, [](const char *, int * validation_result, size_t *)
+    {
+      *validation_result = RMW_NAMESPACE_VALID;
+      return RMW_RET_OK;
+    });
+
+  RCLCPP_EXPECT_THROW_EQ(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    std::runtime_error("valid rmw node namespace but invalid rcl node namespace"));
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_fini_error) {
+  auto mock_node_fini = mocking_utils::inject_on_return(
+    "lib:rclcpp", rcl_node_fini, RCL_RET_ERROR);
+
+  EXPECT_NO_THROW(std::make_shared<rclcpp::Node>("node", "ns").reset());
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_guard_condition_fini_error) {
+  auto mock_node_fini = mocking_utils::inject_on_return(
+    "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR);
+
+  EXPECT_NO_THROW(std::make_shared<rclcpp::Node>("node", "ns").reset());
+}

rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp

@@ -15,14 +15,31 @@
 #include <gtest/gtest.h>
 
 #include <memory>
+#include <string>
 #include <utility>
 
+#include "rcl/graph.h"
 #include "rcl/node_options.h"
+#include "rcl/remap.h"
 #include "rclcpp/node.hpp"
 #include "rclcpp/node_interfaces/node_base.hpp"
 #include "rclcpp/node_interfaces/node_graph.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "rcutils/strdup.h"
 #include "test_msgs/msg/empty.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+namespace
+{
+
+constexpr char node_name[] = "node";
+constexpr char node_namespace[] = "ns";
+constexpr char absolute_namespace[] = "/ns";
+
+}  // namespace
 
 class TestNodeGraph : public ::testing::Test
 {
@@ -30,150 +47,305 @@ public:
   void SetUp()
   {
     rclcpp::init(0, nullptr);
+    node_ = std::make_shared<rclcpp::Node>(node_name, node_namespace);
+
+    // This dynamic cast is not necessary for the unittests, but instead is used to ensure
+    // the proper type is being tested and covered.
+    node_graph_ =
+      dynamic_cast<rclcpp::node_interfaces::NodeGraph *>(node_->get_node_graph_interface().get());
+    ASSERT_NE(nullptr, node_graph_);
   }
 
   void TearDown()
   {
     rclcpp::shutdown();
   }
+
+protected:
+  std::shared_ptr<rclcpp::Node> node() {return node_;}
+
+  const rclcpp::node_interfaces::NodeGraph * node_graph() const {return node_graph_;}
+
+private:
+  std::shared_ptr<rclcpp::Node> node_;
+  rclcpp::node_interfaces::NodeGraph * node_graph_;
 };
 
 TEST_F(TestNodeGraph, construct_from_node)
 {
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
-
-  // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
-  // the proper type is being tested and covered.
-  const auto * node_graph =
-    dynamic_cast<rclcpp::node_interfaces::NodeGraph *>(node->get_node_graph_interface().get());
-  ASSERT_NE(nullptr, node_graph);
-
-  auto topic_names_and_types = node_graph->get_topic_names_and_types(false);
+  auto topic_names_and_types = node_graph()->get_topic_names_and_types(false);
   EXPECT_LT(0u, topic_names_and_types.size());
 
-  auto service_names_and_types = node_graph->get_service_names_and_types();
+  auto service_names_and_types = node_graph()->get_service_names_and_types();
   EXPECT_LT(0u, service_names_and_types.size());
 
-  auto names = node_graph->get_node_names();
+  auto names = node_graph()->get_node_names();
   EXPECT_EQ(1u, names.size());
 
-  auto names_and_namespaces = node_graph->get_node_names_and_namespaces();
+  auto names_and_namespaces = node_graph()->get_node_names_and_namespaces();
   EXPECT_EQ(1u, names_and_namespaces.size());
 
-  EXPECT_EQ(0u, node_graph->count_publishers("not_a_topic"));
-  EXPECT_EQ(0u, node_graph->count_subscribers("not_a_topic"));
+  EXPECT_EQ(0u, node_graph()->count_publishers("not_a_topic"));
+  EXPECT_EQ(0u, node_graph()->count_subscribers("not_a_topic"));
+  EXPECT_NE(nullptr, node_graph()->get_graph_guard_condition());
+
+  // get_graph_event is non-const
+  EXPECT_NE(nullptr, node()->get_node_graph_interface()->get_graph_event());
+  EXPECT_EQ(1u, node_graph()->count_graph_users());
 }
 
 TEST_F(TestNodeGraph, get_topic_names_and_types)
 {
-  auto node = std::make_shared<rclcpp::Node>("node2", "ns");
-  const auto * node_graph =
-    dynamic_cast<rclcpp::node_interfaces::NodeGraph *>(node->get_node_graph_interface().get());
-  ASSERT_NE(nullptr, node_graph);
-  auto topic_names_and_types = node_graph->get_topic_names_and_types();
+  auto topic_names_and_types = node_graph()->get_topic_names_and_types();
   EXPECT_LT(0u, topic_names_and_types.size());
 }
 
+TEST_F(TestNodeGraph, get_topic_names_and_types_rcl_error)
+{
+  auto mock_get_topic_names = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_topic_names_and_types, RCL_RET_ERROR);
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
+
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_topic_names_and_types(),
+    std::runtime_error(
+      "failed to get topic names and types: error not set, failed also to cleanup topic names and"
+      " types, leaking memory: error not set"));
+}
+
+TEST_F(TestNodeGraph, get_topic_names_and_types_rcl_names_and_types_fini_error)
+{
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
+
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_topic_names_and_types(),
+    std::runtime_error("could not destroy topic names and types: error not set"));
+}
+
 TEST_F(TestNodeGraph, get_service_names_and_types)
 {
-  auto node = std::make_shared<rclcpp::Node>("node2", "ns");
-  const auto * node_graph =
-    dynamic_cast<rclcpp::node_interfaces::NodeGraph *>(node->get_node_graph_interface().get());
-  ASSERT_NE(nullptr, node_graph);
-  auto service_names_and_types = node_graph->get_service_names_and_types();
+  auto service_names_and_types = node_graph()->get_service_names_and_types();
   EXPECT_LT(0u, service_names_and_types.size());
 }
 
+TEST_F(TestNodeGraph, get_service_names_and_types_rcl_error)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_service_names_and_types, RCL_RET_ERROR);
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_service_names_and_types(),
+    std::runtime_error(
+      "failed to get service names and types: error not set, failed also to cleanup service names"
+      " and types, leaking memory: error not set"));
+}
+
+TEST_F(TestNodeGraph, get_service_names_and_types_rcl_names_and_types_fini)
+{
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_service_names_and_types(),
+    std::runtime_error("could not destroy service names and types: error not set"));
+}
+
 TEST_F(TestNodeGraph, get_service_names_and_types_by_node)
 {
-  auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
-  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);
+  auto callback = [](
+    const test_msgs::srv::Empty::Request::SharedPtr,
+    test_msgs::srv::Empty::Response::SharedPtr) {};
+  auto service =
+    node()->create_service<test_msgs::srv::Empty>("node1_service", std::move(callback));
+
+  const std::string node2_name = "node2";
+  auto node2 = std::make_shared<rclcpp::Node>(node2_name, node_namespace);
+
+  // 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);
+
+  // 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(node_name, absolute_namespace);
+  auto services_of_node2 =
+    node_graph()->get_service_names_and_types_by_node(node2_name, absolute_namespace);
+
+  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(node_name, absolute_namespace);
+    services_of_node2 =
+      node_graph()->get_service_names_and_types_by_node(node2_name, absolute_namespace);
+    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_service_names_and_types_by_node_rcl_errors)
+{
+  auto callback = [](
+    const test_msgs::srv::Empty::Request::SharedPtr,
+    test_msgs::srv::Empty::Response::SharedPtr) {};
+  auto service =
+    node()->create_service<test_msgs::srv::Empty>("node1_service", std::move(callback));
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_service_names_and_types_by_node, RCL_RET_ERROR);
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_service_names_and_types_by_node(node_name, node_namespace),
+    std::runtime_error(
+      "failed to get service names and types by node: error not set, failed also to cleanup"
+      " service names and types, leaking memory: error not set"));
+}
+
+TEST_F(TestNodeGraph, get_service_names_and_types_by_node_names_and_types_fini_error)
+{
+  auto callback = [](
+    const test_msgs::srv::Empty::Request::SharedPtr,
+    test_msgs::srv::Empty::Response::SharedPtr) {};
+  auto service =
+    node()->create_service<test_msgs::srv::Empty>("node1_service", std::move(callback));
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
 
   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());
+    node_graph()->get_service_names_and_types_by_node(node_name, absolute_namespace),
+    rclcpp::exceptions::RCLError);
 }
 
 TEST_F(TestNodeGraph, get_node_names_and_namespaces)
 {
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  const auto * node_graph =
-    dynamic_cast<rclcpp::node_interfaces::NodeGraph *>(node->get_node_graph_interface().get());
-  ASSERT_NE(nullptr, node_graph);
-
-  auto names_and_namespaces = node_graph->get_node_names_and_namespaces();
+  auto names_and_namespaces = node_graph()->get_node_names_and_namespaces();
   EXPECT_EQ(1u, names_and_namespaces.size());
 }
 
+TEST_F(TestNodeGraph, get_node_names_and_namespaces_rcl_errors)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_node_names, RCL_RET_ERROR);
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcutils_string_array_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_node_names_and_namespaces(),
+    std::runtime_error(
+      "failed to get node names: error not set, failed also to cleanup node names, leaking memory:"
+      " error not set, failed also to cleanup node namespaces, leaking memory: error not set"));
+}
+
+TEST_F(TestNodeGraph, get_node_names_and_namespaces_fini_errors)
+{
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcutils_string_array_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_node_names_and_namespaces(),
+    std::runtime_error("could not destroy node names, could not destroy node namespaces"));
+}
+
+TEST_F(TestNodeGraph, count_publishers_rcl_error)
+{
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_count_publishers, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->count_publishers("topic"),
+    std::runtime_error("could not count publishers: error not set"));
+}
+
+TEST_F(TestNodeGraph, count_subscribers_rcl_error)
+{
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_count_subscribers, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->count_subscribers("topic"),
+    std::runtime_error("could not count subscribers: error not set"));
+}
+
 TEST_F(TestNodeGraph, notify_shutdown)
 {
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  auto * node_graph =
-    dynamic_cast<rclcpp::node_interfaces::NodeGraph *>(node->get_node_graph_interface().get());
-  ASSERT_NE(nullptr, node_graph);
-
-  EXPECT_NO_THROW(node_graph->notify_shutdown());
+  EXPECT_NO_THROW(node()->get_node_graph_interface()->notify_shutdown());
 }
 
 TEST_F(TestNodeGraph, wait_for_graph_change)
 {
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
-  auto * node_graph =
-    dynamic_cast<rclcpp::node_interfaces::NodeGraph *>(node->get_node_graph_interface().get());
-  ASSERT_NE(nullptr, node_graph);
-
-  EXPECT_NO_THROW(node_graph->notify_graph_change());
+  auto node_graph_interface = node()->get_node_graph_interface();
+  EXPECT_NO_THROW(node_graph_interface->notify_graph_change());
   EXPECT_THROW(
-    node_graph->wait_for_graph_change(nullptr, std::chrono::milliseconds(1)),
+    node_graph_interface->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)),
+    node_graph_interface->wait_for_graph_change(event, std::chrono::milliseconds(0)),
     rclcpp::exceptions::EventNotRegisteredError);
 }
 
+TEST_F(TestNodeGraph, notify_graph_change_rcl_error)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  EXPECT_THROW(
+    node()->get_node_graph_interface()->notify_graph_change(),
+    rclcpp::exceptions::RCLError);
+}
+
 TEST_F(TestNodeGraph, get_info_by_topic)
 {
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
   const rclcpp::QoS publisher_qos(1);
-  auto publisher = node->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
   auto callback = [](const test_msgs::msg::Empty::SharedPtr) {};
 
   const rclcpp::QoS subscriber_qos(10);
   auto subscription =
-    node->create_subscription<test_msgs::msg::Empty>(
+    node()->create_subscription<test_msgs::msg::Empty>(
     "topic", subscriber_qos, std::move(callback));
 
-  const auto * node_graph =
-    dynamic_cast<rclcpp::node_interfaces::NodeGraph *>(node->get_node_graph_interface().get());
-  ASSERT_NE(nullptr, node_graph);
+  EXPECT_EQ(0u, node_graph()->get_publishers_info_by_topic("topic", true).size());
 
-  auto publishers = node_graph->get_publishers_info_by_topic("topic", false);
+  auto publishers = node_graph()->get_publishers_info_by_topic("topic", false);
   ASSERT_EQ(1u, publishers.size());
 
   auto publisher_endpoint_info = publishers[0];
   const auto const_publisher_endpoint_info = publisher_endpoint_info;
-  EXPECT_STREQ("node", publisher_endpoint_info.node_name().c_str());
-  EXPECT_STREQ("node", const_publisher_endpoint_info.node_name().c_str());
-  EXPECT_STREQ("/ns", publisher_endpoint_info.node_namespace().c_str());
-  EXPECT_STREQ("/ns", const_publisher_endpoint_info.node_namespace().c_str());
+  EXPECT_STREQ(node_name, publisher_endpoint_info.node_name().c_str());
+  EXPECT_STREQ(node_name, const_publisher_endpoint_info.node_name().c_str());
+  EXPECT_STREQ(absolute_namespace, publisher_endpoint_info.node_namespace().c_str());
+  EXPECT_STREQ(absolute_namespace, const_publisher_endpoint_info.node_namespace().c_str());
   EXPECT_STREQ("test_msgs/msg/Empty", publisher_endpoint_info.topic_type().c_str());
   EXPECT_STREQ("test_msgs/msg/Empty", const_publisher_endpoint_info.topic_type().c_str());
   EXPECT_EQ(rclcpp::EndpointType::Publisher, publisher_endpoint_info.endpoint_type());
   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();
@@ -184,3 +356,90 @@ TEST_F(TestNodeGraph, get_info_by_topic)
   }
   EXPECT_FALSE(endpoint_gid_is_all_zeros);
 }
+
+TEST_F(TestNodeGraph, get_info_by_topic_rcl_node_get_options_error)
+{
+  const rclcpp::QoS publisher_qos(1);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_node_get_options, nullptr);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_publishers_info_by_topic("topic", false),
+    std::runtime_error("Need valid node options in get_info_by_topic()"));
+}
+
+// Required for mocking_utils below
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >)
+
+TEST_F(TestNodeGraph, get_info_by_topic_rcl_remap_topic_name_error)
+{
+  const rclcpp::QoS publisher_qos(1);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_remap_topic_name, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_publishers_info_by_topic("topic", false),
+    std::runtime_error("Failed to remap topic name /ns/topic: error not set"));
+}
+
+TEST_F(TestNodeGraph, get_info_by_topic_rcl_remap_topic_name_nullptr)
+{
+  const rclcpp::QoS publisher_qos(1);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+
+  // Should be cleaned up by get_info_by_topic
+  char * some_string = rcutils_strdup("", rcl_get_default_allocator());
+  ASSERT_NE(nullptr, some_string);
+  auto mock =
+    mocking_utils::patch(
+    "lib:rclcpp", rcl_remap_topic_name, [&some_string](
+      const rcl_arguments_t *, const rcl_arguments_t *, const char *, const char *, const char *,
+      rcl_allocator_t, char ** output_name)
+    {
+      *output_name = some_string;
+      return RCL_RET_OK;
+    });
+  EXPECT_NO_THROW(node_graph()->get_publishers_info_by_topic("topic", false));
+}
+
+TEST_F(TestNodeGraph, get_info_by_topic_rcl_errors)
+{
+  const rclcpp::QoS publisher_qos(1);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_publishers_info_by_topic, RCL_RET_ERROR);
+  auto mock_info_array_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_topic_endpoint_info_array_fini, RCL_RET_ERROR);
+  EXPECT_THROW(
+    node_graph()->get_publishers_info_by_topic("topic", false),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestNodeGraph, get_info_by_topic_unsupported)
+{
+  const rclcpp::QoS publisher_qos(1);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_publishers_info_by_topic, RCL_RET_UNSUPPORTED);
+  EXPECT_THROW(
+    node_graph()->get_publishers_info_by_topic("topic", false),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestNodeGraph, get_info_by_topic_endpoint_info_array_fini_error)
+{
+  const rclcpp::QoS publisher_qos(1);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+
+  auto mock_info_array_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_topic_endpoint_info_array_fini, RCL_RET_ERROR);
+  EXPECT_THROW(
+    node_graph()->get_publishers_info_by_topic("topic", false),
+    rclcpp::exceptions::RCLError);
+}

rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp

@@ -28,31 +28,47 @@
 #include "rclcpp/node.hpp"
 #include "rclcpp/node_interfaces/node_parameters.hpp"
 
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
 class TestNodeParameters : public ::testing::Test
 {
 public:
   void SetUp()
   {
     rclcpp::init(0, nullptr);
+    rclcpp::NodeOptions options;
+    options.allow_undeclared_parameters(true);
+    node = std::make_shared<rclcpp::Node>("node", "ns", options);
+
+    // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
+    // the proper type is being tested and covered.
+    node_parameters =
+      dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
+      node->get_node_parameters_interface().get());
+    ASSERT_NE(nullptr, node_parameters);
   }
 
   void TearDown()
   {
     rclcpp::shutdown();
   }
+
+protected:
+  std::shared_ptr<rclcpp::Node> node;
+  rclcpp::node_interfaces::NodeParameters * node_parameters;
 };
 
+TEST_F(TestNodeParameters, construct_destruct_rcl_errors) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_arguments_get_param_overrides, RCL_RET_ERROR);
+  EXPECT_THROW(
+    std::make_shared<rclcpp::Node>("node2", "ns").reset(),
+    rclcpp::exceptions::RCLError);
+}
+
 TEST_F(TestNodeParameters, list_parameters)
 {
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
-
-  // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
-  // the proper type is being tested and covered.
-  auto * node_parameters =
-    dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
-    node->get_node_parameters_interface().get());
-  ASSERT_NE(nullptr, node_parameters);
-
   std::vector<std::string> prefixes;
   const auto list_result = node_parameters->list_parameters(prefixes, 1u);
 
@@ -73,17 +89,113 @@ TEST_F(TestNodeParameters, list_parameters)
   EXPECT_NE(
     std::find(list_result2.names.begin(), list_result2.names.end(), parameter_name),
     list_result2.names.end());
+
+  // Check prefixes
+  const std::string parameter_name2 = "prefix.new_parameter";
+  const rclcpp::ParameterValue value2(true);
+  const rcl_interfaces::msg::ParameterDescriptor descriptor2;
+  const auto added_parameter_value2 =
+    node_parameters->declare_parameter(parameter_name2, value2, descriptor2, false);
+  EXPECT_EQ(value.get<bool>(), added_parameter_value.get<bool>());
+  prefixes = {"prefix"};
+  auto list_result3 = node_parameters->list_parameters(prefixes, 2u);
+  EXPECT_EQ(1u, list_result3.names.size());
+  EXPECT_NE(
+    std::find(list_result3.names.begin(), list_result3.names.end(), parameter_name2),
+    list_result3.names.end());
+
+  // Check if prefix equals parameter name
+  prefixes = {"new_parameter"};
+  auto list_result4 = node_parameters->list_parameters(prefixes, 2u);
+  EXPECT_EQ(1u, list_result4.names.size());
+  EXPECT_NE(
+    std::find(list_result4.names.begin(), list_result4.names.end(), parameter_name),
+    list_result4.names.end());
 }
 
 TEST_F(TestNodeParameters, parameter_overrides)
 {
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::NodeOptions node_options;
+  node_options.automatically_declare_parameters_from_overrides(true);
+  node_options.append_parameter_override("param1", true);
+  node_options.append_parameter_override("param2", 42);
 
-  auto * node_parameters =
+  std::shared_ptr<rclcpp::Node> node2 = std::make_shared<rclcpp::Node>("node2", "ns", node_options);
+
+  auto * node_parameters_interface =
     dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
-    node->get_node_parameters_interface().get());
+    node2->get_node_parameters_interface().get());
   ASSERT_NE(nullptr, node_parameters);
 
-  const auto & parameter_overrides = node_parameters->get_parameter_overrides();
-  EXPECT_EQ(0u, parameter_overrides.size());
+  const auto & parameter_overrides = node_parameters_interface->get_parameter_overrides();
+  EXPECT_EQ(2u, parameter_overrides.size());
+}
+
+TEST_F(TestNodeParameters, set_parameters) {
+  rclcpp::NodeOptions node_options;
+  node_options.allow_undeclared_parameters(true);
+
+  rcl_interfaces::msg::ParameterDescriptor bool_descriptor;
+  bool_descriptor.name = "bool_parameter";
+  bool_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
+  bool_descriptor.read_only = false;
+  node_parameters->declare_parameter(
+    "bool_parameter", rclcpp::ParameterValue(false), bool_descriptor, false);
+
+  rcl_interfaces::msg::ParameterDescriptor read_only_descriptor;
+  read_only_descriptor.name = "read_only_parameter";
+  read_only_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
+  read_only_descriptor.read_only = true;
+  node_parameters->declare_parameter(
+    "read_only_parameter", rclcpp::ParameterValue(42), read_only_descriptor, false);
+
+  const std::vector<rclcpp::Parameter> parameters = {
+    rclcpp::Parameter("bool_parameter", true),
+    rclcpp::Parameter("read_only_parameter", 42),
+  };
+  auto result = node_parameters->set_parameters(parameters);
+  ASSERT_EQ(parameters.size(), result.size());
+  EXPECT_TRUE(result[0].successful);
+  EXPECT_FALSE(result[1].successful);
+  EXPECT_STREQ(
+    "parameter 'read_only_parameter' cannot be set because it is read-only",
+    result[1].reason.c_str());
+
+  RCLCPP_EXPECT_THROW_EQ(
+    node_parameters->set_parameters({rclcpp::Parameter("", true)}),
+    rclcpp::exceptions::InvalidParametersException("parameter name must not be empty"));
+
+  result = node_parameters->set_parameters({rclcpp::Parameter("undeclared_parameter", 3.14159)});
+  ASSERT_EQ(1u, result.size());
+  EXPECT_TRUE(result[0].successful);
+}
+
+TEST_F(TestNodeParameters, add_remove_parameters_callback) {
+  rcl_interfaces::msg::ParameterDescriptor bool_descriptor;
+  bool_descriptor.name = "bool_parameter";
+  bool_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
+  bool_descriptor.read_only = false;
+  node_parameters->declare_parameter(
+    "bool_parameter", rclcpp::ParameterValue(false), bool_descriptor, false);
+  const std::vector<rclcpp::Parameter> parameters = {rclcpp::Parameter("bool_parameter", true)};
+
+  const std::string reason = "some totally not made up reason";
+  auto callback = [reason](const std::vector<rclcpp::Parameter> &) {
+      rcl_interfaces::msg::SetParametersResult result;
+      result.successful = false;
+      result.reason = reason;
+      return result;
+    };
+
+  auto handle = node_parameters->add_on_set_parameters_callback(callback);
+  auto result = node_parameters->set_parameters(parameters);
+  ASSERT_EQ(1u, result.size());
+  EXPECT_FALSE(result[0].successful);
+  EXPECT_EQ(reason, result[0].reason);
+
+  EXPECT_NO_THROW(node_parameters->remove_on_set_parameters_callback(handle.get()));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    node_parameters->remove_on_set_parameters_callback(handle.get()),
+    std::runtime_error("Callback doesn't exist"));
 }

rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp

@@ -16,12 +16,16 @@
 
 #include <memory>
 #include <string>
+#include <vector>
 
 #include "rcl/node_options.h"
 #include "rclcpp/node.hpp"
 #include "rclcpp/node_interfaces/node_services.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
 class TestService : public rclcpp::ServiceBase
 {
 public:
@@ -51,25 +55,31 @@ public:
   void SetUp()
   {
     rclcpp::init(0, nullptr);
+
+    rclcpp::NodeOptions options{};
+    options.arguments(std::vector<std::string>{"-r", "foo:=bar"});
+    node = std::make_shared<rclcpp::Node>("node", "ns", options);
+
+    // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
+    // the proper type is being tested and covered.
+    node_services =
+      dynamic_cast<rclcpp::node_interfaces::NodeServices *>(
+      node->get_node_services_interface().get());
+    ASSERT_NE(nullptr, node_services);
   }
 
   void TearDown()
   {
     rclcpp::shutdown();
   }
+
+protected:
+  std::shared_ptr<rclcpp::Node> node;
+  rclcpp::node_interfaces::NodeServices * node_services;
 };
 
 TEST_F(TestNodeService, add_service)
 {
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
-
-  // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
-  // the proper type is being tested and covered.
-  auto * node_services =
-    dynamic_cast<rclcpp::node_interfaces::NodeServices *>(
-    node->get_node_services_interface().get());
-  ASSERT_NE(nullptr, node_services);
-
   auto service = std::make_shared<TestService>(node.get());
   auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   EXPECT_NO_THROW(
@@ -80,22 +90,24 @@ TEST_F(TestNodeService, add_service)
 
   auto callback_group_in_different_node =
     different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-  EXPECT_THROW(
+  RCLCPP_EXPECT_THROW_EQ(
     node_services->add_service(service, callback_group_in_different_node),
-    std::runtime_error);
+    std::runtime_error("Cannot create service, group not in node."));
+}
+
+TEST_F(TestNodeService, add_service_rcl_trigger_guard_condition_error)
+{
+  auto service = std::make_shared<TestService>(node.get());
+  auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_services->add_service(service, callback_group),
+    std::runtime_error("Failed to notify wait set on service creation: error not set"));
 }
 
 TEST_F(TestNodeService, add_client)
 {
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
-
-  // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
-  // the proper type is being tested and covered.
-  auto * node_services =
-    dynamic_cast<rclcpp::node_interfaces::NodeServices *>(
-    node->get_node_services_interface().get());
-  ASSERT_NE(nullptr, node_services);
-
   auto client = std::make_shared<TestClient>(node.get());
   auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   EXPECT_NO_THROW(node_services->add_client(client, callback_group));
@@ -105,7 +117,28 @@ TEST_F(TestNodeService, add_client)
 
   auto callback_group_in_different_node =
     different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-  EXPECT_THROW(
+  RCLCPP_EXPECT_THROW_EQ(
     node_services->add_client(client, callback_group_in_different_node),
-    std::runtime_error);
+    std::runtime_error("Cannot create client, group not in node."));
+}
+
+TEST_F(TestNodeService, add_client_rcl_trigger_guard_condition_error)
+{
+  auto client = std::make_shared<TestClient>(node.get());
+  auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_services->add_client(client, callback_group),
+    std::runtime_error("Failed to notify wait set on client creation: error not set"));
+}
+
+TEST_F(TestNodeService, resolve_service_name)
+{
+  EXPECT_EQ("/ns/bar", node_services->resolve_service_name("foo", false));
+  EXPECT_EQ("/ns/foo", node_services->resolve_service_name("foo", true));
+  EXPECT_EQ("/foo", node_services->resolve_service_name("/foo", true));
+  EXPECT_THROW(
+    node_services->resolve_service_name("this is not a valid name!~>", true),
+    rclcpp::exceptions::RCLError);
 }

rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp

@@ -22,6 +22,9 @@
 #include "rclcpp/node_interfaces/node_timers.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
 class TestTimer : public rclcpp::TimerBase
 {
 public:
@@ -39,23 +42,27 @@ public:
   void SetUp()
   {
     rclcpp::init(0, nullptr);
+    node = std::make_shared<rclcpp::Node>("node", "ns");
+
+    // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
+    // the proper type is being tested and covered.
+    node_timers =
+      dynamic_cast<rclcpp::node_interfaces::NodeTimers *>(node->get_node_timers_interface().get());
+    ASSERT_NE(nullptr, node_timers);
   }
 
   void TearDown()
   {
     rclcpp::shutdown();
   }
+
+protected:
+  std::shared_ptr<rclcpp::Node> node;
+  rclcpp::node_interfaces::NodeTimers * node_timers;
 };
 
 TEST_F(TestNodeTimers, add_timer)
 {
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
-
-  // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
-  // the proper type is being tested and covered.
-  auto node_timers =
-    dynamic_cast<rclcpp::node_interfaces::NodeTimers *>(node->get_node_timers_interface().get());
-  ASSERT_NE(nullptr, node_timers);
   auto timer = std::make_shared<TestTimer>(node.get());
   auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   EXPECT_NO_THROW(node_timers->add_timer(timer, callback_group));
@@ -65,7 +72,19 @@ TEST_F(TestNodeTimers, add_timer)
 
   auto callback_group_in_different_node =
     different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-  EXPECT_THROW(
+  RCLCPP_EXPECT_THROW_EQ(
     node_timers->add_timer(timer, callback_group_in_different_node),
-    std::runtime_error);
+    std::runtime_error("Cannot create timer, group not in node."));
+}
+
+TEST_F(TestNodeTimers, add_timer_rcl_trigger_guard_condition_error)
+{
+  auto timer = std::make_shared<TestTimer>(node.get());
+  auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_timers->add_timer(timer, callback_group),
+    std::runtime_error("Failed to notify wait set on timer creation: error not set"));
 }

rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp

@@ -17,6 +17,7 @@
 #include <memory>
 #include <string>
 #include <type_traits>
+#include <vector>
 
 #include "rcl/node_options.h"
 #include "rclcpp/node.hpp"
@@ -24,6 +25,9 @@
 #include "rclcpp/rclcpp.hpp"
 #include "test_msgs/msg/empty.hpp"
 
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
 namespace
 {
 
@@ -77,23 +81,29 @@ public:
   void SetUp()
   {
     rclcpp::init(0, nullptr);
+    rclcpp::NodeOptions options{};
+    options.arguments(std::vector<std::string>{"-r", "foo:=bar"});
+    node = std::make_shared<rclcpp::Node>("node", "ns", options);
+
+    // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
+    // the proper type is being tested and covered.
+    node_topics =
+      dynamic_cast<rclcpp::node_interfaces::NodeTopics *>(node->get_node_topics_interface().get());
+    ASSERT_NE(nullptr, node_topics);
   }
 
   void TearDown()
   {
     rclcpp::shutdown();
   }
+
+protected:
+  std::shared_ptr<rclcpp::Node> node;
+  rclcpp::node_interfaces::NodeTopics * node_topics;
 };
 
 TEST_F(TestNodeTopics, add_publisher)
 {
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
-
-  // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
-  // the proper type is being tested and covered.
-  auto * node_topics =
-    dynamic_cast<rclcpp::node_interfaces::NodeTopics *>(node->get_node_topics_interface().get());
-  ASSERT_NE(nullptr, node_topics);
   auto publisher = std::make_shared<TestPublisher>(node.get());
   auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   EXPECT_NO_THROW(node_topics->add_publisher(publisher, callback_group));
@@ -108,12 +118,20 @@ TEST_F(TestNodeTopics, add_publisher)
     std::runtime_error);
 }
 
+TEST_F(TestNodeTopics, add_publisher_rcl_trigger_guard_condition_error)
+{
+  auto publisher = std::make_shared<TestPublisher>(node.get());
+  auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_topics->add_publisher(publisher, callback_group),
+    std::runtime_error("Failed to notify wait set on publisher creation: error not set"));
+}
+
 TEST_F(TestNodeTopics, add_subscription)
 {
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
-  auto * node_topics =
-    dynamic_cast<rclcpp::node_interfaces::NodeTopics *>(node->get_node_topics_interface().get());
-  ASSERT_NE(nullptr, node_topics);
   auto subscription = std::make_shared<TestSubscription>(node.get());
   auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   EXPECT_NO_THROW(node_topics->add_subscription(subscription, callback_group));
@@ -127,3 +145,25 @@ TEST_F(TestNodeTopics, add_subscription)
     node_topics->add_subscription(subscription, callback_group_in_different_node),
     std::runtime_error);
 }
+
+TEST_F(TestNodeTopics, add_subscription_rcl_trigger_guard_condition_error)
+{
+  auto subscription = std::make_shared<TestSubscription>(node.get());
+  auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_topics->add_subscription(subscription, callback_group),
+    std::runtime_error("failed to notify wait set on subscription creation: error not set"));
+}
+
+TEST_F(TestNodeTopics, resolve_topic_name)
+{
+  EXPECT_EQ("/ns/bar", node_topics->resolve_topic_name("foo", false));
+  EXPECT_EQ("/ns/foo", node_topics->resolve_topic_name("foo", true));
+  EXPECT_EQ("/foo", node_topics->resolve_topic_name("/foo", true));
+  EXPECT_THROW(
+    node_topics->resolve_topic_name("this is not a valid name!~>", true),
+    rclcpp::exceptions::RCLError);
+}

rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp

@@ -22,6 +22,9 @@
 #include "rclcpp/node_interfaces/node_waitables.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
 class TestWaitable : public rclcpp::Waitable
 {
 public:
@@ -36,23 +39,26 @@ public:
   void SetUp()
   {
     rclcpp::init(0, nullptr);
+    node = std::make_shared<rclcpp::Node>("node", "ns");
+
+    node_waitables =
+      dynamic_cast<rclcpp::node_interfaces::NodeWaitables *>(
+      node->get_node_waitables_interface().get());
+    ASSERT_NE(nullptr, node_waitables);
   }
 
   void TearDown()
   {
     rclcpp::shutdown();
   }
+
+protected:
+  std::shared_ptr<rclcpp::Node> node;
+  rclcpp::node_interfaces::NodeWaitables * node_waitables;
 };
 
 TEST_F(TestNodeWaitables, add_remove_waitable)
 {
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
-
-  auto * node_waitables =
-    dynamic_cast<rclcpp::node_interfaces::NodeWaitables *>(
-    node->get_node_waitables_interface().get());
-  ASSERT_NE(nullptr, node_waitables);
-
   std::shared_ptr<rclcpp::Node> node2 = std::make_shared<rclcpp::Node>("node2", "ns");
 
   auto callback_group1 = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
@@ -60,9 +66,25 @@ TEST_F(TestNodeWaitables, add_remove_waitable)
   auto waitable = std::make_shared<TestWaitable>();
   EXPECT_NO_THROW(
     node_waitables->add_waitable(waitable, callback_group1));
-  EXPECT_THROW(
+  RCLCPP_EXPECT_THROW_EQ(
     node_waitables->add_waitable(waitable, callback_group2),
-    std::runtime_error);
+    std::runtime_error("Cannot create waitable, group not in node."));
   EXPECT_NO_THROW(node_waitables->remove_waitable(waitable, callback_group1));
   EXPECT_NO_THROW(node_waitables->remove_waitable(waitable, callback_group2));
+
+  auto waitable2 = std::make_shared<TestWaitable>();
+  EXPECT_NO_THROW(node_waitables->add_waitable(waitable2, nullptr));
+  EXPECT_NO_THROW(node_waitables->remove_waitable(waitable2, nullptr));
+}
+
+TEST_F(TestNodeWaitables, add_waitable_rcl_trigger_guard_condition_error)
+{
+  auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto waitable = std::make_shared<TestWaitable>();
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_waitables->add_waitable(waitable, callback_group),
+    std::runtime_error("Failed to notify wait set on waitable creation: error not set"));
 }

rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp

@@ -0,0 +1,928 @@
+// 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
+  {
+    allocator_memory_strategy_.reset();
+    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,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 <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"
+
+// Note: This is a long running test with rmw_connext_cpp, if you change this file, please check
+// that this test can complete fully, or adjust the timeout as necessary.
+// See https://github.com/ros2/rmw_connext/issues/325 for resolution]
+
+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_SUITE(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_client.cpp

@@ -16,12 +16,17 @@
 
 #include <string>
 #include <memory>
+#include <utility>
 
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 #include "rcl_interfaces/srv/list_parameters.hpp"
 
+#include "../mocking_utils/patch.hpp"
+
+#include "test_msgs/srv/empty.hpp"
+
 class TestClient : public ::testing::Test
 {
 protected:
@@ -30,6 +35,11 @@ protected:
     rclcpp::init(0, nullptr);
   }
 
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
   void SetUp()
   {
     node = std::make_shared<rclcpp::Node>("my_node", "/ns");
@@ -48,6 +58,12 @@ class TestClientSub : public ::testing::Test
 protected:
   static void SetUpTestCase()
   {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
   }
 
   void SetUp()
@@ -106,6 +122,38 @@ TEST_F(TestClient, construction_with_free_function) {
   }
 }
 
+TEST_F(TestClient, construct_with_rcl_error) {
+  {
+    // reset() is not necessary for this exception, but handles unused return value warning
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_client_init, RCL_RET_ERROR);
+    EXPECT_THROW(
+      node->create_client<test_msgs::srv::Empty>("service").reset(),
+      rclcpp::exceptions::RCLError);
+  }
+  {
+    // reset() is required for this one
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_client_fini, RCL_RET_ERROR);
+    EXPECT_NO_THROW(node->create_client<test_msgs::srv::Empty>("service").reset());
+  }
+}
+
+TEST_F(TestClient, wait_for_service) {
+  const std::string service_name = "service";
+  auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+  EXPECT_FALSE(client->wait_for_service(std::chrono::nanoseconds(0)));
+  EXPECT_FALSE(client->wait_for_service(std::chrono::milliseconds(10)));
+
+  auto callback = [](
+    const test_msgs::srv::Empty::Request::SharedPtr,
+    test_msgs::srv::Empty::Response::SharedPtr) {};
+
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(service_name, std::move(callback));
+
+  EXPECT_TRUE(client->wait_for_service(std::chrono::nanoseconds(-1)));
+  EXPECT_TRUE(client->service_is_ready());
+}
+
 /*
    Testing client construction and destruction for subnodes.
  */
@@ -123,3 +171,160 @@ TEST_F(TestClientSub, construction_and_destruction) {
     }, rclcpp::exceptions::InvalidServiceNameError);
   }
 }
+
+class TestClientWithServer : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("node", "ns");
+
+    auto callback = [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {};
+
+    service = node->create_service<test_msgs::srv::Empty>(service_name, std::move(callback));
+  }
+
+  ::testing::AssertionResult SendEmptyRequestAndWait(
+    std::chrono::milliseconds timeout = std::chrono::milliseconds(1000))
+  {
+    using SharedFuture = rclcpp::Client<test_msgs::srv::Empty>::SharedFuture;
+
+    auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+    if (!client->wait_for_service()) {
+      return ::testing::AssertionFailure() << "Waiting for service failed";
+    }
+
+    auto request = std::make_shared<test_msgs::srv::Empty::Request>();
+    bool received_response = false;
+    ::testing::AssertionResult request_result = ::testing::AssertionSuccess();
+    auto callback = [&received_response, &request_result](SharedFuture future_response) {
+        if (nullptr == future_response.get()) {
+          request_result = ::testing::AssertionFailure() << "Future response was null";
+        }
+        received_response = true;
+      };
+
+    auto future = client->async_send_request(request, std::move(callback));
+
+    auto start = std::chrono::steady_clock::now();
+    while (!received_response &&
+      (std::chrono::steady_clock::now() - start) < timeout)
+    {
+      rclcpp::spin_some(node);
+    }
+
+    if (!received_response) {
+      return ::testing::AssertionFailure() << "Waiting for response timed out";
+    }
+
+    return request_result;
+  }
+
+  std::shared_ptr<rclcpp::Node> node;
+  std::shared_ptr<rclcpp::Service<test_msgs::srv::Empty>> service;
+  const std::string service_name{"empty_service"};
+};
+
+TEST_F(TestClientWithServer, async_send_request) {
+  EXPECT_TRUE(SendEmptyRequestAndWait());
+}
+
+TEST_F(TestClientWithServer, async_send_request_callback_with_request) {
+  using SharedFutureWithRequest =
+    rclcpp::Client<test_msgs::srv::Empty>::SharedFutureWithRequest;
+
+  auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+  ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1)));
+
+  auto request = std::make_shared<test_msgs::srv::Empty::Request>();
+  bool received_response = false;
+  auto callback = [&request, &received_response](SharedFutureWithRequest future) {
+      auto request_response_pair = future.get();
+      EXPECT_EQ(request, request_response_pair.first);
+      EXPECT_NE(nullptr, request_response_pair.second);
+      received_response = true;
+    };
+  auto future = client->async_send_request(request, std::move(callback));
+
+  auto start = std::chrono::steady_clock::now();
+  while (!received_response &&
+    (std::chrono::steady_clock::now() - start) < std::chrono::seconds(1))
+  {
+    rclcpp::spin_some(node);
+  }
+  EXPECT_TRUE(received_response);
+}
+
+TEST_F(TestClientWithServer, async_send_request_rcl_send_request_error) {
+  // Checking rcl_send_request in rclcpp::Client::async_send_request()
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_request, RCL_RET_ERROR);
+  EXPECT_THROW(SendEmptyRequestAndWait(), rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestClientWithServer, async_send_request_rcl_service_server_is_available_error) {
+  {
+    // Checking rcl_service_server_is_available in rclcpp::ClientBase::service_is_ready
+    auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_service_server_is_available, RCL_RET_NODE_INVALID);
+    EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError);
+  }
+  {
+    // Checking rcl_service_server_is_available exception in rclcpp::ClientBase::service_is_ready
+    auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_service_server_is_available, RCL_RET_ERROR);
+    EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError);
+  }
+  {
+    // Checking rcl_service_server_is_available exception in rclcpp::ClientBase::service_is_ready
+    auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_service_server_is_available, RCL_RET_ERROR);
+    EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError);
+  }
+}
+
+TEST_F(TestClientWithServer, take_response) {
+  auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+  ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1)));
+  auto request = std::make_shared<test_msgs::srv::Empty::Request>();
+  auto request_header = client->create_request_header();
+  test_msgs::srv::Empty::Response response;
+
+  client->async_send_request(request);
+  EXPECT_FALSE(client->take_response(response, *request_header.get()));
+
+  {
+    // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_take_response, RCL_RET_OK);
+    EXPECT_TRUE(client->take_response(response, *request_header.get()));
+  }
+  {
+    // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_take_response, RCL_RET_CLIENT_TAKE_FAILED);
+    EXPECT_FALSE(client->take_response(response, *request_header.get()));
+  }
+  {
+    // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_take_response, RCL_RET_ERROR);
+    EXPECT_THROW(
+      client->take_response(response, *request_header.get()),
+      rclcpp::exceptions::RCLError);
+  }
+}

rclcpp/test/rclcpp/test_duration.cpp

@@ -25,6 +25,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp/duration.hpp"
 
+#include "../utils/rclcpp_gtest_macros.hpp"
 
 using namespace std::chrono_literals;
 
@@ -41,6 +42,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());
@@ -66,7 +68,7 @@ TEST_F(TestDuration, operators) {
 TEST_F(TestDuration, chrono_overloads) {
   int64_t ns = 123456789l;
   auto chrono_ns = std::chrono::nanoseconds(ns);
-  auto d1 = rclcpp::Duration(ns);
+  auto d1 = rclcpp::Duration::from_nanoseconds(ns);
   auto d2 = rclcpp::Duration(chrono_ns);
   auto d3 = rclcpp::Duration(123456789ns);
   EXPECT_EQ(d1, d2);
@@ -83,11 +85,11 @@ TEST_F(TestDuration, chrono_overloads) {
 }
 
 TEST_F(TestDuration, overflows) {
-  rclcpp::Duration max(std::numeric_limits<rcl_duration_value_t>::max());
-  rclcpp::Duration min(std::numeric_limits<rcl_duration_value_t>::min());
+  auto max = rclcpp::Duration::from_nanoseconds(std::numeric_limits<rcl_duration_value_t>::max());
+  auto min = rclcpp::Duration::from_nanoseconds(std::numeric_limits<rcl_duration_value_t>::min());
 
-  rclcpp::Duration one(1);
-  rclcpp::Duration negative_one(-1);
+  rclcpp::Duration one(1ns);
+  rclcpp::Duration negative_one(-1ns);
 
   EXPECT_THROW(max + one, std::overflow_error);
   EXPECT_THROW(min - one, std::underflow_error);
@@ -104,7 +106,7 @@ TEST_F(TestDuration, overflows) {
 }
 
 TEST_F(TestDuration, negative_duration) {
-  rclcpp::Duration assignable_duration = rclcpp::Duration(0) - rclcpp::Duration(5, 0);
+  rclcpp::Duration assignable_duration = rclcpp::Duration(0ns) - rclcpp::Duration(5, 0);
 
   {
     // avoid windows converting a literal number less than -INT_MAX to unsigned int C4146
@@ -138,22 +140,24 @@ static const int64_t ONE_SEC_IN_NS = 1000 * 1000 * 1000;
 static const int64_t ONE_AND_HALF_SEC_IN_NS = 3 * HALF_SEC_IN_NS;
 
 TEST_F(TestDuration, from_seconds) {
-  EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration::from_seconds(0.0));
-  EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration::from_seconds(0));
+  EXPECT_EQ(rclcpp::Duration(0ns), rclcpp::Duration::from_seconds(0.0));
+  EXPECT_EQ(rclcpp::Duration(0ns), rclcpp::Duration::from_seconds(0));
   EXPECT_EQ(rclcpp::Duration(1, HALF_SEC_IN_NS), rclcpp::Duration::from_seconds(1.5));
-  EXPECT_EQ(rclcpp::Duration(-ONE_AND_HALF_SEC_IN_NS), rclcpp::Duration::from_seconds(-1.5));
+  EXPECT_EQ(
+    rclcpp::Duration::from_nanoseconds(-ONE_AND_HALF_SEC_IN_NS),
+    rclcpp::Duration::from_seconds(-1.5));
 }
 
 TEST_F(TestDuration, std_chrono_constructors) {
-  EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration(0.0s));
-  EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration(0s));
+  EXPECT_EQ(rclcpp::Duration(0ns), rclcpp::Duration(0.0s));
+  EXPECT_EQ(rclcpp::Duration(0ns), rclcpp::Duration(0s));
   EXPECT_EQ(rclcpp::Duration(1, HALF_SEC_IN_NS), rclcpp::Duration(1.5s));
   EXPECT_EQ(rclcpp::Duration(-1, 0), rclcpp::Duration(-1s));
 }
 
 TEST_F(TestDuration, conversions) {
   {
-    const rclcpp::Duration duration(HALF_SEC_IN_NS);
+    auto duration = rclcpp::Duration::from_nanoseconds(HALF_SEC_IN_NS);
     const auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
     EXPECT_EQ(duration_msg.sec, 0);
     EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS);
@@ -168,7 +172,7 @@ TEST_F(TestDuration, conversions) {
   }
 
   {
-    const rclcpp::Duration duration(ONE_SEC_IN_NS);
+    auto duration = rclcpp::Duration::from_nanoseconds(ONE_SEC_IN_NS);
     const auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
     EXPECT_EQ(duration_msg.sec, 1);
     EXPECT_EQ(duration_msg.nanosec, 0u);
@@ -183,7 +187,7 @@ TEST_F(TestDuration, conversions) {
   }
 
   {
-    const rclcpp::Duration duration(ONE_AND_HALF_SEC_IN_NS);
+    auto duration = rclcpp::Duration::from_nanoseconds(ONE_AND_HALF_SEC_IN_NS);
     auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
     EXPECT_EQ(duration_msg.sec, 1);
     EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS);
@@ -198,7 +202,7 @@ TEST_F(TestDuration, conversions) {
   }
 
   {
-    rclcpp::Duration duration(-HALF_SEC_IN_NS);
+    auto duration = rclcpp::Duration::from_nanoseconds(-HALF_SEC_IN_NS);
     auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
     EXPECT_EQ(duration_msg.sec, -1);
     EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS);
@@ -211,7 +215,7 @@ TEST_F(TestDuration, conversions) {
   }
 
   {
-    rclcpp::Duration duration(-ONE_SEC_IN_NS);
+    auto duration = rclcpp::Duration::from_nanoseconds(-ONE_SEC_IN_NS);
     auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
     EXPECT_EQ(duration_msg.sec, -1);
     EXPECT_EQ(duration_msg.nanosec, 0u);
@@ -224,7 +228,7 @@ TEST_F(TestDuration, conversions) {
   }
 
   {
-    rclcpp::Duration duration(-ONE_AND_HALF_SEC_IN_NS);
+    auto duration = rclcpp::Duration::from_nanoseconds(-ONE_AND_HALF_SEC_IN_NS);
     auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
     EXPECT_EQ(duration_msg.sec, -2);
     EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS);
@@ -236,3 +240,27 @@ TEST_F(TestDuration, conversions) {
     EXPECT_EQ(chrono_duration.count(), -ONE_AND_HALF_SEC_IN_NS);
   }
 }
+
+TEST_F(TestDuration, test_some_constructors) {
+  builtin_interfaces::msg::Duration duration_msg;
+  duration_msg.sec = 1;
+  duration_msg.nanosec = 1000;
+  rclcpp::Duration duration_from_msg(duration_msg);
+  EXPECT_EQ(RCL_S_TO_NS(1) + 1000, duration_from_msg.nanoseconds());
+
+  rcl_duration_t duration_struct;
+  duration_struct.nanoseconds = 4000;
+  rclcpp::Duration duration_from_struct(duration_struct);
+  EXPECT_EQ(4000, duration_from_struct.nanoseconds());
+}
+
+TEST_F(TestDuration, test_some_exceptions) {
+  rclcpp::Duration test_duration(0ns);
+  RCLCPP_EXPECT_THROW_EQ(
+    test_duration =
+    rclcpp::Duration::from_nanoseconds(INT64_MAX) - rclcpp::Duration(-1ns),
+    std::overflow_error("duration subtraction leads to int64_t overflow"));
+  RCLCPP_EXPECT_THROW_EQ(
+    test_duration = test_duration * (std::numeric_limits<double>::infinity()),
+    std::runtime_error("abnormal scale in rclcpp::Duration"));
+}

rclcpp/test/rclcpp/test_executor.cpp

@@ -1,4 +1,4 @@
-// Copyright 2017 Open Source Robotics Foundation, Inc.
+// 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.
@@ -14,186 +14,515 @@
 
 #include <gtest/gtest.h>
 
-#include <algorithm>
 #include <chrono>
-#include <limits>
 #include <memory>
+#include <stdexcept>
 #include <string>
 
-#include "rcl/error_handling.h"
-#include "rcl/time.h"
-#include "rclcpp/clock.hpp"
-#include "rclcpp/duration.hpp"
-#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/executor.hpp"
+#include "rclcpp/memory_strategy.hpp"
+#include "rclcpp/strategies/allocator_memory_strategy.hpp"
 
-using namespace std::chrono_literals;
+#include "../mocking_utils/patch.hpp"
+#include "../utils/rclcpp_gtest_macros.hpp"
 
-class TestExecutors : public ::testing::Test
+// This file tests the abstract rclcpp::Executor class.  For tests of the concrete classes
+// that implement this class, please see the test/rclcpp/executors subdirectory.
+
+class DummyExecutor : public rclcpp::Executor
 {
-protected:
-  static void SetUpTestCase()
+public:
+  DummyExecutor()
+  : rclcpp::Executor()
+  {
+  }
+
+  void spin() override
+  {
+  }
+
+  void spin_nanoseconds(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node)
+  {
+    spin_node_once_nanoseconds(node, std::chrono::milliseconds(100));
+  }
+
+  rclcpp::memory_strategy::MemoryStrategy * memory_strategy_ptr()
+  {
+    return memory_strategy_.get();
+  }
+
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr local_get_node_by_group(
+    rclcpp::CallbackGroup::SharedPtr group)
+  {
+    return get_node_by_group(weak_groups_to_nodes_, group);
+  }
+
+  rclcpp::CallbackGroup::SharedPtr local_get_group_by_timer(rclcpp::TimerBase::SharedPtr timer)
+  {
+    return get_group_by_timer(timer);
+  }
+};
+
+class TestExecutor : public ::testing::Test
+{
+public:
+  void SetUp()
   {
     rclcpp::init(0, nullptr);
   }
 
-  void SetUp()
-  {
-    node = std::make_shared<rclcpp::Node>("my_node");
-  }
-
   void TearDown()
   {
-    node.reset();
+    rclcpp::shutdown();
   }
-
-  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));
-  }
+// Required for mocking_utils below
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, >)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >)
+
+TEST_F(TestExecutor, constructor_bad_guard_condition_init) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_guard_condition_init, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    new DummyExecutor(),
+    std::runtime_error(
+      "Failed to create interrupt guard condition in Executor constructor: error not set"));
 }
 
-// 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());
+TEST_F(TestExecutor, constructor_bad_wait_set_init) {
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_init, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    new DummyExecutor(),
+    std::runtime_error("Failed to create wait set in Executor constructor: error not set"));
 }
 
-// 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_F(TestExecutor, add_callback_group_twice) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  dummy.add_callback_group(cb_group, node->get_node_base_interface(), false);
+  cb_group->get_associated_with_executor_atomic().exchange(false);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.add_callback_group(cb_group, node->get_node_base_interface(), false),
+    std::runtime_error("Callback group was already added to executor."));
+}
 
-  // test success
-  future = std::async(
-    []() {
-      return;
+TEST_F(TestExecutor, add_callback_group_failed_trigger_guard_condition) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.add_callback_group(cb_group, node->get_node_base_interface(), true),
+    std::runtime_error("Failed to trigger guard condition on callback group add: error not set"));
+}
+
+TEST_F(TestExecutor, remove_callback_group_null_node) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  dummy.add_callback_group(cb_group, node->get_node_base_interface(), true);
+
+  node.reset();
+
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.remove_callback_group(cb_group, false),
+    std::runtime_error("Node must not be deleted before its callback group(s)."));
+}
+
+TEST_F(TestExecutor, remove_callback_group_failed_trigger_guard_condition) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  dummy.add_callback_group(cb_group, node->get_node_base_interface(), true);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.remove_callback_group(cb_group, true),
+    std::runtime_error(
+      "Failed to trigger guard condition on callback group remove: error not set"));
+}
+
+TEST_F(TestExecutor, remove_node_not_associated) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.remove_node(node->get_node_base_interface(), false),
+    std::runtime_error("Node needs to be associated with an executor."));
+}
+
+TEST_F(TestExecutor, remove_node_associated_with_different_executor) {
+  DummyExecutor dummy1;
+  auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
+  dummy1.add_node(node1->get_node_base_interface(), false);
+
+  DummyExecutor dummy2;
+  auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
+  dummy2.add_node(node2->get_node_base_interface(), false);
+
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy2.remove_node(node1->get_node_base_interface(), false),
+    std::runtime_error("Node needs to be associated with this executor."));
+}
+
+TEST_F(TestExecutor, spin_node_once_nanoseconds) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool timer_fired = false;
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&timer_fired]() {timer_fired = true;});
+
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  EXPECT_FALSE(timer_fired);
+  dummy.spin_nanoseconds(node->get_node_base_interface());
+  EXPECT_TRUE(timer_fired);
+}
+
+TEST_F(TestExecutor, spin_node_some) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool timer_fired = false;
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&timer_fired]() {timer_fired = true;});
+
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  EXPECT_FALSE(timer_fired);
+  dummy.spin_node_some(node);
+  EXPECT_TRUE(timer_fired);
+}
+
+TEST_F(TestExecutor, spin_all_invalid_duration) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.spin_all(std::chrono::nanoseconds(-1)),
+    std::invalid_argument("max_duration must be positive"));
+}
+
+TEST_F(TestExecutor, spin_some_in_spin_some) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool spin_some_in_spin_some = false;
+  auto timer =
+    node->create_wall_timer(
+    std::chrono::milliseconds(1), [&]() {
+      try {
+        dummy.spin_some(std::chrono::milliseconds(1));
+      } catch (const std::runtime_error & err) {
+        if (err.what() == std::string("spin_some() called while already spinning")) {
+          spin_some_in_spin_some = true;
+        }
+      }
     });
-  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);
+  dummy.add_node(node);
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  EXPECT_FALSE(spin_some_in_spin_some);
+  dummy.spin_some(std::chrono::milliseconds(1));
+  EXPECT_TRUE(spin_some_in_spin_some);
 }
 
-// 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;
+TEST_F(TestExecutor, spin_some_elapsed) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool timer_called = false;
+  auto timer =
+    node->create_wall_timer(
+    std::chrono::milliseconds(1), [&]() {
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+      timer_called = true;
     });
-  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);
+  dummy.add_node(node);
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  dummy.spin_some(std::chrono::milliseconds(1));
+
+  ASSERT_TRUE(timer_called);
 }
 
-class TestWaitable : public rclcpp::Waitable
-{
-public:
-  TestWaitable()
-  {
-    rcl_guard_condition_options_t guard_condition_options =
-      rcl_guard_condition_get_default_options();
+TEST_F(TestExecutor, spin_once_in_spin_once) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool spin_once_in_spin_once = false;
+  auto timer =
+    node->create_wall_timer(
+    std::chrono::milliseconds(1), [&]() {
+      try {
+        dummy.spin_once(std::chrono::milliseconds(1));
+      } catch (const std::runtime_error & err) {
+        if (err.what() == std::string("spin_once() called while already spinning")) {
+          spin_once_in_spin_once = true;
+        }
+      }
+    });
 
-    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);
-  }
+  dummy.add_node(node);
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  EXPECT_FALSE(spin_once_in_spin_once);
+  dummy.spin_once(std::chrono::milliseconds(1));
+  EXPECT_TRUE(spin_once_in_spin_once);
+}
+
+TEST_F(TestExecutor, cancel_failed_trigger_guard_condition) {
+  DummyExecutor dummy;
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.cancel(),
+    std::runtime_error("Failed to trigger guard condition in cancel: error not set"));
+}
+
+TEST_F(TestExecutor, set_memory_strategy_nullptr) {
+  DummyExecutor dummy;
+
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.set_memory_strategy(nullptr),
+    std::runtime_error("Received NULL memory strategy in executor."));
+}
+
+TEST_F(TestExecutor, set_memory_strategy) {
+  DummyExecutor dummy;
+  rclcpp::memory_strategy::MemoryStrategy::SharedPtr strategy =
+    std::make_shared<
+    rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>();
+
+  dummy.set_memory_strategy(strategy);
+  EXPECT_EQ(dummy.memory_strategy_ptr(), strategy.get());
+}
+
+TEST_F(TestExecutor, spin_once_failed_trigger_guard_condition) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {});
+
+  dummy.add_node(node);
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.spin_once(std::chrono::milliseconds(1)),
+    std::runtime_error(
+      "Failed to trigger guard condition from execute_any_executable: error not set"));
+}
+
+TEST_F(TestExecutor, spin_some_fail_wait_set_clear) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {});
+
+  dummy.add_node(node);
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.spin_some(std::chrono::milliseconds(1)),
+    std::runtime_error("Couldn't clear wait set: error not set"));
+}
+
+TEST_F(TestExecutor, spin_some_fail_wait_set_resize) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {});
+
+  dummy.add_node(node);
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_resize, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.spin_some(std::chrono::milliseconds(1)),
+    std::runtime_error("Couldn't resize the wait set: error not set"));
+}
+
+TEST_F(TestExecutor, spin_some_fail_add_handles_to_wait_set) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {});
+
+  dummy.add_node(node);
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait_set_add_subscription,
+    RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.spin_some(std::chrono::milliseconds(1)),
+    std::runtime_error("Couldn't fill wait set"));
+}
+
+TEST_F(TestExecutor, spin_some_fail_wait) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {});
+
+  dummy.add_node(node);
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.spin_some(std::chrono::milliseconds(1)),
+    std::runtime_error("rcl_wait() failed: error not set"));
+}
+
+TEST_F(TestExecutor, get_node_by_group_null_group) {
+  DummyExecutor dummy;
+  ASSERT_EQ(nullptr, dummy.local_get_node_by_group(nullptr));
+}
+
+TEST_F(TestExecutor, get_node_by_group) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  dummy.add_callback_group(cb_group, node->get_node_base_interface(), false);
+  ASSERT_EQ(node->get_node_base_interface().get(), dummy.local_get_node_by_group(cb_group).get());
+}
+
+TEST_F(TestExecutor, get_node_by_group_not_found) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  ASSERT_EQ(nullptr, dummy.local_get_node_by_group(cb_group).get());
+}
+
+TEST_F(TestExecutor, get_group_by_timer_nullptr) {
+  DummyExecutor dummy;
+  ASSERT_EQ(nullptr, dummy.local_get_group_by_timer(nullptr));
+}
+
+TEST_F(TestExecutor, get_group_by_timer) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group);
+  dummy.add_node(node);
+
+  ASSERT_EQ(cb_group.get(), dummy.local_get_group_by_timer(timer).get());
+}
+
+TEST_F(TestExecutor, get_group_by_timer_with_deleted_group) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group);
+  dummy.add_node(node);
+
+  cb_group.reset();
+
+  ASSERT_EQ(nullptr, dummy.local_get_group_by_timer(timer).get());
+}
+
+TEST_F(TestExecutor, get_group_by_timer_add_callback_group) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group);
+  dummy.add_callback_group(cb_group, node->get_node_base_interface(), false);
+
+  ASSERT_EQ(cb_group.get(), dummy.local_get_group_by_timer(timer).get());
+}
+
+TEST_F(TestExecutor, spin_until_future_complete_in_spin_until_future_complete) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool spin_until_future_complete_in_spin_until_future_complete = false;
+  auto timer =
+    node->create_wall_timer(
+    std::chrono::milliseconds(1), [&]() {
+      try {
+        std::promise<void> promise;
+        std::future<void> future = promise.get_future();
+        dummy.spin_until_future_complete(future, std::chrono::milliseconds(1));
+      } catch (const std::runtime_error & err) {
+        if (err.what() == std::string(
+          "spin_until_future_complete() called while already spinning"))
+        {
+          spin_until_future_complete_in_spin_until_future_complete = true;
+        }
+      }
+    });
+
+  dummy.add_node(node);
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  EXPECT_FALSE(spin_until_future_complete_in_spin_until_future_complete);
+  std::promise<void> promise;
+  std::future<void> future = promise.get_future();
+  dummy.spin_until_future_complete(future, std::chrono::milliseconds(1));
+  EXPECT_TRUE(spin_until_future_complete_in_spin_until_future_complete);
+}
+
+TEST_F(TestExecutor, spin_node_once_base_interface) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool spin_called = false;
+  auto timer =
+    node->create_wall_timer(
+    std::chrono::milliseconds(1), [&]() {
+      spin_called = true;
+    });
+
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  EXPECT_FALSE(spin_called);
+  dummy.spin_node_once(node->get_node_base_interface());
+  EXPECT_TRUE(spin_called);
+}
+
+TEST_F(TestExecutor, spin_node_once_node) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool spin_called = false;
+  auto timer =
+    node->create_wall_timer(
+    std::chrono::milliseconds(1), [&]() {
+      spin_called = true;
+    });
+
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  EXPECT_FALSE(spin_called);
+  dummy.spin_node_once(node);
+  EXPECT_TRUE(spin_called);
+}
+
+TEST_F(TestExecutor, spin_until_future_complete_future_already_complete) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  std::promise<void> promise;
+  std::future<void> future = promise.get_future();
+  promise.set_value();
+  EXPECT_EQ(
+    rclcpp::FutureReturnCode::SUCCESS,
+    dummy.spin_until_future_complete(future, std::chrono::milliseconds(1)));
 }

rclcpp/test/rclcpp/test_expand_topic_or_service_name.cpp

@@ -14,9 +14,20 @@
 
 #include <gtest/gtest.h>
 
+#include <stdexcept>
+
+#include "rcl/expand_topic_name.h"
+#include "rcl/validate_topic_name.h"
+#include "rmw/validate_full_topic_name.h"
+#include "rmw/validate_namespace.h"
+#include "rmw/validate_node_name.h"
+
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/expand_topic_or_service_name.hpp"
 
+#include "../mocking_utils/patch.hpp"
+#include "../utils/rclcpp_gtest_macros.hpp"
+
 /*
    Testing expand_topic_or_service_name.
  */
@@ -78,3 +89,156 @@ TEST(TestExpandTopicOrServiceName, exceptions) {
     }, rclcpp::exceptions::InvalidServiceNameError);
   }
 }
+
+// Required for mocking_utils below
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >)
+
+TEST(TestExpandTopicOrServiceName, rcutils_string_map_init_fail_bad_alloc) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcutils_string_map_init, RCUTILS_RET_BAD_ALLOC);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::bad_alloc());
+}
+
+TEST(TestExpandTopicOrServiceName, rcutils_string_map_init_fail_other) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcutils_string_map_init, RCUTILS_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::runtime_error("error not set"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcl_get_default_topic_name_substitution_fail) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_default_topic_name_substitutions, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::runtime_error("error not set"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcl_get_default_topic_name_substitution_and_map_fini_fail) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_default_topic_name_substitutions, RCL_RET_ERROR);
+  auto mock2 = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcutils_string_map_fini, RCUTILS_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::runtime_error("error not set"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcutils_string_map_fini_fail_bad_alloc) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcutils_string_map_fini, RCUTILS_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::runtime_error("error not set"));
+}
+
+TEST(TestExpandTopicOrServiceName, rmw_valid_full_topic_name_fail_invalid_argument) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_full_topic_name, RMW_RET_INVALID_ARGUMENT);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLInvalidArgument(
+      RCL_RET_INVALID_ARGUMENT, rcl_get_error_state(), "failed to validate full topic name"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcl_expand_topic_name_fail) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_TOPIC_NAME_INVALID);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::runtime_error("topic name unexpectedly valid"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcl_validate_topic_name_fail) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_TOPIC_NAME_INVALID);
+  auto mock2 = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_validate_topic_name, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, rcl_get_error_state(), "failed to validate full topic name"));
+}
+
+TEST(TestExpandTopicOrServiceName, rmw_validate_node_name_fail_invalid_argument) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAME);
+  auto mock2 = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_node_name, RMW_RET_INVALID_ARGUMENT);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLInvalidArgument(
+      RCL_RET_INVALID_ARGUMENT, rcl_get_error_state(), "failed to validate node name"));
+}
+
+TEST(TestExpandTopicOrServiceName, rmw_validate_node_name_fail_other) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAME);
+  auto mock2 = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_node_name, RMW_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, rcl_get_error_state(), "failed to validate node name"));
+}
+
+TEST(TestExpandTopicOrServiceName, rmw_validate_namespace_fail_invalid_argument) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAMESPACE);
+  auto mock2 = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_namespace, RMW_RET_INVALID_ARGUMENT);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLInvalidArgument(
+      RCL_RET_INVALID_ARGUMENT, rcl_get_error_state(), "failed to validate namespace"));
+}
+
+TEST(TestExpandTopicOrServiceName, rmw_validate_namespace_fail_other) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAMESPACE);
+  auto mock2 = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_namespace, RMW_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, rcl_get_error_state(), "failed to validate namespace"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcl_expand_topic_name_fail_other) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLError(RCL_RET_ERROR, rcl_get_error_state(), "error not set"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcl_expand_topic_name_fail_invalid_node_name) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAME);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::runtime_error("invalid rcl node name but valid rmw node name"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcl_expand_topic_name_fail_invalid_node_namespace) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAMESPACE);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::runtime_error("invalid rcl namespace but valid rmw namespace"));
+}
+
+TEST(TestExpandTopicOrServiceName, rmw_validate_full_topic_name_fail_other) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_full_topic_name, RMW_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, rcl_get_error_state(), "failed to validate full topic name"));
+}

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

@@ -0,0 +1,42 @@
+// 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 <sstream>
+#include <string>
+
+#include "rclcpp/future_return_code.hpp"
+
+TEST(TestFutureReturnCode, to_string) {
+  EXPECT_EQ(
+    "Unknown enum value (-1)", rclcpp::to_string(rclcpp::FutureReturnCode(-1)));
+  EXPECT_EQ(
+    "SUCCESS (0)", rclcpp::to_string(rclcpp::FutureReturnCode::SUCCESS));
+  EXPECT_EQ(
+    "INTERRUPTED (1)", rclcpp::to_string(rclcpp::FutureReturnCode::INTERRUPTED));
+  EXPECT_EQ(
+    "TIMEOUT (2)", rclcpp::to_string(rclcpp::FutureReturnCode::TIMEOUT));
+  EXPECT_EQ(
+    "Unknown enum value (3)", rclcpp::to_string(rclcpp::FutureReturnCode(3)));
+  EXPECT_EQ(
+    "Unknown enum value (100)", rclcpp::to_string(rclcpp::FutureReturnCode(100)));
+}
+
+TEST(FutureReturnCode, ostream) {
+  std::ostringstream ostream;
+
+  ostream << rclcpp::FutureReturnCode::SUCCESS;
+  ASSERT_EQ("SUCCESS (0)", ostream.str());
+}

rclcpp/test/rclcpp/test_graph_listener.cpp

@@ -0,0 +1,293 @@
+// 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/contexts/default_context.hpp"
+#include "rclcpp/graph_listener.hpp"
+#include "rclcpp/node_interfaces/node_graph.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "../mocking_utils/patch.hpp"
+#include "../utils/rclcpp_gtest_macros.hpp"
+
+namespace
+{
+
+constexpr char node_name[] = "node";
+constexpr char node_namespace[] = "ns";
+constexpr char shutdown_error_str[] = "GraphListener already shutdown";
+
+}  // namespace
+
+class TestGraphListener : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+    node_ = std::make_shared<rclcpp::Node>(node_name, node_namespace);
+
+    node_graph_ = node_->get_node_graph_interface();
+    ASSERT_NE(nullptr, node_graph_);
+
+    graph_listener_ =
+      std::make_shared<rclcpp::graph_listener::GraphListener>(
+      rclcpp::contexts::get_global_default_context());
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+
+protected:
+  std::shared_ptr<rclcpp::Node> node() {return node_;}
+  rclcpp::node_interfaces::NodeGraphInterface * node_graph() {return node_graph_.get();}
+  std::shared_ptr<rclcpp::graph_listener::GraphListener> graph_listener() {return graph_listener_;}
+
+private:
+  std::shared_ptr<rclcpp::Node> node_;
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
+  std::shared_ptr<rclcpp::graph_listener::GraphListener> graph_listener_;
+};
+
+/* Run base class init/shutdown */
+TEST_F(TestGraphListener, construction_and_destruction) {
+  EXPECT_FALSE(graph_listener()->has_node(node_graph()));
+  EXPECT_FALSE(graph_listener()->is_shutdown());
+}
+
+// Required for mocking_utils below
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, >)
+
+/* Error creating a new graph listener */
+TEST_F(TestGraphListener, error_construct_graph_listener) {
+  using rclcpp::contexts::get_global_default_context;
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_guard_condition_init, RCL_RET_ERROR);
+
+  RCLCPP_EXPECT_THROW_EQ(
+  {
+    auto graph_listener_error =
+    std::make_shared<rclcpp::graph_listener::GraphListener>(get_global_default_context());
+    graph_listener_error.reset();
+  }, std::runtime_error("failed to create interrupt guard condition: error not set"));
+}
+
+// Required for mocking_utils below
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >)
+
+/* Errors that occur when initializing the graph_listener */
+TEST_F(TestGraphListener, error_start_graph_listener) {
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_wait_set_init, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      graph_listener()->start_if_not_started(),
+      std::runtime_error("failed to initialize wait set: error not set"));
+  }
+  {
+    EXPECT_NO_THROW(graph_listener()->shutdown());
+    RCLCPP_EXPECT_THROW_EQ(
+      graph_listener()->start_if_not_started(),
+      std::runtime_error(shutdown_error_str));
+  }
+}
+
+class TestGraphListenerProtectedMethods : public rclcpp::graph_listener::GraphListener
+{
+public:
+  explicit TestGraphListenerProtectedMethods(std::shared_ptr<rclcpp::Context> parent_context)
+  : rclcpp::graph_listener::GraphListener{parent_context}
+  {}
+
+  void run_protected()
+  {
+    this->run();
+  }
+
+  void mock_start_thread()
+  {
+    // This function prepares the loop thread to be run, but leave
+    // early with the failure thrown. That way the graph_listener wait_set
+    // is init, without being started
+    auto mock_wait_set_init = mocking_utils::inject_on_return(
+      "lib:rclcpp", rcl_wait_set_init, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      this->start_if_not_started(),
+      std::runtime_error("failed to initialize wait set: error not set"));
+  }
+};
+
+/* Errors running graph protected methods */
+TEST_F(TestGraphListener, error_run_graph_listener_destroy_context) {
+  auto context_to_destroy = std::make_shared<rclcpp::contexts::DefaultContext>();
+  context_to_destroy->init(0, nullptr);
+  auto graph_listener_error =
+    std::make_shared<TestGraphListenerProtectedMethods>(context_to_destroy);
+  context_to_destroy.reset();
+  EXPECT_NO_THROW(graph_listener_error->run_protected());
+}
+
+TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_set_clear) {
+  auto global_context = rclcpp::contexts::get_global_default_context();
+  auto graph_listener_test =
+    std::make_shared<TestGraphListenerProtectedMethods>(global_context);
+  graph_listener_test->mock_start_thread();
+  auto mock_wait_set_clear = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener_test->run_protected(),
+    std::runtime_error("failed to clear wait set: error not set"));
+}
+
+TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_set_add_guard_condition) {
+  auto global_context = rclcpp::contexts::get_global_default_context();
+  auto graph_listener_test =
+    std::make_shared<TestGraphListenerProtectedMethods>(global_context);
+  graph_listener_test->mock_start_thread();
+  auto mock_wait_set_clear = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait_set_add_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener_test->run_protected(),
+    std::runtime_error("failed to add interrupt guard condition to wait set: error not set"));
+}
+
+TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_set_add_guard_condition_twice) {
+  auto global_context = rclcpp::contexts::get_global_default_context();
+  auto graph_listener_test =
+    std::make_shared<TestGraphListenerProtectedMethods>(global_context);
+  graph_listener_test->mock_start_thread();
+  auto mock = mocking_utils::patch(
+    "lib:rclcpp", rcl_wait_set_add_guard_condition, [](auto, ...) {
+      static int counter = 1;
+      if (counter == 1) {
+        counter++;
+        return RCL_RET_OK;
+      } else {
+        return RCL_RET_ERROR;
+      }
+    });
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener_test->run_protected(),
+    std::runtime_error("failed to add shutdown guard condition to wait set: error not set"));
+}
+
+TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_error) {
+  auto global_context = rclcpp::contexts::get_global_default_context();
+  auto graph_listener_test =
+    std::make_shared<TestGraphListenerProtectedMethods>(global_context);
+  graph_listener_test->mock_start_thread();
+  auto mock_wait_set_clear = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener_test->run_protected(),
+    std::runtime_error("failed to wait on wait set: error not set"));
+}
+
+TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_timeout) {
+  auto global_context = rclcpp::contexts::get_global_default_context();
+  auto graph_listener_test =
+    std::make_shared<TestGraphListenerProtectedMethods>(global_context);
+  graph_listener_test->mock_start_thread();
+  auto mock_wait_set_clear = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait, RCL_RET_TIMEOUT);
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener_test->run_protected(),
+    std::runtime_error("rcl_wait unexpectedly timed out"));
+}
+
+/* Add/Remove node usage */
+TEST_F(TestGraphListener, test_graph_listener_add_remove_node) {
+  EXPECT_FALSE(graph_listener()->has_node(node_graph()));
+
+  graph_listener()->add_node(node_graph());
+  EXPECT_TRUE(graph_listener()->has_node(node_graph()));
+
+  graph_listener()->remove_node(node_graph());
+  EXPECT_FALSE(graph_listener()->has_node(node_graph()));
+}
+
+/* Add/Remove node error usage */
+TEST_F(TestGraphListener, test_errors_graph_listener_add_remove_node) {
+  // nullptrs tests
+  EXPECT_FALSE(graph_listener()->has_node(nullptr));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener()->add_node(nullptr),
+    std::invalid_argument("node is nullptr"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener()->remove_node(nullptr),
+    std::invalid_argument("node is nullptr"));
+
+  // Already added
+  graph_listener()->add_node(node_graph());
+  EXPECT_TRUE(graph_listener()->has_node(node_graph()));
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener()->add_node(node_graph()),
+    std::runtime_error("node already added"));
+
+  // Remove node not found
+  graph_listener()->remove_node(node_graph());
+  EXPECT_FALSE(graph_listener()->has_node(node_graph()));
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener()->remove_node(node_graph()),
+    std::runtime_error("node not found"));
+
+  // Add and remove after shutdown
+  EXPECT_NO_THROW(graph_listener()->shutdown());
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener()->add_node(node_graph()),
+    std::runtime_error(shutdown_error_str));
+  // Remove works the same
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener()->remove_node(node_graph()),
+    std::runtime_error("node not found"));
+}
+
+/* Shutdown errors */
+TEST_F(TestGraphListener, test_graph_listener_shutdown_wait_fini_error_nothrow) {
+  graph_listener()->start_if_not_started();
+  auto mock_wait_set_fini = mocking_utils::inject_on_return(
+    "lib:rclcpp", rcl_wait_set_fini, RCL_RET_ERROR);
+  // Exception is logged when using nothrow_t
+  EXPECT_NO_THROW(graph_listener()->shutdown(std::nothrow_t()));
+}
+
+TEST_F(TestGraphListener, test_graph_listener_shutdown_wait_fini_error_throw) {
+  graph_listener()->start_if_not_started();
+  auto mock_wait_set_fini = mocking_utils::inject_on_return(
+    "lib:rclcpp", rcl_wait_set_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener()->shutdown(),
+    std::runtime_error("failed to finalize wait set: error not set"));
+}
+
+TEST_F(TestGraphListener, test_graph_listener_shutdown_guard_fini_error_throw) {
+  graph_listener()->start_if_not_started();
+  auto mock_wait_set_fini = mocking_utils::inject_on_return(
+    "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener()->shutdown(),
+    std::runtime_error("failed to finalize interrupt guard condition: error not set"));
+}

rclcpp/test/rclcpp/test_guard_condition.cpp

@@ -18,6 +18,8 @@
 
 #include "rclcpp/rclcpp.hpp"
 
+#include "../mocking_utils/patch.hpp"
+
 class TestGuardCondition : public ::testing::Test
 {
 protected:
@@ -54,6 +56,14 @@ TEST_F(TestGuardCondition, construction_and_destruction) {
       (void)gc;
     }, rclcpp::exceptions::RCLInvalidArgument);
   }
+
+  {
+    auto mock = mocking_utils::inject_on_return(
+      "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR);
+    auto gc = std::make_shared<rclcpp::GuardCondition>();
+    // This just logs an error on destruction
+    EXPECT_NO_THROW(gc.reset());
+  }
 }
 
 /*
@@ -82,6 +92,13 @@ TEST_F(TestGuardCondition, get_rcl_guard_condition) {
 TEST_F(TestGuardCondition, trigger) {
   {
     auto gc = std::make_shared<rclcpp::GuardCondition>();
-    gc->trigger();
+    EXPECT_NO_THROW(gc->trigger());
+
+    {
+      auto mock = mocking_utils::patch_and_return(
+        "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+      auto gc = std::make_shared<rclcpp::GuardCondition>();
+      EXPECT_THROW(gc->trigger(), rclcpp::exceptions::RCLError);
+    }
   }
 }