5.0.0

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

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

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

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

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

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

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

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

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

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

This reverts commit d444e09131c42d6eece1338443b8ffb4f5f17370.

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

Propagate error message from a failed result request.

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

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

* Guard against mutliple calls to invalidate

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

rclcpp/CHANGELOG.rst

@@ -2,129 +2,84 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-2.3.1 (2021-04-14)
+5.0.0 (2020-09-18)
 ------------------
-* Update quality declaration links (re: `ros2/docs.ros2.org#52 <https://github.com/ros2/docs.ros2.org/issues/52>`_) (`#1616 <https://github.com/ros2/rclcpp/issues/1616>`_)
-* Fix documented example in create_publisher (`#1558 <https://github.com/ros2/rclcpp/issues/1558>`_) (`#1559 <https://github.com/ros2/rclcpp/issues/1559>`_)
-* Fix runtime error: reference binding to null pointer of type (`#1547 <https://github.com/ros2/rclcpp/issues/1547>`_) (`#1548 <https://github.com/ros2/rclcpp/issues/1548>`_)
-* Contributors: Jacob Perron, Simon Honigmann, Tomoya Fujita
-
-2.3.0 (2020-12-09)
-------------------
-* Update QD to QL 1 (`#1480 <https://github.com/ros2/rclcpp/issues/1480>`_)
-* Add performance tests for parameter transport (`#1470 <https://github.com/ros2/rclcpp/issues/1470>`_)
-* Add benchmarks for node parameters interface (`#1470 <https://github.com/ros2/rclcpp/issues/1470>`_)
-* Fix NodeOptions copy constructor (`#1376 <https://github.com/ros2/rclcpp/issues/1376>`_) (`#1451 <https://github.com/ros2/rclcpp/issues/1451>`_)
-* Avoid reference cycle to fix memory leak (`#1301 <https://github.com/ros2/rclcpp/issues/1301>`_) (`#1450 <https://github.com/ros2/rclcpp/issues/1450>`_)
-* Bump rclcpp packages to Quality Level 2 (`#1445 <https://github.com/ros2/rclcpp/issues/1445>`_) (`#1446 <https://github.com/ros2/rclcpp/issues/1446>`_)
-* Added executor benchmark tests (`#1413 <https://github.com/ros2/rclcpp/issues/1413>`_)
-* 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>`_)
-* Initial benchmark tests for rclcpp::init/shutdown create/destroy node (`#1411 <https://github.com/ros2/rclcpp/issues/1411>`_)
-* Use global namespace for parameter events subscription topic (`#1257 <https://github.com/ros2/rclcpp/issues/1257>`_) (`#1261 <https://github.com/ros2/rclcpp/issues/1261>`_)
-* Refactor test CMakeLists.txt (`#1422 <https://github.com/ros2/rclcpp/issues/1422>`_) and moving rosidl_generate_interfaces_call (`#1424 <https://github.com/ros2/rclcpp/issues/1424>`_)  (`#1437 <https://github.com/ros2/rclcpp/issues/1437>`_)
-* Update tracetools' QL in rclcpp's QD (`#1428 <https://github.com/ros2/rclcpp/issues/1428>`_) (`#1430 <https://github.com/ros2/rclcpp/issues/1430>`_)
-* Add coverage statement (`#1367 <https://github.com/ros2/rclcpp/issues/1367>`_)
-* Update tracetools' QL to 2 in rclcpp's QD (`#1187 <https://github.com/ros2/rclcpp/issues/1187>`_)
-* Fix reference to rclcpp in its QD (`#1161 <https://github.com/ros2/rclcpp/issues/1161>`_)
-* 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>`_)
-* 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>`_)
-* 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>`_)
-* Only exchange intra_process waitable if nonnull (`#1317 <https://github.com/ros2/rclcpp/issues/1317>`_)
-* 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.
-* Improve 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>`_)
-* Add coverage tests graph_listener (`#1330 <https://github.com/ros2/rclcpp/issues/1330>`_)
-* Add executor unit tests `#1336 <https://github.com/ros2/rclcpp/issues/1336>`_ (`#1395 <https://github.com/ros2/rclcpp/issues/1395>`_)
-* Add coverage for client API (`#1329 <https://github.com/ros2/rclcpp/issues/1329>`_)
-* Increase service coverage (`#1332 <https://github.com/ros2/rclcpp/issues/1332>`_)
-* Add ostream test for FutureReturnCode (`#1327 <https://github.com/ros2/rclcpp/issues/1327>`_) (`#1393 <https://github.com/ros2/rclcpp/issues/1393>`_)
-* Increase coverage of publisher/subscription API (`#1325 <https://github.com/ros2/rclcpp/issues/1325>`_)
-* Add coverage for missing API (except executors) (`#1326 <https://github.com/ros2/rclcpp/issues/1326>`_)
-* 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>`_)
+* 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>`_)
-* Ability to configure domain_id via InitOptions (`#1165 <https://github.com/ros2/rclcpp/issues/1165>`_)
+* 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>`_)
-* Parameterize test executors for all executor types (`#1222 <https://github.com/ros2/rclcpp/issues/1222>`_) (`#1386 <https://github.com/ros2/rclcpp/issues/1386>`_)
-* Derive and throw exception in spin_some spin_all for StaticSingleThreadedExecutor (`#1385 <https://github.com/ros2/rclcpp/issues/1385>`_)
 * 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>`_)
-* Unit tests for node interfaces (`#1202 <https://github.com/ros2/rclcpp/issues/1202>`_)
-* Unit tests for some header-only functions/classes (`#1181 <https://github.com/ros2/rclcpp/issues/1181>`_)
-* Add unit tests for logging functionality (`#1184 <https://github.com/ros2/rclcpp/issues/1184>`_)
+* 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>`_)
-* Check period duration in create_wall_timer (`#1178 <https://github.com/ros2/rclcpp/issues/1178>`_)
+* 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>`_)
-* Remove finalization of guard_condition from GraphListener::__shutdown() (`#1401 <https://github.com/ros2/rclcpp/issues/1401>`_)
-* Contributors: Alejandro Hernández Cordero, Chen Lihui, Chris Lalancette, Christophe Bedard, Devin Bonnie, Dirk Thomas, Jacob Perron, Jorge Perez, Louise Poubel, Michel Hidalgo, Scott K Logan, Stephen Brawner, tomoya
+* 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
 
-2.2.0 (2020-10-07)
+3.0.0 (2020-06-18)
 ------------------
-* Fix implementation of NodeOptions::use_global_arguments() (`#1176 <https://github.com/ros2/rclcpp/issues/1176>`_) (`#1372 <https://github.com/ros2/rclcpp/issues/1372>`_)
-* Fix conversion of negative durations to messages (`#1188 <https://github.com/ros2/rclcpp/issues/1188>`_) (`#1371 <https://github.com/ros2/rclcpp/issues/1371>`_)
-* Call vector.erase with end iterator overload (`#1314 <https://github.com/ros2/rclcpp/issues/1314>`_) (`#1380 <https://github.com/ros2/rclcpp/issues/1380>`_)
-* Check waitable for nullptr during constructor (`#1315 <https://github.com/ros2/rclcpp/issues/1315>`_) (`#1379 <https://github.com/ros2/rclcpp/issues/1379>`_)
-* Fix pub/sub count API tests. (`#1203 <https://github.com/ros2/rclcpp/issues/1203>`_) (`#1319 <https://github.com/ros2/rclcpp/issues/1319>`_)
-* Reorganize test directory and split CMakeLists.txt (`#1173 <https://github.com/ros2/rclcpp/issues/1173>`_) (`#1262 <https://github.com/ros2/rclcpp/issues/1262>`_)
-* Add operator!= for duration (`#1236 <https://github.com/ros2/rclcpp/issues/1236>`_) (`#1278 <https://github.com/ros2/rclcpp/issues/1278>`_)
-* Contributors: Ivan Santiago Paunovic, Jacob Perron, Jannik Abbenseth, Johannes Meyer, Michel Hidalgo, Stephen Brawner
-
-2.1.0 (2020-08-03)
-------------------
-* Warn about unused result of add_on_set_parameters_callback (`#1238 <https://github.com/ros2/rclcpp/issues/1238>`_) (`#1244 <https://github.com/ros2/rclcpp/issues/1244>`_)
-* Add missing RCLCPP_PUBLIC to ~StaticExecutorEntitiesCollector (`#1227 <https://github.com/ros2/rclcpp/issues/1227>`_) (`#1228 <https://github.com/ros2/rclcpp/issues/1228>`_)
-* Remove recreation of entities_collector (`#1217 <https://github.com/ros2/rclcpp/issues/1217>`_) (`#1224 <https://github.com/ros2/rclcpp/issues/1224>`_)
-* Contributors: Jacob Perron, brawner
-
-2.0.2 (2020-07-07)
-------------------
-* Link against thread library where necessary (`#1210 <https://github.com/ros2/rclcpp/issues/1210>`_) (`#1214 <https://github.com/ros2/rclcpp/issues/1214>`_)
-* Fix exception message on rcl_clock_init (`#1182 <https://github.com/ros2/rclcpp/issues/1182>`_) (`#1193 <https://github.com/ros2/rclcpp/issues/1193>`_)
-* Contributors: Dirk Thomas, tomoya
-
-2.0.1 (2020-06-23)
-------------------
-* Add create_publisher include to create_subscription (`#1180 <https://github.com/ros2/rclcpp/issues/1180>`_) (`#1192 <https://github.com/ros2/rclcpp/issues/1192>`_)
-* Fix get_node_time_source_interface() docstring (`#988 <https://github.com/ros2/rclcpp/issues/988>`_) (`#1185 <https://github.com/ros2/rclcpp/issues/1185>`_)
-* Fixed doxygen warnings (`#1163 <https://github.com/ros2/rclcpp/issues/1163>`_) (`#1191 <https://github.com/ros2/rclcpp/issues/1191>`_)
+* Check period duration in create_wall_timer (`#1178 <https://github.com/ros2/rclcpp/issues/1178>`_)
+* Fix get_node_time_source_interface() docstring (`#988 <https://github.com/ros2/rclcpp/issues/988>`_)
+* Add message lost subscription event (`#1164 <https://github.com/ros2/rclcpp/issues/1164>`_)
+* Add spin_all method to Executor (`#1156 <https://github.com/ros2/rclcpp/issues/1156>`_)
+* Reorganize test directory and split CMakeLists.txt (`#1173 <https://github.com/ros2/rclcpp/issues/1173>`_)
 * Check if context is valid when looping in spin_some (`#1167 <https://github.com/ros2/rclcpp/issues/1167>`_)
+* Add check for invalid topic statistics publish period (`#1151 <https://github.com/ros2/rclcpp/issues/1151>`_)
 * Fix spin_until_future_complete: check spinning value (`#1023 <https://github.com/ros2/rclcpp/issues/1023>`_)
-  Make Executor::spin_once_impl private before backporting, to avoid API compatibility issues
-* Add check for invalid topic statistics publish period (`#1151 <https://github.com/ros2/rclcpp/issues/1151>`_) (`#1172 <https://github.com/ros2/rclcpp/issues/1172>`_)
-* Contributors: Alejandro Hernández Cordero, Devin Bonnie, DongheeYe, Ivan Santiago Paunovic, Jacob Perron, Stephen Brawner
+* Fix doxygen warnings (`#1163 <https://github.com/ros2/rclcpp/issues/1163>`_)
+* Fix reference to rclcpp in its Quality declaration (`#1161 <https://github.com/ros2/rclcpp/issues/1161>`_)
+* Allow spin_until_future_complete to accept any future like object (`#1113 <https://github.com/ros2/rclcpp/issues/1113>`_)
+* Contributors: Alejandro Hernández Cordero, Christophe Bedard, Devin Bonnie, Dirk Thomas, DongheeYe, Ivan Santiago Paunovic, Jacob Perron, Sarthak Mittal, brawner, tomoya
 
 2.0.0 (2020-06-01)
 ------------------

rclcpp/QUALITY_DECLARATION.md

@@ -2,15 +2,15 @@ This document is a declaration of software quality for the `rclcpp` package, bas
 
 # rclcpp Quality Declaration
 
-The package `rclcpp` claims to be in the **Quality Level 1** category when it is used with a **Quality Level 1** middleware.
+The package `rclcpp` claims to be in the **Quality Level 3** category.
 
-Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html) of the ROS2 developer guide.
+Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-quality-categories) of the ROS2 developer guide.
 
 ## Version Policy [1]
 
 ### Version Scheme [1.i]
 
-`rclcpp` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#versioning).
+`rclcpp` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning).
 
 ### Version Stability [1.ii]
 
@@ -39,11 +39,11 @@ Headers under the folder `detail` are not considered part of the public API and
 
 ## Change Control Process [2]
 
-`rclcpp` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#change-control-process).
+`rclcpp` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process).
 
 ### Change Requests [2.i]
 
-All changes will occur through a pull request, check [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#change-control-process) for additional information.
+All changes will occur through a pull request, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information.
 
 ### Contributor Origin [2.ii]
 
@@ -51,17 +51,18 @@ This package uses DCO as its confirmation of contributor origin policy. More inf
 
 ### Peer Review Policy [2.iii]
 
-All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#change-control-process) for additional information.
+All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information.
 
 ### Continuous Integration [2.iv]
 
 All pull requests must pass CI on all [tier 1 platforms](https://www.ros.org/reps/rep-2000.html#support-tiers)
 
-Though there are no nightly jobs for foxy outside of linux, each change is tested on ci.ros2.org.
-* [linux-aarch64](https://ci.ros2.org/job/ci_linux-aarch64)
-* [linux](https://ci.ros2.org/job/ci_linux)
-* [mac_osx](https://ci.ros2.org/job/ci_osx)
-* [windows](https://ci.ros2.org/job/ci_windows)
+Currently nightly results can be seen here:
+
+* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp/)
+* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp/)
+* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp/)
+* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp/)
 
 ###  Documentation Policy [2.v]
 
@@ -71,36 +72,37 @@ All pull requests must resolve related documentation changes before merging.
 
 ### Feature Documentation [3.i]
 
-`rclcpp` has a [feature list](http://docs.ros2.org/foxy/api/rclcpp/) and each item in the list links to the corresponding feature documentation. There is documentation for all of the features, and new features require documentation before being added.
+`rclcpp` has a [feature list](http://docs.ros2.org/latest/api/rclcpp/) and each item in the list links to the corresponding feature documentation. There is documentation for all of the features, and new features require documentation before being added.
 
 ### Public API Documentation [3.ii]
 
-The API is publicly available in its [ROS 2 API documentation](http://docs.ros2.org/foxy/api/rclcpp/).
+The API is publicly available in its [ROS 2 API documentation](http://docs.ros2.org/latest/api/rclcpp/).
 
 ### License [3.iii]
 
 The license for `rclcpp` is Apache 2.0, and a summary is in each source file, the type is declared in the [`package.xml`](./package.xml) manifest file, and a full copy of the license is in the [`LICENSE`](../LICENSE) file.
 
-There is an automated test which runs a linter that ensures each file has a license statement. [Here](http://build.ros2.org/view/Fpr/job/Fpr__rclcpp__ubuntu_focal_amd64/lastCompletedBuild/testReport/rclcpp/) can be found a list with the latest results of the various linters being run on the package.
+There is an automated test which runs a linter that ensures each file has a license statement. [Here](http://build.ros2.org/view/Fpr/job/Fpr__rclcpp__ubuntu_focal_amd64/lastSuccessfulBuild/testReport/rclcpp/) can be found a list with the latest results of the various linters being run on the package.
 
 ### Copyright Statements [3.iv]
 
 The copyright holders each provide a statement of copyright in each source code file in `rclcpp`.
 
-There is an automated test which runs a linter that ensures each file has at least one copyright statement. Latest linter result report can be seen [here](http://build.ros2.org/view/Fpr/job/Fpr__rclcpp__ubuntu_focal_amd64/lastCompletedBuild/testReport/rclcpp/copyright/).
+There is an automated test which runs a linter that ensures each file has at least one copyright statement. Latest linter result report can be seen [here](http://build.ros2.org/view/Fpr/job/Fpr__rclcpp__ubuntu_focal_amd64/lastSuccessfulBuild/testReport/rclcpp/copyright/).
 
 ## Testing [4]
 
 ### Feature Testing [4.i]
 
-Each feature in `rclcpp` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp/tree/foxy/test) directory.
+Each feature in `rclcpp` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp/tree/master/test) directory.
 New features are required to have tests before being added.
 
-Though there are no nightly jobs for foxy outside of linux, each change is tested on ci.ros2.org.
-* [linux-aarch64](https://ci.ros2.org/job/ci_linux-aarch64)
-* [linux](https://ci.ros2.org/job/ci_linux)
-* [mac_osx](https://ci.ros2.org/job/ci_osx)
-* [windows](https://ci.ros2.org/job/ci_windows)
+Currently nightly test results can be seen here:
+
+* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp/)
+* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp/)
+* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp/)
+* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp/)
 
 ### Public API Testing [4.ii]
 
@@ -109,7 +111,7 @@ The tests aim to cover both typical usage and corner cases, but are quantified b
 
 ### Coverage [4.iii]
 
-`rclcpp` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#code-coverage), and opts to use line coverage instead of branch coverage.
+`rclcpp` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#code-coverage), and opts to use line coverage instead of branch coverage.
 
 This includes:
 
@@ -119,27 +121,21 @@ This includes:
 
 Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers.
 
-Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_foxy_coverage/lastCompletedBuild/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://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#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.
+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).
 
 ### Performance [4.iv]
 
-`rclcpp` follows the recommendations for performance testing of C/C++ code in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#performance), and opts to do performance analysis on each release rather than each change.
-
-The performance tests of `rclcpp` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/foxy/rclcpp/test/benchmark).
-
-System level performance benchmarks that cover features of `rclcpp` can be found at:
-* [Benchmarks](http://build.ros2.org/view/Fci/job/Fci__benchmark_ubuntu_focal_amd64/BenchmarkTable/)
-* [Performance](http://build.ros2.org/view/Fci/job/Fci__nightly-performance_ubuntu_focal_amd64/lastCompletedBuild/)
-
-Changes that introduce regressions in performance must be adequately justified in order to be accepted and merged.
+It is not yet defined if this package requires performance testing and how addresses this topic.
 
 ### Linters and Static Analysis [4.v]
 
-`rclcpp` uses and passes all the ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/foxy/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms.
+`rclcpp` uses and passes all the ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms.
 
-Currently nightly test results can be seen [here](http://build.ros2.org/view/Fpr/job/Fpr__rclcpp__ubuntu_focal_amd64/lastCompletedBuild/testReport/rclcpp).
+Currently nightly test results can be seen here:
+* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp/)
+* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp/)
+* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp/)
+* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp/)
 
 ## Dependencies [5]
 
@@ -157,49 +153,49 @@ It also has several test dependencies, which do not affect the resulting quality
 
 The `libstatistics_collector` package provides lightweight aggregation utilities to collect statistics and measure message metrics.
 
-It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros-tooling/libstatistics_collector/tree/foxy-devel/QUALITY_DECLARATION.md).
+It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros-tooling/libstatistics_collector/tree/master/QUALITY_DECLARATION.md).
 
 #### `rcl`
 
 `rcl` a library to support implementation of language specific ROS 2 Client Libraries.
 
-It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/foxy/rcl/QUALITY_DECLARATION.md).
+It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl/QUALITY_DECLARATION.md).
 
 #### `rcl_yaml_param_parser`
 
 The `rcl_yaml_param_parser` package provides an API that is used to parse YAML configuration files which may be used to configure ROS and specific nodes.
 
-It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/tree/foxy/rcl_yaml_param_parser/QUALITY_DECLARATION.md).
+It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rcl/tree/master/rcl_yaml_param_parser/QUALITY_DECLARATION.md).
 
 #### `rcpputils`
 
 The `rcpputils` package provides an API which contains common utilities and data structures useful when programming in C++.
 
-It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcpputils/blob/foxy/QUALITY_DECLARATION.md).
+It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rcpputils/blob/master/QUALITY_DECLARATION.md).
 
 #### `rcutils`
 
 The `rcutils` package provides an API which contains common utilities and data structures useful when programming in C.
 
-It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcutils/blob/foxy/QUALITY_DECLARATION.md).
+It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rcutils/blob/master/QUALITY_DECLARATION.md).
 
 #### `rmw`
 
 `rmw` is the ROS 2 middleware library.
 
-It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rmw/blob/foxy/rmw/QUALITY_DECLARATION.md).
+It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rmw/blob/master/rmw/QUALITY_DECLARATION.md).
 
 #### `statistics_msgs`
 
 The `statistics_msgs` package contains ROS 2 message definitions for reporting statistics for topics and system resources.
 
-It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/foxy/statistics_msgs/QUALITY_DECLARATION.md).
+It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/statistics_msgs/QUALITY_DECLARATION.md).
 
 #### `tracetools`
 
 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 1**, see its [Quality Declaration document](https://gitlab.com/ros-tracing/ros2_tracing/-/blob/foxy/tracetools/QUALITY_DECLARATION.md).
+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).
 
 ### Direct Runtime non-ROS Dependency [5.iii]
 
