WIP: Reorganize test CMakeLists and add benchmark dir

* Clear members for StaticExecutorEntitiesCollector to avoid shared_ptr dependency

Signed-off-by: Chen Lihui <Lihui.Chen@sony.com>
Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>
Co-authored-by: Chris Lalancette <clalancette@openrobotics.org>

README.md

@@ -8,10 +8,10 @@ rclcpp provides the standard C++ API for interacting with ROS 2.
 
 `#include "rclcpp/rclcpp.hpp"` allows use of the most common elements of the ROS 2 system.
 
-Visit the [rclcpp API documentation](http://docs.ros2.org/foxy/api/rclcpp/) for a complete list of its main components.
+Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) for a complete list of its main components.
 
 ### Examples
 
-The ROS 2 tutorials [Writing a simple publisher and subscriber](https://docs.ros.org/en/foxy/Tutorials/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html).
-and [Writing a simple service and client](https://docs.ros.org/en/foxy/Tutorials/Writing-A-Simple-Cpp-Service-And-Client.html)
+The ROS 2 tutorials [Writing a simple publisher and subscriber](https://index.ros.org/doc/ros2/Tutorials/Writing-A-Simple-Cpp-Publisher-And-Subscriber/)
+and [Writing a simple service and client](https://index.ros.org/doc/ros2/Tutorials/Writing-A-Simple-Cpp-Service-And-Client/)
 contain some examples of rclcpp APIs in use.

rclcpp/CHANGELOG.rst

@@ -2,157 +2,84 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-2.4.2 (2022-07-25)
+5.0.0 (2020-09-18)
 ------------------
-* Add statistics for handle_loaned_message (`#1927 <https://github.com/ros2/rclcpp/issues/1927>`_) (`#1934 <https://github.com/ros2/rclcpp/issues/1934>`_)
-* Add test-dep ament_cmake_google_benchmark (`#1904 <https://github.com/ros2/rclcpp/issues/1904>`_) (`#1910 <https://github.com/ros2/rclcpp/issues/1910>`_)
-* Use parantheses around logging macro parameter (`#1820 <https://github.com/ros2/rclcpp/issues/1820>`_) (`#1823 <https://github.com/ros2/rclcpp/issues/1823>`_)
-* Contributors: Abrar Rahman Protyasha, Barry Xu, Gaël Écorchard
-
-2.4.1 (2022-01-31)
-------------------
-* Fix subscription instrumentation for ConstSharedPtr[WithInfo]Callback (`#1872 <https://github.com/ros2/rclcpp/issues/1872>`_)
-* Add node_waitables\_ to copy constructor (backport `#1799 <https://github.com/ros2/rclcpp/issues/1799>`_) (`#1834 <https://github.com/ros2/rclcpp/issues/1834>`_)
-* Fix returning invalid namespace if sub_namespace is empty (`#1658 <https://github.com/ros2/rclcpp/issues/1658>`_) (`#1811 <https://github.com/ros2/rclcpp/issues/1811>`_)
-* [service] Don't use a weak_ptr to avoid leaking (`#1668 <https://github.com/ros2/rclcpp/issues/1668>`_) (`#1669 <https://github.com/ros2/rclcpp/issues/1669>`_)
-* Use dynamic_pointer_cast to detect allocator mismatch in intra process manager (backport `#1643 <https://github.com/ros2/rclcpp/issues/1643>`_) (`#1645 <https://github.com/ros2/rclcpp/issues/1645>`_)
-* Contributors: Abrar Rahman Protyasha, Christophe Bedard, Ivan Santiago Paunovic, M. Hofstätter, Michel Hidalgo, Tomoya Fujita, William Woodall
-
-2.4.0 (2021-09-01)
-------------------
-* Guard against integer overflow in duration conversion (`#1584 <https://github.com/ros2/rclcpp/issues/1584>`_) (`#1761 <https://github.com/ros2/rclcpp/issues/1761>`_)
-* Update for checking correct variable (`#1534 <https://github.com/ros2/rclcpp/issues/1534>`_) (`#1760 <https://github.com/ros2/rclcpp/issues/1760>`_)
-* Fix SEGV caused by order of destruction of Node sub-interfaces (`#1469 <https://github.com/ros2/rclcpp/issues/1469>`_) (`#1736 <https://github.com/ros2/rclcpp/issues/1736>`_)
-* Add wait for message API (`#1705 <https://github.com/ros2/rclcpp/issues/1705>`_) (`#1737 <https://github.com/ros2/rclcpp/issues/1737>`_)
-* Fix documentation bug (`#1719 <https://github.com/ros2/rclcpp/issues/1719>`_) (`#1721 <https://github.com/ros2/rclcpp/issues/1721>`_)
-* Fix clock thread issue (`#1266 <https://github.com/ros2/rclcpp/issues/1266>`_) (`#1267 <https://github.com/ros2/rclcpp/issues/1267>`_) (`#1685 <https://github.com/ros2/rclcpp/issues/1685>`_)
-* Allow timers to keep up the intended rate in MultiThreadedExecutor `#1516 <https://github.com/ros2/rclcpp/issues/1516>`_ (`#1636 <https://github.com/ros2/rclcpp/issues/1636>`_)
-  Backports `#1516 <https://github.com/ros2/rclcpp/issues/1516>`_ and follow-up fix `#1628 <https://github.com/ros2/rclcpp/issues/1628>`_
-* Contributors: Chen Lihui, Colin MacKenzie, Daisuke Sato, Jacob Perron, Karsten Knese, Tomoya Fujita, hsgwa, William Woodall
-
-2.3.1 (2021-04-14)
-------------------
-* 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/CMakeLists.txt

@@ -39,7 +39,6 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/clock.cpp
   src/rclcpp/context.cpp
   src/rclcpp/contexts/default_context.cpp
-  src/rclcpp/detail/mutex_two_priorities.cpp
   src/rclcpp/detail/rmw_implementation_specific_payload.cpp
   src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp
   src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp
@@ -223,6 +222,12 @@ if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 
+  # For benchmark tests. Currently there is a bug where these targets can't be
+  # found if these calls are added to the CMakeLists.txt in test/rclcpp/benchmark
+  find_package(benchmark REQUIRED)
+  find_package(osrf_testing_tools_cpp REQUIRED)
+  find_package(performance_test_fixture REQUIRED)
+
   add_subdirectory(test)
 endif()
 

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,23 @@ 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).
+Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastSuccessfulBuild/cobertura/src_ros2_rclcpp_rclcpp_src_rclcpp/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://index.ros.org/doc/ros2/Contributing/ROS-2-On-boarding-Guide/#note-on-coverage-runs).
 
 `rclcpp` has a line coverage `>= 95%`, which is calculated over all directories within `rclcpp` with the exception of the `experimental` directory.
 
 ### Performance [4.iv]
 
-`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 +155,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 +207,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,32 +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 (const_shared_ptr_callback_) {
-      TRACEPOINT(
-        rclcpp_callback_register,
-        (const void *)this,
-        get_symbol(const_shared_ptr_callback_));
-    } else if (const_shared_ptr_with_info_callback_) {
-      TRACEPOINT(
-        rclcpp_callback_register,
-        (const void *)this,
-        get_symbol(const_shared_ptr_with_info_callback_));
     } else if (unique_ptr_callback_) {
       TRACEPOINT(
         rclcpp_callback_register,
-        (const void *)this,
+        static_cast<const void *>(this),
         get_symbol(unique_ptr_callback_));
     } else if (unique_ptr_with_info_callback_) {
       TRACEPOINT(
         rclcpp_callback_register,
-        (const void *)this,
+        static_cast<const void *>(this),
         get_symbol(unique_ptr_with_info_callback_));
     }
 #endif  // TRACETOOLS_DISABLED

rclcpp/include/rclcpp/callback_group.hpp

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

rclcpp/include/rclcpp/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/detail/mutex_two_priorities.hpp

@@ -1,76 +0,0 @@
-// Copyright 2021 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef RCLCPP__DETAIL__MUTEX_TWO_PRIORITIES_HPP_
-#define RCLCPP__DETAIL__MUTEX_TWO_PRIORITIES_HPP_
-
-#include <condition_variable>
-#include <mutex>
-
-namespace rclcpp
-{
-namespace detail
-{
-/// \internal A mutex that has two locking mechanism, one with higher priority than the other.
-/**
- * After the current mutex owner release the lock, a thread that used the high
- * priority mechanism will have priority over threads that used the low priority mechanism.
- */
-class MutexTwoPriorities
-{
-public:
-  class HighPriorityLockable
-  {
-public:
-    explicit HighPriorityLockable(MutexTwoPriorities & parent);
-
-    void lock();
-
-    void unlock();
-
-private:
-    MutexTwoPriorities & parent_;
-  };
-
-  class LowPriorityLockable
-  {
-public:
-    explicit LowPriorityLockable(MutexTwoPriorities & parent);
-
-    void lock();
-
-    void unlock();
-
-private:
-    MutexTwoPriorities & parent_;
-  };
-
-  HighPriorityLockable
-  get_high_priority_lockable();
-
-  LowPriorityLockable
-  get_low_priority_lockable();
-
-private:
-  std::condition_variable hp_cv_;
-  std::condition_variable lp_cv_;
-  std::mutex cv_mutex_;
-  size_t hp_waiting_count_{0u};
-  bool data_taken_{false};
-};
-
-}  // namespace detail
-}  // namespace rclcpp
-
-#endif  // RCLCPP__DETAIL__MUTEX_TWO_PRIORITIES_HPP_

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

@@ -22,7 +22,6 @@
 #include <thread>
 #include <unordered_map>
 
-#include "rclcpp/detail/mutex_two_priorities.hpp"
 #include "rclcpp/executor.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/memory_strategies.hpp"
@@ -82,17 +81,12 @@ protected:
 private:
   RCLCPP_DISABLE_COPY(MultiThreadedExecutor)
 
-  std::mutex wait_mutex_;  // Unused. Leave it for ABI compatibility.
+  std::mutex wait_mutex_;
   size_t number_of_threads_;
   bool yield_before_execute_;
   std::chrono::nanoseconds next_exec_timeout_;
 
   std::set<TimerBase::SharedPtr> scheduled_timers_;
-  static std::unordered_map<MultiThreadedExecutor *,
-    std::shared_ptr<detail::MutexTwoPriorities>> wait_mutex_set_;
-  static std::mutex shared_wait_mutex_;
-  // These variables are declared as static variables for ABI-compatibiliity.
-  // And they mimic member variables needed to backport from master.
 };
 
 }  // namespace executors

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

rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp

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

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

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

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

@@ -202,8 +202,7 @@ public:
       // None of the buffers require ownership, so we promote the pointer
       std::shared_ptr<MessageT> msg = std::move(message);
 
-      this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
-        msg, sub_ids.take_shared_subscriptions);
+      this->template add_shared_msg_to_buffers<MessageT>(msg, sub_ids.take_shared_subscriptions);
     } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT
       sub_ids.take_shared_subscriptions.size() <= 1)
     {
@@ -228,7 +227,7 @@ public:
       // for the buffers that do not require ownership
       auto shared_msg = std::allocate_shared<MessageT, MessageAllocatorT>(*allocator, *message);
 
-      this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
+      this->template add_shared_msg_to_buffers<MessageT>(
         shared_msg, sub_ids.take_shared_subscriptions);
       this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter>(
         std::move(message), sub_ids.take_ownership_subscriptions, allocator);
@@ -264,7 +263,7 @@ public:
       // If there are no owning, just convert to shared.
       std::shared_ptr<MessageT> shared_msg = std::move(message);
       if (!sub_ids.take_shared_subscriptions.empty()) {
-        this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
+        this->template add_shared_msg_to_buffers<MessageT>(
           shared_msg, sub_ids.take_shared_subscriptions);
       }
       return shared_msg;
@@ -274,7 +273,7 @@ public:
       auto shared_msg = std::allocate_shared<MessageT, MessageAllocatorT>(*allocator, *message);
 
       if (!sub_ids.take_shared_subscriptions.empty()) {
-        this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
+        this->template add_shared_msg_to_buffers<MessageT>(
           shared_msg,
           sub_ids.take_shared_subscriptions);
       }
@@ -351,10 +350,7 @@ private:
   bool
   can_communicate(PublisherInfo pub_info, SubscriptionInfo sub_info) const;
 
-  template<
-    typename MessageT,
-    typename Alloc,
-    typename Deleter>
+  template<typename MessageT>
   void
   add_shared_msg_to_buffers(
     std::shared_ptr<const MessageT> message,
@@ -367,16 +363,10 @@ private:
       }
       auto subscription_base = subscription_it->second.subscription;
 
-      auto subscription = std::dynamic_pointer_cast<
-        rclcpp::experimental::SubscriptionIntraProcess<MessageT, Alloc, Deleter>
+      auto subscription = std::static_pointer_cast<
+        rclcpp::experimental::SubscriptionIntraProcess<MessageT>
         >(subscription_base);
-      if (nullptr == subscription) {
-        throw std::runtime_error(
-                "failed to dynamic cast SubscriptionIntraProcessBase to "
-                "SubscriptionIntraProcess<MessageT, Alloc, Deleter>, which "
-                "can happen when the publisher and subscription use different "
-                "allocator types, which is not supported");
-      }
+
       subscription->provide_intra_process_message(message);
     }
   }
@@ -401,16 +391,9 @@ private:
       }
       auto subscription_base = subscription_it->second.subscription;
 
-      auto subscription = std::dynamic_pointer_cast<
-        rclcpp::experimental::SubscriptionIntraProcess<MessageT, Alloc, Deleter>
+      auto subscription = std::static_pointer_cast<
+        rclcpp::experimental::SubscriptionIntraProcess<MessageT>
         >(subscription_base);
-      if (nullptr == subscription) {
-        throw std::runtime_error(
-                "failed to dynamic cast SubscriptionIntraProcessBase to "
-                "SubscriptionIntraProcess<MessageT, Alloc, Deleter>, which "
-                "can happen when the publisher and subscription use different "
-                "allocator types, which is not supported");
-      }
 
       if (std::next(it) == subscription_ids.end()) {
         // If this is the last subscription, give up ownership

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -92,8 +92,8 @@ public:
 
     TRACEPOINT(
       rclcpp_subscription_callback_added,
-      (const void *)this,
-      (const void *)&any_callback_);
+      static_cast<const void *>(this),
+      static_cast<const void *>(&any_callback_));
     // The callback object gets copied, so if registration is done too early/before this point
     // (e.g. in `AnySubscriptionCallback::set()`), its address won't match any address used later
     // in subsequent tracepoints.
@@ -102,6 +102,16 @@ public:
 #endif
   }
 
+  ~SubscriptionIntraProcess()
+  {
+    if (rcl_guard_condition_fini(&gc_) != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "Failed to destroy guard condition: %s",
+        rcutils_get_error_string().str);
+    }
+  }
+
   bool
   is_ready(rcl_wait_set_t * wait_set)
   {

rclcpp/include/rclcpp/future_return_code.hpp

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

rclcpp/include/rclcpp/init_options.hpp

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

rclcpp/include/rclcpp/logger.hpp

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

rclcpp/include/rclcpp/memory_strategy.hpp

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

rclcpp/include/rclcpp/node.hpp

@@ -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.
@@ -892,15 +874,12 @@ public:
   std::map<std::string, std::vector<std::string>>
   get_service_names_and_types() const;
 
-  /// Return a map of existing service names to list of service types for a specific node.
+  /// Return the number of publishers that are advertised on a given topic.
   /**
-   * This function only considers services - not clients.
-   * The returned names are the actual names used and do not have remap rules applied.
-   *
-   * \param[in] node_name name of the node.
-   * \param[in] namespace_ namespace of the node.
-   * \return a map of existing service names to list of service types.
-   * \throws std::runtime_error anything that rcl_error can throw.
+   * \param[in] node_name the node_name on which to count the publishers.
+   * \param[in] namespace_ the namespace of the node associated with the name
+   * \return number of publishers that are advertised on a given topic.
+   * \throws std::runtime_error if publishers could not be counted
    */
   RCLCPP_PUBLIC
   std::map<std::string, std::vector<std::string>>
@@ -908,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
@@ -937,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.
@@ -963,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.
@@ -1197,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_interfaces/node_time_source.hpp

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

rclcpp/include/rclcpp/node_options.hpp

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

rclcpp/include/rclcpp/parameter_client.hpp

@@ -73,12 +73,21 @@ public:
    * \param[in] qos_profile (optional) The rmw qos profile to use to subscribe
    * \param[in] group (optional) The async parameter client will be added to this callback group.
    */
-  RCLCPP_PUBLIC
+  template<typename NodeT>
   AsyncParametersClient(
-    const rclcpp::Node::SharedPtr node,
+    const std::shared_ptr<NodeT> node,
     const std::string & remote_node_name = "",
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters,
-    rclcpp::CallbackGroup::SharedPtr group = nullptr);
+    rclcpp::CallbackGroup::SharedPtr group = nullptr)
+  : AsyncParametersClient(
+      node->get_node_base_interface(),
+      node->get_node_topics_interface(),
+      node->get_node_graph_interface(),
+      node->get_node_services_interface(),
+      remote_node_name,
+      qos_profile,
+      group)
+  {}
 
   /// Constructor
   /**
@@ -87,12 +96,21 @@ public:
    * \param[in] qos_profile (optional) The rmw qos profile to use to subscribe
    * \param[in] group (optional) The async parameter client will be added to this callback group.
    */
-  RCLCPP_PUBLIC
+  template<typename NodeT>
   AsyncParametersClient(
-    rclcpp::Node * node,
+    NodeT * node,
     const std::string & remote_node_name = "",
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters,
-    rclcpp::CallbackGroup::SharedPtr group = nullptr);
+    rclcpp::CallbackGroup::SharedPtr group = nullptr)
+  : AsyncParametersClient(
+      node->get_node_base_interface(),
+      node->get_node_topics_interface(),
+      node->get_node_graph_interface(),
+      node->get_node_services_interface(),
+      remote_node_name,
+      qos_profile,
+      group)
+  {}
 
   RCLCPP_PUBLIC
   std::shared_future<std::vector<rclcpp::Parameter>>
@@ -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

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

rclcpp/include/rclcpp/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

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

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

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

rclcpp/include/rclcpp/timer.hpp

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

rclcpp/include/rclcpp/wait_for_message.hpp

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

rclcpp/package.xml

@@ -2,10 +2,13 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp</name>
-  <version>2.4.2</version>
+  <version>5.0.0</version>
   <description>The ROS client library in C++.</description>
-  <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
+  <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
+  <maintainer email="mabel@openrobotics.org">Mabel Zhang</maintainer>
+  <maintainer email="william@openrobotics.org">William Woodall</maintainer>
   <license>Apache License 2.0</license>
+  <author email="dthomas@openrobotics.org">Dirk Thomas</author>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
@@ -32,7 +35,6 @@
   <depend>tracetools</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
-  <test_depend>ament_cmake_google_benchmark</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

rclcpp/resource/logging.hpp.em

@@ -149,7 +149,7 @@ def get_rclcpp_suffix_from_features(features):
 @[ if params]@
 @(''.join(['      ' + p + ', \\\n' for p in params if p != stream_arg]))@
 @[ end if]@
-      (logger).get_name(), \
+      logger.get_name(), \
 @[ if 'stream' not in feature_combination]@
       rclcpp::get_c_string(RCLCPP_FIRST_ARG(__VA_ARGS__, "")), \
         RCLCPP_ALL_BUT_FIRST_ARGS(__VA_ARGS__,"")); \

rclcpp/src/rclcpp/callback_group.cpp

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

rclcpp/src/rclcpp/context.cpp

@@ -199,13 +199,17 @@ Context::init(
     throw rclcpp::ContextAlreadyInitialized();
   }
   this->clean_up();
-  rcl_context_.reset(new rcl_context_t, __delete_context);
-  *rcl_context_.get() = rcl_get_zero_initialized_context();
-  rcl_ret_t ret = rcl_init(argc, argv, init_options.get_rcl_init_options(), rcl_context_.get());
+  rcl_context_t * context = new rcl_context_t;
+  if (!context) {
+    throw std::runtime_error("failed to allocate memory for rcl context");
+  }
+  *context = rcl_get_zero_initialized_context();
+  rcl_ret_t ret = rcl_init(argc, argv, init_options.get_rcl_init_options(), context);
   if (RCL_RET_OK != ret) {
-    rcl_context_.reset();
+    delete context;
     rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl");
   }
+  rcl_context_.reset(context, __delete_context);
 
   if (init_options.auto_initialize_logging()) {
     logging_mutex_ = get_global_logging_mutex();
@@ -275,6 +279,17 @@ Context::get_init_options()
   return init_options_;
 }
 
+size_t
+Context::get_domain_id() const
+{
+  size_t domain_id;
+  rcl_ret_t ret = rcl_context_get_domain_id(rcl_context_.get(), &domain_id);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get domain id from context");
+  }
+  return domain_id;
+}
+
 std::string
 Context::shutdown_reason()
 {

rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp

@@ -1,104 +0,0 @@
-// Copyright 2021 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include "rclcpp/detail/mutex_two_priorities.hpp"
-
-#include <mutex>
-
-namespace rclcpp
-{
-namespace detail
-{
-
-using LowPriorityLockable = MutexTwoPriorities::LowPriorityLockable;
-using HighPriorityLockable = MutexTwoPriorities::HighPriorityLockable;
-
-HighPriorityLockable::HighPriorityLockable(MutexTwoPriorities & parent)
-: parent_(parent)
-{}
-
-void
-HighPriorityLockable::lock()
-{
-  std::unique_lock<std::mutex> guard{parent_.cv_mutex_};
-  if (parent_.data_taken_) {
-    ++parent_.hp_waiting_count_;
-    while (parent_.data_taken_) {
-      parent_.hp_cv_.wait(guard);
-    }
-    --parent_.hp_waiting_count_;
-  }
-  parent_.data_taken_ = true;
-}
-
-void
-HighPriorityLockable::unlock()
-{
-  bool notify_lp{false};
-  {
-    std::lock_guard<std::mutex> guard{parent_.cv_mutex_};
-    parent_.data_taken_ = false;
-    notify_lp = 0u == parent_.hp_waiting_count_;
-  }
-  if (notify_lp) {
-    parent_.lp_cv_.notify_one();
-  } else {
-    parent_.hp_cv_.notify_one();
-  }
-}
-
-LowPriorityLockable::LowPriorityLockable(MutexTwoPriorities & parent)
-: parent_(parent)
-{}
-
-void
-LowPriorityLockable::lock()
-{
-  std::unique_lock<std::mutex> guard{parent_.cv_mutex_};
-  while (parent_.data_taken_ || parent_.hp_waiting_count_) {
-    parent_.lp_cv_.wait(guard);
-  }
-  parent_.data_taken_ = true;
-}
-
-void
-LowPriorityLockable::unlock()
-{
-  bool notify_lp{false};
-  {
-    std::lock_guard<std::mutex> guard{parent_.cv_mutex_};
-    parent_.data_taken_ = false;
-    notify_lp = 0u == parent_.hp_waiting_count_;
-  }
-  if (notify_lp) {
-    parent_.lp_cv_.notify_one();
-  } else {
-    parent_.hp_cv_.notify_one();
-  }
-}
-
-HighPriorityLockable
-MutexTwoPriorities::get_high_priority_lockable()
-{
-  return HighPriorityLockable{*this};
-}
-
-LowPriorityLockable
-MutexTwoPriorities::get_low_priority_lockable()
-{
-  return LowPriorityLockable{*this};
-}
-
-}  // namespace detail
-}  // namespace rclcpp

rclcpp/src/rclcpp/duration.cpp

@@ -67,27 +67,13 @@ Duration::operator builtin_interfaces::msg::Duration() const
 {
   builtin_interfaces::msg::Duration msg_duration;
   constexpr rcl_duration_value_t kDivisor = RCL_S_TO_NS(1);
-  constexpr int32_t max_s = std::numeric_limits<int32_t>::max();
-  constexpr int32_t min_s = std::numeric_limits<int32_t>::min();
-  constexpr uint32_t max_ns = std::numeric_limits<uint32_t>::max();
   const auto result = std::div(rcl_duration_.nanoseconds, kDivisor);
   if (result.rem >= 0) {
-    // saturate if we will overflow
-    if (result.quot > max_s) {
-      msg_duration.sec = max_s;
-      msg_duration.nanosec = max_ns;
-    } else {
-      msg_duration.sec = static_cast<int32_t>(result.quot);
-      msg_duration.nanosec = static_cast<uint32_t>(result.rem);
-    }
+    msg_duration.sec = static_cast<std::int32_t>(result.quot);
+    msg_duration.nanosec = static_cast<std::uint32_t>(result.rem);
   } else {
-    if (result.quot <= min_s) {
-      msg_duration.sec = min_s;
-      msg_duration.nanosec = 0u;
-    } else {
-      msg_duration.sec = static_cast<int32_t>(result.quot - 1);
-      msg_duration.nanosec = static_cast<uint32_t>(kDivisor + result.rem);
-    }
+    msg_duration.sec = static_cast<std::int32_t>(result.quot - 1);
+    msg_duration.nanosec = static_cast<std::uint32_t>(kDivisor + result.rem);
   }
   return msg_duration;
 }
@@ -252,14 +238,11 @@ Duration::to_rmw_time() const
     throw std::runtime_error("rmw_time_t cannot be negative");
   }
 
-  // Purposefully avoid creating from builtin_interfaces::msg::Duration
-  // to avoid possible overflow converting from int64_t to int32_t, then back to uint64_t
+  // reuse conversion logic from msg creation
+  builtin_interfaces::msg::Duration msg = *this;
   rmw_time_t result;
-  constexpr rcl_duration_value_t kDivisor = RCL_S_TO_NS(1);
-  const auto div_result = std::div(rcl_duration_.nanoseconds, kDivisor);
-  result.sec = static_cast<uint64_t>(div_result.quot);
-  result.nsec = static_cast<uint64_t>(div_result.rem);
-
+  result.sec = static_cast<uint64_t>(msg.sec);
+  result.nsec = static_cast<uint64_t>(msg.nanosec);
   return result;
 }
 

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

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -17,19 +17,13 @@
 #include <chrono>
 #include <functional>
 #include <memory>
-#include <unordered_map>
 #include <vector>
 
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/scope_exit.hpp"
 
-using rclcpp::detail::MutexTwoPriorities;
 using rclcpp::executors::MultiThreadedExecutor;
 
-std::unordered_map<MultiThreadedExecutor *, std::shared_ptr<MutexTwoPriorities>>
-MultiThreadedExecutor::wait_mutex_set_;
-std::mutex MultiThreadedExecutor::shared_wait_mutex_;
-
 MultiThreadedExecutor::MultiThreadedExecutor(
   const rclcpp::ExecutorOptions & options,
   size_t number_of_threads,
@@ -39,11 +33,6 @@ MultiThreadedExecutor::MultiThreadedExecutor(
   yield_before_execute_(yield_before_execute),
   next_exec_timeout_(next_exec_timeout)
 {
-  {
-    std::lock_guard<std::mutex> wait_lock(
-      MultiThreadedExecutor::shared_wait_mutex_);
-    wait_mutex_set_[this] = std::make_shared<MutexTwoPriorities>();
-  }
   number_of_threads_ = number_of_threads ? number_of_threads : std::thread::hardware_concurrency();
   if (number_of_threads_ == 0) {
     number_of_threads_ = 1;
@@ -62,9 +51,7 @@ MultiThreadedExecutor::spin()
   std::vector<std::thread> threads;
   size_t thread_id = 0;
   {
-    auto wait_mutex = MultiThreadedExecutor::wait_mutex_set_[this];
-    auto low_priority_wait_mutex = wait_mutex->get_low_priority_lockable();
-    std::lock_guard<MutexTwoPriorities::LowPriorityLockable> wait_lock(low_priority_wait_mutex);
+    std::lock_guard<std::mutex> wait_lock(wait_mutex_);
     for (; thread_id < number_of_threads_ - 1; ++thread_id) {
       auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id);
       threads.emplace_back(func);
@@ -89,9 +76,7 @@ MultiThreadedExecutor::run(size_t)
   while (rclcpp::ok(this->context_) && spinning.load()) {
     rclcpp::AnyExecutable any_exec;
     {
-      auto wait_mutex = MultiThreadedExecutor::wait_mutex_set_[this];
-      auto low_priority_wait_mutex = wait_mutex->get_low_priority_lockable();
-      std::lock_guard<MutexTwoPriorities::LowPriorityLockable> wait_lock(low_priority_wait_mutex);
+      std::lock_guard<std::mutex> wait_lock(wait_mutex_);
       if (!rclcpp::ok(this->context_) || !spinning.load()) {
         return;
       }
@@ -118,9 +103,7 @@ MultiThreadedExecutor::run(size_t)
     execute_any_executable(any_exec);
 
     if (any_exec.timer) {
-      auto wait_mutex = MultiThreadedExecutor::wait_mutex_set_[this];
-      auto high_priority_wait_mutex = wait_mutex->get_high_priority_lockable();
-      std::lock_guard<MutexTwoPriorities::HighPriorityLockable> wait_lock(high_priority_wait_mutex);
+      std::lock_guard<std::mutex> wait_lock(wait_mutex_);
       auto it = scheduled_timers_.find(any_exec.timer);
       if (it != scheduled_timers_.end()) {
         scheduled_timers_.erase(it);

rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp

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

rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp

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

rclcpp/src/rclcpp/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

@@ -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

@@ -54,12 +54,6 @@ extend_sub_namespace(const std::string & existing_sub_namespace, const std::stri
             extension.c_str(),
             "a sub-namespace should not have a leading /",
             0);
-  } else if (existing_sub_namespace.empty() && extension.empty()) {
-    throw rclcpp::exceptions::NameValidationError(
-            "sub_namespace",
-            extension.c_str(),
-            "sub-nodes should not extend nodes by an empty sub-namespace",
-            0);
   }
 
   std::string new_sub_namespace;
@@ -85,11 +79,7 @@ create_effective_namespace(const std::string & node_namespace, const std::string
   // and do not need trimming of `/` and other things, as they were validated
   // in other functions already.
 
-  // A node may not have a sub_namespace if it is no sub_node. In this case,
-  // just return the original namespace
-  if (sub_namespace.empty()) {
-    return node_namespace;
-  } else if (node_namespace.back() == '/') {
+  if (node_namespace.back() == '/') {
     // this is the special case where node_namespace is just `/`
     return node_namespace + sub_namespace;
   } else {
@@ -148,7 +138,8 @@ Node::Node(
       node_services_,
       node_logging_,
       node_clock_,
-      node_parameters_
+      node_parameters_,
+      options.clock_qos()
     )),
   node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())),
   node_options_(options),
@@ -168,8 +159,6 @@ Node::Node(
   node_services_(other.node_services_),
   node_clock_(other.node_clock_),
   node_parameters_(other.node_parameters_),
-  node_time_source_(other.node_time_source_),
-  node_waitables_(other.node_waitables_),
   node_options_(other.node_options_),
   sub_namespace_(extend_sub_namespace(other.get_sub_namespace(), sub_namespace)),
   effective_namespace_(create_effective_namespace(other.get_namespace(), sub_namespace_))
@@ -196,18 +185,7 @@ Node::Node(
 }
 
 Node::~Node()
-{
-  // release sub-interfaces in an order that allows them to consult with node_base during tear-down
-  node_waitables_.reset();
-  node_time_source_.reset();
-  node_parameters_.reset();
-  node_clock_.reset();
-  node_services_.reset();
-  node_topics_.reset();
-  node_timers_.reset();
-  node_logging_.reset();
-  node_graph_.reset();
-}
+{}
 
 const char *
 Node::get_name() const
@@ -234,15 +212,11 @@ Node::get_logger() const
 }
 
 rclcpp::CallbackGroup::SharedPtr
-Node::create_callback_group(rclcpp::CallbackGroupType group_type)
+Node::create_callback_group(
+  rclcpp::CallbackGroupType group_type,
+  bool automatically_add_to_executor_with_node)
 {
-  return node_base_->create_callback_group(group_type);
-}
-
-bool
-Node::group_in_node(rclcpp::CallbackGroup::SharedPtr group)
-{
-  return node_base_->callback_group_in_node(group);
+  return node_base_->create_callback_group(group_type, automatically_add_to_executor_with_node);
 }
 
 const rclcpp::ParameterValue &
@@ -351,28 +325,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_interfaces/node_time_source.cpp

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

rclcpp/src/rclcpp/node_options.cpp

@@ -77,7 +77,9 @@ NodeOptions::operator=(const NodeOptions & other)
     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->clock_qos_ = other.clock_qos_;
     this->parameter_event_qos_ = other.parameter_event_qos_;
+    this->rosout_qos_ = other.rosout_qos_;
     this->parameter_event_publisher_options_ = other.parameter_event_publisher_options_;
     this->allow_undeclared_parameters_ = other.allow_undeclared_parameters_;
     this->automatically_declare_parameters_from_overrides_ =
@@ -96,8 +98,8 @@ 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_;
+    node_options_->rosout_qos = this->rosout_qos_.get_rmw_qos_profile();
 
     int c_argc = 0;
     std::unique_ptr<const char *[]> c_argv;
@@ -257,6 +259,19 @@ NodeOptions::start_parameter_event_publisher(bool start_parameter_event_publishe
   return *this;
 }
 
+const rclcpp::QoS &
+NodeOptions::clock_qos() const
+{
+  return this->clock_qos_;
+}
+
+NodeOptions &
+NodeOptions::clock_qos(const rclcpp::QoS & clock_qos)
+{
+  this->clock_qos_ = clock_qos;
+  return *this;
+}
+
 const rclcpp::QoS &
 NodeOptions::parameter_event_qos() const
 {
@@ -270,6 +285,20 @@ NodeOptions::parameter_event_qos(const rclcpp::QoS & parameter_event_qos)
   return *this;
 }
 
+const rclcpp::QoS &
+NodeOptions::rosout_qos() const
+{
+  return this->rosout_qos_;
+}
+
+NodeOptions &
+NodeOptions::rosout_qos(const rclcpp::QoS & rosout_qos)
+{
+  this->node_options_.reset();
+  this->rosout_qos_ = rosout_qos;
+  return *this;
+}
+
 const rclcpp::PublisherOptionsBase &
 NodeOptions::parameter_event_publisher_options() const
 {
@@ -326,36 +355,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

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

rclcpp/src/rclcpp/publisher_base.cpp

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

rclcpp/src/rclcpp/qos.cpp

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

rclcpp/src/rclcpp/subscription_base.cpp

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

rclcpp/src/rclcpp/time_source.cpp

@@ -33,17 +33,16 @@
 namespace rclcpp
 {
 
-TimeSource::TimeSource(std::shared_ptr<rclcpp::Node> node)
+TimeSource::TimeSource(std::shared_ptr<rclcpp::Node> node, const rclcpp::QoS & qos)
 : logger_(rclcpp::get_logger("rclcpp")),
-  clock_subscription_(nullptr),
-  ros_time_active_(false)
+  qos_(qos)
 {
   this->attachNode(node);
 }
 
-TimeSource::TimeSource()
+TimeSource::TimeSource(const rclcpp::QoS & qos)
 : logger_(rclcpp::get_logger("rclcpp")),
-  ros_time_active_(false)
+  qos_(qos)
 {
 }
 
@@ -237,7 +236,7 @@ void TimeSource::create_clock_sub()
   clock_subscription_ = rclcpp::create_subscription<rosgraph_msgs::msg::Clock>(
     node_topics_,
     "/clock",
-    rclcpp::QoS(QoSInitialization::from_rmw(rmw_qos_profile_default)),
+    rclcpp::QoS(KeepLast(1)).best_effort(),
     std::bind(&TimeSource::clock_cb, this, std::placeholders::_1)
   );
 }

rclcpp/test/CMakeLists.txt

@@ -4,7 +4,14 @@ find_package(test_msgs REQUIRED)
 
 include(cmake/rclcpp_add_build_failure_test.cmake)
 
-add_subdirectory(benchmark)
+rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs
+  msg/Header.msg
+  msg/MessageWithHeader.msg
+  DEPENDENCIES builtin_interfaces
+  LIBRARY_NAME ${PROJECT_NAME}
+  SKIP_INSTALL
+)
+
 add_subdirectory(rclcpp)
 
 ament_add_gtest(test_rclcpp_gtest_macros utils/test_rclcpp_gtest_macros.cpp)

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/rclcpp/CMakeLists.txt

@@ -1,17 +1,11 @@
+add_subdirectory(benchmark)
+
 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)
@@ -97,6 +91,15 @@ if(TARGET test_create_subscription)
     "test_msgs"
   )
 endif()
+ament_add_gtest(test_add_callback_groups_to_executor
+  test_add_callback_groups_to_executor.cpp
+  TIMEOUT 120)
+if(TARGET test_add_callback_groups_to_executor)
+  target_link_libraries(test_add_callback_groups_to_executor ${PROJECT_NAME})
+  ament_target_dependencies(test_add_callback_groups_to_executor
+    "test_msgs"
+  )
+endif()
 ament_add_gtest(test_expand_topic_or_service_name test_expand_topic_or_service_name.cpp)
 if(TARGET test_expand_topic_or_service_name)
   ament_target_dependencies(test_expand_topic_or_service_name
@@ -134,13 +137,6 @@ if(TARGET test_intra_process_manager)
   )
   target_link_libraries(test_intra_process_manager ${PROJECT_NAME})
 endif()
-ament_add_gmock(test_intra_process_manager_with_allocators test_intra_process_manager_with_allocators.cpp)
-if(TARGET test_intra_process_manager_with_allocators)
-  ament_target_dependencies(test_intra_process_manager_with_allocators
-    "test_msgs"
-  )
-  target_link_libraries(test_intra_process_manager_with_allocators ${PROJECT_NAME})
-endif()
 ament_add_gtest(test_ring_buffer_implementation test_ring_buffer_implementation.cpp)
 if(TARGET test_ring_buffer_implementation)
   ament_target_dependencies(test_ring_buffer_implementation
@@ -515,13 +511,6 @@ if(TARGET test_utilities)
   target_link_libraries(test_utilities ${PROJECT_NAME} mimick)
 endif()
 
-ament_add_gtest(test_wait_for_message test_wait_for_message.cpp)
-if(TARGET test_wait_for_message)
-  ament_target_dependencies(test_wait_for_message
-    "test_msgs")
-  target_link_libraries(test_wait_for_message ${PROJECT_NAME})
-endif()
-
 ament_add_gtest(test_init test_init.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}")
 if(TARGET test_init)
@@ -638,6 +627,12 @@ if(TARGET test_thread_safe_synchronization)
   target_link_libraries(test_thread_safe_synchronization ${PROJECT_NAME})
 endif()
 
+ament_add_gtest(test_rosout_qos test_rosout_qos.cpp)
+if(TARGET test_rosout_qos)
+  ament_target_dependencies(test_rosout_qos "rcl")
+  target_link_libraries(test_rosout_qos ${PROJECT_NAME})
+endif()
+
 ament_add_gtest(test_executor test_executor.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}"
   TIMEOUT 120)

rclcpp/test/rclcpp/benchmark/CMakeLists.txt

@@ -0,0 +1,29 @@
+# Give cppcheck hints about macro definitions coming from outside this package
+get_target_property(
+  ament_cmake_cppcheck_ADDITIONAL_INCLUDE_DIRS
+  performance_test_fixture::performance_test_fixture
+  INTERFACE_INCLUDE_DIRECTORIES)
+
+function(add_rclcpp_benchmark NAME SOURCE)
+  cmake_parse_arguments(RCLCPP_BENCHMARK "" "LIBRARIES" "")
+  if(RCLCPP_UNPARSED_ARGUMENTS)
+    message(FATAL_ERROR "Unrecognized arguments for 'add_rclcpp_benchmark' (${RCLCPP_UNPARSED_ARGUMENTS})")
+    return()
+  endif()
+  find_package(${rmw_implementation} REQUIRED)
+  message(STATUS "Adding ${NAME} for '${rmw_implementation}'")
+  set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation})
+
+  set(full_benchmark_name ${NAME}${target_suffix})
+  add_performance_test(${full_benchmark_name} ${SOURCE})
+  if(TARGET ${full_benchmark_name})
+    target_link_libraries(${full_benchmark_name} ${PROJECT_NAME} ${LIBRARIES})
+  endif()
+endfunction()
+
+# Add new benchmarks inside this macro
+macro(rclcpp_benchmarks)
+  add_rclcpp_benchmark(benchmark_client benchmark_client.cpp)
+endmacro()
+
+call_for_each_rmw_implementation(rclcpp_benchmarks)

rclcpp/test/rclcpp/benchmark/benchmark_client.cpp

@@ -0,0 +1,13 @@
+// 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.

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:
@@ -306,6 +457,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;

rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp

@@ -14,13 +14,18 @@
 
 #include <gtest/gtest.h>
 
+#include <chrono>
 #include <memory>
+#include <stdexcept>
 
 #include "rclcpp/rclcpp.hpp"
 
 #include "test_msgs/msg/empty.hpp"
 #include "test_msgs/srv/empty.hpp"
 
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
 namespace
 {
 
@@ -105,7 +110,9 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node) {
   EXPECT_NO_THROW(entities_collector_->add_node(node1->get_node_base_interface()));
 
   // Check adding second time
-  EXPECT_THROW(entities_collector_->add_node(node1->get_node_base_interface()), std::runtime_error);
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->add_node(node1->get_node_base_interface()),
+    std::runtime_error("Node has already been added to an executor."));
 
   auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
   EXPECT_FALSE(entities_collector_->remove_node(node2->get_node_base_interface()));
@@ -114,6 +121,10 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node) {
   EXPECT_TRUE(entities_collector_->remove_node(node1->get_node_base_interface()));
   EXPECT_FALSE(entities_collector_->remove_node(node1->get_node_base_interface()));
   EXPECT_TRUE(entities_collector_->remove_node(node2->get_node_base_interface()));
+
+  auto node3 = std::make_shared<rclcpp::Node>("node3", "ns");
+  node3->get_node_base_interface()->get_associated_with_executor_atomic().exchange(true);
+  EXPECT_FALSE(entities_collector_->remove_node(node3->get_node_base_interface()));
 }
 
 TEST_F(TestStaticExecutorEntitiesCollector, init_bad_arguments) {
@@ -296,3 +307,316 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) {
   // Still one for the executor
   EXPECT_EQ(1u, entities_collector_->get_number_of_waitables());
 }
+
+TEST_F(TestStaticExecutorEntitiesCollector, add_callback_group) {
+  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
+  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, add_callback_group_after_add_node) {
+  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_node(node->get_node_base_interface());
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->add_callback_group(cb_group, node->get_node_base_interface()),
+    std::runtime_error("Callback group has already been added to an executor."));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, add_callback_group_twice) {
+  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
+  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
+  cb_group->get_associated_with_executor_atomic().exchange(false);
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->add_callback_group(cb_group, node->get_node_base_interface()),
+    std::runtime_error("Callback group was already added to executor."));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_clear_error) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+
+  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
+  rclcpp::GuardCondition guard_condition(shared_context);
+  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
+
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+
+  {
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      entities_collector_->execute(),
+      std::runtime_error("Couldn't clear wait set"));
+  }
+
+  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_resize_error) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+
+  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
+  rclcpp::GuardCondition guard_condition(shared_context);
+  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
+
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+
+  {
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_resize, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      entities_collector_->execute(),
+      std::runtime_error("Couldn't resize the wait set: error not set"));
+  }
+
+  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_not_initialized) {
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->refresh_wait_set(std::chrono::nanoseconds(1000)),
+    std::runtime_error("Couldn't clear wait set"));
+  rcl_reset_error();
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_rcl_wait_failed) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+
+  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
+  rclcpp::GuardCondition guard_condition(shared_context);
+  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
+
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+
+  {
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      entities_collector_->refresh_wait_set(std::chrono::nanoseconds(1000)),
+      std::runtime_error("rcl_wait() failed: error not set"));
+  }
+
+  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_add_handles_to_wait_set_failed) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+
+  // Create 1 of each entity type
+  auto subscription =
+    node->create_subscription<test_msgs::msg::Empty>(
+    "topic", rclcpp::QoS(10), [](test_msgs::msg::Empty::SharedPtr) {});
+  auto timer =
+    node->create_wall_timer(std::chrono::seconds(60), []() {});
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  auto waitable = std::make_shared<TestWaitable>();
+
+  node->get_node_waitables_interface()->add_waitable(waitable, nullptr);
+
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+
+  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
+
+  rclcpp::GuardCondition guard_condition(shared_context);
+  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
+
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_wait_set_add_subscription,
+      RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      entities_collector_->refresh_wait_set(std::chrono::nanoseconds(1000)),
+      std::runtime_error("Couldn't fill wait set"));
+  }
+
+  entities_collector_->remove_node(node->get_node_base_interface());
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, add_to_wait_set_nullptr) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+
+  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
+  rclcpp::GuardCondition guard_condition(shared_context);
+  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
+
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->add_to_wait_set(nullptr),
+    std::runtime_error("Executor waitable: couldn't add guard condition to wait set"));
+  rcl_reset_error();
+
+  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, fill_memory_strategy_invalid_group) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+
+  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
+  rclcpp::GuardCondition guard_condition(shared_context);
+  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
+
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
+  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 2u);
+
+  cb_group.reset();
+
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
+
+  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, remove_callback_group_after_node) {
+  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
+  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
+
+  node.reset();
+
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->remove_callback_group(cb_group),
+    std::runtime_error("Node must not be deleted before its callback group(s)."));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, remove_callback_group_twice) {
+  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
+  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
+
+  entities_collector_->remove_callback_group(cb_group);
+
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->remove_callback_group(cb_group),
+    std::runtime_error("Callback group needs to be associated with executor."));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, remove_node_opposite_order) {
+  auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
+  EXPECT_NO_THROW(entities_collector_->add_node(node1->get_node_base_interface()));
+
+  auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
+  EXPECT_NO_THROW(entities_collector_->add_node(node2->get_node_base_interface()));
+
+  EXPECT_TRUE(entities_collector_->remove_node(node2->get_node_base_interface()));
+}
+
+TEST_F(
+  TestStaticExecutorEntitiesCollector,
+  add_callback_groups_from_nodes_associated_to_executor_add) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+
+  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
+  rclcpp::GuardCondition guard_condition(shared_context);
+  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
+
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+
+  cb_group->get_associated_with_executor_atomic().exchange(false);
+  entities_collector_->execute();
+
+  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
+}

rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp

@@ -15,13 +15,20 @@
 #include <gtest/gtest.h>
 
 #include <chrono>
+#include <future>
 #include <memory>
+#include <stdexcept>
 
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp/executors.hpp"
 
+#include "test_msgs/srv/empty.hpp"
+
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
 using namespace std::chrono_literals;
 
 class TestStaticSingleThreadedExecutor : public ::testing::Test
@@ -44,5 +51,102 @@ 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);
 }
+
+TEST_F(TestStaticSingleThreadedExecutor, add_callback_group_trigger_guard_failed) {
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      executor.add_callback_group(cb_group, node->get_node_base_interface(), true),
+      std::runtime_error("error not set"));
+  }
+}
+
+TEST_F(TestStaticSingleThreadedExecutor, add_node_trigger_guard_failed) {
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      executor.add_node(node),
+      std::runtime_error("error not set"));
+  }
+}
+
+TEST_F(TestStaticSingleThreadedExecutor, remove_callback_group_trigger_guard_failed) {
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  executor.add_callback_group(cb_group, node->get_node_base_interface(), true);
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      executor.remove_callback_group(cb_group, true),
+      std::runtime_error("error not set"));
+  }
+}
+
+TEST_F(TestStaticSingleThreadedExecutor, remove_node_failed) {
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      executor.remove_node(node, true),
+      std::runtime_error("Node needs to be associated with this executor."));
+  }
+}
+
+TEST_F(TestStaticSingleThreadedExecutor, remove_node_trigger_guard_failed) {
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+
+  executor.add_node(node);
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      executor.remove_node(node, true),
+      std::runtime_error("error not set"));
+  }
+}
+
+TEST_F(TestStaticSingleThreadedExecutor, execute_service) {
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  executor.add_node(node);
+
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+
+  std::promise<void> promise;
+  std::future<void> future = promise.get_future();
+  EXPECT_EQ(
+    rclcpp::FutureReturnCode::TIMEOUT,
+    executor.spin_until_future_complete(future, std::chrono::milliseconds(1)));
+
+  executor.remove_node(node);
+  executor.spin_until_future_complete(future, std::chrono::milliseconds(1));
+}

rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp

@@ -91,7 +91,7 @@ TEST_F(TestNodeGraph, construct_from_node)
 
   // 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(1u, node_graph()->count_graph_users());
 }
 
 TEST_F(TestNodeGraph, get_topic_names_and_types)

rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp

@@ -125,7 +125,7 @@ TEST_F(TestNodeParameters, parameter_overrides)
   auto * node_parameters_interface =
     dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
     node2->get_node_parameters_interface().get());
-  ASSERT_NE(nullptr, node_parameters_interface);
+  ASSERT_NE(nullptr, node_parameters);
 
   const auto & parameter_overrides = node_parameters_interface->get_parameter_overrides();
   EXPECT_EQ(2u, parameter_overrides.size());

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

rclcpp/test/rclcpp/test_duration.cpp

@@ -138,7 +138,6 @@ TEST_F(TestDuration, maximum_duration) {
 static const int64_t HALF_SEC_IN_NS = 500 * 1000 * 1000;
 static const int64_t ONE_SEC_IN_NS = 1000 * 1000 * 1000;
 static const int64_t ONE_AND_HALF_SEC_IN_NS = 3 * HALF_SEC_IN_NS;
-static const int64_t MAX_NANOSECONDS = std::numeric_limits<int64_t>::max();
 
 TEST_F(TestDuration, from_seconds) {
   EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration::from_seconds(0.0));
@@ -238,34 +237,6 @@ TEST_F(TestDuration, conversions) {
     auto chrono_duration = duration.to_chrono<std::chrono::nanoseconds>();
     EXPECT_EQ(chrono_duration.count(), -ONE_AND_HALF_SEC_IN_NS);
   }
-
-  {
-    auto duration = rclcpp::Duration(MAX_NANOSECONDS);
-
-    const auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
-    EXPECT_EQ(duration_msg.sec, std::numeric_limits<int32_t>::max());
-    EXPECT_EQ(duration_msg.nanosec, std::numeric_limits<uint32_t>::max());
-
-    auto rmw_time = duration.to_rmw_time();
-    EXPECT_EQ(rmw_time.sec, 9223372036u);
-    EXPECT_EQ(rmw_time.nsec, 854775807u);
-
-    auto chrono_duration = duration.to_chrono<std::chrono::nanoseconds>();
-    EXPECT_EQ(chrono_duration.count(), MAX_NANOSECONDS);
-  }
-
-  {
-    auto duration = rclcpp::Duration(-MAX_NANOSECONDS);
-
-    const auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
-    EXPECT_EQ(duration_msg.sec, std::numeric_limits<int32_t>::min());
-    EXPECT_EQ(duration_msg.nanosec, 0u);
-
-    EXPECT_THROW(duration.to_rmw_time(), std::runtime_error);
-
-    auto chrono_duration = duration.to_chrono<std::chrono::nanoseconds>();
-    EXPECT_EQ(chrono_duration.count(), -MAX_NANOSECONDS);
-  }
 }
 
 TEST_F(TestDuration, test_some_constructors) {

rclcpp/test/rclcpp/test_executor.cpp

@@ -51,6 +51,12 @@ public:
     return memory_strategy_.get();
   }
 
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr local_get_node_by_group(
+    rclcpp::CallbackGroup::SharedPtr group)
+  {
+    return get_node_by_group(weak_groups_to_nodes_, group);
+  }
+
   rclcpp::CallbackGroup::SharedPtr local_get_group_by_timer(rclcpp::TimerBase::SharedPtr timer)
   {
     return get_group_by_timer(timer);
@@ -97,6 +103,87 @@ TEST_F(TestExecutor, constructor_bad_wait_set_init) {
     std::runtime_error("Failed to create wait set in Executor constructor: error not set"));
 }
 
+TEST_F(TestExecutor, add_callback_group_twice) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  dummy.add_callback_group(cb_group, node->get_node_base_interface(), false);
+  cb_group->get_associated_with_executor_atomic().exchange(false);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.add_callback_group(cb_group, node->get_node_base_interface(), false),
+    std::runtime_error("Callback group was already added to executor."));
+}
+
+TEST_F(TestExecutor, add_callback_group_failed_trigger_guard_condition) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.add_callback_group(cb_group, node->get_node_base_interface(), true),
+    std::runtime_error("Failed to trigger guard condition on callback group add: error not set"));
+}
+
+TEST_F(TestExecutor, remove_callback_group_null_node) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  dummy.add_callback_group(cb_group, node->get_node_base_interface(), true);
+
+  node.reset();
+
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.remove_callback_group(cb_group, false),
+    std::runtime_error("Node must not be deleted before its callback group(s)."));
+}
+
+TEST_F(TestExecutor, remove_callback_group_failed_trigger_guard_condition) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  dummy.add_callback_group(cb_group, node->get_node_base_interface(), true);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.remove_callback_group(cb_group, true),
+    std::runtime_error(
+      "Failed to trigger guard condition on callback group remove: error not set"));
+}
+
+TEST_F(TestExecutor, remove_node_not_associated) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.remove_node(node->get_node_base_interface(), false),
+    std::runtime_error("Node needs to be associated with an executor."));
+}
+
+TEST_F(TestExecutor, remove_node_associated_with_different_executor) {
+  DummyExecutor dummy1;
+  auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
+  dummy1.add_node(node1->get_node_base_interface(), false);
+
+  DummyExecutor dummy2;
+  auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
+  dummy2.add_node(node2->get_node_base_interface(), false);
+
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy2.remove_node(node1->get_node_base_interface(), false),
+    std::runtime_error("Node needs to be associated with this executor."));
+}
+
 TEST_F(TestExecutor, spin_node_once_nanoseconds) {
   DummyExecutor dummy;
   auto node = std::make_shared<rclcpp::Node>("node", "ns");
@@ -125,6 +212,15 @@ TEST_F(TestExecutor, spin_node_some) {
   EXPECT_TRUE(timer_fired);
 }
 
+TEST_F(TestExecutor, spin_all_invalid_duration) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.spin_all(std::chrono::nanoseconds(-1)),
+    std::invalid_argument("max_duration must be positive"));
+}
+
 TEST_F(TestExecutor, spin_some_in_spin_some) {
   DummyExecutor dummy;
   auto node = std::make_shared<rclcpp::Node>("node", "ns");
@@ -291,6 +387,28 @@ TEST_F(TestExecutor, spin_some_fail_wait) {
     std::runtime_error("rcl_wait() failed: error not set"));
 }
 
+TEST_F(TestExecutor, get_node_by_group_null_group) {
+  DummyExecutor dummy;
+  ASSERT_EQ(nullptr, dummy.local_get_node_by_group(nullptr));
+}
+
+TEST_F(TestExecutor, get_node_by_group) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  dummy.add_callback_group(cb_group, node->get_node_base_interface(), false);
+  ASSERT_EQ(node->get_node_base_interface().get(), dummy.local_get_node_by_group(cb_group).get());
+}
+
+TEST_F(TestExecutor, get_node_by_group_not_found) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  ASSERT_EQ(nullptr, dummy.local_get_node_by_group(cb_group).get());
+}
+
 TEST_F(TestExecutor, get_group_by_timer_nullptr) {
   DummyExecutor dummy;
   ASSERT_EQ(nullptr, dummy.local_get_group_by_timer(nullptr));
@@ -322,6 +440,48 @@ TEST_F(TestExecutor, get_group_by_timer_with_deleted_group) {
   ASSERT_EQ(nullptr, dummy.local_get_group_by_timer(timer).get());
 }
 
+TEST_F(TestExecutor, get_group_by_timer_add_callback_group) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group);
+  dummy.add_callback_group(cb_group, node->get_node_base_interface(), false);
+
+  ASSERT_EQ(cb_group.get(), dummy.local_get_group_by_timer(timer).get());
+}
+
+TEST_F(TestExecutor, spin_until_future_complete_in_spin_until_future_complete) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool spin_until_future_complete_in_spin_until_future_complete = false;
+  auto timer =
+    node->create_wall_timer(
+    std::chrono::milliseconds(1), [&]() {
+      try {
+        std::promise<void> promise;
+        std::future<void> future = promise.get_future();
+        dummy.spin_until_future_complete(future, std::chrono::milliseconds(1));
+      } catch (const std::runtime_error & err) {
+        if (err.what() == std::string(
+          "spin_until_future_complete() called while already spinning"))
+        {
+          spin_until_future_complete_in_spin_until_future_complete = true;
+        }
+      }
+    });
+
+  dummy.add_node(node);
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  EXPECT_FALSE(spin_until_future_complete_in_spin_until_future_complete);
+  std::promise<void> promise;
+  std::future<void> future = promise.get_future();
+  dummy.spin_until_future_complete(future, std::chrono::milliseconds(1));
+  EXPECT_TRUE(spin_until_future_complete_in_spin_until_future_complete);
+}
+
 TEST_F(TestExecutor, spin_node_once_base_interface) {
   DummyExecutor dummy;
   auto node = std::make_shared<rclcpp::Node>("node", "ns");
@@ -355,3 +515,14 @@ TEST_F(TestExecutor, spin_node_once_node) {
   dummy.spin_node_once(node);
   EXPECT_TRUE(spin_called);
 }
+
+TEST_F(TestExecutor, spin_until_future_complete_future_already_complete) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  std::promise<void> promise;
+  std::future<void> future = promise.get_future();
+  promise.set_value();
+  EXPECT_EQ(
+    rclcpp::FutureReturnCode::SUCCESS,
+    dummy.spin_until_future_complete(future, std::chrono::milliseconds(1)));
+}

rclcpp/test/rclcpp/test_find_weak_nodes.cpp

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

rclcpp/test/rclcpp/test_init_options.cpp

@@ -65,6 +65,20 @@ TEST(TestInitOptions, test_initialize_logging) {
   }
 }
 
+TEST(TestInitOptions, test_domain_id) {
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto options = rclcpp::InitOptions(allocator);
+  size_t domain_id = RCL_DEFAULT_DOMAIN_ID;
+  EXPECT_EQ(RCL_RET_OK, rcl_get_default_domain_id(&domain_id));
+
+  options.use_default_domain_id();
+  EXPECT_EQ(domain_id, options.get_domain_id());
+  options.set_domain_id(42);
+  EXPECT_EQ((size_t)42, options.get_domain_id());
+  options.use_default_domain_id();
+  EXPECT_EQ(domain_id, options.get_domain_id());
+}
+
 // Required for mocking_utils below
 MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==)
 MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=)
@@ -97,3 +111,30 @@ TEST(TestInitOptions, copy_constructor_rcl_init_options_copy_failed) {
     options2.operator=(options),
     std::runtime_error("failed to copy rcl init options: error not set"));
 }
+
+TEST(TestInitOptions, use_default_domain_id_rcl_get_default_domain_id_failed) {
+  rclcpp::InitOptions options;
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_default_domain_id, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    options.use_default_domain_id(),
+    std::runtime_error("failed to get default domain id: error not set"));
+}
+
+TEST(TestInitOptions, set_domain_id_rcl_init_options_set_domain_id_failed) {
+  rclcpp::InitOptions options;
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_init_options_set_domain_id, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    options.set_domain_id(0),
+    std::runtime_error("failed to set domain id to rcl init options: error not set"));
+}
+
+TEST(TestInitOptions, get_domain_id_rcl_init_options_get_domain_id_failed) {
+  rclcpp::InitOptions options;
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_init_options_get_domain_id, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    options.get_domain_id(),
+    std::runtime_error("failed to get domain id from rcl init options: error not set"));
+}

rclcpp/test/rclcpp/test_intra_process_manager.cpp

@@ -216,10 +216,7 @@ public:
   const char * topic_name;
 };
 
-template<
-  typename MessageT,
-  typename Alloc = std::allocator<void>,
-  typename Deleter = std::default_delete<MessageT>>
+template<typename MessageT>
 class SubscriptionIntraProcess : public SubscriptionIntraProcessBase
 {
 public:

rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp

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

rclcpp/test/rclcpp/test_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_logging.cpp

@@ -249,12 +249,6 @@ TEST_F(TestLoggingMacros, test_throttle) {
   }
 }
 
-TEST_F(TestLoggingMacros, test_parameter_expression) {
-  RCLCPP_DEBUG_STREAM(*&g_logger, "message");
-  EXPECT_EQ(1u, g_log_calls);
-  EXPECT_EQ("message", g_last_log_event.message);
-}
-
 bool log_function(rclcpp::Logger logger)
 {
   RCLCPP_INFO(logger, "successful log");

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,92 @@ 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);
 
+    // Nothing in the weak_groups_to_nodes, so find fails.
+    EXPECT_EQ(
+      nullptr,
+      memory_strategy()->get_node_by_group(callback_group, weak_groups_to_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()));
     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
+  // Callback group still exists, so lookup returns nullptr because node is destroyed.
   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 +355,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 +400,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

@@ -94,7 +94,6 @@ TEST_F(TestNode, get_name_and_namespace) {
     auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
     EXPECT_STREQ("my_node", node->get_name());
     EXPECT_STREQ("/ns", node->get_namespace());
-    EXPECT_STREQ("/ns", node->get_effective_namespace().c_str());
     EXPECT_STREQ("/ns/my_node", node->get_fully_qualified_name());
   }
   {
@@ -109,35 +108,30 @@ TEST_F(TestNode, get_name_and_namespace) {
     auto node = std::make_shared<rclcpp::Node>("my_node", "ns");
     EXPECT_STREQ("my_node", node->get_name());
     EXPECT_STREQ("/ns", node->get_namespace());
-    EXPECT_STREQ("/ns", node->get_effective_namespace().c_str());
     EXPECT_STREQ("/ns/my_node", node->get_fully_qualified_name());
   }
   {
     auto node = std::make_shared<rclcpp::Node>("my_node");
     EXPECT_STREQ("my_node", node->get_name());
     EXPECT_STREQ("/", node->get_namespace());
-    EXPECT_STREQ("/", node->get_effective_namespace().c_str());
     EXPECT_STREQ("/my_node", node->get_fully_qualified_name());
   }
   {
     auto node = std::make_shared<rclcpp::Node>("my_node", "");
     EXPECT_STREQ("my_node", node->get_name());
     EXPECT_STREQ("/", node->get_namespace());
-    EXPECT_STREQ("/", node->get_effective_namespace().c_str());
     EXPECT_STREQ("/my_node", node->get_fully_qualified_name());
   }
   {
     auto node = std::make_shared<rclcpp::Node>("my_node", "/my/ns");
     EXPECT_STREQ("my_node", node->get_name());
     EXPECT_STREQ("/my/ns", node->get_namespace());
-    EXPECT_STREQ("/my/ns", node->get_effective_namespace().c_str());
     EXPECT_STREQ("/my/ns/my_node", node->get_fully_qualified_name());
   }
   {
     auto node = std::make_shared<rclcpp::Node>("my_node", "my/ns");
     EXPECT_STREQ("my_node", node->get_name());
     EXPECT_STREQ("/my/ns", node->get_namespace());
-    EXPECT_STREQ("/my/ns", node->get_effective_namespace().c_str());
     EXPECT_STREQ("/my/ns/my_node", node->get_fully_qualified_name());
   }
   {
@@ -276,13 +270,6 @@ TEST_F(TestNode, subnode_construction_and_destruction) {
       auto subnode = node->create_sub_node("~sub_ns");
     }, rclcpp::exceptions::InvalidNamespaceError);
   }
-  {
-    ASSERT_THROW(
-    {
-      auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
-      auto subnode = node->create_sub_node("");
-    }, rclcpp::exceptions::NameValidationError);
-  }
 }
 
 TEST_F(TestNode, get_logger) {
@@ -2373,206 +2360,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)
 {

rclcpp/test/rclcpp/test_qos.cpp

@@ -165,6 +165,9 @@ TEST(TestQoS, DerivedTypes) {
   rclcpp::ParameterEventsQoS parameter_events_qos;
   EXPECT_EQ(rmw_qos_profile_parameter_events, parameter_events_qos.get_rmw_qos_profile());
 
+  rclcpp::RosoutQoS rosout_qos;
+  EXPECT_EQ(rcl_qos_profile_rosout_default, rosout_qos.get_rmw_qos_profile());
+
   rclcpp::SystemDefaultsQoS system_default_qos;
   const rclcpp::KeepLast expected_initialization(RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT);
   const rclcpp::QoS expected_default(expected_initialization);