@@ -209,11 +205,11 @@ It is **Quality Level 1**, see its [Quality Declaration document](https://gitlab
 
 `rclcpp` supports all of the tier 1 platforms as described in [REP-2000](https://www.ros.org/reps/rep-2000.html#support-tiers), and tests each change against all of them.
 
-Though there are no nightly jobs for foxy outside of linux, each change is tested on ci.ros2.org.
-* [linux-aarch64](https://ci.ros2.org/job/ci_linux-aarch64)
-* [linux](https://ci.ros2.org/job/ci_linux)
-* [mac_osx](https://ci.ros2.org/job/ci_osx)
-* [windows](https://ci.ros2.org/job/ci_windows)
+Currently nightly build status can be seen here:
+* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/rclcpp/)
+* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/rclcpp/)
+* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/rclcpp/)
+* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/rclcpp/)
 
 ## Security
 

rclcpp/README.md

@@ -6,4 +6,4 @@ Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) fo
 
 ## Quality Declaration
 
-This package claims to be in the **Quality Level 1** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details.
+This package claims to be in the **Quality Level 3** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details.

rclcpp/include/rclcpp/any_service_callback.hpp

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

rclcpp/include/rclcpp/any_subscription_callback.hpp

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

rclcpp/include/rclcpp/callback_group.hpp

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

rclcpp/include/rclcpp/client.hpp

@@ -361,7 +361,8 @@ public:
     SharedPromiseWithRequest promise = std::make_shared<PromiseWithRequest>();
     SharedFutureWithRequest future_with_request(promise->get_future());
 
-    auto wrapping_cb = [future_with_request, promise, request, &cb](SharedFuture future) {
+    auto wrapping_cb = [future_with_request, promise, request,
+        cb = std::forward<CallbackWithRequestType>(cb)](SharedFuture future) {
         auto response = future.get();
         promise->set_value(std::make_pair(request, response));
         cb(future_with_request);

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,14 +117,8 @@ create_subscription(
       rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>
       >(node_topics->get_node_base_interface()->get_name(), publisher);
 
-    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 sub_call_back = [subscription_topic_stats]() {
+        subscription_topic_stats->publish_message();
       };
 
     auto node_timer_interface = node_topics->get_node_timers_interface();

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
@@ -160,7 +279,7 @@ public:
   void
   spin_node_some(std::shared_ptr<rclcpp::Node> node);
 
-  /// Complete all available queued work without blocking.
+  /// Collect work once and execute all available work, optionally within a duration.
   /**
    * This function can be overridden. The default implementation is suitable for a
    * single-threaded model of execution.
@@ -175,6 +294,23 @@ public:
   virtual void
   spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0));
 
+  /// Collect and execute work repeatedly within a duration or until no more work is available.
+  /**
+   * This function can be overridden. The default implementation is suitable for a
+   * single-threaded model of execution.
+   * Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function
+   * to block (which may have unintended consequences).
+   * If the time that waitables take to be executed is longer than the period on which new waitables
+   * become ready, this method will execute work repeatedly until `max_duration` has elapsed.
+   *
+   * \param[in] max_duration The maximum amount of time to spend executing work. Must be positive.
+   * Note that spin_all() may take longer than this time as it only returns once max_duration has
+   * been exceeded.
+   */
+  RCLCPP_PUBLIC
+  virtual void
+  spin_all(std::chrono::nanoseconds max_duration);
+
   RCLCPP_PUBLIC
   virtual void
   spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
@@ -189,10 +325,10 @@ public:
    *   code.
    * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
    */
-  template<typename ResponseT, typename TimeRepT = int64_t, typename TimeT = std::milli>
+  template<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::milli>
   FutureReturnCode
   spin_until_future_complete(
-    const std::shared_future<ResponseT> & future,
+    const FutureT & future,
     std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
   {
     // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
@@ -270,6 +406,10 @@ protected:
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
     std::chrono::nanoseconds timeout);
 
+  RCLCPP_PUBLIC
+  void
+  spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive);
+
   /// Find the next available executable and do the work associated with it.
   /**
    * \param[in] any_exec Union structure that can hold any executable type (timer, subscription,
@@ -306,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;
 
@@ -342,13 +539,29 @@ protected:
 
   RCLCPP_DISABLE_COPY(Executor)
 
-  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
-  std::list<const rcl_guard_condition_t *> guard_conditions_;
-
-private:
   RCLCPP_PUBLIC
   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_;
 };
 
 namespace executor

rclcpp/include/rclcpp/executors.hpp

@@ -66,12 +66,12 @@ using rclcpp::executors::SingleThreadedExecutor;
  *   If the time spent inside the blocking loop exceeds this timeout, return a `TIMEOUT` return code.
  * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
  */
-template<typename ResponseT, typename TimeRepT = int64_t, typename TimeT = std::milli>
+template<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::milli>
 rclcpp::FutureReturnCode
 spin_node_until_future_complete(
   rclcpp::Executor & executor,
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  const std::shared_future<ResponseT> & future,
+  const FutureT & future,
   std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
@@ -82,13 +82,13 @@ spin_node_until_future_complete(
   return retcode;
 }
 
-template<typename NodeT = rclcpp::Node, typename ResponseT, typename TimeRepT = int64_t,
+template<typename NodeT = rclcpp::Node, typename FutureT, typename TimeRepT = int64_t,
   typename TimeT = std::milli>
 rclcpp::FutureReturnCode
 spin_node_until_future_complete(
   rclcpp::Executor & executor,
   std::shared_ptr<NodeT> node_ptr,
-  const std::shared_future<ResponseT> & future,
+  const FutureT & future,
   std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   return rclcpp::executors::spin_node_until_future_complete(
@@ -104,7 +104,7 @@ template<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::mi
 rclcpp::FutureReturnCode
 spin_until_future_complete(
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  const std::shared_future<FutureT> & future,
+  const FutureT & future,
   std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   rclcpp::executors::SingleThreadedExecutor executor;
@@ -116,7 +116,7 @@ template<typename NodeT = rclcpp::Node, typename FutureT, typename TimeRepT = in
 rclcpp::FutureReturnCode
 spin_until_future_complete(
   std::shared_ptr<NodeT> node_ptr,
-  const std::shared_future<FutureT> & future,
+  const FutureT & future,
   std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout);

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

rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp

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

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

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

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

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

rclcpp/include/rclcpp/init_options.hpp

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

rclcpp/include/rclcpp/logger.hpp

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

rclcpp/include/rclcpp/memory_strategy.hpp

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

rclcpp/include/rclcpp/node.hpp

@@ -141,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
@@ -161,7 +163,7 @@ public:
    * pub = node->create_publisher<MsgT>("chatter", QoS(10));  // implicitly KeepLast
    * pub = node->create_publisher<MsgT>("chatter", QoS(KeepLast(10)));
    * pub = node->create_publisher<MsgT>("chatter", QoS(KeepAll()));
-   * pub = node->create_publisher<MsgT>("chatter", QoS(1).best_effort().durability_volatile());
+   * pub = node->create_publisher<MsgT>("chatter", QoS(1).best_effort().volatile());
    * {
    *   rclcpp::QoS custom_qos(KeepLast(10), rmw_qos_profile_sensor_data);
    *   pub = node->create_publisher<MsgT>("chatter", custom_qos);
@@ -845,26 +847,6 @@ public:
   void
   remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler);
 
-  /// Register a callback to be called anytime a parameter is about to be changed.
-  /**
-   * \deprecated Use add_on_set_parameters_callback instead.
-   * With this method, only one callback can be set at a time. The callback that was previously
-   * set by this method is returned or `nullptr` if no callback was previously set.
-   *
-   * The callbacks added with `add_on_set_parameters_callback` are stored in a different place.
-   * `remove_on_set_parameters_callback` can't be used with the callbacks registered with this
-   * method. For removing it, use `set_on_parameters_set_callback(nullptr)`.
-   *
-   * \param[in] callback The callback to be called when the value for a
-   *   parameter is about to be set.
-   * \return The previous callback that was registered, if there was one,
-   *   otherwise nullptr.
-   */
-  [[deprecated("use add_on_set_parameters_callback(OnParametersSetCallbackType callback) instead")]]
-  RCLCPP_PUBLIC
-  OnParametersSetCallbackType
-  set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback);
-
   /// Get the fully-qualified names of all available nodes.
   /**
    * The fully-qualified name includes the local namespace and name of the node.
@@ -905,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
@@ -934,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.
@@ -960,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.
@@ -1194,10 +1182,6 @@ protected:
 private:
   RCLCPP_DISABLE_COPY(Node)
 
-  RCLCPP_PUBLIC
-  bool
-  group_in_node(CallbackGroup::SharedPtr group);
-
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
   rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -170,11 +170,6 @@ public:
   void
   remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) override;
 
-  [[deprecated("use add_on_set_parameters_callback(OnParametersSetCallbackType callback) instead")]]
-  RCLCPP_PUBLIC
-  OnParametersSetCallbackType
-  set_on_parameters_set_callback(OnParametersSetCallbackType callback) override;
-
   RCLCPP_PUBLIC
   const std::map<std::string, rclcpp::ParameterValue> &
   get_parameter_overrides() const override;

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -191,17 +191,6 @@ public:
   void
   remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) = 0;
 
-  /// Register a callback for when parameters are being set, return an existing one.
-  /**
-   * \deprecated Use add_on_set_parameters_callback instead.
-   * \sa rclcpp::Node::set_on_parameters_set_callback
-   */
-  [[deprecated("use add_on_set_parameters_callback(OnParametersSetCallbackType callback) instead")]]
-  RCLCPP_PUBLIC
-  virtual
-  OnParametersSetCallbackType
-  set_on_parameters_set_callback(OnParametersSetCallbackType callback) = 0;
-
   /// Return the initial parameter values used by the NodeParameters to override default values.
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/node_options.hpp

@@ -327,11 +327,6 @@ public:
   NodeOptions &
   allocator(rcl_allocator_t allocator);
 
-protected:
-  /// Retrieve the ROS_DOMAIN_ID environment variable and populate options.
-  size_t
-  get_domain_id_from_env() const;
-
 private:
   // This is mutable to allow for a const accessor which lazily creates the node options instance.
   /// Underlying rcl_node_options structure.

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>>
@@ -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/publisher.hpp

@@ -68,7 +68,7 @@ public:
    * \param[in] node_base NodeBaseInterface pointer that is used in part of the setup.
    * \param[in] topic Name of the topic to publish to.
    * \param[in] qos QoS profile for Subcription.
-   * \param[in] options options for the subscription.
+   * \param[in] options Options for the subscription.
    */
   Publisher(
     rclcpp::node_interfaces::NodeBaseInterface * node_base,

rclcpp/include/rclcpp/publisher_options.hpp

@@ -92,7 +92,12 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
   get_allocator() const
   {
     if (!this->allocator) {
-      return std::make_shared<Allocator>();
+      // TODO(wjwwood): I would like to use the commented line instead, but
+      //   cppcheck 1.89 fails with:
+      //     Syntax Error: AST broken, binary operator '>' doesn't have two operands.
+      // return std::make_shared<Allocator>();
+      std::shared_ptr<Allocator> tmp(new Allocator());
+      return tmp;
     }
     return this->allocator;
   }

rclcpp/include/rclcpp/qos.hpp

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

rclcpp/include/rclcpp/qos_event.hpp

@@ -34,6 +34,7 @@ using QOSDeadlineRequestedInfo = rmw_requested_deadline_missed_status_t;
 using QOSDeadlineOfferedInfo = rmw_offered_deadline_missed_status_t;
 using QOSLivelinessChangedInfo = rmw_liveliness_changed_status_t;
 using QOSLivelinessLostInfo = rmw_liveliness_lost_status_t;
+using QOSMessageLostInfo = rmw_message_lost_status_t;
 using QOSOfferedIncompatibleQoSInfo = rmw_offered_qos_incompatible_event_status_t;
 using QOSRequestedIncompatibleQoSInfo = rmw_requested_qos_incompatible_event_status_t;
 
@@ -41,6 +42,7 @@ using QOSDeadlineRequestedCallbackType = std::function<void (QOSDeadlineRequeste
 using QOSDeadlineOfferedCallbackType = std::function<void (QOSDeadlineOfferedInfo &)>;
 using QOSLivelinessChangedCallbackType = std::function<void (QOSLivelinessChangedInfo &)>;
 using QOSLivelinessLostCallbackType = std::function<void (QOSLivelinessLostInfo &)>;
+using QOSMessageLostCallbackType = std::function<void (QOSMessageLostInfo &)>;
 using QOSOfferedIncompatibleQoSCallbackType = std::function<void (QOSOfferedIncompatibleQoSInfo &)>;
 using QOSRequestedIncompatibleQoSCallbackType =
   std::function<void (QOSRequestedIncompatibleQoSInfo &)>;
@@ -59,6 +61,7 @@ struct SubscriptionEventCallbacks
   QOSDeadlineRequestedCallbackType deadline_callback;
   QOSLivelinessChangedCallbackType liveliness_callback;
   QOSRequestedIncompatibleQoSCallbackType incompatible_qos_callback;
+  QOSMessageLostCallbackType message_lost_callback;
 };
 
 class UnsupportedEventTypeException : public exceptions::RCLErrorBase, public std::runtime_error

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

@@ -92,7 +92,7 @@ public:
    * \param[in] topic_name Name of the topic to subscribe to.
    * \param[in] qos QoS profile for Subcription.
    * \param[in] callback User defined callback to call when a message is received.
-   * \param[in] options options for the subscription.
+   * \param[in] options Options for the subscription.
    * \param[in] message_memory_strategy The memory strategy to be used for managing message memory.
    * \param[in] subscription_topic_statistics Optional pointer to a topic statistics subcription.
    * \throws std::invalid_argument if the QoS is uncompatible with intra-process (if one
@@ -144,6 +144,11 @@ public:
         // pass
       }
     }
+    if (options.event_callbacks.message_lost_callback) {
+      this->add_event_handler(
+        options.event_callbacks.message_lost_callback,
+        RCL_SUBSCRIPTION_MESSAGE_LOST);
+    }
 
     // Setup intra process publishing if requested.
     if (rclcpp::detail::resolve_use_intra_process(options, *node_base)) {
@@ -179,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;
@@ -195,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/subscription_base.hpp

@@ -66,7 +66,7 @@ public:
    * \param[in] node_base NodeBaseInterface pointer used in parts of the setup.
    * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic.
    * \param[in] topic_name Name of the topic to subscribe to.
-   * \param[in] subscription_options options for the subscription.
+   * \param[in] subscription_options Options for the subscription.
    * \param[in] is_serialized is true if the message will be delivered still serialized
    */
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/subscription_options.hpp

@@ -101,7 +101,7 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
     rcl_subscription_options_t result = rcl_subscription_get_default_options();
     using AllocatorTraits = std::allocator_traits<Allocator>;
     using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
-    auto message_alloc = std::make_shared<MessageAllocatorT>(*this->get_allocator().get());
+    auto message_alloc = std::make_shared<MessageAllocatorT>(*allocator.get());
     result.allocator = allocator::get_rcl_allocator<MessageT>(*message_alloc);
     result.qos = qos.get_rmw_qos_profile();
     result.rmw_subscription_options.ignore_local_publications = this->ignore_local_publications;

rclcpp/include/rclcpp/time.hpp

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

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

rclcpp/include/rclcpp/timer.hpp

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

rclcpp/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp</name>
-  <version>2.3.1</version>
+  <version>5.0.0</version>
   <description>The ROS client library in C++.</description>
   <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
   <license>Apache License 2.0</license>
@@ -36,7 +36,6 @@
   <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

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

rclcpp/src/rclcpp/executor.cpp

@@ -14,21 +14,25 @@
 
 #include <algorithm>
 #include <memory>
+#include <map>
 #include <string>
 #include <type_traits>
+#include <utility>
+#include <vector>
 
 #include "rcl/allocator.h"
 #include "rcl/error_handling.h"
 
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/executor.hpp"
-#include "rclcpp/executors/static_single_threaded_executor.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/scope_exit.hpp"
 #include "rclcpp/utilities.hpp"
 
 #include "rcutils/logging_macros.h"
 
+using namespace std::chrono_literals;
+
 using rclcpp::exceptions::throw_from_rcl_error;
 using rclcpp::AnyExecutable;
 using rclcpp::Executor;
@@ -75,25 +79,39 @@ Executor::Executor(const rclcpp::ExecutorOptions & options)
         "failed to destroy guard condition: %s", rcl_get_error_string().str);
       rcl_reset_error();
     }
-    throw_from_rcl_error(ret, "Failed to create wait set in Executor constructor");
+    throw std::runtime_error("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();
+  // Disassocate all callback groups.
+  for (auto & pair : weak_groups_to_nodes_) {
+    auto group = pair.first.lock();
+    if (group) {
+      std::atomic_bool & has_executor = group->get_associated_with_executor_atomic();
       has_executor.store(false);
     }
   }
+  // Disassocate all nodes.
+  std::for_each(
+    weak_nodes_.begin(), weak_nodes_.end(), []
+      (rclcpp::node_interfaces::NodeBaseInterface::WeakPtr weak_node_ptr) {
+      auto shared_node_ptr = weak_node_ptr.lock();
+      if (shared_node_ptr) {
+        std::atomic_bool & has_executor = shared_node_ptr->get_associated_with_executor_atomic();
+        has_executor.store(false);
+      }
+    });
   weak_nodes_.clear();
-  for (auto & guard_condition : guard_conditions_) {
+  weak_groups_associated_with_executor_to_nodes_.clear();
+  weak_groups_to_nodes_associated_with_executor_.clear();
+  weak_groups_to_nodes_.clear();
+  for (const auto & pair : weak_nodes_to_guard_conditions_) {
+    auto & guard_condition = pair.second;
     memory_strategy_->remove_guard_condition(guard_condition);
   }
-  guard_conditions_.clear();
+  weak_nodes_to_guard_conditions_.clear();
 
   // Finalize the wait set.
   if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) {
@@ -114,6 +132,116 @@ Executor::~Executor()
   context_->release_interrupt_guard_condition(&wait_set_, std::nothrow);
 }
 
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+Executor::get_all_callback_groups()
+{
+  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
+  for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
+    groups.push_back(group_node_ptr.first);
+  }
+  for (auto const & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) {
+    groups.push_back(group_node_ptr.first);
+  }
+  return groups;
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+Executor::get_manually_added_callback_groups()
+{
+  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
+  for (auto const & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
+    groups.push_back(group_node_ptr.first);
+  }
+  return groups;
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+Executor::get_automatically_added_callback_groups_from_nodes()
+{
+  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
+  for (auto const & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) {
+    groups.push_back(group_node_ptr.first);
+  }
+  return groups;
+}
+
+void
+Executor::add_callback_groups_from_nodes_associated_to_executor()
+{
+  for (auto & weak_node : weak_nodes_) {
+    auto node = weak_node.lock();
+    if (node) {
+      auto group_ptrs = node->get_callback_groups();
+      std::for_each(
+        group_ptrs.begin(), group_ptrs.end(),
+        [this, node](rclcpp::CallbackGroup::WeakPtr group_ptr)
+        {
+          auto shared_group_ptr = group_ptr.lock();
+          if (shared_group_ptr && shared_group_ptr->automatically_add_to_executor_with_node() &&
+          !shared_group_ptr->get_associated_with_executor_atomic().load())
+          {
+            this->add_callback_group_to_map(
+              shared_group_ptr,
+              node,
+              weak_groups_to_nodes_associated_with_executor_,
+              true);
+          }
+        });
+    }
+  }
+}
+
+void
+Executor::add_callback_group_to_map(
+  rclcpp::CallbackGroup::SharedPtr group_ptr,
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
+  bool notify)
+{
+  // If the callback_group already has an executor
+  std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic();
+  if (has_executor.exchange(true)) {
+    throw std::runtime_error("Callback group has already been added to an executor.");
+  }
+  bool is_new_node = !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) &&
+    !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_);
+  rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
+  auto insert_info =
+    weak_groups_to_nodes.insert(std::make_pair(weak_group_ptr, node_ptr));
+  bool was_inserted = insert_info.second;
+  if (!was_inserted) {
+    throw std::runtime_error("Callback group was already added to executor.");
+  }
+  // Also add to the map that contains all callback groups
+  weak_groups_to_nodes_.insert(std::make_pair(weak_group_ptr, node_ptr));
+  if (is_new_node) {
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr);
+    weak_nodes_to_guard_conditions_[node_weak_ptr] = node_ptr->get_notify_guard_condition();
+    if (notify) {
+      // Interrupt waiting to handle new node
+      if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+        throw std::runtime_error(rcl_get_error_string().str);
+      }
+    }
+    // Add the node's notify condition to the guard condition handles
+    std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
+    memory_strategy_->add_guard_condition(node_ptr->get_notify_guard_condition());
+  }
+}
+
+void
+Executor::add_callback_group(
+  rclcpp::CallbackGroup::SharedPtr group_ptr,
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  bool notify)
+{
+  this->add_callback_group_to_map(
+    group_ptr,
+    node_ptr,
+    weak_groups_associated_with_executor_to_nodes_,
+    notify);
+}
+
 void
 Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
 {
@@ -122,25 +250,67 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt
   if (has_executor.exchange(true)) {
     throw std::runtime_error("Node has already been added to an executor.");
   }
-  // Check to ensure node not already added
-  for (auto & weak_node : weak_nodes_) {
-    auto node = weak_node.lock();
-    if (node == node_ptr) {
-      // TODO(jacquelinekay): Use a different error here?
-      throw std::runtime_error("Cannot add node to executor, node already added.");
+  for (auto & weak_group : node_ptr->get_callback_groups()) {
+    auto group_ptr = weak_group.lock();
+    if (group_ptr != nullptr && !group_ptr->get_associated_with_executor_atomic().load() &&
+      group_ptr->automatically_add_to_executor_with_node())
+    {
+      this->add_callback_group_to_map(
+        group_ptr,
+        node_ptr,
+        weak_groups_to_nodes_associated_with_executor_,
+        notify);
     }
   }
   weak_nodes_.push_back(node_ptr);
-  guard_conditions_.push_back(node_ptr->get_notify_guard_condition());
-  if (notify) {
-    // Interrupt waiting to handle new node
-    if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-      throw std::runtime_error(rcl_get_error_string().str);
+}
+
+void
+Executor::remove_callback_group_from_map(
+  rclcpp::CallbackGroup::SharedPtr group_ptr,
+  WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
+  bool notify)
+{
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr;
+  rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
+  auto iter = weak_groups_to_nodes.find(weak_group_ptr);
+  if (iter != weak_groups_to_nodes.end()) {
+    node_ptr = iter->second.lock();
+    if (node_ptr == nullptr) {
+      throw std::runtime_error("Node must not be deleted before its callback group(s).");
     }
+    weak_groups_to_nodes.erase(iter);
+    weak_groups_to_nodes_.erase(group_ptr);
+    std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic();
+    has_executor.store(false);
+  } else {
+    throw std::runtime_error("Callback group needs to be associated with executor.");
   }
-  // Add the node's notify condition to the guard condition handles
-  std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
-  memory_strategy_->add_guard_condition(node_ptr->get_notify_guard_condition());
+  // If the node was matched and removed, interrupt waiting.
+  if (!has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) &&
+    !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_))
+  {
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr);
+    weak_nodes_to_guard_conditions_.erase(node_weak_ptr);
+    if (notify) {
+      if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+        throw std::runtime_error(rcl_get_error_string().str);
+      }
+    }
+    std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
+    memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition());
+  }
+}
+
+void
+Executor::remove_callback_group(
+  rclcpp::CallbackGroup::SharedPtr group_ptr,
+  bool notify)
+{
+  this->remove_callback_group_from_map(
+    group_ptr,
+    weak_groups_associated_with_executor_to_nodes_,
+    notify);
 }
 
 void
@@ -152,34 +322,47 @@ Executor::add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify)
 void
 Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
 {
-  bool node_removed = false;
-  {
-    auto node_it = weak_nodes_.begin();
-    auto gc_it = guard_conditions_.begin();
-    while (node_it != weak_nodes_.end()) {
-      bool matched = (node_it->lock() == node_ptr);
-      if (matched) {
-        node_it = weak_nodes_.erase(node_it);
-        gc_it = guard_conditions_.erase(gc_it);
-        node_removed = true;
-      } else {
-        ++node_it;
-        ++gc_it;
-      }
+  if (!node_ptr->get_associated_with_executor_atomic().load()) {
+    throw std::runtime_error("Node needs to be associated with an executor.");
+  }
+
+  bool found_node = false;
+  auto node_it = weak_nodes_.begin();
+  while (node_it != weak_nodes_.end()) {
+    bool matched = (node_it->lock() == node_ptr);
+    if (matched) {
+      found_node = true;
+      node_it = weak_nodes_.erase(node_it);
+    } else {
+      ++node_it;
     }
   }
+  if (!found_node) {
+    throw std::runtime_error("Node needs to be associated with this executor.");
+  }
+  std::vector<rclcpp::CallbackGroup::SharedPtr> found_group_ptrs;
+  std::for_each(
+    weak_groups_to_nodes_associated_with_executor_.begin(),
+    weak_groups_to_nodes_associated_with_executor_.end(),
+    [&found_group_ptrs, node_ptr](std::pair<rclcpp::CallbackGroup::WeakPtr,
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> key_value_pair) {
+      auto weak_node_ptr = key_value_pair.second;
+      auto shared_node_ptr = weak_node_ptr.lock();
+      auto group_ptr = key_value_pair.first.lock();
+      if (shared_node_ptr == node_ptr) {
+        found_group_ptrs.push_back(group_ptr);
+      }
+    });
+  std::for_each(
+    found_group_ptrs.begin(), found_group_ptrs.end(), [this, notify]
+      (rclcpp::CallbackGroup::SharedPtr group_ptr) {
+      remove_callback_group_from_map(
+        group_ptr,
+        weak_groups_to_nodes_associated_with_executor_,
+        notify);
+    });
   std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
   has_executor.store(false);
-  if (notify) {
-    // If the node was matched and removed, interrupt waiting
-    if (node_removed) {
-      if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-        throw std::runtime_error(rcl_get_error_string().str);
-      }
-    }
-  }
-  std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
-  memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition());
 }
 
 void
@@ -213,15 +396,22 @@ Executor::spin_node_some(std::shared_ptr<rclcpp::Node> node)
   this->spin_node_some(node->get_node_base_interface());
 }
 
-void
-Executor::spin_some(std::chrono::nanoseconds max_duration)
+void Executor::spin_some(std::chrono::nanoseconds max_duration)
 {
-  if (nullptr != dynamic_cast<executors::StaticSingleThreadedExecutor *>(this)) {
-    throw rclcpp::exceptions::UnimplementedError(
-            "spin_some is not implemented for StaticSingleThreadedExecutor, use spin or "
-            "spin_until_future_complete");
-  }
+  return this->spin_some_impl(max_duration, false);
+}
 
+void Executor::spin_all(std::chrono::nanoseconds max_duration)
+{
+  if (max_duration <= 0ns) {
+    throw std::invalid_argument("max_duration must be positive");
+  }
+  return this->spin_some_impl(max_duration, true);
+}
+
+void
+Executor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive)
+{
   auto start = std::chrono::steady_clock::now();
   auto max_duration_not_elapsed = [max_duration, start]() {
       if (std::chrono::nanoseconds(0) == max_duration) {
@@ -239,14 +429,20 @@ Executor::spin_some(std::chrono::nanoseconds max_duration)
     throw std::runtime_error("spin_some() called while already spinning");
   }
   RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
-  // non-blocking call to pre-load all available work
-  wait_for_work(std::chrono::milliseconds::zero());
+  bool work_available = false;
   while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) {
     AnyExecutable any_exec;
+    if (!work_available) {
+      wait_for_work(std::chrono::milliseconds::zero());
+    }
     if (get_next_ready_executable(any_exec)) {
       execute_any_executable(any_exec);
+      work_available = true;
     } else {
-      break;
+      if (!work_available || !exhaustive) {
+        break;
+      }
+      work_available = false;
     }
   }
 }
@@ -263,11 +459,6 @@ Executor::spin_once_impl(std::chrono::nanoseconds timeout)
 void
 Executor::spin_once(std::chrono::nanoseconds timeout)
 {
-  if (nullptr != dynamic_cast<executors::StaticSingleThreadedExecutor *>(this)) {
-    throw rclcpp::exceptions::UnimplementedError(
-            "spin_once is not implemented for StaticSingleThreadedExecutor, use spin or "
-            "spin_until_future_complete");
-  }
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin_once() called while already spinning");
   }
@@ -279,9 +470,8 @@ void
 Executor::cancel()
 {
   spinning.store(false);
-  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");
+  if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+    throw std::runtime_error(rcl_get_error_string().str);
   }
 }
 
@@ -319,9 +509,8 @@ 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.
-  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");
+  if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+    throw std::runtime_error(rcl_get_error_string().str);
   }
 }
 
@@ -466,45 +655,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
-    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");
+    if (rcl_wait_set_clear(&wait_set_) != RCL_RET_OK) {
+      throw std::runtime_error("Couldn't clear wait set");
     }
 
     // The size of waitables are accounted for in size of the other entities
-    ret = rcl_wait_set_resize(
+    rcl_ret_t 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_from_rcl_error(ret, "Couldn't resize the wait set");
+      throw std::runtime_error(
+              std::string("Couldn't resize the wait set : ") + rcl_get_error_string().str);
     }
 
     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) {
@@ -522,22 +733,18 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
 }
 
 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
-Executor::get_node_by_group(rclcpp::CallbackGroup::SharedPtr group)
+Executor::get_node_by_group(
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes,
+  rclcpp::CallbackGroup::SharedPtr group)
 {
   if (!group) {
     return nullptr;
   }
-  for (auto & weak_node : weak_nodes_) {
-    auto node = weak_node.lock();
-    if (!node) {
-      continue;
-    }
-    for (auto & weak_group : node->get_callback_groups()) {
-      auto callback_group = weak_group.lock();
-      if (callback_group == group) {
-        return node;
-      }
-    }
+  rclcpp::CallbackGroup::WeakPtr weak_group_ptr(group);
+  const auto finder = weak_groups_to_nodes.find(weak_group_ptr);
+  if (finder != weak_groups_to_nodes.end()) {
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr = finder->second.lock();
+    return node_ptr;
   }
   return nullptr;
 }
@@ -545,80 +752,103 @@ Executor::get_node_by_group(rclcpp::CallbackGroup::SharedPtr group)
 rclcpp::CallbackGroup::SharedPtr
 Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer)
 {
-  for (auto & weak_node : weak_nodes_) {
-    auto node = weak_node.lock();
-    if (!node) {
+  for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) {
+    auto group = pair.first.lock();
+    if (!group) {
       continue;
     }
-    for (auto & weak_group : node->get_callback_groups()) {
-      auto group = weak_group.lock();
-      if (!group) {
-        continue;
-      }
-      auto timer_ref = group->find_timer_ptrs_if(
-        [timer](const rclcpp::TimerBase::SharedPtr & timer_ptr) -> bool {
-          return timer_ptr == timer;
-        });
-      if (timer_ref) {
-        return group;
-      }
+    auto timer_ref = group->find_timer_ptrs_if(
+      [timer](const rclcpp::TimerBase::SharedPtr & timer_ptr) -> bool {
+        return timer_ptr == timer;
+      });
+    if (timer_ref) {
+      return group;
     }
   }
-  return rclcpp::CallbackGroup::SharedPtr();
+
+  for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) {
+    auto group = pair.first.lock();
+    if (!group) {
+      continue;
+    }
+    auto timer_ref = group->find_timer_ptrs_if(
+      [timer](const rclcpp::TimerBase::SharedPtr & timer_ptr) -> bool {
+        return timer_ptr == timer;
+      });
+    if (timer_ref) {
+      return group;
+    }
+  }
+  return nullptr;
 }
 
 bool
 Executor::get_next_ready_executable(AnyExecutable & any_executable)
+{
+  bool success = get_next_ready_executable_from_map(any_executable, weak_groups_to_nodes_);
+  return success;
+}
+
+bool
+Executor::get_next_ready_executable_from_map(
+  AnyExecutable & any_executable,
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes)
 {
   bool success = false;
   // Check the timers to see if there are any that are ready
-  memory_strategy_->get_next_timer(any_executable, weak_nodes_);
+  memory_strategy_->get_next_timer(any_executable, weak_groups_to_nodes);
   if (any_executable.timer) {
     success = true;
   }
   if (!success) {
     // Check the subscriptions to see if there are any that are ready
-    memory_strategy_->get_next_subscription(any_executable, weak_nodes_);
+    memory_strategy_->get_next_subscription(any_executable, weak_groups_to_nodes);
     if (any_executable.subscription) {
       success = true;
     }
   }
   if (!success) {
     // Check the services to see if there are any that are ready
-    memory_strategy_->get_next_service(any_executable, weak_nodes_);
+    memory_strategy_->get_next_service(any_executable, weak_groups_to_nodes);
     if (any_executable.service) {
       success = true;
     }
   }
   if (!success) {
     // Check the clients to see if there are any that are ready
-    memory_strategy_->get_next_client(any_executable, weak_nodes_);
+    memory_strategy_->get_next_client(any_executable, weak_groups_to_nodes);
     if (any_executable.client) {
       success = true;
     }
   }
   if (!success) {
     // Check the waitables to see if there are any that are ready
-    memory_strategy_->get_next_waitable(any_executable, weak_nodes_);
+    memory_strategy_->get_next_waitable(any_executable, weak_groups_to_nodes);
     if (any_executable.waitable) {
       success = true;
     }
   }
-  // At this point any_exec should be valid with either a valid subscription
+  // At this point any_executable should be valid with either a valid subscription
   // or a valid timer, or it should be a null shared_ptr
+  if (success) {
+    rclcpp::CallbackGroup::WeakPtr weak_group_ptr = any_executable.callback_group;
+    auto iter = weak_groups_to_nodes.find(weak_group_ptr);
+    if (iter == weak_groups_to_nodes.end()) {
+      success = false;
+    }
+  }
+
   if (success) {
     // If it is valid, check to see if the group is mutually exclusive or
-    // not, then mark it accordingly
-    using callback_group::CallbackGroupType;
-    if (
-      any_executable.callback_group &&
-      any_executable.callback_group->type() == CallbackGroupType::MutuallyExclusive)
+    // not, then mark it accordingly ..Check if the callback_group belongs to this executor
+    if (any_executable.callback_group && any_executable.callback_group->type() == \
+      CallbackGroupType::MutuallyExclusive)
     {
       // It should not have been taken otherwise
       assert(any_executable.callback_group->can_be_taken_from().load());
       // Set to false to indicate something is being run from this group
-      // This is reset to true either when the any_exec is executed or when the
-      // any_exec is destructued
+      // This is reset to true either when the any_executable is executed or when the
+      // any_executable is destructued
       any_executable.callback_group->can_be_taken_from().store(false);
     }
   }
@@ -645,3 +875,18 @@ Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanos
   }
   return success;
 }
+
+// Returns true iff the weak_groups_to_nodes map has node_ptr as the value in any of its entry.
+bool
+Executor::has_node(
+  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes) const
+{
+  return std::find_if(
+    weak_groups_to_nodes.begin(),
+    weak_groups_to_nodes.end(),
+    [&](const WeakCallbackGroupsToNodesMap::value_type & other) -> bool {
+      auto other_ptr = other.second.lock();
+      return other_ptr == node_ptr;
+    }) != weak_groups_to_nodes.end();
+}

rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp

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

rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp

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

rclcpp/src/rclcpp/graph_listener.cpp

@@ -371,6 +371,17 @@ GraphListener::__shutdown(bool should_throw)
         } else {
           parent_context_ptr->release_interrupt_guard_condition(&wait_set_, std::nothrow);
         }
+      } else {
+        ret = rcl_guard_condition_fini(shutdown_guard_condition_);
+        if (RCL_RET_OK != ret) {
+          if (should_throw) {
+            throw_from_rcl_error(ret, "failed to finalize shutdown guard condition");
+          } else {
+            RCLCPP_ERROR(
+              rclcpp::get_logger("rclcpp"),
+              "failed to finalize shutdown guard condition");
+          }
+        }
       }
       shutdown_guard_condition_ = nullptr;
     }

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 initialize rcl init options");
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialized rcl init options");
   }
 }
 
@@ -64,7 +64,8 @@ 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");
@@ -82,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());
@@ -101,4 +109,38 @@ InitOptions::get_rcl_init_options() const
   return init_options_.get();
 }
 
+void
+InitOptions::use_default_domain_id()
+{
+  size_t domain_id = RCL_DEFAULT_DOMAIN_ID;
+  rcl_ret_t ret = rcl_get_default_domain_id(&domain_id);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get default domain id");
+  }
+  set_domain_id(domain_id);
+}
+
+void
+InitOptions::set_domain_id(size_t domain_id)
+{
+  std::lock_guard<std::mutex> init_options_lock(init_options_mutex_);
+  rcl_ret_t ret = rcl_init_options_set_domain_id(init_options_.get(), domain_id);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to set domain id to rcl init options");
+  }
+}
+
+size_t
+InitOptions::get_domain_id() const
+{
+  std::lock_guard<std::mutex> init_options_lock(init_options_mutex_);
+  size_t domain_id;
+  rcl_ret_t ret = rcl_init_options_get_domain_id(init_options_.get(), &domain_id);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get domain id from rcl init options");
+  }
+
+  return domain_id;
+}
+
 }  // namespace rclcpp

rclcpp/src/rclcpp/logger.cpp

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

rclcpp/src/rclcpp/memory_strategy.cpp

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

rclcpp/src/rclcpp/node.cpp

@@ -211,15 +211,11 @@ Node::get_logger() const
 }
 
 rclcpp::CallbackGroup::SharedPtr
-Node::create_callback_group(rclcpp::CallbackGroupType group_type)
+Node::create_callback_group(
+  rclcpp::CallbackGroupType group_type,
+  bool automatically_add_to_executor_with_node)
 {
-  return node_base_->create_callback_group(group_type);
-}
-
-bool
-Node::group_in_node(rclcpp::CallbackGroup::SharedPtr group)
-{
-  return node_base_->callback_group_in_node(group);
+  return node_base_->create_callback_group(group_type, automatically_add_to_executor_with_node);
 }
 
 const rclcpp::ParameterValue &
@@ -328,28 +324,6 @@ Node::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * co
   return node_parameters_->remove_on_set_parameters_callback(callback);
 }
 
-// suppress deprecated function warning
-#if !defined(_WIN32)
-# pragma GCC diagnostic push
-# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
-#else  // !defined(_WIN32)
-# pragma warning(push)
-# pragma warning(disable: 4996)
-#endif
-
-rclcpp::Node::OnParametersSetCallbackType
-Node::set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback)
-{
-  return node_parameters_->set_on_parameters_set_callback(callback);
-}
-
-// remove warning suppression
-#if !defined(_WIN32)
-# pragma GCC diagnostic pop
-#else  // !defined(_WIN32)
-# pragma warning(pop)
-#endif
-
 std::vector<std::string>
 Node::get_node_names() const
 {

rclcpp/src/rclcpp/node_interfaces/node_base.cpp

@@ -216,11 +216,16 @@ NodeBase::get_shared_rcl_node_handle() const
 }
 
 rclcpp::CallbackGroup::SharedPtr
-NodeBase::create_callback_group(rclcpp::CallbackGroupType group_type)
+NodeBase::create_callback_group(
+  rclcpp::CallbackGroupType group_type,
+  bool automatically_add_to_executor_with_node)
 {
   using rclcpp::CallbackGroup;
   using rclcpp::CallbackGroupType;
-  auto group = CallbackGroup::SharedPtr(new CallbackGroup(group_type));
+  auto group = CallbackGroup::SharedPtr(
+    new CallbackGroup(
+      group_type,
+      automatically_add_to_executor_with_node));
   callback_groups_.push_back(group);
   return group;
 }

rclcpp/src/rclcpp/node_interfaces/node_graph.cpp

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

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -874,18 +874,6 @@ NodeParameters::add_on_set_parameters_callback(OnParametersSetCallbackType callb
   return handle;
 }
 
-NodeParameters::OnParametersSetCallbackType
-NodeParameters::set_on_parameters_set_callback(OnParametersSetCallbackType callback)
-{
-  std::lock_guard<std::recursive_mutex> lock(mutex_);
-
-  ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
-
-  auto existing_callback = on_parameters_set_callback_;
-  on_parameters_set_callback_ = callback;
-  return existing_callback;
-}
-
 const std::map<std::string, rclcpp::ParameterValue> &
 NodeParameters::get_parameter_overrides() const
 {

rclcpp/src/rclcpp/node_options.cpp

@@ -67,7 +67,6 @@ 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_;
@@ -76,13 +75,11 @@ 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->start_parameter_event_publisher_ = other.start_parameter_event_publisher_;
-    this->parameter_event_qos_ = other.parameter_event_qos_;
-    this->parameter_event_publisher_options_ = other.parameter_event_publisher_options_;
+    this->allocator_ = other.allocator_;
     this->allow_undeclared_parameters_ = other.allow_undeclared_parameters_;
     this->automatically_declare_parameters_from_overrides_ =
       other.automatically_declare_parameters_from_overrides_;
-    this->allocator_ = other.allocator_;
+    this->node_options_.reset();
   }
   return *this;
 }
@@ -96,7 +93,6 @@ NodeOptions::get_rcl_node_options() const
     *node_options_ = rcl_node_get_default_options();
     node_options_->allocator = this->allocator_;
     node_options_->use_global_arguments = this->use_global_arguments_;
-    node_options_->domain_id = this->get_domain_id_from_env();
     node_options_->enable_rosout = this->enable_rosout_;
 
     int c_argc = 0;
@@ -326,36 +322,4 @@ NodeOptions::allocator(rcl_allocator_t allocator)
   return *this;
 }
 
-// TODO(wjwwood): reuse rcutils_get_env() to avoid code duplication.
-//   See also: https://github.com/ros2/rcl/issues/119
-size_t
-NodeOptions::get_domain_id_from_env() const
-{
-  // Determine the domain id based on the options and the ROS_DOMAIN_ID env variable.
-  size_t domain_id = std::numeric_limits<size_t>::max();
-  char * ros_domain_id = nullptr;
-  const char * env_var = "ROS_DOMAIN_ID";
-#ifndef _WIN32
-  ros_domain_id = getenv(env_var);
-#else
-  size_t ros_domain_id_size;
-  _dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var);
-#endif
-  if (ros_domain_id) {
-    uint32_t number = static_cast<uint32_t>(strtoul(ros_domain_id, NULL, 0));
-    if (number == (std::numeric_limits<uint32_t>::max)()) {
-#ifdef _WIN32
-      // free the ros_domain_id before throwing, if getenv was used on Windows
-      free(ros_domain_id);
-#endif
-      throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number");
-    }
-    domain_id = static_cast<size_t>(number);
-#ifdef _WIN32
-    free(ros_domain_id);
-#endif
-  }
-  return domain_id;
-}
-
 }  // namespace rclcpp

rclcpp/src/rclcpp/parameter_client.cpp

@@ -15,12 +15,7 @@
 #include "rclcpp/parameter_client.hpp"
 
 #include <algorithm>
-#include <chrono>
-#include <functional>
-#include <future>
-#include <iterator>
 #include <memory>
-#include <stdexcept>
 #include <string>
 #include <vector>
 
@@ -104,36 +99,6 @@ AsyncParametersClient::AsyncParametersClient(
   node_services_interface->add_client(describe_parameters_base, group);
 }
 
-AsyncParametersClient::AsyncParametersClient(
-  const rclcpp::Node::SharedPtr node,
-  const std::string & remote_node_name,
-  const rmw_qos_profile_t & qos_profile,
-  rclcpp::CallbackGroup::SharedPtr group)
-: AsyncParametersClient(
-    node->get_node_base_interface(),
-    node->get_node_topics_interface(),
-    node->get_node_graph_interface(),
-    node->get_node_services_interface(),
-    remote_node_name,
-    qos_profile,
-    group)
-{}
-
-AsyncParametersClient::AsyncParametersClient(
-  rclcpp::Node * node,
-  const std::string & remote_node_name,
-  const rmw_qos_profile_t & qos_profile,
-  rclcpp::CallbackGroup::SharedPtr group)
-: AsyncParametersClient(
-    node->get_node_base_interface(),
-    node->get_node_topics_interface(),
-    node->get_node_graph_interface(),
-    node->get_node_services_interface(),
-    remote_node_name,
-    qos_profile,
-    group)
-{}
-
 std::shared_future<std::vector<rclcpp::Parameter>>
 AsyncParametersClient::get_parameters(
   const std::vector<std::string> & names,
@@ -342,78 +307,6 @@ AsyncParametersClient::wait_for_service_nanoseconds(std::chrono::nanoseconds tim
   return true;
 }
 
-SyncParametersClient::SyncParametersClient(
-  rclcpp::Node::SharedPtr node,
-  const std::string & remote_node_name,
-  const rmw_qos_profile_t & qos_profile)
-: SyncParametersClient(
-    std::make_shared<rclcpp::executors::SingleThreadedExecutor>(),
-    node,
-    remote_node_name,
-    qos_profile)
-{}
-
-SyncParametersClient::SyncParametersClient(
-  rclcpp::Executor::SharedPtr executor,
-  rclcpp::Node::SharedPtr node,
-  const std::string & remote_node_name,
-  const rmw_qos_profile_t & qos_profile)
-: SyncParametersClient(
-    executor,
-    node->get_node_base_interface(),
-    node->get_node_topics_interface(),
-    node->get_node_graph_interface(),
-    node->get_node_services_interface(),
-    remote_node_name,
-    qos_profile)
-{}
-
-SyncParametersClient::SyncParametersClient(
-  rclcpp::Node * node,
-  const std::string & remote_node_name,
-  const rmw_qos_profile_t & qos_profile)
-: SyncParametersClient(
-    std::make_shared<rclcpp::executors::SingleThreadedExecutor>(),
-    node,
-    remote_node_name,
-    qos_profile)
-{}
-
-SyncParametersClient::SyncParametersClient(
-  rclcpp::Executor::SharedPtr executor,
-  rclcpp::Node * node,
-  const std::string & remote_node_name,
-  const rmw_qos_profile_t & qos_profile)
-: SyncParametersClient(
-    executor,
-    node->get_node_base_interface(),
-    node->get_node_topics_interface(),
-    node->get_node_graph_interface(),
-    node->get_node_services_interface(),
-    remote_node_name,
-    qos_profile)
-{}
-
-SyncParametersClient::SyncParametersClient(
-  rclcpp::Executor::SharedPtr executor,
-  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
-  const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
-  const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
-  const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
-  const std::string & remote_node_name,
-  const rmw_qos_profile_t & qos_profile)
-: executor_(executor), node_base_interface_(node_base_interface)
-{
-  async_parameters_client_ =
-    std::make_shared<AsyncParametersClient>(
-    node_base_interface,
-    node_topics_interface,
-    node_graph_interface,
-    node_services_interface,
-    remote_node_name,
-    qos_profile);
-}
-
 std::vector<rclcpp::Parameter>
 SyncParametersClient::get_parameters(const std::vector<std::string> & parameter_names)
 {

rclcpp/src/rclcpp/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 were not declared before setting";
+        response->result.reason = "One or more parameters wer not declared before setting";
       }
     },
     qos_profile, nullptr);

rclcpp/src/rclcpp/time_source.cpp

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

rclcpp/src/rclcpp/timer.cpp

@@ -76,9 +76,8 @@ TimerBase::~TimerBase()
 void
 TimerBase::cancel()
 {
-  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");
+  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);
   }
 }
 
@@ -96,9 +95,8 @@ TimerBase::is_canceled()
 void
 TimerBase::reset()
 {
-  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");
+  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);
   }
 }
 
@@ -106,9 +104,8 @@ bool
 TimerBase::is_ready()
 {
   bool ready = false;
-  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");
+  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);
   }
   return ready;
 }
@@ -117,10 +114,14 @@ std::chrono::nanoseconds
 TimerBase::time_until_trigger()
 {
   int64_t time_until_next_call = 0;
-  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");
+  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);
   }
   return std::chrono::nanoseconds(time_until_next_call);
 }

rclcpp/src/rclcpp/utilities.cpp

@@ -14,8 +14,6 @@
 
 #include "rclcpp/utilities.hpp"
 
-#include <chrono>
-#include <functional>
 #include <string>
 #include <vector>
 

rclcpp/test/CMakeLists.txt

@@ -1,11 +1,601 @@
+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_subdirectory(benchmark)
-add_subdirectory(rclcpp)
+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
+)
+
+# Increasing timeout because connext can take a long time to destroy nodes
+# TODO(brawner) remove when destroying Node for Connext is resolved. See:
+# https://github.com/ros2/rclcpp/issues/1250
+ament_add_gtest(
+  test_allocator_memory_strategy
+  rclcpp/strategies/test_allocator_memory_strategy.cpp
+  TIMEOUT 360)
+if(TARGET test_allocator_memory_strategy)
+  ament_target_dependencies(test_allocator_memory_strategy
+    "rcl"
+    "test_msgs"
+  )
+  target_link_libraries(test_allocator_memory_strategy ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_message_pool_memory_strategy rclcpp/strategies/test_message_pool_memory_strategy.cpp)
+if(TARGET test_message_pool_memory_strategy)
+  ament_target_dependencies(test_message_pool_memory_strategy
+    "rcl"
+    "test_msgs"
+  )
+  target_link_libraries(test_message_pool_memory_strategy ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_any_service_callback rclcpp/test_any_service_callback.cpp)
+if(TARGET test_any_service_callback)
+  ament_target_dependencies(test_any_service_callback
+    "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_add_callback_groups_to_executor rclcpp/test_add_callback_groups_to_executor.cpp)
+if(TARGET test_add_callback_groups_to_executor)
+  target_link_libraries(test_add_callback_groups_to_executor ${PROJECT_NAME})
+  ament_target_dependencies(test_add_callback_groups_to_executor
+    "test_msgs"
+  )
+endif()
+ament_add_gtest(test_expand_topic_or_service_name rclcpp/test_expand_topic_or_service_name.cpp)
+if(TARGET test_expand_topic_or_service_name)
+  ament_target_dependencies(test_expand_topic_or_service_name
+    "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_memory_strategy rclcpp/test_memory_strategy.cpp)
+ament_target_dependencies(test_memory_strategy
+  "test_msgs"
+)
+target_link_libraries(test_memory_strategy ${PROJECT_NAME})
+
+ament_add_gtest(test_message_memory_strategy rclcpp/test_message_memory_strategy.cpp)
+ament_target_dependencies(test_message_memory_strategy
+  "test_msgs"
+)
+target_link_libraries(test_message_memory_strategy ${PROJECT_NAME})
+
+ament_add_gtest(test_node rclcpp/test_node.cpp TIMEOUT 240)
+if(TARGET test_node)
+  ament_target_dependencies(test_node
+    "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_init_options rclcpp/test_init_options.cpp)
+if(TARGET test_init_options)
+  ament_target_dependencies(test_init_options "rcl")
+  target_link_libraries(test_init_options ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_parameter_client rclcpp/test_parameter_client.cpp)
+if(TARGET test_parameter_client)
+  ament_target_dependencies(test_parameter_client
+    "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"
+    "test_msgs"
+  )
+  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_type_support rclcpp/test_type_support.cpp)
+if(TARGET test_type_support)
+  ament_target_dependencies(test_type_support
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "test_msgs"
+  )
+  target_link_libraries(test_type_support ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_find_weak_nodes rclcpp/test_find_weak_nodes.cpp)
+if(TARGET test_find_weak_nodes)
+  ament_target_dependencies(test_find_weak_nodes
+    "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_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()
+
+# TODO(brawner) remove when destroying Node for Connext is resolved. See:
+# https://github.com/ros2/rclcpp/issues/1250
+ament_add_gtest(
+  test_executors
+  rclcpp/executors/test_executors.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}"
+  TIMEOUT 180)
+if(TARGET test_executors)
+  ament_target_dependencies(test_executors
+    "rcl"
+    "test_msgs")
+  target_link_libraries(test_executors ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_static_single_threaded_executor rclcpp/executors/test_static_single_threaded_executor.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_static_single_threaded_executor)
+  target_link_libraries(test_static_single_threaded_executor ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_multi_threaded_executor rclcpp/executors/test_multi_threaded_executor.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_multi_threaded_executor)
+  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 rclcpp/executors/test_static_executor_entities_collector.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_static_executor_entities_collector)
+  ament_target_dependencies(test_static_executor_entities_collector
+    "rcl"
+    "test_msgs")
+  target_link_libraries(test_static_executor_entities_collector ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_guard_condition rclcpp/test_guard_condition.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_guard_condition)
+  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})
+endif()
+
+ament_add_gtest(test_dynamic_storage rclcpp/wait_set_policies/test_dynamic_storage.cpp)
+if(TARGET test_dynamic_storage)
+  ament_target_dependencies(test_dynamic_storage "rcl" "test_msgs")
+  target_link_libraries(test_dynamic_storage ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_storage_policy_common rclcpp/wait_set_policies/test_storage_policy_common.cpp)
+if(TARGET test_storage_policy_common)
+  ament_target_dependencies(test_storage_policy_common "rcl" "test_msgs")
+  target_link_libraries(test_storage_policy_common ${PROJECT_NAME} mimick)
+endif()
+
+ament_add_gtest(test_static_storage rclcpp/wait_set_policies/test_static_storage.cpp)
+if(TARGET test_static_storage)
+  ament_target_dependencies(test_static_storage "rcl" "test_msgs")
+  target_link_libraries(test_static_storage ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_thread_safe_synchronization rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp)
+if(TARGET test_thread_safe_synchronization)
+  ament_target_dependencies(test_thread_safe_synchronization "rcl" "test_msgs")
+  target_link_libraries(test_thread_safe_synchronization ${PROJECT_NAME})
+endif()
 
 ament_add_gtest(test_rclcpp_gtest_macros utils/test_rclcpp_gtest_macros.cpp)
 if(TARGET test_rclcpp_gtest_macros)

rclcpp/test/benchmark/CMakeLists.txt

@@ -1,44 +0,0 @@
-find_package(ament_cmake_google_benchmark REQUIRED)
-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_executor benchmark_executor.cpp)
-if(TARGET benchmark_executor)
-  target_link_libraries(benchmark_executor ${PROJECT_NAME})
-  ament_target_dependencies(benchmark_executor test_msgs)
-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_node_parameters_interface benchmark_node_parameters_interface.cpp)
-if(TARGET benchmark_node_parameters_interface)
-  target_link_libraries(benchmark_node_parameters_interface ${PROJECT_NAME})
-endif()
-
-ament_add_google_benchmark(benchmark_parameter_client benchmark_parameter_client.cpp)
-if(TARGET benchmark_parameter_client)
-  target_link_libraries(benchmark_parameter_client ${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

@@ -1,153 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <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_executor.cpp

@@ -1,389 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <memory>
-#include <string>
-#include <utility>
-#include <vector>
-
-#include "performance_test_fixture/performance_test_fixture.hpp"
-
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp/scope_exit.hpp"
-#include "test_msgs/msg/empty.hpp"
-
-using namespace std::chrono_literals;
-using performance_test_fixture::PerformanceTest;
-
-constexpr unsigned int kNumberOfNodes = 10;
-
-class PerformanceTestExecutor : public PerformanceTest
-{
-public:
-  void SetUp(benchmark::State & st)
-  {
-    rclcpp::init(0, nullptr);
-    callback_count = 0;
-    for (unsigned int i = 0u; i < kNumberOfNodes; i++) {
-      nodes.push_back(std::make_shared<rclcpp::Node>("my_node_" + std::to_string(i)));
-
-      publishers.push_back(
-        nodes[i]->create_publisher<test_msgs::msg::Empty>(
-          "/empty_msgs_" + std::to_string(i), rclcpp::QoS(10)));
-
-      auto callback = [this](test_msgs::msg::Empty::SharedPtr) {this->callback_count++;};
-      subscriptions.push_back(
-        nodes[i]->create_subscription<test_msgs::msg::Empty>(
-          "/empty_msgs_" + std::to_string(i), rclcpp::QoS(10), std::move(callback)));
-    }
-    PerformanceTest::SetUp(st);
-  }
-  void TearDown(benchmark::State & st)
-  {
-    PerformanceTest::TearDown(st);
-    subscriptions.clear();
-    publishers.clear();
-    nodes.clear();
-    rclcpp::shutdown();
-  }
-
-  test_msgs::msg::Empty empty_msgs;
-  std::vector<rclcpp::Node::SharedPtr> nodes;
-  std::vector<rclcpp::Publisher<test_msgs::msg::Empty>::SharedPtr> publishers;
-  std::vector<rclcpp::Subscription<test_msgs::msg::Empty>::SharedPtr> subscriptions;
-  int callback_count;
-};
-
-BENCHMARK_F(PerformanceTestExecutor, single_thread_executor_spin_some)(benchmark::State & st)
-{
-  rclcpp::executors::SingleThreadedExecutor executor;
-  for (unsigned int i = 0u; i < kNumberOfNodes; i++) {
-    executor.add_node(nodes[i]);
-    publishers[i]->publish(empty_msgs);
-    executor.spin_some(100ms);
-  }
-
-  callback_count = 0;
-  reset_heap_counters();
-
-  for (auto _ : st) {
-    st.PauseTiming();
-    for (unsigned int i = 0u; i < kNumberOfNodes; i++) {
-      publishers[i]->publish(empty_msgs);
-    }
-    st.ResumeTiming();
-
-    executor.spin_some(100ms);
-  }
-  if (callback_count == 0) {
-    st.SkipWithError("No message was received");
-  }
-}
-
-BENCHMARK_F(PerformanceTestExecutor, multi_thread_executor_spin_some)(benchmark::State & st)
-{
-  rclcpp::executors::MultiThreadedExecutor executor;
-  for (unsigned int i = 0u; i < kNumberOfNodes; i++) {
-    executor.add_node(nodes[i]);
-    publishers[i]->publish(empty_msgs);
-    executor.spin_some(100ms);
-  }
-
-  callback_count = 0;
-  reset_heap_counters();
-
-  for (auto _ : st) {
-    st.PauseTiming();
-    for (unsigned int i = 0u; i < kNumberOfNodes; i++) {
-      publishers[i]->publish(empty_msgs);
-    }
-    st.ResumeTiming();
-
-    executor.spin_some(100ms);
-  }
-  if (callback_count == 0) {
-    st.SkipWithError("No message was received");
-  }
-}
-
-class PerformanceTestExecutorSimple : public PerformanceTest
-{
-public:
-  void SetUp(benchmark::State & st)
-  {
-    rclcpp::init(0, nullptr);
-    node = std::make_shared<rclcpp::Node>("my_node");
-
-    PerformanceTest::SetUp(st);
-  }
-  void TearDown(benchmark::State & st)
-  {
-    PerformanceTest::TearDown(st);
-    node.reset();
-    rclcpp::shutdown();
-  }
-
-  rclcpp::Node::SharedPtr node;
-};
-
-
-BENCHMARK_F(PerformanceTestExecutorSimple, single_thread_executor_add_node)(benchmark::State & st)
-{
-  rclcpp::executors::SingleThreadedExecutor executor;
-  for (auto _ : st) {
-    executor.add_node(node);
-    st.PauseTiming();
-    executor.remove_node(node);
-    st.ResumeTiming();
-  }
-}
-
-BENCHMARK_F(
-  PerformanceTestExecutorSimple, single_thread_executor_remove_node)(benchmark::State & st)
-{
-  rclcpp::executors::SingleThreadedExecutor executor;
-  for (auto _ : st) {
-    st.PauseTiming();
-    executor.add_node(node);
-    st.ResumeTiming();
-    executor.remove_node(node);
-  }
-}
-
-BENCHMARK_F(PerformanceTestExecutorSimple, multi_thread_executor_add_node)(benchmark::State & st)
-{
-  rclcpp::executors::MultiThreadedExecutor executor;
-  for (auto _ : st) {
-    executor.add_node(node);
-    st.PauseTiming();
-    executor.remove_node(node);
-    st.ResumeTiming();
-  }
-}
-
-BENCHMARK_F(PerformanceTestExecutorSimple, multi_thread_executor_remove_node)(benchmark::State & st)
-{
-  rclcpp::executors::MultiThreadedExecutor executor;
-  for (auto _ : st) {
-    st.PauseTiming();
-    executor.add_node(node);
-    st.ResumeTiming();
-    executor.remove_node(node);
-  }
-}
-
-BENCHMARK_F(
-  PerformanceTestExecutorSimple,
-  static_single_thread_executor_add_node)(benchmark::State & st)
-{
-  rclcpp::executors::StaticSingleThreadedExecutor executor;
-  for (auto _ : st) {
-    executor.add_node(node);
-    st.PauseTiming();
-    executor.remove_node(node);
-    st.ResumeTiming();
-  }
-}
-
-BENCHMARK_F(
-  PerformanceTestExecutorSimple,
-  static_single_thread_executor_remove_node)(benchmark::State & st)
-{
-  rclcpp::executors::StaticSingleThreadedExecutor executor;
-  for (auto _ : st) {
-    st.PauseTiming();
-    executor.add_node(node);
-    st.ResumeTiming();
-    executor.remove_node(node);
-  }
-}
-
-BENCHMARK_F(
-  PerformanceTestExecutorSimple,
-  static_single_thread_executor_spin_until_future_complete)(benchmark::State & st)
-{
-  rclcpp::executors::StaticSingleThreadedExecutor executor;
-  // test success of an immediately finishing future
-  std::promise<bool> promise;
-  std::future<bool> future = promise.get_future();
-  promise.set_value(true);
-  auto shared_future = future.share();
-
-  auto ret = executor.spin_until_future_complete(shared_future, 100ms);
-  if (ret != rclcpp::FutureReturnCode::SUCCESS) {
-    st.SkipWithError(rcutils_get_error_string().str);
-  }
-
-  reset_heap_counters();
-
-  for (auto _ : st) {
-    // static_single_thread_executor has a special design. We need to add/remove the node each
-    // time you call spin
-    st.PauseTiming();
-    executor.add_node(node);
-    st.ResumeTiming();
-
-    ret = executor.spin_until_future_complete(shared_future, 100ms);
-    if (ret != rclcpp::FutureReturnCode::SUCCESS) {
-      st.SkipWithError(rcutils_get_error_string().str);
-      break;
-    }
-    st.PauseTiming();
-    executor.remove_node(node);
-    st.ResumeTiming();
-  }
-}
-
-BENCHMARK_F(
-  PerformanceTestExecutorSimple,
-  single_thread_executor_spin_node_until_future_complete)(benchmark::State & st)
-{
-  rclcpp::executors::SingleThreadedExecutor executor;
-  // test success of an immediately finishing future
-  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, node, shared_future, 1s);
-  if (ret != rclcpp::FutureReturnCode::SUCCESS) {
-    st.SkipWithError(rcutils_get_error_string().str);
-  }
-
-  reset_heap_counters();
-
-  for (auto _ : st) {
-    ret = rclcpp::executors::spin_node_until_future_complete(
-      executor, node, shared_future, 1s);
-    if (ret != rclcpp::FutureReturnCode::SUCCESS) {
-      st.SkipWithError(rcutils_get_error_string().str);
-      break;
-    }
-  }
-}
-
-BENCHMARK_F(
-  PerformanceTestExecutorSimple,
-  multi_thread_executor_spin_node_until_future_complete)(benchmark::State & st)
-{
-  rclcpp::executors::MultiThreadedExecutor executor;
-  // test success of an immediately finishing future
-  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, node, shared_future, 1s);
-  if (ret != rclcpp::FutureReturnCode::SUCCESS) {
-    st.SkipWithError(rcutils_get_error_string().str);
-  }
-
-  reset_heap_counters();
-
-  for (auto _ : st) {
-    ret = rclcpp::executors::spin_node_until_future_complete(
-      executor, node, shared_future, 1s);
-    if (ret != rclcpp::FutureReturnCode::SUCCESS) {
-      st.SkipWithError(rcutils_get_error_string().str);
-      break;
-    }
-  }
-}
-
-BENCHMARK_F(
-  PerformanceTestExecutorSimple,
-  static_single_thread_executor_spin_node_until_future_complete)(benchmark::State & st)
-{
-  rclcpp::executors::StaticSingleThreadedExecutor executor;
-  // test success of an immediately finishing future
-  std::promise<bool> promise;
-  std::future<bool> future = promise.get_future();
-  promise.set_value(true);
-  auto shared_future = future.share();
-
-  reset_heap_counters();
-
-  for (auto _ : st) {
-    auto ret = rclcpp::executors::spin_node_until_future_complete(
-      executor, node, shared_future, 1s);
-    if (ret != rclcpp::FutureReturnCode::SUCCESS) {
-      st.SkipWithError(rcutils_get_error_string().str);
-      break;
-    }
-  }
-}
-
-BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_future_complete)(benchmark::State & st)
-{
-  // test success of an immediately finishing future
-  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);
-  if (ret != rclcpp::FutureReturnCode::SUCCESS) {
-    st.SkipWithError(rcutils_get_error_string().str);
-  }
-
-  reset_heap_counters();
-
-  for (auto _ : st) {
-    ret = rclcpp::spin_until_future_complete(node, shared_future, 1s);
-    if (ret != rclcpp::FutureReturnCode::SUCCESS) {
-      st.SkipWithError(rcutils_get_error_string().str);
-      break;
-    }
-  }
-}
-
-BENCHMARK_F(
-  PerformanceTestExecutorSimple,
-  static_executor_entities_collector_execute)(benchmark::State & st)
-{
-  rclcpp::executors::StaticExecutorEntitiesCollector::SharedPtr entities_collector_ =
-    std::make_shared<rclcpp::executors::StaticExecutorEntitiesCollector>();
-  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();
-  rcl_ret_t ret = rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator);
-  if (ret != RCL_RET_OK) {
-    st.SkipWithError(rcutils_get_error_string().str);
-  }
-  RCLCPP_SCOPE_EXIT(
-  {
-    rcl_ret_t ret = rcl_wait_set_fini(&wait_set);
-    if (ret != RCL_RET_OK) {
-      st.SkipWithError(rcutils_get_error_string().str);
-    }
-  });
-
-  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());
-
-  reset_heap_counters();
-
-  for (auto _ : st) {
-    entities_collector_->execute();
-  }
-}

rclcpp/test/benchmark/benchmark_init_shutdown.cpp

@@ -1,53 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include "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

@@ -1,77 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <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_node_parameters_interface.cpp

@@ -1,262 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <memory>
-#include <string>
-#include <vector>
-
-#include "performance_test_fixture/performance_test_fixture.hpp"
-
-#include "rclcpp/rclcpp.hpp"
-
-class NodeParametersInterfaceTest : public performance_test_fixture::PerformanceTest
-{
-public:
-  NodeParametersInterfaceTest()
-  : node_name("my_node"),
-    param_prefix("my_prefix"),
-    param1_name(param_prefix + ".my_param_1"),
-    param2_name(param_prefix + ".my_param_2"),
-    param3_name(param_prefix + ".my_param_3")
-  {
-  }
-
-  void SetUp(benchmark::State & state)
-  {
-    rclcpp::init(0, nullptr);
-    node = std::make_shared<rclcpp::Node>(node_name);
-
-    node->declare_parameter(param1_name);
-    node->declare_parameter(param2_name);
-    node->declare_parameter(param3_name);
-    node->undeclare_parameter(param3_name);
-
-    performance_test_fixture::PerformanceTest::SetUp(state);
-  }
-
-  void TearDown(benchmark::State & state)
-  {
-    performance_test_fixture::PerformanceTest::TearDown(state);
-
-    node.reset();
-    rclcpp::shutdown();
-  }
-
-  const std::string node_name;
-  const std::string param_prefix;
-  const std::string param1_name;
-  const std::string param2_name;
-  const std::string param3_name;
-
-protected:
-  rclcpp::Node::SharedPtr node;
-};
-
-BENCHMARK_F(NodeParametersInterfaceTest, declare_undeclare)(benchmark::State & state)
-{
-  for (auto _ : state) {
-    node->declare_parameter(param3_name);
-    node->undeclare_parameter(param3_name);
-  }
-}
-
-BENCHMARK_F(NodeParametersInterfaceTest, has_parameter_hit)(benchmark::State & state)
-{
-  for (auto _ : state) {
-    if (!node->has_parameter(param1_name)) {
-      state.SkipWithError("Parameter was expected");
-      break;
-    }
-  }
-}
-
-BENCHMARK_F(NodeParametersInterfaceTest, has_parameter_miss)(benchmark::State & state)
-{
-  for (auto _ : state) {
-    if (node->has_parameter(param3_name)) {
-      state.SkipWithError("Parameter was not expected");
-      break;
-    }
-  }
-}
-
-BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_bool)(benchmark::State & state)
-{
-  const std::vector<rclcpp::Parameter> param_values1
-  {
-    rclcpp::Parameter(param1_name, true),
-    rclcpp::Parameter(param2_name, false),
-  };
-  const std::vector<rclcpp::Parameter> param_values2
-  {
-    rclcpp::Parameter(param1_name, false),
-    rclcpp::Parameter(param2_name, true),
-  };
-
-  reset_heap_counters();
-
-  for (auto _ : state) {
-    node->set_parameters(param_values2);
-    node->set_parameters(param_values1);
-  }
-}
-
-BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_atomically_bool)(benchmark::State & state)
-{
-  const std::vector<rclcpp::Parameter> param_values1
-  {
-    rclcpp::Parameter(param1_name, true),
-    rclcpp::Parameter(param2_name, false),
-  };
-  const std::vector<rclcpp::Parameter> param_values2
-  {
-    rclcpp::Parameter(param1_name, false),
-    rclcpp::Parameter(param2_name, true),
-  };
-
-  reset_heap_counters();
-
-  for (auto _ : state) {
-    node->set_parameters_atomically(param_values2);
-    node->set_parameters_atomically(param_values1);
-  }
-}
-
-BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_callback_bool)(benchmark::State & state)
-{
-  const std::vector<rclcpp::Parameter> param_values1
-  {
-    rclcpp::Parameter(param1_name, true),
-    rclcpp::Parameter(param2_name, false),
-  };
-  const std::vector<rclcpp::Parameter> param_values2
-  {
-    rclcpp::Parameter(param1_name, false),
-    rclcpp::Parameter(param2_name, true),
-  };
-
-  rcl_interfaces::msg::SetParametersResult callback_result;
-  bool callback_received = false;
-  callback_result.successful = true;
-  auto callback =
-    [&callback_result, &callback_received](const std::vector<rclcpp::Parameter> &) {
-      callback_received = true;
-      return callback_result;
-    };
-  auto handle = node->add_on_set_parameters_callback(callback);
-
-  reset_heap_counters();
-
-  for (auto _ : state) {
-    node->set_parameters(param_values2);
-    node->set_parameters(param_values1);
-  }
-
-  if (!callback_received) {
-    state.SkipWithError("Callback is not functioning");
-  }
-
-  node->remove_on_set_parameters_callback(handle.get());
-}
-
-BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_string)(benchmark::State & state)
-{
-  const std::vector<rclcpp::Parameter> param_values1
-  {
-    rclcpp::Parameter(param1_name, "param 1 value A"),
-    rclcpp::Parameter(param2_name, "param 2 value B"),
-  };
-  const std::vector<rclcpp::Parameter> param_values2
-  {
-    rclcpp::Parameter(param1_name, "param 1 value B"),
-    rclcpp::Parameter(param2_name, "param 2 value A"),
-  };
-
-  reset_heap_counters();
-
-  for (auto _ : state) {
-    node->set_parameters(param_values2);
-    node->set_parameters(param_values1);
-  }
-}
-
-BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_array)(benchmark::State & state)
-{
-  const std::vector<rclcpp::Parameter> param_values1
-  {
-    rclcpp::Parameter(param1_name, std::vector<int> {0, 1, 2}),
-    rclcpp::Parameter(param2_name, std::vector<int> {3, 4, 5}),
-  };
-  const std::vector<rclcpp::Parameter> param_values2
-  {
-    rclcpp::Parameter(param1_name, std::vector<int> {4, 5, 6}),
-    rclcpp::Parameter(param2_name, std::vector<int> {7, 8, 9}),
-  };
-
-  reset_heap_counters();
-
-  for (auto _ : state) {
-    node->set_parameters(param_values2);
-    node->set_parameters(param_values1);
-  }
-}
-
-BENCHMARK_F(NodeParametersInterfaceTest, get_parameter)(benchmark::State & state)
-{
-  rclcpp::Parameter param1_value;
-
-  reset_heap_counters();
-
-  for (auto _ : state) {
-    node->get_parameter(param1_name, param1_value);
-  }
-}
-
-BENCHMARK_F(NodeParametersInterfaceTest, list_parameters_hit)(benchmark::State & state)
-{
-  rcl_interfaces::msg::ListParametersResult param_list;
-  const std::vector<std::string> prefixes
-  {
-    param_prefix,
-  };
-
-  reset_heap_counters();
-
-  for (auto _ : state) {
-    param_list = node->list_parameters(prefixes, 10);
-    if (param_list.names.size() != 2) {
-      state.SkipWithError("Expected node names");
-      break;
-    }
-  }
-}
-
-BENCHMARK_F(NodeParametersInterfaceTest, list_parameters_miss)(benchmark::State & state)
-{
-  rcl_interfaces::msg::ListParametersResult param_list;
-  const std::vector<std::string> prefixes
-  {
-    "your_param",
-  };
-
-  reset_heap_counters();
-
-  for (auto _ : state) {
-    param_list = node->list_parameters(prefixes, 10);
-    if (param_list.names.size() != 0) {
-      state.SkipWithError("Expected no node names");
-      break;
-    }
-  }
-}

rclcpp/test/benchmark/benchmark_parameter_client.cpp

@@ -1,324 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <memory>
-#include <vector>
-#include <string>
-
-#include "benchmark/benchmark.h"
-
-#include "rclcpp/rclcpp.hpp"
-
-class RemoteNodeTest : public benchmark::Fixture
-{
-public:
-  RemoteNodeTest()
-  : remote_node_name("my_remote_node")
-  {
-  }
-
-#ifdef __GNUC__
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Woverloaded-virtual"
-#endif
-  void SetUp(benchmark::State &)
-  {
-    remote_context = std::make_shared<rclcpp::Context>();
-    remote_context->init(0, nullptr, rclcpp::InitOptions().auto_initialize_logging(false));
-
-    rclcpp::ExecutorOptions exec_options;
-    exec_options.context = remote_context;
-
-    remote_executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(exec_options);
-
-    remote_node = std::make_shared<rclcpp::Node>(
-      remote_node_name, rclcpp::NodeOptions().context(remote_context));
-    remote_executor->add_node(remote_node);
-
-    remote_thread = std::thread(&rclcpp::executors::SingleThreadedExecutor::spin, remote_executor);
-  }
-
-  void TearDown(benchmark::State &)
-  {
-    remote_executor->cancel();
-    remote_context->shutdown("Test is complete");
-    remote_thread.join();
-
-    remote_node.reset();
-    remote_executor.reset();
-    remote_context.reset();
-  }
-#ifdef __GNUC__
-#pragma GCC diagnostic pop
-#endif
-
-  const std::string remote_node_name;
-
-protected:
-  rclcpp::Context::SharedPtr remote_context;
-  rclcpp::executors::SingleThreadedExecutor::SharedPtr remote_executor;
-  rclcpp::Node::SharedPtr remote_node;
-  std::thread remote_thread;
-};
-
-class ParameterClientTest : public RemoteNodeTest
-{
-public:
-  ParameterClientTest()
-  : node_name("my_node"),
-    param_prefix("my_prefix"),
-    param1_name(param_prefix + ".my_param_1"),
-    param2_name(param_prefix + ".my_param_2"),
-    param3_name(param_prefix + ".my_param_3")
-  {
-  }
-
-  void SetUp(benchmark::State & state)
-  {
-    RemoteNodeTest::SetUp(state);
-
-    rclcpp::init(0, nullptr);
-    node = std::make_shared<rclcpp::Node>(node_name);
-
-    remote_node->declare_parameter(
-      param1_name, rclcpp::ParameterValue("param1_value"));
-    remote_node->declare_parameter(
-      param2_name, rclcpp::ParameterValue(std::vector<int> {1, 2, 3}));
-    remote_node->declare_parameter(param3_name);
-    remote_node->undeclare_parameter(param3_name);
-
-    params_client = std::make_shared<rclcpp::SyncParametersClient>(node, remote_node_name);
-
-    if (!params_client->wait_for_service()) {
-      state.SkipWithError("Client failed to become ready");
-    }
-  }
-
-  void TearDown(benchmark::State & state)
-  {
-    RemoteNodeTest::TearDown(state);
-
-    rclcpp::shutdown();
-    node.reset();
-    params_client.reset();
-  }
-
-  const std::string node_name;
-  const std::string param_prefix;
-  const std::string param1_name;
-  const std::string param2_name;
-  const std::string param3_name;
-
-protected:
-  rclcpp::Node::SharedPtr node;
-  rclcpp::SyncParametersClient::SharedPtr params_client;
-};
-
-static bool result_is_successful(rcl_interfaces::msg::SetParametersResult result)
-{
-  return result.successful;
-}
-
-BENCHMARK_F(ParameterClientTest, create_destroy_client)(benchmark::State & state)
-{
-  for (auto _ : state) {
-    params_client.reset();
-    params_client = std::make_shared<rclcpp::SyncParametersClient>(node, remote_node_name);
-    if (!params_client->wait_for_service()) {
-      state.SkipWithError("Client failed to become ready");
-      break;
-    }
-  }
-}
-
-BENCHMARK_F(ParameterClientTest, has_parameter_hit)(benchmark::State & state)
-{
-  for (auto _ : state) {
-    if (!params_client->has_parameter(param1_name)) {
-      state.SkipWithError("Parameter was expected");
-      break;
-    }
-  }
-}
-
-BENCHMARK_F(ParameterClientTest, has_parameter_miss)(benchmark::State & state)
-{
-  for (auto _ : state) {
-    if (params_client->has_parameter(param3_name)) {
-      state.SkipWithError("Parameter was not expected");
-      break;
-    }
-  }
-}
-
-BENCHMARK_F(ParameterClientTest, set_parameters_bool)(benchmark::State & state)
-{
-  const std::vector<rclcpp::Parameter> param_values1
-  {
-    rclcpp::Parameter(param1_name, true),
-    rclcpp::Parameter(param2_name, false),
-  };
-  const std::vector<rclcpp::Parameter> param_values2
-  {
-    rclcpp::Parameter(param1_name, false),
-    rclcpp::Parameter(param2_name, true),
-  };
-
-  for (auto _ : state) {
-    std::vector<rcl_interfaces::msg::SetParametersResult> results =
-      params_client->set_parameters(param_values2);
-    if (!std::all_of(results.begin(), results.end(), result_is_successful)) {
-      state.SkipWithError("Failed to set one or more parameters");
-      break;
-    }
-
-    results = params_client->set_parameters(param_values1);
-    if (!std::all_of(results.begin(), results.end(), result_is_successful)) {
-      state.SkipWithError("Failed to set one or more parameters");
-      break;
-    }
-  }
-}
-
-BENCHMARK_F(ParameterClientTest, set_parameters_atomically_bool)(benchmark::State & state)
-{
-  const std::vector<rclcpp::Parameter> param_values1
-  {
-    rclcpp::Parameter(param1_name, true),
-    rclcpp::Parameter(param2_name, false),
-  };
-  const std::vector<rclcpp::Parameter> param_values2
-  {
-    rclcpp::Parameter(param1_name, false),
-    rclcpp::Parameter(param2_name, true),
-  };
-
-  for (auto _ : state) {
-    rcl_interfaces::msg::SetParametersResult result =
-      params_client->set_parameters_atomically(param_values2);
-    if (!result.successful) {
-      state.SkipWithError(("Failed to set parameters: " + result.reason).c_str());
-      break;
-    }
-
-    result = params_client->set_parameters_atomically(param_values1);
-    if (!result.successful) {
-      state.SkipWithError(("Failed to set parameters: " + result.reason).c_str());
-      break;
-    }
-  }
-}
-
-BENCHMARK_F(ParameterClientTest, set_parameters_string)(benchmark::State & state)
-{
-  const std::vector<rclcpp::Parameter> param_values1
-  {
-    rclcpp::Parameter(param1_name, "param 1 value A"),
-    rclcpp::Parameter(param2_name, "param 2 value B"),
-  };
-  const std::vector<rclcpp::Parameter> param_values2
-  {
-    rclcpp::Parameter(param1_name, "param 1 value B"),
-    rclcpp::Parameter(param2_name, "param 2 value A"),
-  };
-
-  for (auto _ : state) {
-    std::vector<rcl_interfaces::msg::SetParametersResult> results =
-      params_client->set_parameters(param_values2);
-    if (!std::all_of(results.begin(), results.end(), result_is_successful)) {
-      state.SkipWithError("Failed to set one or more parameters");
-      break;
-    }
-
-    results = params_client->set_parameters(param_values1);
-    if (!std::all_of(results.begin(), results.end(), result_is_successful)) {
-      state.SkipWithError("Failed to set one or more parameters");
-      break;
-    }
-  }
-}
-
-BENCHMARK_F(ParameterClientTest, set_parameters_array)(benchmark::State & state)
-{
-  const std::vector<rclcpp::Parameter> param_values1
-  {
-    rclcpp::Parameter(param1_name, std::vector<int> {0, 1, 2}),
-    rclcpp::Parameter(param2_name, std::vector<int> {3, 4, 5}),
-  };
-  const std::vector<rclcpp::Parameter> param_values2
-  {
-    rclcpp::Parameter(param1_name, std::vector<int> {4, 5, 6}),
-    rclcpp::Parameter(param2_name, std::vector<int> {7, 8, 9}),
-  };
-
-  for (auto _ : state) {
-    std::vector<rcl_interfaces::msg::SetParametersResult> results =
-      params_client->set_parameters(param_values2);
-    if (!std::all_of(results.begin(), results.end(), result_is_successful)) {
-      state.SkipWithError("Failed to set one or more parameters");
-      break;
-    }
-
-    results = params_client->set_parameters(param_values1);
-    if (!std::all_of(results.begin(), results.end(), result_is_successful)) {
-      state.SkipWithError("Failed to set one or more parameters");
-      break;
-    }
-  }
-}
-
-BENCHMARK_F(ParameterClientTest, get_parameters)(benchmark::State & state)
-{
-  for (auto _ : state) {
-    std::vector<rclcpp::Parameter> results = params_client->get_parameters({param1_name});
-    if (results.size() != 1 || results[0].get_name() != param1_name) {
-      state.SkipWithError("Got the wrong parameter(s)");
-      break;
-    }
-  }
-}
-
-BENCHMARK_F(ParameterClientTest, list_parameters_hit)(benchmark::State & state)
-{
-  const std::vector<std::string> prefixes
-  {
-    param_prefix,
-  };
-
-  for (auto _ : state) {
-    rcl_interfaces::msg::ListParametersResult param_list =
-      params_client->list_parameters(prefixes, 10);
-    if (param_list.names.size() != 2) {
-      state.SkipWithError("Expected parameters");
-      break;
-    }
-  }
-}
-
-BENCHMARK_F(ParameterClientTest, list_parameters_miss)(benchmark::State & state)
-{
-  const std::vector<std::string> prefixes
-  {
-    "your_prefix",
-  };
-
-  for (auto _ : state) {
-    rcl_interfaces::msg::ListParametersResult param_list =
-      params_client->list_parameters(prefixes, 10);
-    if (param_list.names.size() != 0) {
-      state.SkipWithError("Expected no parameters");
-      break;
-    }
-  }
-}

rclcpp/test/benchmark/benchmark_service.cpp

@@ -1,141 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <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

@@ -253,7 +253,7 @@ struct PatchTraits<ID, void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)>
     mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5);
 };
 
-/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6)
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)
 /// free functions.
 /**
  * \tparam ID Numerical identifier of the patch. Ought to be unique.
@@ -270,62 +270,6 @@ struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6)>
     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.

rclcpp/test/rclcpp/CMakeLists.txt

@@ -1,638 +0,0 @@
-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_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_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

@@ -1,67 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <gtest/gtest.h>
-
-#include <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

@@ -1,101 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <gtest/gtest.h>
-
-#include <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

@@ -1,58 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <gtest/gtest.h>
-
-#include "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

@@ -241,6 +241,157 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) {
   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:
@@ -259,14 +410,6 @@ public:
     }
   }
 
-  ~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
   {
@@ -306,6 +449,46 @@ private:
   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;
@@ -343,72 +526,3 @@ TYPED_TEST(TestExecutorsStable, spinSome) {
 
   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

@@ -159,7 +159,6 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_basic_node) {
   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());
@@ -207,7 +206,6 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) {
 
   // 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());
@@ -269,7 +267,6 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) {
   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,

rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp

@@ -44,5 +44,6 @@ TEST_F(TestStaticSingleThreadedExecutor, check_unimplemented) {
   executor.add_node(node);
 
   EXPECT_THROW(executor.spin_some(), rclcpp::exceptions::UnimplementedError);
+  EXPECT_THROW(executor.spin_all(0ns), rclcpp::exceptions::UnimplementedError);
   EXPECT_THROW(executor.spin_once(0ns), rclcpp::exceptions::UnimplementedError);
 }

rclcpp/test/rclcpp/node_interfaces/test_node_base.cpp

@@ -21,11 +21,6 @@
 #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
 {
@@ -41,12 +36,6 @@ 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");
@@ -69,125 +58,3 @@ 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,172 +15,105 @@
 #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
 {
 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)
 {
-  auto topic_names_and_types = node_graph()->get_topic_names_and_types(false);
+  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);
   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_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()->get_node_graph_interface()->count_graph_users());
+  EXPECT_EQ(0u, node_graph->count_publishers("not_a_topic"));
+  EXPECT_EQ(0u, node_graph->count_subscribers("not_a_topic"));
 }
 
 TEST_F(TestNodeGraph, get_topic_names_and_types)
 {
-  auto topic_names_and_types = node_graph()->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();
   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 service_names_and_types = node_graph()->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();
   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 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);
+    node1->create_service<test_msgs::srv::Empty>("node1_service", std::move(callback));
+  auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
+  const auto * node_graph =
+    dynamic_cast<rclcpp::node_interfaces::NodeGraph *>(node1->get_node_graph_interface().get());
+  ASSERT_NE(nullptr, node_graph);
 
   // rcl_get_service_names_and_types_by_node() expects the node to exist, otherwise it fails
   EXPECT_THROW(
-    node_graph()->get_service_names_and_types_by_node("not_a_node", "not_absolute_namespace"),
+    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);
+    node_graph->get_service_names_and_types_by_node("node1", "/ns");
   auto services_of_node2 =
-    node_graph()->get_service_names_and_types_by_node(node2_name, absolute_namespace);
+    node_graph->get_service_names_and_types_by_node("node2", "/ns");
 
   auto start = std::chrono::steady_clock::now();
   while (std::chrono::steady_clock::now() - start < std::chrono::seconds(1)) {
-    services_of_node1 =
-      node_graph()->get_service_names_and_types_by_node(node_name, absolute_namespace);
-    services_of_node2 =
-      node_graph()->get_service_names_and_types_by_node(node2_name, absolute_namespace);
+    services_of_node1 = node_graph->get_service_names_and_types_by_node("node1", "/ns");
+    services_of_node2 = node_graph->get_service_names_and_types_by_node("node2", "/ns");
     if (services_of_node1.find("/ns/node1_service") != services_of_node1.end()) {
       break;
     }
@@ -190,134 +123,70 @@ TEST_F(TestNodeGraph, get_service_names_and_types_by_node)
   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(node_name, absolute_namespace),
-    rclcpp::exceptions::RCLError);
-}
-
 TEST_F(TestNodeGraph, get_node_names_and_namespaces)
 {
-  auto names_and_namespaces = node_graph()->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();
   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)
 {
-  EXPECT_NO_THROW(node()->get_node_graph_interface()->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());
 }
 
 TEST_F(TestNodeGraph, wait_for_graph_change)
 {
-  auto node_graph_interface = node()->get_node_graph_interface();
-  EXPECT_NO_THROW(node_graph_interface->notify_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());
   EXPECT_THROW(
-    node_graph_interface->wait_for_graph_change(nullptr, std::chrono::milliseconds(1)),
+    node_graph->wait_for_graph_change(nullptr, std::chrono::milliseconds(1)),
     rclcpp::exceptions::InvalidEventError);
 
   auto event = std::make_shared<rclcpp::Event>();
   EXPECT_THROW(
-    node_graph_interface->wait_for_graph_change(event, std::chrono::milliseconds(0)),
+    node_graph->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));
 
-  EXPECT_EQ(0u, node_graph()->get_publishers_info_by_topic("topic", true).size());
+  const auto * node_graph =
+    dynamic_cast<rclcpp::node_interfaces::NodeGraph *>(node->get_node_graph_interface().get());
+  ASSERT_NE(nullptr, node_graph);
 
-  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_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("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("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());
@@ -356,90 +225,3 @@ 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,47 +28,31 @@
 #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);
 
@@ -89,113 +73,17 @@ 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)
 {
-  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);
+  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
 
-  std::shared_ptr<rclcpp::Node> node2 = std::make_shared<rclcpp::Node>("node2", "ns", node_options);
-
-  auto * node_parameters_interface =
+  auto * node_parameters =
     dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
-    node2->get_node_parameters_interface().get());
+    node->get_node_parameters_interface().get());
   ASSERT_NE(nullptr, node_parameters);
 
-  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"));
+  const auto & parameter_overrides = node_parameters->get_parameter_overrides();
+  EXPECT_EQ(0u, parameter_overrides.size());
 }

rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp

@@ -22,9 +22,6 @@
 #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:
@@ -54,29 +51,25 @@ 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_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(
@@ -87,24 +80,22 @@ TEST_F(TestNodeService, add_service)
 
   auto callback_group_in_different_node =
     different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-  RCLCPP_EXPECT_THROW_EQ(
+  EXPECT_THROW(
     node_services->add_service(service, callback_group_in_different_node),
-    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"));
+    std::runtime_error);
 }
 
 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));
@@ -114,18 +105,7 @@ TEST_F(TestNodeService, add_client)
 
   auto callback_group_in_different_node =
     different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-  RCLCPP_EXPECT_THROW_EQ(
+  EXPECT_THROW(
     node_services->add_client(client, callback_group_in_different_node),
-    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"));
+    std::runtime_error);
 }

rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp

@@ -22,9 +22,6 @@
 #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:
@@ -42,27 +39,23 @@ 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));
@@ -72,19 +65,7 @@ TEST_F(TestNodeTimers, add_timer)
 
   auto callback_group_in_different_node =
     different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-  RCLCPP_EXPECT_THROW_EQ(
+  EXPECT_THROW(
     node_timers->add_timer(timer, callback_group_in_different_node),
-    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"));
+    std::runtime_error);
 }

rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp

@@ -24,9 +24,6 @@
 #include "rclcpp/rclcpp.hpp"
 #include "test_msgs/msg/empty.hpp"
 
-#include "../../mocking_utils/patch.hpp"
-#include "../../utils/rclcpp_gtest_macros.hpp"
-
 namespace
 {
 
@@ -80,27 +77,23 @@ 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_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));
@@ -115,20 +108,12 @@ 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));
@@ -142,15 +127,3 @@ 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"));
-}

rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp

@@ -22,9 +22,6 @@
 #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:
@@ -39,26 +36,23 @@ 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);
@@ -66,25 +60,9 @@ TEST_F(TestNodeWaitables, add_remove_waitable)
   auto waitable = std::make_shared<TestWaitable>();
   EXPECT_NO_THROW(
     node_waitables->add_waitable(waitable, callback_group1));
-  RCLCPP_EXPECT_THROW_EQ(
+  EXPECT_THROW(
     node_waitables->add_waitable(waitable, callback_group2),
-    std::runtime_error("Cannot create waitable, group not in node."));
+    std::runtime_error);
   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

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <list>
+#include <map>
 #include <memory>
 #include <string>
 #include <vector>
@@ -26,7 +27,9 @@
 #include "test_msgs/srv/empty.hpp"
 
 using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy;
-using WeakNodeList = std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>;
+typedef std::map<rclcpp::CallbackGroup::WeakPtr,
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+    std::owner_less<rclcpp::CallbackGroup::WeakPtr>> WeakCallbackGroupsToNodesMap;
 
 static bool test_waitable_result = false;
 
@@ -83,7 +86,6 @@ public:
 
   void TearDown() override
   {
-    allocator_memory_strategy_.reset();
     rclcpp::shutdown();
   }
 
@@ -188,10 +190,19 @@ protected:
     std::shared_ptr<rclcpp::Node> node,
     const RclWaitSetSizes & expected)
   {
-    WeakNodeList nodes;
-    nodes.push_back(node->get_node_base_interface());
-
-    allocator_memory_strategy()->collect_entities(nodes);
+    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(
@@ -212,9 +223,19 @@ protected:
     std::shared_ptr<rclcpp::Node> node,
     const RclWaitSetSizes & insufficient_capacities)
   {
-    WeakNodeList nodes;
-    nodes.push_back(node->get_node_base_interface());
-    allocator_memory_strategy()->collect_entities(nodes);
+    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();
@@ -281,26 +302,65 @@ protected:
   ::testing::AssertionResult TestGetNextEntity(
     std::shared_ptr<rclcpp::Node> node_with_entity1,
     std::shared_ptr<rclcpp::Node> node_with_entity2,
-    std::function<rclcpp::AnyExecutable(WeakNodeList)> get_next_entity_func)
+    std::function<rclcpp::AnyExecutable(WeakCallbackGroupsToNodesMap)> get_next_entity_func)
   {
-    WeakNodeList nodes;
-    auto basic_node = std::make_shared<rclcpp::Node>("basic_node", "ns");
-    nodes.push_back(node_with_entity1->get_node_base_interface());
-    nodes.push_back(basic_node->get_node_base_interface());
-    allocator_memory_strategy()->collect_entities(nodes);
+    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(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";
     }
 
-    WeakNodeList uncollected_nodes;
     auto basic_node2 = std::make_shared<rclcpp::Node>("basic_node2", "ns");
-    uncollected_nodes.push_back(node_with_entity2->get_node_base_interface());
-    uncollected_nodes.push_back(basic_node2->get_node_base_interface());
-
-    rclcpp::AnyExecutable failed_result = get_next_entity_func(uncollected_nodes);
+    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"
@@ -312,16 +372,33 @@ protected:
 
   ::testing::AssertionResult TestGetNextEntityMutuallyExclusive(
     std::shared_ptr<rclcpp::Node> node_with_entity,
-    std::function<rclcpp::AnyExecutable(WeakNodeList)> get_next_entity_func)
+    std::function<rclcpp::AnyExecutable(WeakCallbackGroupsToNodesMap)> get_next_entity_func)
   {
-    WeakNodeList nodes;
     auto basic_node = std::make_shared<rclcpp::Node>("basic_node", "ns");
-
-    auto node_base = node_with_entity->get_node_base_interface();
     auto basic_node_base = basic_node->get_node_base_interface();
-    nodes.push_back(node_base);
-    nodes.push_back(basic_node_base);
-    allocator_memory_strategy()->collect_entities(nodes);
+    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
@@ -331,7 +408,7 @@ protected:
       callback_group->can_be_taken_from() = false;
     }
 
-    rclcpp::AnyExecutable result = get_next_entity_func(nodes);
+    rclcpp::AnyExecutable result = get_next_entity_func(weak_groups_to_nodes);
 
     if (nullptr != result.node_base) {
       return ::testing::AssertionFailure() <<
@@ -358,10 +435,19 @@ private:
 };
 
 TEST_F(TestAllocatorMemoryStrategy, construct_destruct) {
-  WeakNodeList nodes;
   auto basic_node = create_node_with_disabled_callback_groups("basic_node");
-  nodes.push_back(basic_node->get_node_base_interface());
-  allocator_memory_strategy()->collect_entities(nodes);
+  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());
@@ -449,9 +535,18 @@ TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_timer) {
 
 TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_bad_arguments) {
   auto node = create_node_with_subscription("subscription_node");
-  WeakNodeList nodes;
-  nodes.push_back(node->get_node_base_interface());
-  allocator_memory_strategy()->collect_entities(nodes);
+  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();
@@ -528,9 +623,9 @@ 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 WeakNodeList & nodes) {
+  auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) {
       rclcpp::AnyExecutable result;
-      allocator_memory_strategy()->get_next_subscription(result, nodes);
+      allocator_memory_strategy()->get_next_subscription(result, weak_groups_to_nodes);
       return result;
     };
 
@@ -541,9 +636,9 @@ 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 WeakNodeList & nodes) {
+  auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) {
       rclcpp::AnyExecutable result;
-      allocator_memory_strategy()->get_next_service(result, nodes);
+      allocator_memory_strategy()->get_next_service(result, weak_groups_to_nodes);
       return result;
     };
 
@@ -554,9 +649,9 @@ 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 WeakNodeList & nodes) {
+  auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) {
       rclcpp::AnyExecutable result;
-      allocator_memory_strategy()->get_next_client(result, nodes);
+      allocator_memory_strategy()->get_next_client(result, weak_groups_to_nodes);
       return result;
     };
 
@@ -567,9 +662,9 @@ 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 WeakNodeList & nodes) {
+  auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) {
       rclcpp::AnyExecutable result;
-      allocator_memory_strategy()->get_next_timer(result, nodes);
+      allocator_memory_strategy()->get_next_timer(result, weak_groups_to_nodes);
       return result;
     };
 
@@ -584,9 +679,9 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_waitable) {
   node1->get_node_waitables_interface()->add_waitable(waitable1, nullptr);
   node2->get_node_waitables_interface()->add_waitable(waitable2, nullptr);
 
-  auto get_next_entity = [this](const WeakNodeList & nodes) {
+  auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) {
       rclcpp::AnyExecutable result;
-      allocator_memory_strategy()->get_next_waitable(result, nodes);
+      allocator_memory_strategy()->get_next_waitable(result, weak_groups_to_nodes);
       return result;
     };
 
@@ -596,9 +691,9 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_waitable) {
 TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_mutually_exclusive) {
   auto node = create_node_with_subscription("node");
 
-  auto get_next_entity = [this](const WeakNodeList & nodes) {
+  auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) {
       rclcpp::AnyExecutable result;
-      allocator_memory_strategy()->get_next_subscription(result, nodes);
+      allocator_memory_strategy()->get_next_subscription(result, weak_groups_to_nodes);
       return result;
     };
 
@@ -608,9 +703,9 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_mutually_exclusive) {
 TEST_F(TestAllocatorMemoryStrategy, get_next_service_mutually_exclusive) {
   auto node = create_node_with_service("node");
 
-  auto get_next_entity = [this](const WeakNodeList & nodes) {
+  auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) {
       rclcpp::AnyExecutable result;
-      allocator_memory_strategy()->get_next_service(result, nodes);
+      allocator_memory_strategy()->get_next_service(result, weak_groups_to_nodes);
       return result;
     };
 
@@ -620,9 +715,9 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_service_mutually_exclusive) {
 TEST_F(TestAllocatorMemoryStrategy, get_next_client_mutually_exclusive) {
   auto node = create_node_with_client("node");
 
-  auto get_next_entity = [this](const WeakNodeList & nodes) {
+  auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) {
       rclcpp::AnyExecutable result;
-      allocator_memory_strategy()->get_next_client(result, nodes);
+      allocator_memory_strategy()->get_next_client(result, weak_groups_to_nodes);
       return result;
     };
 
@@ -632,9 +727,9 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_client_mutually_exclusive) {
 TEST_F(TestAllocatorMemoryStrategy, get_next_timer_mutually_exclusive) {
   auto node = create_node_with_timer("node");
 
-  auto get_next_entity = [this](const WeakNodeList & nodes) {
+  auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) {
       rclcpp::AnyExecutable result;
-      allocator_memory_strategy()->get_next_timer(result, nodes);
+      allocator_memory_strategy()->get_next_timer(result, weak_groups_to_nodes);
       return result;
     };
 
@@ -648,13 +743,14 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_waitable_mutually_exclusive) {
     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 WeakNodeList & nodes) {
+  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, nodes);
+      allocator_memory_strategy()->get_next_waitable(result, weak_groups_to_nodes);
       return result;
     };
 
@@ -662,9 +758,8 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_waitable_mutually_exclusive) {
 }
 
 TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_out_of_scope) {
-  WeakNodeList nodes;
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
   auto node = create_node_with_disabled_callback_groups("node");
-  nodes.push_back(node->get_node_base_interface());
   // Force subscription to go out of scope and cleanup after collecting entities.
   {
     rclcpp::SubscriptionOptions subscription_options;
@@ -682,19 +777,28 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_out_of_scope) {
       test_msgs::msg::Empty, decltype(subscription_callback)>(
       "topic", qos, std::move(subscription_callback), subscription_options);
 
-    allocator_memory_strategy()->collect_entities(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(1u, allocator_memory_strategy()->number_of_ready_subscriptions());
 
   rclcpp::AnyExecutable result;
-  allocator_memory_strategy()->get_next_subscription(result, nodes);
+  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) {
-  WeakNodeList nodes;
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
   auto node = create_node_with_disabled_callback_groups("node");
-  nodes.push_back(node->get_node_base_interface());
   // Force service to go out of scope and cleanup after collecting entities.
   {
     auto callback_group =
@@ -707,19 +811,38 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_service_out_of_scope) {
     auto service = node->create_service<test_msgs::srv::Empty>(
       "service", std::move(service_callback), rmw_qos_profile_services_default, callback_group);
 
-    allocator_memory_strategy()->collect_entities(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(1u, allocator_memory_strategy()->number_of_ready_services());
 
   rclcpp::AnyExecutable result;
-  allocator_memory_strategy()->get_next_service(result, nodes);
+  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) {
-  WeakNodeList nodes;
   auto node = create_node_with_disabled_callback_groups("node");
-  nodes.push_back(node->get_node_base_interface());
+  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 =
@@ -728,49 +851,69 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_client_out_of_scope) {
     auto client = node->create_client<test_msgs::srv::Empty>(
       "service", rmw_qos_profile_services_default, callback_group);
 
-    allocator_memory_strategy()->collect_entities(nodes);
+    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, nodes);
+  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) {
-  WeakNodeList nodes;
   auto node = create_node_with_disabled_callback_groups("node");
-  nodes.push_back(node->get_node_base_interface());
-
+  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);
-
-    allocator_memory_strategy()->collect_entities(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(1u, allocator_memory_strategy()->number_of_ready_timers());
 
   rclcpp::AnyExecutable result;
-  allocator_memory_strategy()->get_next_timer(result, nodes);
+  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) {
-  WeakNodeList nodes;
   auto node = create_node_with_disabled_callback_groups("node");
-  nodes.push_back(node->get_node_base_interface());
-
+  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);
-    allocator_memory_strategy()->collect_entities(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 waitable = std::make_shared<TestWaitable>();
     node->get_node_waitables_interface()->add_waitable(waitable, callback_group);
     allocator_memory_strategy()->add_waitable_handle(waitable);
@@ -779,6 +922,6 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_waitable_out_of_scope) {
   EXPECT_EQ(1u, allocator_memory_strategy()->number_of_waitables());
 
   rclcpp::AnyExecutable result;
-  allocator_memory_strategy()->get_next_waitable(result, nodes);
+  allocator_memory_strategy()->get_next_waitable(result, weak_groups_to_nodes);
   EXPECT_EQ(nullptr, result.node_base);
 }

rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp

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

rclcpp/test/rclcpp/test_client.cpp

@@ -16,17 +16,12 @@
 
 #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:
@@ -35,11 +30,6 @@ protected:
     rclcpp::init(0, nullptr);
   }
 
-  static void TearDownTestCase()
-  {
-    rclcpp::shutdown();
-  }
-
   void SetUp()
   {
     node = std::make_shared<rclcpp::Node>("my_node", "/ns");
@@ -58,12 +48,6 @@ class TestClientSub : public ::testing::Test
 protected:
   static void SetUpTestCase()
   {
-    rclcpp::init(0, nullptr);
-  }
-
-  static void TearDownTestCase()
-  {
-    rclcpp::shutdown();
   }
 
   void SetUp()
@@ -122,38 +106,6 @@ 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.
  */
@@ -171,160 +123,3 @@ 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,7 +25,6 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp/duration.hpp"
 
-#include "../utils/rclcpp_gtest_macros.hpp"
 
 using namespace std::chrono_literals;
 
@@ -238,26 +237,3 @@ 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(0u);
-  RCLCPP_EXPECT_THROW_EQ(
-    test_duration = rclcpp::Duration(INT64_MAX) - rclcpp::Duration(-1),
-    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,357 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <gtest/gtest.h>
-
-#include <chrono>
-#include <memory>
-#include <stdexcept>
-#include <string>
-
-#include "rclcpp/executor.hpp"
-#include "rclcpp/memory_strategy.hpp"
-#include "rclcpp/strategies/allocator_memory_strategy.hpp"
-
-#include "../mocking_utils/patch.hpp"
-#include "../utils/rclcpp_gtest_macros.hpp"
-
-// 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
-{
-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::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 TearDown()
-  {
-    rclcpp::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, >)
-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"));
-}
-
-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"));
-}
-
-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_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;
-        }
-      }
-    });
-
-  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);
-}
-
-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;
-    });
-
-  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);
-}
-
-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;
-        }
-      }
-    });
-
-  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_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, 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);
-}

rclcpp/test/rclcpp/test_expand_topic_or_service_name.cpp

@@ -14,20 +14,9 @@
 
 #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.
  */
@@ -89,156 +78,3 @@ 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

@@ -1,42 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <gtest/gtest.h>
-
-#include <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

@@ -1,293 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <gtest/gtest.h>
-#include <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,8 +18,6 @@
 
 #include "rclcpp/rclcpp.hpp"
 
-#include "../mocking_utils/patch.hpp"
-
 class TestGuardCondition : public ::testing::Test
 {
 protected:
@@ -56,14 +54,6 @@ 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());
-  }
 }
 
 /*
@@ -92,13 +82,6 @@ TEST_F(TestGuardCondition, get_rcl_guard_condition) {
 TEST_F(TestGuardCondition, trigger) {
   {
     auto gc = std::make_shared<rclcpp::GuardCondition>();
-    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);
-    }
+    gc->trigger();
   }
 }

rclcpp/test/rclcpp/test_init_options.cpp

@@ -14,7 +14,6 @@
 
 #include <gtest/gtest.h>
 
-#include <stdexcept>
 #include <string>
 #include <vector>
 
@@ -23,8 +22,6 @@
 
 #include "rclcpp/init_options.hpp"
 
-#include "../mocking_utils/patch.hpp"
-#include "../utils/rclcpp_gtest_macros.hpp"
 
 TEST(TestInitOptions, test_construction) {
   rcl_allocator_t allocator = rcl_get_default_allocator();
@@ -65,35 +62,16 @@ TEST(TestInitOptions, test_initialize_logging) {
   }
 }
 
-// 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(TestInitOptions, test_domain_id) {
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto options = rclcpp::InitOptions(allocator);
+  size_t domain_id = RCL_DEFAULT_DOMAIN_ID;
+  EXPECT_EQ(RCL_RET_OK, rcl_get_default_domain_id(&domain_id));
 
-TEST(TestInitOptions, constructor_rcl_init_options_init_failed) {
-  auto mock = mocking_utils::patch_and_return(
-    "lib:rclcpp", rcl_init_options_init, RCL_RET_ERROR);
-  RCLCPP_EXPECT_THROW_EQ(
-    rclcpp::InitOptions(),
-    std::runtime_error("failed to initialize rcl init options: error not set"));
-}
-
-TEST(TestInitOptions, constructor_rcl_init_options_copy_failed) {
-  rcl_init_options_t rcl_opts;
-  auto mock = mocking_utils::patch_and_return(
-    "lib:rclcpp", rcl_init_options_copy, RCL_RET_ERROR);
-  RCLCPP_EXPECT_THROW_EQ(
-    new rclcpp::InitOptions(rcl_opts),
-    std::runtime_error("failed to copy rcl init options: error not set"));
-}
-
-TEST(TestInitOptions, copy_constructor_rcl_init_options_copy_failed) {
-  rclcpp::InitOptions options;
-  rclcpp::InitOptions options2;
-  auto mock = mocking_utils::patch_and_return(
-    "lib:rclcpp", rcl_init_options_copy, RCL_RET_ERROR);
-  RCLCPP_EXPECT_THROW_EQ(
-    options2.operator=(options),
-    std::runtime_error("failed to copy rcl init options: error not set"));
+  options.use_default_domain_id();
+  EXPECT_EQ(domain_id, options.get_domain_id());
+  options.set_domain_id(42);
+  EXPECT_EQ((size_t)42, options.get_domain_id());
+  options.use_default_domain_id();
+  EXPECT_EQ(domain_id, options.get_domain_id());
 }

rclcpp/test/rclcpp/test_loaned_message.cpp

@@ -20,8 +20,6 @@
 
 #include "test_msgs/msg/basic_types.hpp"
 
-#include "../mocking_utils/patch.hpp"
-
 using MessageT = test_msgs::msg::BasicTypes;
 using LoanedMessageT = rclcpp::LoanedMessage<MessageT>;
 
@@ -83,51 +81,3 @@ TEST_F(TestLoanedMessage, release) {
 
   SUCCEED();
 }
-
-TEST_F(TestLoanedMessage, construct_with_loaned_message_publisher) {
-  auto node = std::make_shared<rclcpp::Node>("loaned_message_test_node");
-  auto publisher = node->create_publisher<MessageT>("topic", 10);
-  std::allocator<MessageT> allocator;
-
-  auto mock_can_loan = mocking_utils::patch_and_return(
-    "lib:rclcpp", rcl_publisher_can_loan_messages, true);
-
-  {
-    auto mock_borrow_loaned = mocking_utils::patch_and_return(
-      "self", rcl_borrow_loaned_message, RCL_RET_ERROR);
-
-    EXPECT_THROW(
-      std::make_shared<LoanedMessageT>(*publisher.get(), allocator).reset(),
-      rclcpp::exceptions::RCLError);
-  }
-
-  MessageT message;
-  auto borrow_loaned_message_callback =
-    [&message](
-    const rcl_publisher_t *, const rosidl_message_type_support_t *, void ** ros_message) {
-      *ros_message = &message;
-      return RCL_RET_OK;
-    };
-  auto mock_borrow_loaned = mocking_utils::patch(
-    "self", rcl_borrow_loaned_message, borrow_loaned_message_callback);
-
-  {
-    auto mock_return_loaned = mocking_utils::patch_and_return(
-      "self", rcl_return_loaned_message_from_publisher, RCL_RET_OK);
-
-    auto loaned_message = std::make_shared<LoanedMessageT>(*publisher.get(), allocator);
-    EXPECT_TRUE(loaned_message->is_valid());
-    EXPECT_NO_THROW(loaned_message.reset());
-  }
-
-  {
-    auto loaned_message = std::make_shared<LoanedMessageT>(*publisher.get(), allocator);
-    EXPECT_TRUE(loaned_message->is_valid());
-
-    auto mock_return_loaned = mocking_utils::patch_and_return(
-      "self", rcl_return_loaned_message_from_publisher, RCL_RET_ERROR);
-
-    // No exception, it just logs an error
-    EXPECT_NO_THROW(loaned_message.reset());
-  }
-}

rclcpp/test/rclcpp/test_logger.cpp

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

rclcpp/test/rclcpp/test_memory_strategy.cpp

@@ -15,6 +15,7 @@
 #include <gtest/gtest.h>
 
 #include <list>
+#include <map>
 #include <memory>
 #include <utility>
 
@@ -24,7 +25,9 @@
 #include "test_msgs/srv/empty.hpp"
 
 using rclcpp::memory_strategy::MemoryStrategy;
-using WeakNodeList = std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>;
+typedef std::map<rclcpp::CallbackGroup::WeakPtr,
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+    std::owner_less<rclcpp::CallbackGroup::WeakPtr>> WeakCallbackGroupsToNodesMap;
 
 /**
  * Mock Waitable class
@@ -75,13 +78,22 @@ TEST_F(TestMemoryStrategy, construct_destruct) {
 }
 
 TEST_F(TestMemoryStrategy, get_subscription_by_handle) {
-  WeakNodeList nodes;
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
   std::shared_ptr<const rcl_subscription_t> subscription_handle;
   rclcpp::SubscriptionBase::SharedPtr found_subscription = nullptr;
   {
     auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    nodes.push_back(node->get_node_base_interface());
-    memory_strategy()->collect_entities(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()));
+      });
+    memory_strategy()->collect_entities(weak_groups_to_nodes);
     {
       auto callback_group =
         node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
@@ -97,29 +109,38 @@ TEST_F(TestMemoryStrategy, get_subscription_by_handle) {
 
         EXPECT_EQ(
           subscription,
-          memory_strategy()->get_subscription_by_handle(subscription_handle, nodes));
+          memory_strategy()->get_subscription_by_handle(subscription_handle, weak_groups_to_nodes));
       }  // subscription goes out of scope
       EXPECT_EQ(
         nullptr,
-        memory_strategy()->get_subscription_by_handle(subscription_handle, nodes));
+        memory_strategy()->get_subscription_by_handle(subscription_handle, weak_groups_to_nodes));
     }  // callback_group goes out of scope
     EXPECT_EQ(
       nullptr,
-      memory_strategy()->get_subscription_by_handle(subscription_handle, nodes));
+      memory_strategy()->get_subscription_by_handle(subscription_handle, weak_groups_to_nodes));
   }  // Node goes out of scope
   EXPECT_EQ(
     nullptr,
-    memory_strategy()->get_subscription_by_handle(subscription_handle, nodes));
+    memory_strategy()->get_subscription_by_handle(subscription_handle, weak_groups_to_nodes));
 }
 
 TEST_F(TestMemoryStrategy, get_service_by_handle) {
-  WeakNodeList nodes;
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
   std::shared_ptr<const rcl_service_t> service_handle;
   rclcpp::ServiceBase::SharedPtr found_service = nullptr;
   {
     auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    nodes.push_back(node->get_node_base_interface());
-    memory_strategy()->collect_entities(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()));
+      });
+    memory_strategy()->collect_entities(weak_groups_to_nodes);
     {
       auto callback_group =
         node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
@@ -127,7 +148,11 @@ TEST_F(TestMemoryStrategy, get_service_by_handle) {
         [](const test_msgs::srv::Empty::Request::SharedPtr,
           test_msgs::srv::Empty::Response::SharedPtr) {};
       const rclcpp::QoS qos(10);
-
+      weak_groups_to_nodes.insert(
+        std::pair<rclcpp::CallbackGroup::WeakPtr,
+        rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+          rclcpp::CallbackGroup::WeakPtr(callback_group),
+          node->get_node_base_interface()));
       {
         auto service = node->create_service<test_msgs::srv::Empty>(
           "service", std::move(service_callback),
@@ -137,29 +162,38 @@ TEST_F(TestMemoryStrategy, get_service_by_handle) {
 
         EXPECT_EQ(
           service,
-          memory_strategy()->get_service_by_handle(service_handle, nodes));
+          memory_strategy()->get_service_by_handle(service_handle, weak_groups_to_nodes));
       }  // service goes out of scope
       EXPECT_EQ(
         nullptr,
-        memory_strategy()->get_service_by_handle(service_handle, nodes));
+        memory_strategy()->get_service_by_handle(service_handle, weak_groups_to_nodes));
     }  // callback_group goes out of scope
     EXPECT_EQ(
       nullptr,
-      memory_strategy()->get_service_by_handle(service_handle, nodes));
+      memory_strategy()->get_service_by_handle(service_handle, weak_groups_to_nodes));
   }  // Node goes out of scope
   EXPECT_EQ(
     nullptr,
-    memory_strategy()->get_service_by_handle(service_handle, nodes));
+    memory_strategy()->get_service_by_handle(service_handle, weak_groups_to_nodes));
 }
 
 TEST_F(TestMemoryStrategy, get_client_by_handle) {
-  WeakNodeList nodes;
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
   std::shared_ptr<const rcl_client_t> client_handle;
   rclcpp::ClientBase::SharedPtr found_client = nullptr;
   {
     auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    nodes.push_back(node->get_node_base_interface());
-    memory_strategy()->collect_entities(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()));
+      });
+    memory_strategy()->collect_entities(weak_groups_to_nodes);
     {
       auto callback_group =
         node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
@@ -168,32 +202,46 @@ TEST_F(TestMemoryStrategy, get_client_by_handle) {
           "service", rmw_qos_profile_services_default, callback_group);
 
         client_handle = client->get_client_handle();
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            rclcpp::CallbackGroup::WeakPtr(callback_group),
+            node->get_node_base_interface()));
 
         EXPECT_EQ(
           client,
-          memory_strategy()->get_client_by_handle(client_handle, nodes));
+          memory_strategy()->get_client_by_handle(client_handle, weak_groups_to_nodes));
       }  // client goes out of scope
       EXPECT_EQ(
         nullptr,
-        memory_strategy()->get_client_by_handle(client_handle, nodes));
+        memory_strategy()->get_client_by_handle(client_handle, weak_groups_to_nodes));
     }  // callback_group goes out of scope
     EXPECT_EQ(
       nullptr,
-      memory_strategy()->get_client_by_handle(client_handle, nodes));
+      memory_strategy()->get_client_by_handle(client_handle, weak_groups_to_nodes));
   }  // Node goes out of scope
   EXPECT_EQ(
     nullptr,
-    memory_strategy()->get_client_by_handle(client_handle, nodes));
+    memory_strategy()->get_client_by_handle(client_handle, weak_groups_to_nodes));
 }
 
 TEST_F(TestMemoryStrategy, get_timer_by_handle) {
-  WeakNodeList nodes;
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
   std::shared_ptr<const rcl_timer_t> timer_handle;
   rclcpp::TimerBase::SharedPtr found_timer = nullptr;
   {
     auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    nodes.push_back(node->get_node_base_interface());
-    memory_strategy()->collect_entities(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()));
+      });
+    memory_strategy()->collect_entities(weak_groups_to_nodes);
     {
       auto callback_group =
         node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
@@ -201,58 +249,85 @@ TEST_F(TestMemoryStrategy, get_timer_by_handle) {
         auto timer_callback = []() {};
         auto timer = node->create_wall_timer(
           std::chrono::milliseconds(1), timer_callback, callback_group);
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            rclcpp::CallbackGroup::WeakPtr(callback_group),
+            node->get_node_base_interface()));
 
         timer_handle = timer->get_timer_handle();
 
         EXPECT_EQ(
           timer,
-          memory_strategy()->get_timer_by_handle(timer_handle, nodes));
+          memory_strategy()->get_timer_by_handle(timer_handle, weak_groups_to_nodes));
       }  // timer goes out of scope
       EXPECT_EQ(
         nullptr,
-        memory_strategy()->get_timer_by_handle(timer_handle, nodes));
+        memory_strategy()->get_timer_by_handle(timer_handle, weak_groups_to_nodes));
     }  // callback_group goes out of scope
     EXPECT_EQ(
       nullptr,
-      memory_strategy()->get_timer_by_handle(timer_handle, nodes));
+      memory_strategy()->get_timer_by_handle(timer_handle, weak_groups_to_nodes));
   }  // Node goes out of scope
   EXPECT_EQ(
     nullptr,
-    memory_strategy()->get_timer_by_handle(timer_handle, nodes));
+    memory_strategy()->get_timer_by_handle(timer_handle, weak_groups_to_nodes));
 }
 
 TEST_F(TestMemoryStrategy, get_node_by_group) {
-  WeakNodeList nodes;
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
   rclcpp::CallbackGroup::SharedPtr callback_group = nullptr;
   {
     auto node = std::make_shared<rclcpp::Node>("node", "ns");
     auto node_handle = node->get_node_base_interface();
-    nodes.push_back(node_handle);
-    memory_strategy()->collect_entities(nodes);
+    auto callback_groups = node_handle->get_callback_groups();
+    std::for_each(
+      callback_groups.begin(), callback_groups.end(),
+      [&weak_groups_to_nodes, &node_handle](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            weak_group_ptr,
+            node_handle));
+      });
+    memory_strategy()->collect_entities(weak_groups_to_nodes);
     EXPECT_EQ(
       nullptr,
-      memory_strategy()->get_node_by_group(nullptr, nodes));
+      memory_strategy()->get_node_by_group(nullptr, weak_groups_to_nodes));
 
     callback_group =
       node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-
+    weak_groups_to_nodes.insert(
+      std::pair<rclcpp::CallbackGroup::WeakPtr,
+      rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+        rclcpp::CallbackGroup::WeakPtr(callback_group),
+        node->get_node_base_interface()));
     EXPECT_EQ(
       node_handle,
-      memory_strategy()->get_node_by_group(callback_group, nodes));
+      memory_strategy()->get_node_by_group(callback_group, weak_groups_to_nodes));
   }  // Node goes out of scope
   EXPECT_EQ(
     nullptr,
-    memory_strategy()->get_node_by_group(callback_group, nodes));
+    memory_strategy()->get_node_by_group(callback_group, weak_groups_to_nodes));
 }
 
 TEST_F(TestMemoryStrategy, get_group_by_subscription) {
-  WeakNodeList nodes;
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
   rclcpp::SubscriptionBase::SharedPtr subscription = nullptr;
   rclcpp::CallbackGroup::SharedPtr callback_group = nullptr;
   {
     auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    nodes.push_back(node->get_node_base_interface());
-    memory_strategy()->collect_entities(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()));
+      });
+    memory_strategy()->collect_entities(weak_groups_to_nodes);
     {
       // This group is just used to test that a callback group that is held as a weak pointer
       // by node, doesn't confuse get_group_by_subscription() when it goes out of scope
@@ -273,27 +348,40 @@ TEST_F(TestMemoryStrategy, get_group_by_subscription) {
       subscription = node->create_subscription<
         test_msgs::msg::Empty, decltype(subscription_callback)>(
         "topic", qos, std::move(subscription_callback), subscription_options);
-
+      weak_groups_to_nodes.insert(
+        std::pair<rclcpp::CallbackGroup::WeakPtr,
+        rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+          rclcpp::CallbackGroup::WeakPtr(callback_group),
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr(node->get_node_base_interface())));
       EXPECT_EQ(
         callback_group,
-        memory_strategy()->get_group_by_subscription(subscription, nodes));
+        memory_strategy()->get_group_by_subscription(subscription, weak_groups_to_nodes));
     }  // callback_group goes out of scope
     EXPECT_EQ(
       callback_group,
-      memory_strategy()->get_group_by_subscription(subscription, nodes));
+      memory_strategy()->get_group_by_subscription(subscription, weak_groups_to_nodes));
   }  // Node goes out of scope
   EXPECT_EQ(
     nullptr,
-    memory_strategy()->get_group_by_subscription(subscription, nodes));
+    memory_strategy()->get_group_by_subscription(subscription, weak_groups_to_nodes));
 }
 
 TEST_F(TestMemoryStrategy, get_group_by_service) {
-  WeakNodeList nodes;
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
   rclcpp::ServiceBase::SharedPtr service = nullptr;
   {
     auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    nodes.push_back(node->get_node_base_interface());
-    memory_strategy()->collect_entities(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()));
+      });
+    memory_strategy()->collect_entities(weak_groups_to_nodes);
     {
       auto callback_group =
         node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
@@ -305,96 +393,139 @@ TEST_F(TestMemoryStrategy, get_group_by_service) {
       service = node->create_service<test_msgs::srv::Empty>(
         "service", std::move(service_callback),
         rmw_qos_profile_services_default, callback_group);
-
+      weak_groups_to_nodes.insert(
+        std::pair<rclcpp::CallbackGroup::WeakPtr,
+        rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+          rclcpp::CallbackGroup::WeakPtr(callback_group),
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr(node->get_node_base_interface())));
       EXPECT_EQ(
         callback_group,
-        memory_strategy()->get_group_by_service(service, nodes));
+        memory_strategy()->get_group_by_service(service, weak_groups_to_nodes));
     }  // callback_group goes out of scope
     EXPECT_EQ(
       nullptr,
-      memory_strategy()->get_group_by_service(service, nodes));
+      memory_strategy()->get_group_by_service(service, weak_groups_to_nodes));
   }  // Node goes out of scope
   EXPECT_EQ(
     nullptr,
-    memory_strategy()->get_group_by_service(service, nodes));
+    memory_strategy()->get_group_by_service(service, weak_groups_to_nodes));
 }
 
 TEST_F(TestMemoryStrategy, get_group_by_client) {
-  WeakNodeList nodes;
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
   rclcpp::ClientBase::SharedPtr client = nullptr;
   {
     auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    nodes.push_back(node->get_node_base_interface());
-    memory_strategy()->collect_entities(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()));
+      });
+    memory_strategy()->collect_entities(weak_groups_to_nodes);
     {
       auto callback_group =
         node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 
       client = node->create_client<test_msgs::srv::Empty>(
         "service", rmw_qos_profile_services_default, callback_group);
-
+      weak_groups_to_nodes.insert(
+        std::pair<rclcpp::CallbackGroup::WeakPtr,
+        rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+          rclcpp::CallbackGroup::WeakPtr(callback_group),
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr(node->get_node_base_interface())));
       EXPECT_EQ(
         callback_group,
-        memory_strategy()->get_group_by_client(client, nodes));
+        memory_strategy()->get_group_by_client(client, weak_groups_to_nodes));
     }  // callback_group goes out of scope
     EXPECT_EQ(
       nullptr,
-      memory_strategy()->get_group_by_client(client, nodes));
+      memory_strategy()->get_group_by_client(client, weak_groups_to_nodes));
   }  // Node goes out of scope
   EXPECT_EQ(
     nullptr,
-    memory_strategy()->get_group_by_client(client, nodes));
+    memory_strategy()->get_group_by_client(client, weak_groups_to_nodes));
 }
 
 TEST_F(TestMemoryStrategy, get_group_by_timer) {
-  WeakNodeList nodes;
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
   rclcpp::TimerBase::SharedPtr timer = nullptr;
   {
     auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    nodes.push_back(node->get_node_base_interface());
-    memory_strategy()->collect_entities(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()));
+      });
+    memory_strategy()->collect_entities(weak_groups_to_nodes);
     {
       auto callback_group =
         node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
       auto timer_callback = []() {};
       timer = node->create_wall_timer(
         std::chrono::milliseconds(1), timer_callback, callback_group);
-
+      weak_groups_to_nodes.insert(
+        std::pair<rclcpp::CallbackGroup::WeakPtr,
+        rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+          rclcpp::CallbackGroup::WeakPtr(callback_group),
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr(node->get_node_base_interface())));
       EXPECT_EQ(
         callback_group,
-        memory_strategy()->get_group_by_timer(timer, nodes));
+        memory_strategy()->get_group_by_timer(timer, weak_groups_to_nodes));
     }  // callback_group goes out of scope
     EXPECT_EQ(
       nullptr,
-      memory_strategy()->get_group_by_timer(timer, nodes));
+      memory_strategy()->get_group_by_timer(timer, weak_groups_to_nodes));
   }  // Node goes out of scope
   EXPECT_EQ(
     nullptr,
-    memory_strategy()->get_group_by_timer(timer, nodes));
+    memory_strategy()->get_group_by_timer(timer, weak_groups_to_nodes));
 }
 
 TEST_F(TestMemoryStrategy, get_group_by_waitable) {
-  WeakNodeList nodes;
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
   rclcpp::Waitable::SharedPtr waitable = nullptr;
   {
     auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    nodes.push_back(node->get_node_base_interface());
-    memory_strategy()->collect_entities(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()));
+      });
+    memory_strategy()->collect_entities(weak_groups_to_nodes);
     {
       waitable = std::make_shared<TestWaitable>();
       auto callback_group =
         node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
       node->get_node_waitables_interface()->add_waitable(waitable, callback_group);
-
+      weak_groups_to_nodes.insert(
+        std::pair<rclcpp::CallbackGroup::WeakPtr,
+        rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+          rclcpp::CallbackGroup::WeakPtr(callback_group),
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr(node->get_node_base_interface())));
       EXPECT_EQ(
         callback_group,
-        memory_strategy()->get_group_by_waitable(waitable, nodes));
+        memory_strategy()->get_group_by_waitable(waitable, weak_groups_to_nodes));
     }  // callback_group goes out of scope
     EXPECT_EQ(
       nullptr,
-      memory_strategy()->get_group_by_waitable(waitable, nodes));
+      memory_strategy()->get_group_by_waitable(waitable, weak_groups_to_nodes));
   }  // Node goes out of scope
   EXPECT_EQ(
     nullptr,
-    memory_strategy()->get_group_by_waitable(waitable, nodes));
+    memory_strategy()->get_group_by_waitable(waitable, weak_groups_to_nodes));
 }

rclcpp/test/rclcpp/test_node.cpp

@@ -26,14 +26,7 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include "rcpputils/filesystem_helper.hpp"
-
-#include "rmw/validate_namespace.h"
-
 #include "test_msgs/msg/basic_types.hpp"
-#include "test_msgs/msg/empty.hpp"
-#include "test_msgs/srv/empty.hpp"
-
-#include "../mocking_utils/patch.hpp"
 
 class TestNode : public ::testing::Test
 {
@@ -57,19 +50,7 @@ protected:
 TEST_F(TestNode, construction_and_destruction) {
   {
     auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
-    EXPECT_NE(nullptr, node->get_node_base_interface());
-    EXPECT_NE(nullptr, node->get_node_clock_interface());
-    EXPECT_NE(nullptr, node->get_node_graph_interface());
-    EXPECT_NE(nullptr, node->get_node_logging_interface());
-    EXPECT_NE(nullptr, node->get_node_time_source_interface());
-    EXPECT_NE(nullptr, node->get_node_timers_interface());
-    EXPECT_NE(nullptr, node->get_node_topics_interface());
-    EXPECT_NE(nullptr, node->get_node_services_interface());
-    EXPECT_NE(nullptr, node->get_node_parameters_interface());
-    EXPECT_NE(nullptr, node->get_node_waitables_interface());
-    EXPECT_NE(nullptr, node->get_node_options().get_rcl_node_options());
-    EXPECT_NE(nullptr, node->get_graph_event());
-    EXPECT_NE(nullptr, node->get_clock());
+    (void)node;
   }
 
   {
@@ -298,11 +279,8 @@ TEST_F(TestNode, get_logger) {
 TEST_F(TestNode, get_clock) {
   auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
   auto ros_clock = node->get_clock();
-  EXPECT_NE(nullptr, ros_clock);
+  EXPECT_TRUE(ros_clock != nullptr);
   EXPECT_EQ(ros_clock->get_clock_type(), RCL_ROS_TIME);
-
-  const rclcpp::Node & const_node = *node.get();
-  EXPECT_NE(nullptr, const_node.get_clock());
 }
 
 TEST_F(TestNode, now) {
@@ -807,24 +785,15 @@ TEST_F(TestNode, undeclare_parameter) {
 
 TEST_F(TestNode, has_parameter) {
   auto node = std::make_shared<rclcpp::Node>("test_has_parameter_node"_unq);
-  // normal use
-  auto name = "parameter"_unq;
-  EXPECT_FALSE(node->has_parameter(name));
-  node->declare_parameter(name);
-  EXPECT_TRUE(node->has_parameter(name));
-  node->undeclare_parameter(name);
-  EXPECT_FALSE(node->has_parameter(name));
-}
-
-TEST_F(TestNode, list_parameters) {
-  auto node = std::make_shared<rclcpp::Node>("test_list_parameter_node"_unq);
-  // normal use
-  auto name = "parameter"_unq;
-  const size_t before_size = node->list_parameters({}, 1u).names.size();
-  node->declare_parameter(name);
-  EXPECT_EQ(1u + before_size, node->list_parameters({}, 1u).names.size());
-  node->undeclare_parameter(name);
-  EXPECT_EQ(before_size, node->list_parameters({}, 1u).names.size());
+  {
+    // normal use
+    auto name = "parameter"_unq;
+    EXPECT_FALSE(node->has_parameter(name));
+    node->declare_parameter(name);
+    EXPECT_TRUE(node->has_parameter(name));
+    node->undeclare_parameter(name);
+    EXPECT_FALSE(node->has_parameter(name));
+  }
 }
 
 TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) {
@@ -2360,206 +2329,6 @@ TEST_F(TestNode, get_parameter_types_undeclared_parameters_allowed) {
   }
 }
 
-// suppress deprecated function test warnings
-#if !defined(_WIN32)
-# pragma GCC diagnostic push
-# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
-#else  // !defined(_WIN32)
-# pragma warning(push)
-# pragma warning(disable: 4996)
-#endif
-
-// test that it is possible to call get_parameter within the set_callback
-TEST_F(TestNode, set_on_parameters_set_callback_get_parameter) {
-  auto node = std::make_shared<rclcpp::Node>("test_set_callback_get_parameter_node"_unq);
-
-  int64_t intval = node->declare_parameter("intparam", 42);
-  EXPECT_EQ(intval, 42);
-  double floatval = node->declare_parameter("floatparam", 5.4);
-  EXPECT_EQ(floatval, 5.4);
-
-  double floatout;
-  RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);});    // always reset
-  auto on_set_parameters =
-    [&node, &floatout](const std::vector<rclcpp::Parameter> & parameters) {
-      rcl_interfaces::msg::SetParametersResult result;
-      result.successful = true;
-      if (parameters.size() != 1) {
-        result.successful = false;
-      }
-
-      if (parameters[0].get_value<int>() != 40) {
-        result.successful = false;
-      }
-
-      rclcpp::Parameter floatparam = node->get_parameter("floatparam");
-      if (floatparam.get_value<double>() != 5.4) {
-        result.successful = false;
-      }
-      floatout = floatparam.get_value<double>();
-
-      return result;
-    };
-
-  EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr);
-  ASSERT_NO_THROW(node->set_parameter({"intparam", 40}));
-  ASSERT_EQ(floatout, 5.4);
-}
-
-// test that calling set_parameter inside of a set_callback throws an exception
-TEST_F(TestNode, set_on_parameters_set_callback_set_parameter) {
-  auto node = std::make_shared<rclcpp::Node>("test_set_callback_set_parameter_node"_unq);
-
-  int64_t intval = node->declare_parameter("intparam", 42);
-  EXPECT_EQ(intval, 42);
-  double floatval = node->declare_parameter("floatparam", 5.4);
-  EXPECT_EQ(floatval, 5.4);
-
-  RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);});    // always reset
-  auto on_set_parameters =
-    [&node](const std::vector<rclcpp::Parameter> & parameters) {
-      rcl_interfaces::msg::SetParametersResult result;
-      result.successful = true;
-      if (parameters.size() != 1) {
-        result.successful = false;
-      }
-
-      if (parameters[0].get_value<int>() != 40) {
-        result.successful = false;
-      }
-
-      // This should throw an exception
-      node->set_parameter({"floatparam", 5.6});
-
-      return result;
-    };
-
-  EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr);
-  EXPECT_THROW(
-  {
-    node->set_parameter(rclcpp::Parameter("intparam", 40));
-  }, rclcpp::exceptions::ParameterModifiedInCallbackException);
-}
-
-// test that calling declare_parameter inside of a set_callback throws an exception
-TEST_F(TestNode, set_on_parameters_set_callback_declare_parameter) {
-  auto node = std::make_shared<rclcpp::Node>("test_set_callback_declare_parameter_node"_unq);
-
-  int64_t intval = node->declare_parameter("intparam", 42);
-  EXPECT_EQ(intval, 42);
-  double floatval = node->declare_parameter("floatparam", 5.4);
-  EXPECT_EQ(floatval, 5.4);
-
-  RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);});    // always reset
-  auto on_set_parameters =
-    [&node](const std::vector<rclcpp::Parameter> & parameters) {
-      rcl_interfaces::msg::SetParametersResult result;
-      result.successful = true;
-      if (parameters.size() != 1) {
-        result.successful = false;
-      }
-
-      if (parameters[0].get_value<int>() != 40) {
-        result.successful = false;
-      }
-
-      // This should throw an exception
-      node->declare_parameter("floatparam2", 5.6);
-
-      return result;
-    };
-
-  EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr);
-  EXPECT_THROW(
-  {
-    node->set_parameter(rclcpp::Parameter("intparam", 40));
-  }, rclcpp::exceptions::ParameterModifiedInCallbackException);
-}
-
-// test that calling undeclare_parameter inside a set_callback throws an exception
-TEST_F(TestNode, set_on_parameters_set_callback_undeclare_parameter) {
-  auto node = std::make_shared<rclcpp::Node>("test_set_callback_undeclare_parameter_node"_unq);
-
-  int64_t intval = node->declare_parameter("intparam", 42);
-  EXPECT_EQ(intval, 42);
-  double floatval = node->declare_parameter("floatparam", 5.4);
-  EXPECT_EQ(floatval, 5.4);
-
-  RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);});    // always reset
-  auto on_set_parameters =
-    [&node](const std::vector<rclcpp::Parameter> & parameters) {
-      rcl_interfaces::msg::SetParametersResult result;
-      result.successful = true;
-      if (parameters.size() != 1) {
-        result.successful = false;
-      }
-
-      if (parameters[0].get_value<int>() != 40) {
-        result.successful = false;
-      }
-
-      // This should throw an exception
-      node->undeclare_parameter("floatparam");
-
-      return result;
-    };
-
-  EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr);
-  EXPECT_THROW(
-  {
-    node->set_parameter(rclcpp::Parameter("intparam", 40));
-  }, rclcpp::exceptions::ParameterModifiedInCallbackException);
-}
-
-// test that calling set_on_parameters_set_callback from a set_callback throws an exception
-TEST_F(TestNode, set_on_parameters_set_callback_set_on_parameters_set_callback) {
-  auto node = std::make_shared<rclcpp::Node>("test_set_callback_set_callback_node"_unq);
-
-  int64_t intval = node->declare_parameter("intparam", 42);
-  EXPECT_EQ(intval, 42);
-  double floatval = node->declare_parameter("floatparam", 5.4);
-  EXPECT_EQ(floatval, 5.4);
-
-  RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);});    // always reset
-  auto on_set_parameters =
-    [&node](const std::vector<rclcpp::Parameter> & parameters) {
-      rcl_interfaces::msg::SetParametersResult result;
-      result.successful = true;
-      if (parameters.size() != 1) {
-        result.successful = false;
-      }
-
-      if (parameters[0].get_value<int>() != 40) {
-        result.successful = false;
-      }
-
-      auto bad_parameters =
-        [](const std::vector<rclcpp::Parameter> & parameters) {
-          (void)parameters;
-          rcl_interfaces::msg::SetParametersResult result;
-          return result;
-        };
-
-      // This should throw an exception
-      node->set_on_parameters_set_callback(bad_parameters);
-
-      return result;
-    };
-
-  EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr);
-  EXPECT_THROW(
-  {
-    node->set_parameter(rclcpp::Parameter("intparam", 40));
-  }, rclcpp::exceptions::ParameterModifiedInCallbackException);
-}
-
-// remove warning suppression
-#if !defined(_WIN32)
-# pragma GCC diagnostic pop
-#else  // !defined(_WIN32)
-# pragma warning(pop)
-#endif
-
 void expect_qos_profile_eq(
   const rmw_qos_profile_t & qos1, const rmw_qos_profile_t & qos2, bool is_publisher)
 {
@@ -2686,89 +2455,3 @@ TEST_F(TestNode, get_publishers_subscriptions_info_by_topic) {
     subscription_list = node->get_subscriptions_info_by_topic("13");
   }, rclcpp::exceptions::InvalidTopicNameError);
 }
-
-TEST_F(TestNode, callback_groups) {
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  size_t num_callback_groups_in_basic_node = node->get_callback_groups().size();
-
-  auto group1 = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-  EXPECT_EQ(1u + num_callback_groups_in_basic_node, node->get_callback_groups().size());
-
-  auto group2 = node->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
-  EXPECT_EQ(2u + num_callback_groups_in_basic_node, node->get_callback_groups().size());
-}
-
-// This is tested more thoroughly in node_interfaces/test_node_graph
-TEST_F(TestNode, get_entity_names) {
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  const auto node_names = node->get_node_names();
-  EXPECT_NE(
-    node_names.end(),
-    std::find(node_names.begin(), node_names.end(), node->get_fully_qualified_name()));
-
-  const auto topic_names_and_types = node->get_topic_names_and_types();
-  EXPECT_EQ(topic_names_and_types.end(), topic_names_and_types.find("topic"));
-
-  EXPECT_EQ(0u, node->count_publishers("topic"));
-  EXPECT_EQ(0u, node->count_subscribers("topic"));
-
-  const auto service_names_and_types = node->get_service_names_and_types();
-  EXPECT_EQ(service_names_and_types.end(), service_names_and_types.find("service"));
-
-  const auto service_names_and_types_by_node =
-    node->get_service_names_and_types_by_node("node", "/ns");
-  EXPECT_EQ(
-    service_names_and_types_by_node.end(),
-    service_names_and_types_by_node.find("service"));
-}
-
-TEST_F(TestNode, wait_for_graph_event) {
-  // Even though this node is only used in the std::thread below, it's here to ensure there is no
-  // race condition in its destruction and modification of the node_graph
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-
-  constexpr std::chrono::seconds timeout(10);
-  auto thread_start = std::chrono::steady_clock::now();
-  auto thread_completion = thread_start;
-
-  // This runs until the graph is updated
-  std::thread graph_event_wait_thread([&thread_completion, node, timeout]() {
-      auto event = node->get_graph_event();
-      EXPECT_NO_THROW(node->wait_for_graph_change(event, timeout));
-      thread_completion = std::chrono::steady_clock::now();
-    });
-
-  // Start creating nodes until at least one event triggers in graph_event_wait_thread or until 100
-  // nodes have been created (at which point this is a failure)
-  std::vector<std::shared_ptr<rclcpp::Node>> nodes;
-  while (thread_completion == thread_start && nodes.size() < 100) {
-    nodes.emplace_back(std::make_shared<rclcpp::Node>("node"_unq, "ns"));
-    std::this_thread::sleep_for(std::chrono::milliseconds(1));
-  }
-  graph_event_wait_thread.join();
-  // Nodes will probably only be of size 1
-  EXPECT_LT(0u, nodes.size());
-  EXPECT_GT(100u, nodes.size());
-  EXPECT_NE(thread_start, thread_completion);
-  EXPECT_GT(timeout, thread_completion - thread_start);
-}
-
-TEST_F(TestNode, create_sub_node_rmw_validate_namespace_error) {
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  {
-    auto mock = mocking_utils::patch_and_return(
-      "lib:rclcpp", rmw_validate_namespace, RMW_RET_INVALID_ARGUMENT);
-
-    // reset() is not necessary for this exception, but it handles unused return value warning
-    EXPECT_THROW(
-      node->create_sub_node("ns").reset(),
-      rclcpp::exceptions::RCLInvalidArgument);
-  }
-  {
-    auto mock = mocking_utils::patch_and_return(
-      "lib:rclcpp", rmw_validate_namespace, RMW_RET_ERROR);
-    EXPECT_THROW(
-      node->create_sub_node("ns").reset(),
-      rclcpp::exceptions::RCLError);
-  }
-}

rclcpp/test/rclcpp/test_node_options.cpp

@@ -14,7 +14,6 @@
 
 #include <gtest/gtest.h>
 
-#include <memory>
 #include <string>
 #include <vector>
 
@@ -24,8 +23,6 @@
 
 #include "rclcpp/node_options.hpp"
 
-#include "../mocking_utils/patch.hpp"
-
 
 TEST(TestNodeOptions, ros_args_only) {
   rcl_allocator_t allocator = rcl_get_default_allocator();
@@ -201,118 +198,3 @@ TEST(TestNodeOptions, copy) {
       rcl_arguments_get_count_unparsed(&rcl_options->arguments));
   }
 }
-
-TEST(TestNodeOptions, append_parameter_override) {
-  std::vector<std::string> expected_args{"--unknown-flag", "arg"};
-  auto options = rclcpp::NodeOptions().arguments(expected_args).use_global_arguments(false);
-  rclcpp::Parameter parameter("some_parameter", 10);
-  options.append_parameter_override("some_parameter", 10);
-  EXPECT_EQ(1u, options.parameter_overrides().size());
-  EXPECT_EQ(std::string("some_parameter"), options.parameter_overrides()[0].get_name());
-}
-
-TEST(TestNodeOptions, rcl_node_options_fini_error) {
-  auto mock = mocking_utils::inject_on_return("lib:rclcpp", rcl_node_options_fini, RCL_RET_ERROR);
-  auto options = std::make_shared<rclcpp::NodeOptions>();
-  // Necessary to setup internal pointer
-  ASSERT_NE(nullptr, options->get_rcl_node_options());
-  // If fini fails, this should just log an error and continue
-  EXPECT_NO_THROW(options.reset());
-}
-
-TEST(TestNodeOptions, bool_setters_and_getters) {
-  rclcpp::NodeOptions options;
-
-  options.use_global_arguments(false);
-  EXPECT_FALSE(options.use_global_arguments());
-  EXPECT_FALSE(options.get_rcl_node_options()->use_global_arguments);
-  options.use_global_arguments(true);
-  EXPECT_TRUE(options.use_global_arguments());
-  EXPECT_TRUE(options.get_rcl_node_options()->use_global_arguments);
-
-  options.enable_rosout(false);
-  EXPECT_FALSE(options.enable_rosout());
-  EXPECT_FALSE(options.get_rcl_node_options()->enable_rosout);
-  options.enable_rosout(true);
-  EXPECT_TRUE(options.enable_rosout());
-  EXPECT_TRUE(options.get_rcl_node_options()->enable_rosout);
-
-  options.use_intra_process_comms(false);
-  EXPECT_FALSE(options.use_intra_process_comms());
-  options.use_intra_process_comms(true);
-  EXPECT_TRUE(options.use_intra_process_comms());
-
-  options.enable_topic_statistics(false);
-  EXPECT_FALSE(options.enable_topic_statistics());
-  options.enable_topic_statistics(true);
-  EXPECT_TRUE(options.enable_topic_statistics());
-
-  options.start_parameter_services(false);
-  EXPECT_FALSE(options.start_parameter_services());
-  options.start_parameter_services(true);
-  EXPECT_TRUE(options.start_parameter_services());
-
-  options.allow_undeclared_parameters(false);
-  EXPECT_FALSE(options.allow_undeclared_parameters());
-  options.allow_undeclared_parameters(true);
-  EXPECT_TRUE(options.allow_undeclared_parameters());
-
-  options.start_parameter_event_publisher(false);
-  EXPECT_FALSE(options.start_parameter_event_publisher());
-  options.start_parameter_event_publisher(true);
-  EXPECT_TRUE(options.start_parameter_event_publisher());
-
-  options.automatically_declare_parameters_from_overrides(false);
-  EXPECT_FALSE(options.automatically_declare_parameters_from_overrides());
-  options.automatically_declare_parameters_from_overrides(true);
-  EXPECT_TRUE(options.automatically_declare_parameters_from_overrides());
-}
-
-TEST(TestNodeOptions, parameter_event_qos) {
-  rclcpp::NodeOptions options;
-  rclcpp::QoS qos1(1);
-  rclcpp::QoS qos2(2);
-  EXPECT_NE(qos1, options.parameter_event_qos());
-  EXPECT_NE(qos2, options.parameter_event_qos());
-  options.parameter_event_qos(qos1);
-  EXPECT_EQ(qos1, options.parameter_event_qos());
-  options.parameter_event_qos(qos2);
-  EXPECT_EQ(qos2, options.parameter_event_qos());
-}
-
-TEST(TestNodeOptions, parameter_event_publisher_options) {
-  rclcpp::NodeOptions options;
-  rclcpp::PublisherOptionsBase publisher_options;
-  publisher_options.use_default_callbacks = true;
-  options.parameter_event_publisher_options(publisher_options);
-  EXPECT_TRUE(options.parameter_event_publisher_options().use_default_callbacks);
-
-  publisher_options.use_default_callbacks = false;
-  options.parameter_event_publisher_options(publisher_options);
-  EXPECT_FALSE(options.parameter_event_publisher_options().use_default_callbacks);
-}
-
-TEST(TestNodeOptions, set_get_allocator) {
-  rclcpp::NodeOptions options;
-  EXPECT_NE(nullptr, options.allocator().allocate);
-  EXPECT_NE(nullptr, options.allocator().deallocate);
-  EXPECT_NE(nullptr, options.allocator().reallocate);
-  EXPECT_NE(nullptr, options.allocator().zero_allocate);
-
-  rcl_allocator_t fake_allocator;
-  fake_allocator.allocate = [](size_t, void *) -> void * {return nullptr;};
-  fake_allocator.deallocate = [](void *, void *) {};
-  fake_allocator.reallocate = [](void *, size_t, void *) -> void * {return nullptr;};
-  fake_allocator.zero_allocate = [](size_t, size_t, void *) -> void * {return nullptr;};
-  fake_allocator.state = rcl_get_default_allocator().state;
-
-  options.allocator(fake_allocator);
-  EXPECT_EQ(fake_allocator.allocate, options.allocator().allocate);
-  EXPECT_EQ(fake_allocator.deallocate, options.allocator().deallocate);
-  EXPECT_EQ(fake_allocator.reallocate, options.allocator().reallocate);
-  EXPECT_EQ(fake_allocator.zero_allocate, options.allocator().zero_allocate);
-  EXPECT_EQ(fake_allocator.state, options.allocator().state);
-
-  // Check invalid allocator
-  EXPECT_THROW(options.get_rcl_node_options(), std::bad_alloc);
-}

rclcpp/test/rclcpp/test_parameter.cpp

@@ -16,7 +16,6 @@
 
 #include <algorithm>
 #include <limits>
-#include <memory>
 #include <stdexcept>
 #include <string>
 #include <vector>
@@ -33,20 +32,11 @@ protected:
   }
 };
 
-TEST_F(TestParameter, construct_destruct) {
-  EXPECT_NO_THROW(std::make_shared<rclcpp::Parameter>().reset());
-  EXPECT_NO_THROW(std::make_shared<rclcpp::Parameter>("some_parameter").reset());
-  EXPECT_NO_THROW(std::make_shared<rclcpp::Parameter>("some_parameter", 10).reset());
-}
-
 TEST_F(TestParameter, not_set_variant) {
   // Direct instantiation
   rclcpp::Parameter not_set_variant;
   EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, not_set_variant.get_type());
   EXPECT_EQ("not set", not_set_variant.get_type_name());
-  std::stringstream ss;
-  ss << not_set_variant;
-  EXPECT_EQ("{\"name\": \"\", \"type\": \"not set\", \"value\": \"not set\"}", ss.str());
 
   EXPECT_THROW(not_set_variant.as_bool(), rclcpp::ParameterTypeException);
   EXPECT_THROW(not_set_variant.as_int(), rclcpp::ParameterTypeException);
@@ -66,17 +56,6 @@ TEST_F(TestParameter, not_set_variant) {
   EXPECT_EQ(
     rclcpp::ParameterType::PARAMETER_NOT_SET,
     rclcpp::Parameter::from_parameter_msg(not_set_param).get_type());
-
-  EXPECT_THROW(
-    not_set_variant.get_value<int>(),
-    rclcpp::exceptions::InvalidParameterTypeException);
-
-  // Check == and != operators work as expected
-  EXPECT_EQ(not_set_variant, not_set_variant);
-  rclcpp::Parameter not_set_variant2;
-  EXPECT_EQ(not_set_variant, not_set_variant2);
-  rclcpp::Parameter bool_variant("bool_param", true);
-  EXPECT_NE(not_set_variant, bool_variant);
 }
 
 TEST_F(TestParameter, bool_variant) {
@@ -102,10 +81,6 @@ TEST_F(TestParameter, bool_variant) {
   EXPECT_THROW(bool_variant_true.as_string_array(), rclcpp::ParameterTypeException);
 
   EXPECT_EQ("true", bool_variant_true.value_to_string());
-  std::stringstream ss;
-  ss << bool_variant_true;
-  EXPECT_EQ("{\"name\": \"bool_param\", \"type\": \"bool\", \"value\": \"true\"}", ss.str());
-
 
   rclcpp::Parameter bool_variant_false("bool_param", false);
   EXPECT_FALSE(bool_variant_false.get_value<rclcpp::ParameterType::PARAMETER_BOOL>());
@@ -139,11 +114,6 @@ TEST_F(TestParameter, bool_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_BOOL,
     bool_variant_false.get_value_message().type);
-
-  // Check == and != operators work as expected
-  EXPECT_EQ(bool_variant_true, bool_variant_true);
-  EXPECT_NE(bool_variant_false, bool_variant_true);
-  EXPECT_EQ(bool_variant_true, from_msg_true);
 }
 
 TEST_F(TestParameter, integer_variant) {
@@ -173,9 +143,6 @@ TEST_F(TestParameter, integer_variant) {
   EXPECT_THROW(integer_variant.as_string_array(), rclcpp::ParameterTypeException);
 
   EXPECT_EQ("42", integer_variant.value_to_string());
-  std::stringstream ss;
-  ss << integer_variant;
-  EXPECT_EQ("{\"name\": \"integer_param\", \"type\": \"integer\", \"value\": \"42\"}", ss.str());
 
   rcl_interfaces::msg::Parameter integer_param = integer_variant.to_parameter_msg();
   EXPECT_EQ("integer_param", integer_param.name);
@@ -195,11 +162,6 @@ TEST_F(TestParameter, integer_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER,
     from_msg.get_value_message().type);
-
-  // Check == and != operators work as expected
-  EXPECT_EQ(integer_variant, integer_variant);
-  EXPECT_NE(integer_variant, rclcpp::Parameter("integer_param", 41));
-  EXPECT_EQ(integer_variant, from_msg);
 }
 
 TEST_F(TestParameter, long_integer_variant) {
@@ -229,12 +191,6 @@ TEST_F(TestParameter, long_integer_variant) {
   EXPECT_THROW(long_variant.as_string_array(), rclcpp::ParameterTypeException);
 
   EXPECT_EQ("9223372036854775807", long_variant.value_to_string());
-  std::stringstream ss;
-  ss << long_variant;
-  EXPECT_EQ(
-    "{\"name\": \"long_integer_param\", \"type\": \"integer\", \"value\": "
-    "\"9223372036854775807\"}",
-    ss.str());
 
   rcl_interfaces::msg::Parameter integer_param = long_variant.to_parameter_msg();
   EXPECT_EQ("long_integer_param", integer_param.name);
@@ -254,12 +210,6 @@ TEST_F(TestParameter, long_integer_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER,
     from_msg.get_value_message().type);
-
-
-  // Check == and != operators work as expected
-  EXPECT_EQ(long_variant, long_variant);
-  EXPECT_NE(long_variant, rclcpp::Parameter("long_integer_param", 41));
-  EXPECT_EQ(long_variant, from_msg);
 }
 
 TEST_F(TestParameter, float_variant) {
@@ -289,10 +239,6 @@ TEST_F(TestParameter, float_variant) {
   EXPECT_THROW(float_variant.as_string_array(), rclcpp::ParameterTypeException);
 
   EXPECT_EQ("42.000000", float_variant.value_to_string());
-  std::stringstream ss;
-  ss << float_variant;
-  EXPECT_EQ(
-    "{\"name\": \"float_param\", \"type\": \"double\", \"value\": \"42.000000\"}", ss.str());
 
   rcl_interfaces::msg::Parameter float_param = float_variant.to_parameter_msg();
   EXPECT_EQ("float_param", float_param.name);
@@ -312,11 +258,6 @@ TEST_F(TestParameter, float_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE,
     from_msg.get_value_message().type);
-
-  // Check == and != operators work as expected
-  EXPECT_EQ(float_variant, float_variant);
-  EXPECT_NE(float_variant, rclcpp::Parameter("float_param", 41.0));
-  EXPECT_EQ(float_variant, from_msg);
 }
 
 TEST_F(TestParameter, double_variant) {
@@ -346,10 +287,6 @@ TEST_F(TestParameter, double_variant) {
   EXPECT_THROW(double_variant.as_string_array(), rclcpp::ParameterTypeException);
 
   EXPECT_EQ("-42.100000", double_variant.value_to_string());
-  std::stringstream ss;
-  ss << double_variant;
-  EXPECT_EQ(
-    "{\"name\": \"double_param\", \"type\": \"double\", \"value\": \"-42.100000\"}", ss.str());
 
   rcl_interfaces::msg::Parameter double_param = double_variant.to_parameter_msg();
   EXPECT_EQ("double_param", double_param.name);
@@ -369,11 +306,6 @@ TEST_F(TestParameter, double_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE,
     from_msg.get_value_message().type);
-
-  // Check == and != operators work as expected
-  EXPECT_EQ(double_variant, double_variant);
-  EXPECT_NE(double_variant, rclcpp::Parameter("double_param", -41.2));
-  EXPECT_EQ(double_variant, from_msg);
 }
 
 TEST_F(TestParameter, string_variant) {
@@ -403,9 +335,6 @@ TEST_F(TestParameter, string_variant) {
   EXPECT_THROW(string_variant.as_string_array(), rclcpp::ParameterTypeException);
 
   EXPECT_EQ(TEST_VALUE, string_variant.value_to_string());
-  std::stringstream ss;
-  ss << string_variant;
-  EXPECT_EQ("{\"name\": \"string_param\", \"type\": \"string\", \"value\": \"ROS2\"}", ss.str());
 
   rcl_interfaces::msg::Parameter string_param = string_variant.to_parameter_msg();
   EXPECT_EQ("string_param", string_param.name);
@@ -423,11 +352,6 @@ TEST_F(TestParameter, string_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_STRING,
     from_msg.get_value_message().type);
-
-  // Check == and != operators work as expected
-  EXPECT_EQ(string_variant, string_variant);
-  EXPECT_NE(string_variant, rclcpp::Parameter("string_param", "ROS1"));
-  EXPECT_EQ(string_variant, from_msg);
 }
 
 TEST_F(TestParameter, byte_array_variant) {
@@ -457,12 +381,6 @@ TEST_F(TestParameter, byte_array_variant) {
   EXPECT_THROW(byte_array_variant.as_string_array(), rclcpp::ParameterTypeException);
 
   EXPECT_EQ("[0x52, 0x4f, 0x53, 0x32]", byte_array_variant.value_to_string());
-  std::stringstream ss;
-  ss << byte_array_variant;
-  EXPECT_EQ(
-    "{\"name\": \"byte_array_param\", \"type\": \"byte_array\", \"value\": "
-    "\"[0x52, 0x4f, 0x53, 0x32]\"}",
-    ss.str());
 
   rcl_interfaces::msg::Parameter byte_array_param = byte_array_variant.to_parameter_msg();
   EXPECT_EQ("byte_array_param", byte_array_param.name);
@@ -482,12 +400,6 @@ TEST_F(TestParameter, byte_array_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY,
     from_msg.get_value_message().type);
-
-  // Check == and != operators work as expected
-  const std::vector<uint8_t> TEST_VALUE2 {0x1, 0x2, 0x3, 0x4};
-  EXPECT_EQ(byte_array_variant, byte_array_variant);
-  EXPECT_NE(byte_array_variant, rclcpp::Parameter("byte_array_param", TEST_VALUE2));
-  EXPECT_EQ(byte_array_variant, from_msg);
 }
 
 TEST_F(TestParameter, bool_array_variant) {
@@ -517,12 +429,6 @@ TEST_F(TestParameter, bool_array_variant) {
   EXPECT_THROW(bool_array_variant.as_string_array(), rclcpp::ParameterTypeException);
 
   EXPECT_EQ("[false, true, true, false, false, true]", bool_array_variant.value_to_string());
-  std::stringstream ss;
-  ss << bool_array_variant;
-  EXPECT_EQ(
-    "{\"name\": \"bool_array_param\", \"type\": \"bool_array\", \"value\": "
-    "\"[false, true, true, false, false, true]\"}",
-    ss.str());
 
   rcl_interfaces::msg::Parameter bool_array_param = bool_array_variant.to_parameter_msg();
   EXPECT_EQ("bool_array_param", bool_array_param.name);
@@ -542,12 +448,6 @@ TEST_F(TestParameter, bool_array_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY,
     from_msg.get_value_message().type);
-
-  // Check == and != operators work as expected
-  const std::vector<bool> TEST_VALUE2 {true, true, true};
-  EXPECT_EQ(bool_array_variant, bool_array_variant);
-  EXPECT_NE(bool_array_variant, rclcpp::Parameter("bool_array_param", TEST_VALUE2));
-  EXPECT_EQ(bool_array_variant, from_msg);
 }
 
 TEST_F(TestParameter, integer_array_variant) {
@@ -595,12 +495,6 @@ TEST_F(TestParameter, integer_array_variant) {
   EXPECT_EQ(
     "[42, -99, 2147483647, -2147483648, 0]",
     integer_array_variant.value_to_string());
-  std::stringstream ss;
-  ss << integer_array_variant;
-  EXPECT_EQ(
-    "{\"name\": \"integer_array_param\", \"type\": \"integer_array\", \"value\": "
-    "\"[42, -99, 2147483647, -2147483648, 0]\"}",
-    ss.str());
 
   rcl_interfaces::msg::Parameter integer_array_param = integer_array_variant.to_parameter_msg();
   EXPECT_EQ("integer_array_param", integer_array_param.name);
@@ -633,12 +527,6 @@ TEST_F(TestParameter, integer_array_variant) {
   EXPECT_EQ(
     from_msg.get_value_message().type,
     rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY);
-
-  // Check == and != operators work as expected
-  const std::vector<int> TEST_VALUE2{1, 2, 3, 4, 5};
-  EXPECT_EQ(integer_array_variant, integer_array_variant);
-  EXPECT_NE(integer_array_variant, rclcpp::Parameter("integer_array_param", TEST_VALUE2));
-  EXPECT_EQ(integer_array_variant, from_msg);
 }
 
 TEST_F(TestParameter, long_integer_array_variant) {
@@ -672,12 +560,6 @@ TEST_F(TestParameter, long_integer_array_variant) {
   EXPECT_EQ(
     "[42, -99, 9223372036854775807, -9223372036854775808, 0]",
     long_array_variant.value_to_string());
-  std::stringstream ss;
-  ss << long_array_variant;
-  EXPECT_EQ(
-    "{\"name\": \"long_integer_array_param\", \"type\": \"integer_array\", \"value\": "
-    "\"[42, -99, 9223372036854775807, -9223372036854775808, 0]\"}",
-    ss.str());
 
   rcl_interfaces::msg::Parameter integer_array_param = long_array_variant.to_parameter_msg();
   EXPECT_EQ("long_integer_array_param", integer_array_param.name);
@@ -699,12 +581,6 @@ TEST_F(TestParameter, long_integer_array_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY,
     from_msg.get_value_message().type);
-
-  // Check == and != operators work as expected
-  const std::vector<int64_t> TEST_VALUE2{1, 2, 3, 4, 5};
-  EXPECT_EQ(long_array_variant, long_array_variant);
-  EXPECT_NE(long_array_variant, rclcpp::Parameter("long_integer_array_param", TEST_VALUE2));
-  EXPECT_EQ(long_array_variant, from_msg);
 }
 
 TEST_F(TestParameter, float_array_variant) {
@@ -752,12 +628,6 @@ TEST_F(TestParameter, float_array_variant) {
   EXPECT_EQ(
     "[42.1, -99.1, 3.40282e+38, -3.40282e+38, 0.1]",
     float_array_variant.value_to_string());
-  std::stringstream ss;
-  ss << float_array_variant;
-  EXPECT_EQ(
-    "{\"name\": \"float_array_param\", \"type\": \"double_array\", \"value\": "
-    "\"[42.1, -99.1, 3.40282e+38, -3.40282e+38, 0.1]\"}",
-    ss.str());
 
   rcl_interfaces::msg::Parameter float_array_param = float_array_variant.to_parameter_msg();
   EXPECT_EQ("float_array_param", float_array_param.name);
@@ -790,12 +660,6 @@ TEST_F(TestParameter, float_array_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY,
     from_msg.get_value_message().type);
-
-  // Check == and != operators work as expected
-  const std::vector<float> TEST_VALUE2{1.0, 2.0, 3.0, 4.0};
-  EXPECT_EQ(float_array_variant, float_array_variant);
-  EXPECT_NE(float_array_variant, rclcpp::Parameter("float_array_param", TEST_VALUE2));
-  EXPECT_EQ(float_array_variant, from_msg);
 }
 
 TEST_F(TestParameter, double_array_variant) {
@@ -829,12 +693,6 @@ TEST_F(TestParameter, double_array_variant) {
   EXPECT_EQ(
     "[42.1, -99.1, 1.79769e+308, -1.79769e+308, 0.1]",
     double_array_variant.value_to_string());
-  std::stringstream ss;
-  ss << double_array_variant;
-  EXPECT_EQ(
-    "{\"name\": \"double_array_param\", \"type\": \"double_array\", \"value\": "
-    "\"[42.1, -99.1, 1.79769e+308, -1.79769e+308, 0.1]\"}",
-    ss.str());
 
   rcl_interfaces::msg::Parameter double_array_param = double_array_variant.to_parameter_msg();
   EXPECT_EQ("double_array_param", double_array_param.name);
@@ -856,12 +714,6 @@ TEST_F(TestParameter, double_array_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY,
     from_msg.get_value_message().type);
-
-  // Check == and != operators work as expected
-  const std::vector<double> TEST_VALUE2{1.0, 2.0, 3.0, 4.0};
-  EXPECT_EQ(double_array_variant, double_array_variant);
-  EXPECT_NE(double_array_variant, rclcpp::Parameter("double_array_param", TEST_VALUE2));
-  EXPECT_EQ(double_array_variant, from_msg);
 }
 
 TEST_F(TestParameter, string_array_variant) {
@@ -893,11 +745,6 @@ TEST_F(TestParameter, string_array_variant) {
   EXPECT_THROW(string_array_variant.as_double_array(), rclcpp::ParameterTypeException);
 
   EXPECT_EQ("[R, O, S2]", string_array_variant.value_to_string());
-  std::stringstream ss;
-  ss << string_array_variant;
-  EXPECT_EQ(
-    "{\"name\": \"string_array_param\", \"type\": \"string_array\", \"value\": \"[R, O, S2]\"}",
-    ss.str());
 
   rcl_interfaces::msg::Parameter string_array_param = string_array_variant.to_parameter_msg();
   EXPECT_EQ("string_array_param", string_array_param.name);
@@ -919,31 +766,4 @@ TEST_F(TestParameter, string_array_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY,
     from_msg.get_value_message().type);
-
-  // Check == and != operators work as expected
-  const std::vector<std::string> TEST_VALUE2{"R", "O", "S1"};
-  EXPECT_EQ(string_array_variant, string_array_variant);
-  EXPECT_NE(string_array_variant, rclcpp::Parameter("string_array_param", TEST_VALUE2));
-  EXPECT_EQ(string_array_variant, from_msg);
-}
-
-TEST_F(TestParameter, parameter_vector_stringification) {
-  const std::vector<rclcpp::Parameter> parameters = {
-    rclcpp::Parameter(),
-    rclcpp::Parameter("bool_param", true),
-    rclcpp::Parameter("integer_param", 42),
-    rclcpp::Parameter("double_param", 3.14159),
-    rclcpp::Parameter("string_param", "I'm a string"),
-  };
-
-  std::stringstream ss;
-  ss << parameters;
-  EXPECT_EQ(
-    "{"
-    "\"\": {\"type\": \"not set\", \"value\": \"not set\"}, "
-    "\"bool_param\": {\"type\": \"bool\", \"value\": \"true\"}, "
-    "\"integer_param\": {\"type\": \"integer\", \"value\": \"42\"}, "
-    "\"double_param\": {\"type\": \"double\", \"value\": \"3.141590\"}, "
-    "\"string_param\": {\"type\": \"string\", \"value\": \"I'm a string\"}}",
-    ss.str());
 }