5.1.0

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

README.md

@@ -8,7 +8,7 @@ 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/eloquent/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
 

rclcpp/CHANGELOG.rst

@@ -2,6 +2,280 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+5.1.0 (2020-11-02)
+------------------
+* Deprecate Duration(rcl_duration_value_t) in favor of static Duration::from_nanoseconds(rcl_duration_value_t) (`#1432 <https://github.com/ros2/rclcpp/issues/1432>`_)
+* Avoid parsing arguments twice in `rclcpp::init_and_remove_ros_arguments` (`#1415 <https://github.com/ros2/rclcpp/issues/1415>`_)
+* Add service and client benchmarks (`#1425 <https://github.com/ros2/rclcpp/issues/1425>`_)
+* Set CMakeLists to only use default rmw for benchmarks (`#1427 <https://github.com/ros2/rclcpp/issues/1427>`_)
+* Update tracetools' QL in rclcpp's QD (`#1428 <https://github.com/ros2/rclcpp/issues/1428>`_)
+* Add missing locking to the rclcpp_action::ServerBase. (`#1421 <https://github.com/ros2/rclcpp/issues/1421>`_)
+* Initial benchmark tests for rclcpp::init/shutdown create/destroy node (`#1411 <https://github.com/ros2/rclcpp/issues/1411>`_)
+* Refactor test CMakeLists in prep for benchmarks (`#1422 <https://github.com/ros2/rclcpp/issues/1422>`_)
+* Add methods in topic and service interface to resolve a name (`#1410 <https://github.com/ros2/rclcpp/issues/1410>`_)
+* Update deprecated gtest macros (`#1370 <https://github.com/ros2/rclcpp/issues/1370>`_)
+* Clear members for StaticExecutorEntitiesCollector to avoid shared_ptr dependency (`#1303 <https://github.com/ros2/rclcpp/issues/1303>`_)
+* Increase test timeouts of slow running tests with rmw_connext_cpp (`#1400 <https://github.com/ros2/rclcpp/issues/1400>`_)
+* Avoid self dependency that not destoryed (`#1301 <https://github.com/ros2/rclcpp/issues/1301>`_)
+* Update maintainers (`#1384 <https://github.com/ros2/rclcpp/issues/1384>`_)
+* Add clock qos to node options (`#1375 <https://github.com/ros2/rclcpp/issues/1375>`_)
+* Fix NodeOptions copy constructor (`#1376 <https://github.com/ros2/rclcpp/issues/1376>`_)
+* Make sure to clean the external client/service handle. (`#1296 <https://github.com/ros2/rclcpp/issues/1296>`_)
+* Increase coverage of WaitSetTemplate (`#1368 <https://github.com/ros2/rclcpp/issues/1368>`_)
+* Increase coverage of guard_condition.cpp to 100% (`#1369 <https://github.com/ros2/rclcpp/issues/1369>`_)
+* Add coverage statement (`#1367 <https://github.com/ros2/rclcpp/issues/1367>`_)
+* Tests for LoanedMessage with mocked loaned message publisher (`#1366 <https://github.com/ros2/rclcpp/issues/1366>`_)
+* Add unit tests for qos and qos_event files (`#1352 <https://github.com/ros2/rclcpp/issues/1352>`_)
+* Finish coverage of publisher API (`#1365 <https://github.com/ros2/rclcpp/issues/1365>`_)
+* Finish API coverage on executors. (`#1364 <https://github.com/ros2/rclcpp/issues/1364>`_)
+* Add test for ParameterService (`#1355 <https://github.com/ros2/rclcpp/issues/1355>`_)
+* Add time API coverage tests (`#1347 <https://github.com/ros2/rclcpp/issues/1347>`_)
+* Add timer coverage tests (`#1363 <https://github.com/ros2/rclcpp/issues/1363>`_)
+* Add in additional tests for parameter_client.cpp coverage.
+* Minor fixes to the parameter_service.cpp file.
+* reset rcl_context shared_ptr after calling rcl_init sucessfully (`#1357 <https://github.com/ros2/rclcpp/issues/1357>`_)
+* Improved test publisher - zero qos history depth value exception (`#1360 <https://github.com/ros2/rclcpp/issues/1360>`_)
+* Covered resolve_use_intra_process (`#1359 <https://github.com/ros2/rclcpp/issues/1359>`_)
+* Improve test_subscription_options (`#1358 <https://github.com/ros2/rclcpp/issues/1358>`_)
+* Add in more tests for init_options coverage. (`#1353 <https://github.com/ros2/rclcpp/issues/1353>`_)
+* Test the remaining node public API (`#1342 <https://github.com/ros2/rclcpp/issues/1342>`_)
+* Complete coverage of Parameter and ParameterValue API (`#1344 <https://github.com/ros2/rclcpp/issues/1344>`_)
+* Add in more tests for the utilities. (`#1349 <https://github.com/ros2/rclcpp/issues/1349>`_)
+* Add in two more tests for expand_topic_or_service_name. (`#1350 <https://github.com/ros2/rclcpp/issues/1350>`_)
+* Add tests for node_options API (`#1343 <https://github.com/ros2/rclcpp/issues/1343>`_)
+* Add in more coverage for expand_topic_or_service_name. (`#1346 <https://github.com/ros2/rclcpp/issues/1346>`_)
+* Test exception in spin_until_future_complete. (`#1345 <https://github.com/ros2/rclcpp/issues/1345>`_)
+* Add coverage tests graph_listener (`#1330 <https://github.com/ros2/rclcpp/issues/1330>`_)
+* Add in unit tests for the Executor class.
+* Allow mimick patching of methods with up to 9 arguments.
+* Improve the error messages in the Executor class.
+* Add coverage for client API (`#1329 <https://github.com/ros2/rclcpp/issues/1329>`_)
+* Increase service coverage (`#1332 <https://github.com/ros2/rclcpp/issues/1332>`_)
+* Make more of the static entity collector API private.
+* Const-ify more of the static executor.
+* Add more tests for the static single threaded executor.
+* Many more tests for the static_executor_entities_collector.
+* Get one more line of code coverage in memory_strategy.cpp
+* Bugfix when adding callback group.
+* Fix typos in comments.
+* Remove deprecated executor::FutureReturnCode APIs. (`#1327 <https://github.com/ros2/rclcpp/issues/1327>`_)
+* Increase coverage of publisher/subscription API (`#1325 <https://github.com/ros2/rclcpp/issues/1325>`_)
+* Not finalize guard condition while destructing SubscriptionIntraProcess (`#1307 <https://github.com/ros2/rclcpp/issues/1307>`_)
+* Expose qos setting for /rosout (`#1247 <https://github.com/ros2/rclcpp/issues/1247>`_)
+* Add coverage for missing API (except executors) (`#1326 <https://github.com/ros2/rclcpp/issues/1326>`_)
+* Include topic name in QoS mismatch warning messages (`#1286 <https://github.com/ros2/rclcpp/issues/1286>`_)
+* Add coverage tests context functions (`#1321 <https://github.com/ros2/rclcpp/issues/1321>`_)
+* Increase coverage of node_interfaces, including with mocking rcl errors (`#1322 <https://github.com/ros2/rclcpp/issues/1322>`_)
+* Contributors: Ada-King, Alejandro Hernández Cordero, Audrow Nash, Barry Xu, Chen Lihui, Chris Lalancette, Christophe Bedard, Ivan Santiago Paunovic, Jorge Perez, Morgan Quigley, brawner
+
+5.0.0 (2020-09-18)
+------------------
+* Make node_graph::count_graph_users() const (`#1320 <https://github.com/ros2/rclcpp/issues/1320>`_)
+* Add coverage for wait_set_policies (`#1316 <https://github.com/ros2/rclcpp/issues/1316>`_)
+* Only exchange intra_process waitable if nonnull (`#1317 <https://github.com/ros2/rclcpp/issues/1317>`_)
+* Check waitable for nullptr during constructor (`#1315 <https://github.com/ros2/rclcpp/issues/1315>`_)
+* Call vector.erase with end iterator overload (`#1314 <https://github.com/ros2/rclcpp/issues/1314>`_)
+* Use best effort, keep last, history depth 1 QoS Profile for '/clock' subscriptions (`#1312 <https://github.com/ros2/rclcpp/issues/1312>`_)
+* Add tests type_support module (`#1308 <https://github.com/ros2/rclcpp/issues/1308>`_)
+* Replace std_msgs with test_msgs in executors test (`#1310 <https://github.com/ros2/rclcpp/issues/1310>`_)
+* Add set_level for rclcpp::Logger (`#1284 <https://github.com/ros2/rclcpp/issues/1284>`_)
+* Remove unused private function (rclcpp::Node and rclcpp_lifecycle::Node) (`#1294 <https://github.com/ros2/rclcpp/issues/1294>`_)
+* Adding tests basic getters (`#1291 <https://github.com/ros2/rclcpp/issues/1291>`_)
+* Adding callback groups in executor (`#1218 <https://github.com/ros2/rclcpp/issues/1218>`_)
+* Refactor Subscription Topic Statistics Tests (`#1281 <https://github.com/ros2/rclcpp/issues/1281>`_)
+* Add operator!= for duration (`#1236 <https://github.com/ros2/rclcpp/issues/1236>`_)
+* Fix clock thread issue (`#1266 <https://github.com/ros2/rclcpp/issues/1266>`_) (`#1267 <https://github.com/ros2/rclcpp/issues/1267>`_)
+* Fix topic stats test, wait for more messages, only check the ones with samples (`#1274 <https://github.com/ros2/rclcpp/issues/1274>`_)
+* Add get_domain_id method to rclcpp::Context (`#1271 <https://github.com/ros2/rclcpp/issues/1271>`_)
+* Fixes for unit tests that fail under cyclonedds (`#1270 <https://github.com/ros2/rclcpp/issues/1270>`_)
+* initialize_logging\_ should be copied (`#1272 <https://github.com/ros2/rclcpp/issues/1272>`_)
+* Use static_cast instead of C-style cast for instrumentation (`#1263 <https://github.com/ros2/rclcpp/issues/1263>`_)
+* Make parameter clients use template constructors (`#1249 <https://github.com/ros2/rclcpp/issues/1249>`_)
+* Ability to configure domain_id via InitOptions. (`#1165 <https://github.com/ros2/rclcpp/issues/1165>`_)
+* Simplify and fix allocator memory strategy unit test for connext (`#1252 <https://github.com/ros2/rclcpp/issues/1252>`_)
+* Use global namespace for parameter events subscription topic (`#1257 <https://github.com/ros2/rclcpp/issues/1257>`_)
+* Increase timeouts for connext for long tests (`#1253 <https://github.com/ros2/rclcpp/issues/1253>`_)
+* Adjust test_static_executor_entities_collector for rmw_connext_cpp (`#1251 <https://github.com/ros2/rclcpp/issues/1251>`_)
+* Fix failing test with Connext since it doesn't wait for discovery (`#1246 <https://github.com/ros2/rclcpp/issues/1246>`_)
+* Fix node graph test with Connext and CycloneDDS returning actual data (`#1245 <https://github.com/ros2/rclcpp/issues/1245>`_)
+* Warn about unused result of add_on_set_parameters_callback (`#1238 <https://github.com/ros2/rclcpp/issues/1238>`_)
+* Unittests for memory strategy files, except allocator_memory_strategy (`#1189 <https://github.com/ros2/rclcpp/issues/1189>`_)
+* EXPECT_THROW_EQ and ASSERT_THROW_EQ macros for unittests (`#1232 <https://github.com/ros2/rclcpp/issues/1232>`_)
+* Add unit test for static_executor_entities_collector (`#1221 <https://github.com/ros2/rclcpp/issues/1221>`_)
+* Parameterize test executors for all executor types (`#1222 <https://github.com/ros2/rclcpp/issues/1222>`_)
+* Unit tests for allocator_memory_strategy.cpp part 2 (`#1198 <https://github.com/ros2/rclcpp/issues/1198>`_)
+* Unit tests for allocator_memory_strategy.hpp (`#1197 <https://github.com/ros2/rclcpp/issues/1197>`_)
+* Derive and throw exception in spin_some spin_all for StaticSingleThreadedExecutor (`#1220 <https://github.com/ros2/rclcpp/issues/1220>`_)
+* Make ring buffer thread-safe (`#1213 <https://github.com/ros2/rclcpp/issues/1213>`_)
+* Add missing RCLCPP_PUBLIC to ~StaticExecutorEntitiesCollector (`#1227 <https://github.com/ros2/rclcpp/issues/1227>`_)
+* Document graph functions don't apply remap rules (`#1225 <https://github.com/ros2/rclcpp/issues/1225>`_)
+* Remove recreation of entities_collector (`#1217 <https://github.com/ros2/rclcpp/issues/1217>`_)
+* Contributors: Audrow Nash, Chen Lihui, Christophe Bedard, Daisuke Sato, Devin Bonnie, Dirk Thomas, Ivan Santiago Paunovic, Jacob Perron, Jannik Abbenseth, Jorge Perez, Pedro Pena, Shane Loretz, Stephen Brawner, Tomoya Fujita
+
+4.0.0 (2020-07-09)
+------------------
+* Fix rclcpp::NodeOptions::operator= (`#1211 <https://github.com/ros2/rclcpp/issues/1211>`_)
+* Link against thread library where necessary (`#1210 <https://github.com/ros2/rclcpp/issues/1210>`_)
+* Unit tests for node interfaces (`#1202 <https://github.com/ros2/rclcpp/issues/1202>`_)
+* Remove usage of domain id in node options (`#1205 <https://github.com/ros2/rclcpp/issues/1205>`_)
+* Remove deprecated set_on_parameters_set_callback function (`#1199 <https://github.com/ros2/rclcpp/issues/1199>`_)
+* Fix conversion of negative durations to messages (`#1188 <https://github.com/ros2/rclcpp/issues/1188>`_)
+* Fix implementation of NodeOptions::use_global_arguments() (`#1176 <https://github.com/ros2/rclcpp/issues/1176>`_)
+* Bump to QD to level 3 and fixed links (`#1158 <https://github.com/ros2/rclcpp/issues/1158>`_)
+* Fix pub/sub count API tests (`#1203 <https://github.com/ros2/rclcpp/issues/1203>`_)
+* Update tracetools' QL to 2 in rclcpp's QD (`#1187 <https://github.com/ros2/rclcpp/issues/1187>`_)
+* Fix exception message on rcl_clock_init (`#1182 <https://github.com/ros2/rclcpp/issues/1182>`_)
+* Throw exception if rcl_timer_init fails (`#1179 <https://github.com/ros2/rclcpp/issues/1179>`_)
+* Unit tests for some header-only functions/classes (`#1181 <https://github.com/ros2/rclcpp/issues/1181>`_)
+* Callback should be perfectly-forwarded (`#1183 <https://github.com/ros2/rclcpp/issues/1183>`_)
+* Add unit tests for logging functionality (`#1184 <https://github.com/ros2/rclcpp/issues/1184>`_)
+* Add create_publisher include to create_subscription (`#1180 <https://github.com/ros2/rclcpp/issues/1180>`_)
+* Contributors: Alejandro Hernández Cordero, Christophe Bedard, Claire Wang, Dirk Thomas, Ivan Santiago Paunovic, Johannes Meyer, Michel Hidalgo, Stephen Brawner, tomoya
+
+3.0.0 (2020-06-18)
+------------------
+* Check period duration in create_wall_timer (`#1178 <https://github.com/ros2/rclcpp/issues/1178>`_)
+* 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>`_)
+* 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)
+------------------
+* Added missing virtual destructors. (`#1149 <https://github.com/ros2/rclcpp/issues/1149>`_)
+* Fixed a test which was using different types on the same topic. (`#1150 <https://github.com/ros2/rclcpp/issues/1150>`_)
+* Made ``test_rate`` more reliable on Windows and improve error output when it fails (`#1146 <https://github.com/ros2/rclcpp/issues/1146>`_)
+* Added Security Vulnerability Policy pointing to REP-2006. (`#1130 <https://github.com/ros2/rclcpp/issues/1130>`_)
+* Added missing header in ``logging_mutex.cpp``. (`#1145 <https://github.com/ros2/rclcpp/issues/1145>`_)
+* Changed the WaitSet API to pass a shared pointer by value instead than by const reference when possible. (`#1141 <https://github.com/ros2/rclcpp/issues/1141>`_)
+* Changed ``SubscriptionBase::get_subscription_handle() const`` to return a shared pointer to const value. (`#1140 <https://github.com/ros2/rclcpp/issues/1140>`_)
+* Extended the lifetime of ``rcl_publisher_t`` by holding onto the shared pointer in order to avoid a use after free situation. (`#1119 <https://github.com/ros2/rclcpp/issues/1119>`_)
+* Improved some docblocks (`#1127 <https://github.com/ros2/rclcpp/issues/1127>`_)
+* Fixed a lock-order-inversion (potential deadlock) (`#1135 <https://github.com/ros2/rclcpp/issues/1135>`_)
+* Fixed a potential Construction/Destruction order problem between global contexts vector and Context of static lifetime (`#1132 <https://github.com/ros2/rclcpp/issues/1132>`_)
+* Contributors: Alejandro Hernández Cordero, Chris Lalancette, Ivan Santiago Paunovic, Michel Hidalgo, tomoya
+
+1.1.0 (2020-05-26)
+------------------
+* Deprecate set_on_parameters_set_callback (`#1123 <https://github.com/ros2/rclcpp/issues/1123>`_)
+* Expose get_service_names_and_types_by_node from rcl in rclcpp (`#1131 <https://github.com/ros2/rclcpp/issues/1131>`_)
+* Fix thread safety issues related to logging (`#1125 <https://github.com/ros2/rclcpp/issues/1125>`_)
+* Make sure rmw_publisher_options is initialized in to_rcl_publisher_options (`#1099 <https://github.com/ros2/rclcpp/issues/1099>`_)
+* Remove empty lines within method signatures (`#1128 <https://github.com/ros2/rclcpp/issues/1128>`_)
+* Add API review March 2020 document (`#1031 <https://github.com/ros2/rclcpp/issues/1031>`_)
+* Improve documentation (`#1106 <https://github.com/ros2/rclcpp/issues/1106>`_)
+* Make test multi threaded executor more reliable (`#1105 <https://github.com/ros2/rclcpp/issues/1105>`_)
+* Fixed rep links and added more details to dependencies in quality declaration (`#1116 <https://github.com/ros2/rclcpp/issues/1116>`_)
+* Update quality declarations to reflect version 1.0 (`#1115 <https://github.com/ros2/rclcpp/issues/1115>`_)
+* Contributors: Alejandro Hernández Cordero, ChenYing Kuo, Claire Wang, Dirk Thomas, Ivan Santiago Paunovic, William Woodall, Stephen Brawner
+
+1.0.0 (2020-05-12)
+------------------
+* Remove MANUAL_BY_NODE liveliness API (`#1107 <https://github.com/ros2/rclcpp/issues/1107>`_)
+* Use rosidl_default_generators dependency in test (`#1114 <https://github.com/ros2/rclcpp/issues/1114>`_)
+* Make sure to include what you use (`#1112 <https://github.com/ros2/rclcpp/issues/1112>`_)
+* Mark flaky test with xfail: TestMultiThreadedExecutor (`#1109 <https://github.com/ros2/rclcpp/issues/1109>`_)
+* Contributors: Chris Lalancette, Ivan Santiago Paunovic, Karsten Knese, Louise Poubel
+
+0.9.1 (2020-05-08)
+------------------
+* Fix tests that were not properly torn down (`#1073 <https://github.com/ros2/rclcpp/issues/1073>`_)
+* Added docblock in rclcpp (`#1103 <https://github.com/ros2/rclcpp/issues/1103>`_)
+* Added Quality declaration: rclcpp, rclpp_action, rclcpp_components andrclcpp_lifecycle (`#1100 <https://github.com/ros2/rclcpp/issues/1100>`_)
+* Use RCL_RET_SERVICE_TAKE_FAILED and not RCL_RET_CLIENT_TAKE_FAILED when checking a request take (`#1101 <https://github.com/ros2/rclcpp/issues/1101>`_)
+* Update comment about return value in Executor::get_next_ready_executable (`#1085 <https://github.com/ros2/rclcpp/issues/1085>`_)
+* Contributors: Alejandro Hernández Cordero, Christophe Bedard, Devin Bonnie, Ivan Santiago Paunovic
+
+0.9.0 (2020-04-29)
+------------------
+* Serialized message move constructor (`#1097 <https://github.com/ros2/rclcpp/issues/1097>`_)
+* Enforce a precedence for wildcard matching in parameter overrides. (`#1094 <https://github.com/ros2/rclcpp/issues/1094>`_)
+* Add serialized_message.hpp header (`#1095 <https://github.com/ros2/rclcpp/issues/1095>`_)
+* Add received message age metric to topic statistics (`#1080 <https://github.com/ros2/rclcpp/issues/1080>`_)
+* Deprecate redundant namespaces (`#1083 <https://github.com/ros2/rclcpp/issues/1083>`_)
+* Export targets in addition to include directories / libraries (`#1088 <https://github.com/ros2/rclcpp/issues/1088>`_)
+* Ensure logging is initialized just once (`#998 <https://github.com/ros2/rclcpp/issues/998>`_)
+* Adapt subscription traits to rclcpp::SerializedMessage (`#1092 <https://github.com/ros2/rclcpp/issues/1092>`_)
+* Protect subscriber_statistics_collectors\_ with a mutex (`#1084 <https://github.com/ros2/rclcpp/issues/1084>`_)
+* Remove unused test variable (`#1087 <https://github.com/ros2/rclcpp/issues/1087>`_)
+* Use serialized message (`#1081 <https://github.com/ros2/rclcpp/issues/1081>`_)
+* Integrate topic statistics (`#1072 <https://github.com/ros2/rclcpp/issues/1072>`_)
+* Fix rclcpp interface traits test (`#1086 <https://github.com/ros2/rclcpp/issues/1086>`_)
+* Generate node interfaces' getters and traits (`#1069 <https://github.com/ros2/rclcpp/issues/1069>`_)
+* Use composition for serialized message (`#1082 <https://github.com/ros2/rclcpp/issues/1082>`_)
+* Dnae adas/serialized message (`#1075 <https://github.com/ros2/rclcpp/issues/1075>`_)
+* Reflect changes in rclcpp API (`#1079 <https://github.com/ros2/rclcpp/issues/1079>`_)
+* Fix build regression (`#1078 <https://github.com/ros2/rclcpp/issues/1078>`_)
+* Add NodeDefault option for enabling topic statistics (`#1074 <https://github.com/ros2/rclcpp/issues/1074>`_)
+* Topic Statistics: Add SubscriptionTopicStatistics class (`#1050 <https://github.com/ros2/rclcpp/issues/1050>`_)
+* Add SubscriptionOptions for topic statistics (`#1057 <https://github.com/ros2/rclcpp/issues/1057>`_)
+* Remove warning message from failing to register default callback (`#1067 <https://github.com/ros2/rclcpp/issues/1067>`_)
+* Create a default warning for qos incompatibility (`#1051 <https://github.com/ros2/rclcpp/issues/1051>`_)
+* Add WaitSet class and modify entities to work without executor (`#1047 <https://github.com/ros2/rclcpp/issues/1047>`_)
+* Include what you use (`#1059 <https://github.com/ros2/rclcpp/issues/1059>`_)
+* Rename rosidl_generator_cpp namespace to rosidl_runtime_cpp (`#1060 <https://github.com/ros2/rclcpp/issues/1060>`_)
+* Changed rosidl_generator_c/cpp to rosidl_runtime_c/cpp (`#1014 <https://github.com/ros2/rclcpp/issues/1014>`_)
+* Use constexpr for endpoint type name (`#1055 <https://github.com/ros2/rclcpp/issues/1055>`_)
+* Add InvalidParameterTypeException (`#1027 <https://github.com/ros2/rclcpp/issues/1027>`_)
+* Support for ON_REQUESTED_INCOMPATIBLE_QOS and ON_OFFERED_INCOMPATIBLE_QOS events (`#924 <https://github.com/ros2/rclcpp/issues/924>`_)
+* Fixup clang warning (`#1040 <https://github.com/ros2/rclcpp/issues/1040>`_)
+* Adding a "static" single threaded executor (`#1034 <https://github.com/ros2/rclcpp/issues/1034>`_)
+* Add equality operators for QoS profile (`#1032 <https://github.com/ros2/rclcpp/issues/1032>`_)
+* Remove extra vertical whitespace (`#1030 <https://github.com/ros2/rclcpp/issues/1030>`_)
+* Switch IntraProcessMessage to test_msgs/Empty (`#1017 <https://github.com/ros2/rclcpp/issues/1017>`_)
+* Add new type of exception that may be thrown during creation of publisher/subscription (`#1026 <https://github.com/ros2/rclcpp/issues/1026>`_)
+* Don't check lifespan on publisher QoS (`#1002 <https://github.com/ros2/rclcpp/issues/1002>`_)
+* Fix get_parameter_tyeps of AsyncPrameterClient results are always empty (`#1019 <https://github.com/ros2/rclcpp/issues/1019>`_)
+* Cleanup node interfaces includes (`#1016 <https://github.com/ros2/rclcpp/issues/1016>`_)
+* Add ifdefs to remove tracing-related calls if tracing is disabled (`#1001 <https://github.com/ros2/rclcpp/issues/1001>`_)
+* Include missing header in node_graph.cpp (`#994 <https://github.com/ros2/rclcpp/issues/994>`_)
+* Add missing includes of logging.hpp (`#995 <https://github.com/ros2/rclcpp/issues/995>`_)
+* Zero initialize publisher GID in subscription intra process callback (`#1011 <https://github.com/ros2/rclcpp/issues/1011>`_)
+* Removed ament_cmake dependency (`#989 <https://github.com/ros2/rclcpp/issues/989>`_)
+* Switch to using new rcutils_strerror (`#993 <https://github.com/ros2/rclcpp/issues/993>`_)
+* Ensure all rclcpp::Clock accesses are thread-safe
+* Use a PIMPL for rclcpp::Clock implementation
+* Replace rmw_implementation for rmw dependency in package.xml (`#990 <https://github.com/ros2/rclcpp/issues/990>`_)
+* Add missing service callback registration tracepoint (`#986 <https://github.com/ros2/rclcpp/issues/986>`_)
+* Rename rmw_topic_endpoint_info_array count to size (`#996 <https://github.com/ros2/rclcpp/issues/996>`_)
+* Implement functions to get publisher and subcription informations like QoS policies from topic name (`#960 <https://github.com/ros2/rclcpp/issues/960>`_)
+* Code style only: wrap after open parenthesis if not in one line (`#977 <https://github.com/ros2/rclcpp/issues/977>`_)
+* Accept taking an rvalue ref future in spin_until_future_complete (`#971 <https://github.com/ros2/rclcpp/issues/971>`_)
+* Allow node clock use in logging macros (`#969 <https://github.com/ros2/rclcpp/issues/969>`_) (`#970 <https://github.com/ros2/rclcpp/issues/970>`_)
+* Change order of deprecated and visibility attributes (`#968 <https://github.com/ros2/rclcpp/issues/968>`_)
+* Deprecated is_initialized() (`#967 <https://github.com/ros2/rclcpp/issues/967>`_)
+* Don't specify calling convention in std::_Binder template (`#952 <https://github.com/ros2/rclcpp/issues/952>`_)
+* Added missing include to logging.hpp (`#964 <https://github.com/ros2/rclcpp/issues/964>`_)
+* Assigning make_shared result to variables in test (`#963 <https://github.com/ros2/rclcpp/issues/963>`_)
+* Fix unused parameter warning (`#962 <https://github.com/ros2/rclcpp/issues/962>`_)
+* Stop retaining ownership of the rcl context in GraphListener (`#946 <https://github.com/ros2/rclcpp/issues/946>`_)
+* Clear sub contexts when starting another init-shutdown cycle (`#947 <https://github.com/ros2/rclcpp/issues/947>`_)
+* Avoid possible UB in Clock jump callbacks (`#954 <https://github.com/ros2/rclcpp/issues/954>`_)
+* Handle unknown global ROS arguments (`#951 <https://github.com/ros2/rclcpp/issues/951>`_)
+* Mark get_clock() as override to fix clang warnings (`#939 <https://github.com/ros2/rclcpp/issues/939>`_)
+* Create node clock calls const (try 2) (`#922 <https://github.com/ros2/rclcpp/issues/922>`_)
+* Fix asserts on shared_ptr::use_count; expects long, got uint32 (`#936 <https://github.com/ros2/rclcpp/issues/936>`_)
+* Use absolute topic name for parameter events (`#929 <https://github.com/ros2/rclcpp/issues/929>`_)
+* Add enable_rosout into NodeOptions. (`#900 <https://github.com/ros2/rclcpp/issues/900>`_)
+* Removing "virtual", adding "override" keywords (`#897 <https://github.com/ros2/rclcpp/issues/897>`_)
+* Use weak_ptr to store context in GraphListener (`#906 <https://github.com/ros2/rclcpp/issues/906>`_)
+* Complete published event message when declaring a parameter (`#928 <https://github.com/ros2/rclcpp/issues/928>`_)
+* Fix duration.cpp lint error (`#930 <https://github.com/ros2/rclcpp/issues/930>`_)
+* Intra-process subscriber should use RMW actual qos. (ros2`#913 <https://github.com/ros2/rclcpp/issues/913>`_) (`#914 <https://github.com/ros2/rclcpp/issues/914>`_)
+* Type conversions fixes (`#901 <https://github.com/ros2/rclcpp/issues/901>`_)
+* Add override keyword to functions
+* Remove unnecessary virtual keywords
+* Only check for new work once in spin_some (`#471 <https://github.com/ros2/rclcpp/issues/471>`_) (`#844 <https://github.com/ros2/rclcpp/issues/844>`_)
+* Add addition/subtraction assignment operators to Time (`#748 <https://github.com/ros2/rclcpp/issues/748>`_)
+* Contributors: Alberto Soragna, Alejandro Hernández Cordero, Barry Xu, Chris Lalancette, Christophe Bedard, Claire Wang, Dan Rose, DensoADAS, Devin Bonnie, Dino Hüllmann, Dirk Thomas, DongheeYe, Emerson Knapp, Ivan Santiago Paunovic, Jacob Perron, Jaison Titus, Karsten Knese, Matt Schickler, Miaofei Mei, Michel Hidalgo, Mikael Arguedas, Monika Idzik, Prajakta Gokhale, Roger Strain, Scott K Logan, Sean Kelly, Stephen Brawner, Steven Macenski, Steven! Ragnarök, Todd Malsbary, Tomoya Fujita, William Woodall, Zachary Michaels
+
 0.8.3 (2019-11-19)
 ------------------
 

rclcpp/CMakeLists.txt

@@ -2,6 +2,8 @@ cmake_minimum_required(VERSION 3.5)
 
 project(rclcpp)
 
+find_package(Threads REQUIRED)
+
 find_package(ament_cmake_ros REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(libstatistics_collector REQUIRED)
@@ -23,11 +25,13 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
 endif()
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
+  # About -Wno-sign-conversion: With Clang, -Wconversion implies -Wsign-conversion. There are a number of
+  # implicit sign conversions in rclcpp and gtest.cc, see https://ci.ros2.org/job/ci_osx/9265/.
+  # Hence disabling -Wsign-conversion for now until all those have eventually been fixed.
+  # (from https://github.com/ros2/rclcpp/pull/1188#issuecomment-650229140)
+  add_compile_options(-Wall -Wextra -Wconversion -Wno-sign-conversion -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual)
 endif()
 
-include_directories(include)
-
 set(${PROJECT_NAME}_SRCS
   src/rclcpp/any_executable.cpp
   src/rclcpp/callback_group.cpp
@@ -56,6 +60,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/init_options.cpp
   src/rclcpp/intra_process_manager.cpp
   src/rclcpp/logger.cpp
+  src/rclcpp/logging_mutex.cpp
   src/rclcpp/memory_strategies.cpp
   src/rclcpp/memory_strategy.cpp
   src/rclcpp/message_info.cpp
@@ -102,23 +107,72 @@ configure_file(
   COPYONLY
 )
 # generate header with logging macros
-set(python_code
+set(python_code_logging
   "import em"
   "em.invoke(['-o', 'include/rclcpp/logging.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/logging.hpp.em'])")
-string(REPLACE ";" "$<SEMICOLON>" python_code "${python_code}")
+string(REPLACE ";" "$<SEMICOLON>" python_code_logging "${python_code_logging}")
 add_custom_command(OUTPUT include/rclcpp/logging.hpp
   COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp"
-  COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_code}"
+  COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_code_logging}"
   DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/logging.hpp.em.watch"
   COMMENT "Expanding logging.hpp.em"
   VERBATIM
 )
 list(APPEND ${PROJECT_NAME}_SRCS
   include/rclcpp/logging.hpp)
-include_directories("${CMAKE_CURRENT_BINARY_DIR}/include")
+
+file(GLOB interface_files "include/rclcpp/node_interfaces/node_*_interface.hpp")
+foreach(interface_file ${interface_files})
+  get_filename_component(interface_name ${interface_file} NAME_WE)
+
+  # "watch" template for changes
+  configure_file(
+    "resource/interface_traits.hpp.em"
+    "${CMAKE_CURRENT_BINARY_DIR}/${interface_name}_traits.hpp.em.watch"
+    COPYONLY
+  )
+  set(python_${interface_name}_traits
+    "import em"
+    "em.invoke(['-D', 'interface_name = \\'${interface_name}\\'', '-o', 'include/rclcpp/node_interfaces/${interface_name}_traits.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/interface_traits.hpp.em'])")
+  string(REPLACE ";" "$<SEMICOLON>" python_${interface_name}_traits "${python_${interface_name}_traits}")
+  add_custom_command(OUTPUT include/rclcpp/node_interfaces/${interface_name}_traits.hpp
+    COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp/node_interfaces"
+    COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_${interface_name}_traits}"
+    DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/${interface_name}_traits.hpp.em.watch"
+    COMMENT "Expanding interface_traits.hpp.em into ${interface_name}_traits.hpp"
+    VERBATIM
+  )
+  list(APPEND ${PROJECT_NAME}_SRCS
+    include/rclcpp/node_interfaces/${interface_name}_traits.hpp)
+
+  # "watch" template for changes
+  configure_file(
+    "resource/get_interface.hpp.em"
+    "get_${interface_name}.hpp.em.watch"
+    COPYONLY
+  )
+  set(python_get_${interface_name}
+    "import em"
+    "em.invoke(['-D', 'interface_name = \\'${interface_name}\\'', '-o', 'include/rclcpp/node_interfaces/get_${interface_name}.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/get_interface.hpp.em'])")
+  string(REPLACE ";" "$<SEMICOLON>" python_get_${interface_name} "${python_get_${interface_name}}")
+  add_custom_command(OUTPUT include/rclcpp/node_interfaces/get_${interface_name}.hpp
+    COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp/node_interfaces"
+    COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_get_${interface_name}}"
+    DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/get_${interface_name}.hpp.em.watch"
+    COMMENT "Expanding get_interface.hpp.em into get_${interface_file}.hpp"
+    VERBATIM
+  )
+  list(APPEND ${PROJECT_NAME}_SRCS
+    include/rclcpp/node_interfaces/get_${interface_name}.hpp)
+endforeach()
 
 add_library(${PROJECT_NAME}
   ${${PROJECT_NAME}_SRCS})
+target_include_directories(${PROJECT_NAME} PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>"
+  "$<INSTALL_INTERFACE:include>")
+target_link_libraries(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
 # specific order: dependents before dependencies
 ament_target_dependencies(${PROJECT_NAME}
   "libstatistics_collector"
@@ -140,7 +194,7 @@ target_compile_definitions(${PROJECT_NAME}
   PRIVATE "RCLCPP_BUILDING_LIBRARY")
 
 install(
-  TARGETS ${PROJECT_NAME}
+  TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -149,6 +203,7 @@ install(
 # specific order: dependents before dependencies
 ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})
+ament_export_targets(${PROJECT_NAME})
 
 ament_export_dependencies(libstatistics_collector)
 ament_export_dependencies(rcl)
@@ -164,420 +219,10 @@ ament_export_dependencies(statistics_msgs)
 ament_export_dependencies(tracetools)
 
 if(BUILD_TESTING)
-  find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 
-  find_package(rmw_implementation_cmake REQUIRED)
-
-  find_package(test_msgs REQUIRED)
-
-  include(cmake/rclcpp_add_build_failure_test.cmake)
-
-  add_definitions(-DTEST_RESOURCES_DIRECTORY="${CMAKE_CURRENT_BINARY_DIR}/test/resources")
-
-  ament_add_gtest(test_client test/test_client.cpp)
-  if(TARGET test_client)
-    ament_target_dependencies(test_client
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_client ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_create_timer test/test_create_timer.cpp)
-  if(TARGET test_create_timer)
-    ament_target_dependencies(test_create_timer
-      "rcl_interfaces"
-      "rmw"
-      "rcl"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_create_timer ${PROJECT_NAME})
-    target_include_directories(test_create_timer PRIVATE test/)
-  endif()
-  ament_add_gtest(test_expand_topic_or_service_name test/test_expand_topic_or_service_name.cpp)
-  if(TARGET test_expand_topic_or_service_name)
-    ament_target_dependencies(test_expand_topic_or_service_name
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_function_traits test/test_function_traits.cpp)
-  if(TARGET test_function_traits)
-    ament_target_dependencies(test_function_traits
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-  endif()
-  ament_add_gmock(test_intra_process_manager test/test_intra_process_manager.cpp)
-  if(TARGET test_intra_process_manager)
-    ament_target_dependencies(test_intra_process_manager
-      "rcl"
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_intra_process_manager ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_ring_buffer_implementation test/test_ring_buffer_implementation.cpp)
-  if(TARGET test_ring_buffer_implementation)
-    ament_target_dependencies(test_ring_buffer_implementation
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_ring_buffer_implementation ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_intra_process_buffer test/test_intra_process_buffer.cpp)
-  if(TARGET test_intra_process_buffer)
-    ament_target_dependencies(test_intra_process_buffer
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_intra_process_buffer ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_loaned_message test/test_loaned_message.cpp)
-  ament_target_dependencies(test_loaned_message
-    "test_msgs"
-  )
-  target_link_libraries(test_loaned_message ${PROJECT_NAME})
-
-  ament_add_gtest(test_node test/test_node.cpp TIMEOUT 240)
-  if(TARGET test_node)
-    ament_target_dependencies(test_node
-      "rcl_interfaces"
-      "rcpputils"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-      "test_msgs"
-    )
-    target_link_libraries(test_node ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_node_interfaces__get_node_interfaces
-    test/node_interfaces/test_get_node_interfaces.cpp)
-  if(TARGET test_node_interfaces__get_node_interfaces)
-    target_link_libraries(test_node_interfaces__get_node_interfaces ${PROJECT_NAME})
-  endif()
-
-  # TODO(wjwwood): reenable these build failure tests when I can get Jenkins to ignore their output
-  # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_rclcpp_node
-  #   test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp)
-  # target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node
-  #   ${PROJECT_NAME})
-
-  # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
-  #   test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp)
-  # target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
-  #   ${PROJECT_NAME})
-
-  # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_wrapped_node
-  #   test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp)
-  # target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node
-  #   ${PROJECT_NAME})
-
-  # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_wrapped_node
-  #   test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp)
-  # target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
-  #   ${PROJECT_NAME})
-
-  ament_add_gtest(test_node_global_args test/test_node_global_args.cpp)
-  if(TARGET test_node_global_args)
-    ament_target_dependencies(test_node_global_args
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_node_global_args ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_node_options test/test_node_options.cpp)
-  if(TARGET test_node_options)
-    ament_target_dependencies(test_node_options "rcl")
-    target_link_libraries(test_node_options ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_parameter_client test/test_parameter_client.cpp)
-  if(TARGET test_parameter_client)
-    ament_target_dependencies(test_parameter_client
-      "rcl_interfaces"
-    )
-    target_link_libraries(test_parameter_client ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_parameter_events_filter test/test_parameter_events_filter.cpp)
-  if(TARGET test_parameter_events_filter)
-    ament_target_dependencies(test_parameter_events_filter
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_parameter_events_filter ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_parameter test/test_parameter.cpp)
-  if(TARGET test_parameter)
-    ament_target_dependencies(test_parameter
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_parameter ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_parameter_map test/test_parameter_map.cpp)
-  if(TARGET test_parameter_map)
-    target_link_libraries(test_parameter_map ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_publisher test/test_publisher.cpp)
-  if(TARGET test_publisher)
-    ament_target_dependencies(test_publisher
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-      "test_msgs"
-    )
-    target_link_libraries(test_publisher ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_publisher_subscription_count_api test/test_publisher_subscription_count_api.cpp)
-  if(TARGET test_publisher_subscription_count_api)
-    ament_target_dependencies(test_publisher_subscription_count_api
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-      "test_msgs"
-    )
-    target_link_libraries(test_publisher_subscription_count_api ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_qos test/test_qos.cpp)
-  if(TARGET test_qos)
-    ament_target_dependencies(test_qos
-      "rmw"
-    )
-    target_link_libraries(test_qos
-      ${PROJECT_NAME}
-    )
-  endif()
-  ament_add_gtest(test_qos_event test/test_qos_event.cpp)
-  if(TARGET test_qos_event)
-    ament_target_dependencies(test_qos_event
-      "rmw"
-      "test_msgs"
-    )
-    target_link_libraries(test_qos_event
-      ${PROJECT_NAME}
-    )
-  endif()
-  ament_add_gtest(test_rate test/test_rate.cpp)
-  if(TARGET test_rate)
-    ament_target_dependencies(test_rate
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_rate
-      ${PROJECT_NAME}
-    )
-  endif()
-  ament_add_gtest(test_serialized_message_allocator test/test_serialized_message_allocator.cpp)
-  if(TARGET test_serialized_message_allocator)
-    ament_target_dependencies(test_serialized_message_allocator
-      test_msgs
-    )
-    target_link_libraries(test_serialized_message_allocator
-      ${PROJECT_NAME}
-    )
-  endif()
-  ament_add_gtest(test_serialized_message test/test_serialized_message.cpp)
-  if(TARGET test_serialized_message)
-    ament_target_dependencies(test_serialized_message
-      test_msgs
-    )
-    target_link_libraries(test_serialized_message
-      ${PROJECT_NAME}
-    )
-  endif()
-  ament_add_gtest(test_service test/test_service.cpp)
-  if(TARGET test_service)
-    ament_target_dependencies(test_service
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_service ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_subscription test/test_subscription.cpp)
-  if(TARGET test_subscription)
-    ament_target_dependencies(test_subscription
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-      "test_msgs"
-    )
-    target_link_libraries(test_subscription ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_subscription_publisher_count_api test/test_subscription_publisher_count_api.cpp)
-  if(TARGET test_subscription_publisher_count_api)
-    ament_target_dependencies(test_subscription_publisher_count_api
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-      "test_msgs"
-    )
-    target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_subscription_traits test/test_subscription_traits.cpp)
-  if(TARGET test_subscription_traits)
-    ament_target_dependencies(test_subscription_traits
-      "rcl"
-      "test_msgs"
-    )
-    target_link_libraries(test_subscription_traits ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_find_weak_nodes test/test_find_weak_nodes.cpp)
-  if(TARGET test_find_weak_nodes)
-    ament_target_dependencies(test_find_weak_nodes
-      "rcl"
-    )
-    target_link_libraries(test_find_weak_nodes ${PROJECT_NAME})
-  endif()
-
-  set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}")
-  if(WIN32)
-    set(append_library_dirs "${append_library_dirs}/$<CONFIG>")
-  endif()
-
-  ament_add_gtest(test_externally_defined_services test/test_externally_defined_services.cpp)
-  ament_target_dependencies(test_externally_defined_services
-    "rcl"
-    "test_msgs"
-  )
-  target_link_libraries(test_externally_defined_services ${PROJECT_NAME})
-
-  ament_add_gtest(test_duration test/test_duration.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_duration)
-    ament_target_dependencies(test_duration
-      "rcl")
-    target_link_libraries(test_duration ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_executor test/test_executor.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_executor)
-    ament_target_dependencies(test_executor
-      "rcl")
-    target_link_libraries(test_executor ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_logger test/test_logger.cpp)
-  target_link_libraries(test_logger ${PROJECT_NAME})
-
-  ament_add_gmock(test_logging test/test_logging.cpp)
-  target_link_libraries(test_logging ${PROJECT_NAME})
-
-  ament_add_gtest(test_time test/test_time.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_time)
-    ament_target_dependencies(test_time
-      "rcl")
-    target_link_libraries(test_time ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_timer test/test_timer.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_timer)
-    ament_target_dependencies(test_timer
-      "rcl")
-    target_link_libraries(test_timer ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_time_source test/test_time_source.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_time_source)
-    ament_target_dependencies(test_time_source
-      "rcl")
-    target_link_libraries(test_time_source ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_utilities test/test_utilities.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_utilities)
-    ament_target_dependencies(test_utilities
-      "rcl")
-    target_link_libraries(test_utilities ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_init test/test_init.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_init)
-    ament_target_dependencies(test_init
-      "rcl")
-    target_link_libraries(test_init ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_multi_threaded_executor test/executors/test_multi_threaded_executor.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_multi_threaded_executor)
-    ament_target_dependencies(test_multi_threaded_executor
-      "rcl")
-    target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_guard_condition test/test_guard_condition.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_guard_condition)
-    target_link_libraries(test_guard_condition ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_wait_set test/test_wait_set.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_wait_set)
-    ament_target_dependencies(test_wait_set "test_msgs")
-    target_link_libraries(test_wait_set ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_subscription_topic_statistics test/topic_statistics/test_subscription_topic_statistics.cpp)
-  if(TARGET test_subscription_topic_statistics)
-    ament_target_dependencies(test_subscription_topic_statistics
-      "libstatistics_collector"
-      "rcl_interfaces"
-      "rcutils"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-      "statistics_msgs"
-      "test_msgs")
-    target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_subscription_options test/test_subscription_options.cpp)
-  if(TARGET test_subscription_options)
-    ament_target_dependencies(test_subscription_options "rcl")
-    target_link_libraries(test_subscription_options ${PROJECT_NAME})
-  endif()
-
-  # Install test resources
-  install(
-    DIRECTORY test/resources
-    DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test)
+  add_subdirectory(test)
 endif()
 
 ament_package()

rclcpp/QUALITY_DECLARATION.md

@@ -0,0 +1,220 @@
+This document is a declaration of software quality for the `rclcpp` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html).
+
+# rclcpp Quality Declaration
+
+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://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://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning).
+
+### Version Stability [1.ii]
+
+`rclcpp` is at a stable version, i.e. `>= 1.0.0`.
+The current version can be found in its [package.xml](package.xml), and its change history can be found in its [CHANGELOG](CHANGELOG.rst).
+
+### Public API Declaration [1.iii]
+
+All symbols in the installed headers are considered part of the public API.
+
+Except for the exclusions listed below, all installed headers are in the `include` directory of the package, headers in any other folders are not installed and considered private.
+Headers under the folder `experimental` are not considered part of the public API as they have not yet been stabilized. These symbols are namespaced `rclcpp::experimental`.
+Headers under the folder `detail` are not considered part of the public API and are subject to change without notice. These symbols are namespaced `rclcpp::detail`.
+
+### API Stability Policy [1.iv]
+
+`rclcpp` will not break public API within a released ROS distribution, i.e. no major releases once the ROS distribution is released.
+
+### ABI Stability Policy [1.v]
+
+`rclcpp` contains C++ code and therefore must be concerned with ABI stability, and will maintain ABI stability within a ROS distribution.
+
+### ABI and ABI Stability Within a Released ROS Distribution [1.vi]
+
+`rclcpp` will not break API nor ABI within a released ROS distribution, i.e. no major releases once the ROS distribution is released.
+
+## Change Control Process [2]
+
+`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://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information.
+
+### Contributor Origin [2.ii]
+
+This package uses DCO as its confirmation of contributor origin policy. More information can be found in [CONTRIBUTING](../CONTRIBUTING.md).
+
+### Peer Review Policy [2.iii]
+
+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)
+
+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]
+
+All pull requests must resolve related documentation changes before merging.
+
+## Documentation [3]
+
+### Feature Documentation [3.i]
+
+`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/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/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/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/master/test) directory.
+New features are required to have tests before being added.
+
+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]
+
+Each part of the public API has tests, and new additions or changes to the public API require tests before being added.
+The tests aim to cover both typical usage and corner cases, but are quantified by contributing to code coverage.
+
+### Coverage [4.iii]
+
+`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:
+
+- tracking and reporting line coverage statistics
+- achieving and maintaining a reasonable branch line coverage (90-100%)
+- no lines are manually skipped in coverage calculations
+
+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_coverage/lastSuccessfulBuild/cobertura/src_ros2_rclcpp_rclcpp_src_rclcpp/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://index.ros.org/doc/ros2/Contributing/ROS-2-On-boarding-Guide/#note-on-coverage-runs).
+
+`rclcpp` has a line coverage `>= 95%`, which is calculated over all directories within `rclcpp` with the exception of the `experimental` directory.
+
+### Performance [4.iv]
+
+It is not yet defined if this package requires performance testing and how addresses this topic.
+
+### 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://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:
+* [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]
+
+Below are evaluations of each of `rclcpp`'s run-time and build-time dependencies that have been determined to influence the quality.
+
+It has several "buildtool" dependencies, which do not affect the resulting quality of the package, because they do not contribute to the public library API.
+
+It also has several test dependencies, which do not affect the resulting quality of the package, because they are only used to build and run the test code.
+
+### Direct and Optional Runtime ROS Dependencies [5.i]/[5.ii]
+
+`rclcpp` has the following runtime ROS dependencies:
+
+#### `libstatistics_collector`
+
+The `libstatistics_collector` package provides lightweight aggregation utilities to collect statistics and measure message metrics.
+
+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 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 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 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 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 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 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/master/tracetools/QUALITY_DECLARATION.md).
+
+### Direct Runtime non-ROS Dependency [5.iii]
+
+`rclcpp` has no run-time or build-time dependencies that need to be considered for this declaration.
+
+## Platform Support [6]
+
+`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.
+
+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
+
+### Vulnerability Disclosure Policy [7.i]
+
+This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://www.ros.org/reps/rep-2006.html).

rclcpp/README.md

@@ -0,0 +1,9 @@
+# `rclcpp`
+
+The ROS client library in C++.
+
+Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) for a complete list of its main components and features.
+
+## Quality Declaration
+
+This package claims to be in the **Quality Level 3** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details.

rclcpp/doc/api_review_march_2020.md

@@ -0,0 +1,437 @@
+# API Review for `rclcpp` from March 2020
+
+## Notes
+
+### Off-Topic Questions
+
+> [rclcpp_action] There exists a thread-safe and non-thread-safe way to get the goal result from an action client. We probably want to remove the public interface to the non-thread safe call (or consolidate somehow): https://github.com/ros2/rclcpp/issues/955
+
+`rclcpp_action` is out of scope atm.
+
+**Notes from 2020-03-19**: To be handled in separate API review.
+
+## Architecture
+
+### Calling Syntax and Keeping Node-like Class APIs in Sync
+
+> Currently, much of the API is exposed via the `rclcpp::Node` class, and due to the nature of the current architecture there is a lot of repeated code to expose these methods and then call the implementations which are in other classes like `rclcpp::node_interfaces::NodeTopics`, for example.
+>
+> Also, we have other versions of the class `rclcpp::Node` with different semantics and interfaces, like `rclcpp_lifecycle::LifecycleNode`, and we have been having trouble keeping the interface provided there up to date with how things are done in `rclcpp::Node`. Since `LifecycleNode` has a different API from `Node` in some important cases, it does not just inherit from `Node`.
+>
+> There are two main proposals (as I see it) to try and address this issue, either (a) break up the functionality in `Node` so that it is in separate classes and make `Node` multiple inherit from those classes, and then `LifecycleNode` could selectively inherit from those as well, or (b) change our calling convention from `node->do_thing(...)` to be `do_thing(node, ...)`.
+>
+> For (a) which commonly referred to as the [Policy Based Design Pattern](https://en.wikipedia.org/wiki/Modern_C%2B%2B_Design#Policy-based_design), we'd be reversing previous design decisions which we discussed at length where we decided to use composition over inheritance for various reasons.
+> One of the reasons was testing, with the theory that having simpler separate interfaces we could more easily mock them as needed for testing.
+> The testing goal would still be met, either by keeping the "node_interface" classes as-is or by mocking the classes that node would multiple inherit from, however it's harder to indicate that a function needs a class that multiple inherits from several classes but not others.
+> Also having interdependency between the classes which are inherited from is a bit complicated in this design pattern.
+>
+> For (b), we would be changing how we recommend all code be written (not a trivial thing to do at all), because example code like `auto pub = node->create_publsiher(...);` would be come `auto pub = create_publisher(node, ...);`.
+> This has some distinct advantages, however, in that it allows us to write these functions, like `create_publisher(node, ...)`, so that the node argument can be any class that meets the criteria of the function.
+> That not only means that when we add a feature it automatically works with `Node` and `LifecycleNode` without adding anything to them, it also means that user defined `Node`-like classes will also work, even if they do not inherit from or provide the complete interface for `rclcpp::Node`.
+> Another major downside of this approach is discoverability of the API when using auto-completion in text editors, as `node-><tab>` will often give you a list of methods to explore, but with the new calling convention, there's not way to get an auto complete for code who's first argument is a `Node`-like class.
+>
+> Both of the above approaches address some of the main concerns, which are: keeping `Node` and `LifecycleNode` in sync, reducing the size of the `Node` class so it is more easily maintained, documented, and so that related functions are grouped more clearly.
+
+- https://github.com/ros2/rclcpp/issues/898
+- https://github.com/ros2/rclcpp/issues/509
+- https://github.com/ros2/rclcpp/issues/855
+- https://github.com/ros2/rclcpp/issues/985
+  - subnode feature is in rclcpp::Node only, complicating "node using" API designs
+- http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2014/n4174.pdf
+- https://en.wikipedia.org/wiki/Uniform_Function_Call_Syntax#C++_proposal
+  - "Many programmers are tempted to write member functions to get the benefits of the member function syntax (e.g. "dot-autocomplete" to list member functions);[6] however, this leads to excessive coupling between classes.[7]"
+
+**Suggested Action**: Document the discussion and defer until Foxy.
+
+**Notes from 2020-03-19**:
+
+- Another version of (b) could be to have classes that are constructed with node, e.g. `Publisher(node, ...)` rather than `node->create_publisher(...)`
+- (tfoote) interface class? `NodeInterface<NodeLike>::something(node_like)`
+  - DRY?
+  - `NodeInterface<LifecycleNode>::<tab>` -> only life cycle node methods
+- (karsten) use interface classes directly, e.g. node->get_X_interface()->do_thing()
+- (dirk) use macros to add methods to class
+  - Question: Do we want tab-completable API (specifically list of member functions)?
+- Question: Is consistency in calling between core and non-core features more important than tab-completion?
+- Add better example of adding new feature and not needing to touch `rclcpp::Node`.
+- (dirk) methods and free functions not mutually exclusive.
+
+### Scoped Versus Non-Scoped Entities (e.g. Publishers/Subscriptions)
+
+> Currently, Publisher and Subscription (and similar entities) are scoped, meaning that when they are created they are added to the ROS graph as a side effect, and when they are let out of scope they remove themselves from the graph too.
+> Additionally, they necessarily have shared state with the system, for instance when you are spinning on a node, the executor shares ownership of the Subscriptions with the user.
+> Therefore, the Subscription only gets removed when both the user and executor are done with it.
+>
+> This shared ownership is accomplished with the use of shared pointers and weak pointers.
+>
+> There are a few concerns here, (a) use of shared pointers confuses users, (b) overhead of shared pointers and lack of an ability to use these classes on the stack rather than the heap, and (c) complexity of shutdown of an entity from the users perspective.
+>
+> For (a), some users are overwhelmed by the need to use a shared pointer.
+> In ROS 1 this was avoided by having a class which itself just thinly wraps a shared pointer (see: https://github.com/ros/ros_comm/blob/ac9f88c59a676ca6895e13445fc7d71f398ebe1f/clients/roscpp/include/ros/subscriber.h#L108-L111).
+> This could be achieved in ROS 2 either by doing the same with a wrapper class (at the expense of lots of repeated code), or by eliminating the need for using shared ownership.
+>
+> For (b), for some use cases, especially resource constrained / real-time / safety-critical environments, requiring these classes to be on the heap rather than the stack is at least inconvenient.
+> Additionally, there is a cost associated with using shared pointers, in the storage of shared state and in some implementation the use of locks or at least atomics for thread-safety.
+>
+> For (c), this is the most concerning drawback, because right now when a user lets their shared pointer to a, for example, Subscription go out of scope, a post condition is not that the Subscription is destroyed, nor that it has been removed from the graph.
+> In stead, the behavior is more like "at some point in the future the Subscription will be destroyed and removed from the graph, when the system is done with it".
+> This isn't a very satisfactory contract, as some users may wish to know when the Subscription has been deleted, but cannot easily know that.
+>
+> The benefit to the shared state is a safety net for users.
+> The alternative would be to document that a Subscription, again for example, cannot be deleted until the system is done with it.
+> We'd basically be pushing the responsibility onto the user to ensure the shared ownership is handled properly by the execution of their application, i.e. they create the Subscription, share a reference with the system (adding it by reference to an executor, for example), and they have to make sure the system is done with it before deleting the Subscription.
+>
+>Separately, from the above points, there is the related concern of forcing the user to keep a copy of their entities in scope, whether it be with a shared pointer or a class wrapping one.
+> There is the desire to create it and forget it in some cases.
+> The downside to this is that if/when the user wants to destroy the entity, they have no way of doing that as they have no handle or unique way to address the entity.
+>
+> One proposed solution would be to have a set of "named X" APIs, e.g. `create_named_subscription` rather than just `create_subscription`.
+> This would allow the user to address the Subscription in the future in order to obtain a new reference to it or delete it.
+
+- https://github.com/ros2/rclcpp/issues/506
+- https://github.com/ros2/rclcpp/issues/726
+
+**Suggested Action**: Consolidate to a single issue, and defer.
+
+**Notes from 2020-03-23**:
+
+- (chris) Putting ownership mechanics on user is hard.
+- (dirk) add documentation clearly outlining ownership
+- (shane) warn on unused to catch issues with immediately deleted items
+- (tfoote) debugging output for destruction so it easy to see when reviewing logs
+- (chris) possible to create API that checks for destruction
+  - (william) might lead to complex synchronization issues
+- (tfoote) could add helper classes to make scoped things non-scoped
+  - (shane) concerned that there is no longer "one good way" to do it
+
+### Allow QoS to be configured externally, like we allow remapping of topic names
+
+> Suggestion from @stonier: allow the qos setting on a topic to be changed externally at startup, similar to how we do topic remapping (e.g., do it on the command-line using appropriate syntax).
+>
+> To keep the syntax manageable, we might just allow profiles to be picked.
+
+- https://github.com/ros2/rclcpp/issues/239
+
+**Suggested Action**: Update issue, defer for now.
+
+**Notes from 2020-03-19**:
+
+- (wjwwood) it depends on the QoS setting, but many don't make sense, mostly because they can change some of the behaviors of underlying API
+- (dirk) Should developers expose a parameter instead?
+- (multiple) should be a feature that makes configuring them (after opt-in) consistent
+- (jacob) customers feedback was that this was expected, surprised it was not allowed
+- (karsten) could limit to profiles
+
+## Init/shutdown and Context
+
+### Consider renaming `rclcpp::ok()`
+
+> Old discussion to rename `rclcpp::ok()` to something more specific, like `rclcpp::is_not_shutdown()` or the corollary `rclcpp::is_shutdown()`.
+
+- https://github.com/ros2/rclcpp/issues/3
+
+**Suggested Action**: Defer.
+
+**Notes from 2020-03-19**:
+
+- (shane) preference to not have a negative in the function name
+
+## Executor
+
+### Exposing Scheduling of Tasks in Executor and a Better Default
+
+> Currently there is a hard coded procedure for handling ready tasks in the executor, first timers, then subscriptions, and so on.
+> This scheduling is not fair and results in non-deterministic behavior and starvation issues.
+>
+> We should provide a better default scheduling which is fairer and ideally deterministic, something like round-robin or FIFO.
+>
+> Additionally, we should make it easier to let the user override the scheduling logic in the executor.
+
+- https://github.com/ros2/rclcpp/pull/614
+- https://github.com/ros2/rclcpp/issues/633
+- https://github.com/ros2/rclcpp/issues/392
+
+**Suggested Action**: Follow up on proposals to implement FIFO scheduling and refactor the Executor design to more easily expose the scheduling logic.
+
+**Notes from 2020-03-19**:
+
+- No comments.
+
+### Make it possible to wait on entities (e.g. Subscriptions) without an Executor
+
+> Currently, it is only possible to use things like Timers and Subscriptions and Services with an executor.
+> It should be possible, however, to either poll these entities or wait on them and then decide which to process as a user.
+>
+> This is most easily accomplished with a WaitSet-like class.
+
+- https://github.com/ros2/rclcpp/issues/520
+
+**Suggested Action**: implement WaitSet class in rclcpp so that this is possible, and make "waitable" entities such that they can be polled, e.g. `Subscription`s should have a user facing `take()` method, which can fail if no data is available.
+
+**Notes from 2020-03-19**:
+
+- No comments.
+
+### Make it possible to use multiple executors per node
+
+> Currently, you cannot use more than one executor per node, this limits your options when it comes to distributing work within a node across threads.
+> You can use a multi-threaded executor, or make your own executor which does this, but it is often convenient to be able to spin part of the node separately from the the rest of the node.
+
+- https://github.com/ros2/rclcpp/issues/519
+
+**Suggested Action**: Make this possible, moving the exclusivity to be between an executor and callback groups rather than nodes.
+
+**Notes from 2020-03-19**:
+
+- No comments.
+
+### Implement a Lock-free Executor
+
+> This would presumably be useful for real-time and safety critical systems where locks and any kind of blocking code is considered undesirable.
+
+- https://github.com/ros2/rclcpp/issues/77
+
+**Suggested Action**: Keep in backlog until someone needs it specifically.
+
+**Notes from 2020-03-19**:
+
+- No comments.
+
+### Add implementation of `spin_some()` to the `MultiThreadedExecutor`
+
+> Currently `spin_some()` is only available in the `SingleThreadedExecutor`.
+
+- https://github.com/ros2/rclcpp/issues/85
+
+**Suggested Action**: Defer.
+
+**Notes from 2020-03-19**:
+
+- No comments.
+
+## Node
+
+### Do argument parsing outside of node constructor
+
+> Things that come from command line arguments should be separately passed into the node's constructor rather than passing in arguments and asking the node to do the parsing.
+
+- https://github.com/ros2/rclcpp/issues/492
+
+**Suggested Action**: Defer until after foxy.
+
+**Notes from 2020-03-23**:
+
+- (dirk) may be related to ROS 1 heritage of argc/argv being passed to node directly
+- (shane) impacts rcl API as well, two parts "global options" as well node specific options
+- (dirk) what is the recommendation to users that want to add arguments programmatically
+  - user should be able to get non-ros argc/argv somehow (seems like you can now)
+- (jacob) the argument in NodeOptions are used for application specific argument via component loading as well
+
+## Timer
+
+### Timer based on ROS Time
+
+> `node->create_wall_timer` does exactly what it says; creates a timer that will call the callback when the wall time expires.  But this is almost never what the user wants, since this won’t work properly in simulation.  Suggestion: deprecate `create_wall_timer`, add a new method called `create_timer` that takes the timer to use as one of the arguments, which defaults to ROS_TIME.
+
+- https://github.com/ros2/rclcpp/blob/96ebf59a6045a535730d98fff25e522807c7aa75/rclcpp/include/rclcpp/node.hpp#L219-L230
+- https://github.com/ros2/rclcpp/issues/465
+
+**Suggested Action**: Promote `rclcpp::create_timer()` which is templated on a clock type, as suggested, but leave `create_wall_timer` as a convenience.
+
+**Notes from 2020-03-19**:
+
+- (shane) may be a `rclcpp::create_timer()` that can be used to create a non-wall timer already
+
+## Publisher
+
+## Subscription
+
+### Callback Signature
+
+> Is there a reason the subscription callback must have a smart pointer argument instead of accepting a const-reference argument?
+
+- https://github.com/ros2/rclcpp/blob/96ebf59a6045a535730d98fff25e522807c7aa75/rclcpp/include/rclcpp/any_subscription_callback.hpp#L44-L52
+- https://github.com/ros2/rclcpp/issues/281
+
+**Suggested Action**: Provide const reference as an option, add documentation as to the implications of one callback signature versus others.
+
+**Notes from 2020-03-19**:
+
+- (dirk) have const reference and document it
+
+## Service Server
+
+### Allow for asynchronous Service Server callbacks
+
+> Currently, the only callback signature for Service Servers takes a request and must return a response.
+> This means that all of the activity of the service server has to happen within that function.
+> This can cause issues, specifically if you want to call another service within the current service server's callback, as it causes deadlock issues trying to synchronously call the second service within a spin callback.
+> More generally, it seems likely that long running service server callbacks may be necessary in the future and requiring them to be synchronous would tie up at least on thread in the spinning executor unnecessarily.
+
+- https://github.com/ros2/rclcpp/issues/491
+
+**Suggested Action**: Defer.
+
+**Notes from 2020-03-23**:
+
+- (dirk) likely new API, so possible to backport
+
+## Service Client
+
+### Callback has SharedFuture rather than const reference to response
+
+> Why does the Client::send_async_request take in a callback that has a SharedFuture argument instead of an argument that is simply a const-reference (or smart pointer) to the service response type? The current API seems to imply that the callback ought to check whether the promise is broken or fulfilled before trying to access it. Is that the case? If so, it should be documented in the header.
+
+- https://github.com/ros2/rclcpp/blob/7c1721a0b390be8242a6b824489d0bc861f6a0ad/rclcpp/include/rclcpp/client.hpp#L134
+
+**Suggested Action**: Update ticket and defer.
+
+**Notes from 2020-03-19**:
+
+- (wjwwood) we wanted the user to handle error cases with the future?
+- (dirk) future allows for single callback (rather than one for response and one for error)
+- (jacob) actions uses a "wrapped result" object
+
+### rclcpp missing synchronous `send_request` and issues with deadlocks
+
+> This has been reported by several users, but there is only an `async_send_request` currently. `rclpy` has a synchronous `send_request` but  it has issues with deadlock, specifically if you call it without spinning in another thread then it will deadlock. Or if you call it from within a spin callback when using a single threaded executor, it will deadlock.
+
+- https://discourse.ros.org/t/synchronous-request-to-service-in-callback-results-in-deadlock/12767
+- https://github.com/ros2/rclcpp/issues/975
+- https://github.com/ros2/demos/blob/948b4f4869298f39cfe99d3ae517ad60a72a8909/demo_nodes_cpp/src/services/add_two_ints_client.cpp#L24-L39
+
+**Suggested Action**: Update issue and defer. Also defer decision on reconciling rclpy's send_request.
+
+**Notes from 2020-03-23**:
+
+- (karsten/shane) async spinner helps in rclpy version, rclcpp could emulate
+- (chris) sees three options:
+  - only async (current case in rclcpp)
+  - have sync version, add lots of docs that spinning needs to happen elsewhere (current case for rclpy)
+  - reentrant spinning
+- (william) you either need async/await from language or ".then" syntax (we have this in async_send_request())
+- (chris) more error checking for recursive spinning
+- (chris) weird that rclcpp and rclpy have different API
+- (shane) thinks it is ok to have different API, but rclpy is not ideal
+
+## Parameters
+
+### Expected vs Unexpected parameters
+
+> Allow node author to define expected parameters and what happens when an unexpected parameter is set.
+
+- https://github.com/ros2/rclcpp/issues/475
+- https://github.com/ros2/rclcpp/tree/check_parameters
+
+**Suggested Action**: Defer as nice to have.
+
+**Notes from 2020-03-23**:
+
+- None.
+
+### Implicitly cast integer values for double parameters
+
+> If we try to pass an integer value to a double parameter from the command line or from a parameters YAML file we get a `rclcpp::ParameterTypeException`.
+> For example, passing a parameter from the command line:
+>
+>     ros2 run foo_package foo_node --ros-args -p foo_arg:=1
+>
+> results in the following error:
+>
+>     terminate called after throwing an instance of 'rclcpp::ParameterTypeException'
+>       what():  expected [double] got [integer]
+>
+> and we can fix it by explicitly making our value a floating point number:
+>
+>     ros2 run foo_package foo_node --ros-args -p foo_arg:=1.0
+>
+> But, it seems reasonable to me that if a user forgets to explicitly provide a floating point value that we should implicitly cast an integer to a float (as is done in many programming languages).
+
+- https://github.com/ros2/rclcpp/issues/979
+
+**Suggested Action**: Continue with issue.
+
+**Notes from 2020-03-23**:
+
+- (shane) says "yes please" :)
+
+### Use `std::variant` instead of custom `ParameterValue` class
+
+> This is only possible if C++17 is available, but it would simplify our code, make our interface more standard, and allow us to use constexpr-if to simply our templated code.
+
+**Suggested Action**: Create an issue for future work.
+
+**Notes from 2020-03-23**:
+
+- (chris) not sure churn is worth
+- (ivan) other places for std::variant, like AnySubscriptionCallback
+
+### Cannot set name or value on `Parameter`/`ParameterValue`
+
+> Both `Parameter` and `ParameterValue` are read-only after construction.
+
+- https://github.com/ros2/rclcpp/issues/238
+
+**Suggested Action**: Update issue, possibly close.
+
+**Notes from 2020-03-23**:
+
+- (chris/william) setting values on temporary (local) objects is not reflected in the node, so misleading
+
+## Parameter Clients
+
+### No timeout option with synchronous parameter client calls
+
+> As an example, SyncParametersClient::set_parameters doesn't take a timeout option. So, if anything goes wrong in the service call (e.g. the server goes down), we will get stuck waiting indefinitely.
+
+- https://github.com/ros2/rclcpp/issues/360
+- https://github.com/ros2/rclcpp/blob/96ebf59a6045a535730d98fff25e522807c7aa75/rclcpp/src/rclcpp/parameter_client.cpp#L453-L468
+
+**Suggested Action**: Update issue, decide if it can be taken for Foxy or not.
+
+**Notes from 2020-03-23**:
+
+- (tfoote) Seems like adding a timeout is a good idea.
+
+### Name of AsyncParametersClient inconsistent
+
+> AsyncParameter**s**Client uses plural, when filename is singular (and ParameterService is singular):
+
+- https://github.com/ros2/rclcpp/blob/7c1721a0b390be8242a6b824489d0bc861f6a0ad/rclcpp/include/rclcpp/parameter_client.hpp#L44
+
+**Suggested Action**: Reconcile class and file name, switch to singular name?
+
+**Notes from on-line, post 2020-03-23 meeting**:
+
+- (tfoote) +1 for homogenizing to singular
+
+### `SyncParametersClient::get_parameters` doesn't allow you to detect error cases
+
+> E.g. https://github.com/ros2/rclcpp/blob/249b7d80d8f677edcda05052f598de84f4c2181c/rclcpp/src/rclcpp/parameter_client.cpp#L246-L257 returns an empty vector if something goes wrong which is also a valid response.
+
+- https://github.com/ros2/rclcpp/issues/200
+- https://github.com/ros2/rclcpp/blob/96ebf59a6045a535730d98fff25e522807c7aa75/rclcpp/src/rclcpp/parameter_client.cpp#L412-L426
+
+**Suggested Action**: Throw an exception to indicate if something went wrong and document other expected conditions of the API.
+
+**Notes from on-line, post 2020-03-23 meeting**:
+
+- (tfoote) An empty list is not a valid response unless you passed in an empty list. The return should have the same length as the request in the same order. Any parameters that are not set should return a ParameterVariant with type PARAMETER_NOT_SET. to indicate that it was polled and determined to not be set. Suggested action improve documentation of the API to clarify a short or incomplete.
+- (jacobperron) I think throwing an exception is also a valid action, making it clear that an error occurred.
+- (wjwwood) Using exceptions to indicate an exceptional case (something went wrong) seems reasonable to me too.
+
+## Clock
+
+### Clock Jump callbacks on System or Steady time?
+
+> Currently time jump callbacks are registered via Clock::create_jump_handler(). Jump handlers are only invoked by TimeSource::set_clock(). This is only called if the clock type is RCL_ROS_TIME and ROS time is active.
+
+- https://github.com/ros2/rclcpp/issues/528
+
+**Suggested Action**: Document that time jumping is only detected with ROS time, consider a warning.
+
+**Notes from on-line, post 2020-03-23 meeting**:
+
+- (tfoote) There should be no jumps in steady time. If there's a big change in system time, it doesn't necessarily mean that time jumped, just that you might have been sleeping for a long time. Most ntp systems adjust the slew rate these days instead of jumping but still that's an external process and I don't know of any APIs to introspect the state of the clock. I'm not sure that we have a way to detect jumps in time for system or steady time. To that end I think that we should be clear that we only provide callbacks when simulation time starts or stops, or simulation time jumps. We should also strongly recommend that operators not actively adjust their system clocks while running ROS nodes.
+- (jacobperron) I agree with Tully, if we don't have a way to detect system time jumps then I think we should just document that this only works with ROS time. In addition to documentation, we could log an info or warning message if the user registers jump callback with steady or system time, but it may be unnecessarily noisy.
+

rclcpp/include/rclcpp/any_service_callback.hpp

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

rclcpp/include/rclcpp/any_subscription_callback.hpp

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

rclcpp/include/rclcpp/callback_group.hpp

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

rclcpp/include/rclcpp/client.hpp

@@ -84,22 +84,43 @@ public:
   bool
   take_type_erased_response(void * response_out, rmw_request_id_t & request_header_out);
 
+  /// Return the name of the service.
+  /** \return The name of the service. */
   RCLCPP_PUBLIC
   const char *
   get_service_name() const;
 
+  /// Return the rcl_client_t client handle in a std::shared_ptr.
+  /**
+   * This handle remains valid after the Client is destroyed.
+   * The actual rcl client is not finalized until it is out of scope everywhere.
+   */
   RCLCPP_PUBLIC
   std::shared_ptr<rcl_client_t>
   get_client_handle();
 
+  /// Return the rcl_client_t client handle in a std::shared_ptr.
+  /**
+   * This handle remains valid after the Client is destroyed.
+   * The actual rcl client is not finalized until it is out of scope everywhere.
+   */
   RCLCPP_PUBLIC
   std::shared_ptr<const rcl_client_t>
   get_client_handle() const;
 
+  /// Return if the service is ready.
+  /**
+   * \return `true` if the service is ready, `false` otherwise
+   */
   RCLCPP_PUBLIC
   bool
   service_is_ready() const;
 
+  /// Wait for a service to be ready.
+  /**
+   * \param timeout maximum time to wait
+   * \return `true` if the service is ready and the timeout is not over, `false` otherwise
+   */
   template<typename RepT = int64_t, typename RatioT = std::milli>
   bool
   wait_for_service(
@@ -174,6 +195,17 @@ public:
 
   RCLCPP_SMART_PTR_DEFINITIONS(Client)
 
+  /// Default constructor.
+  /**
+   * The constructor for a Client is almost never called directly.
+   * Instead, clients should be instantiated through the function
+   * rclcpp::create_client().
+   *
+   * \param[in] node_base NodeBaseInterface pointer that is used in part of the setup.
+   * \param[in] node_graph The node graph interface of the corresponding node.
+   * \param[in] service_name Name of the topic to publish to.
+   * \param[in] client_options options for the subscription.
+   */
   Client(
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
     rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
@@ -228,12 +260,20 @@ public:
     return this->take_type_erased_response(&response_out, request_header_out);
   }
 
+  /// Create a shared pointer with the response type
+  /**
+   * \return shared pointer with the response type
+   */
   std::shared_ptr<void>
   create_response() override
   {
     return std::shared_ptr<void>(new typename ServiceT::Response());
   }
 
+  /// Create a shared pointer with a rmw_request_id_t
+  /**
+   * \return shared pointer with a rmw_request_id_t
+   */
   std::shared_ptr<rmw_request_id_t>
   create_request_header() override
   {
@@ -242,6 +282,11 @@ public:
     return std::shared_ptr<rmw_request_id_t>(new rmw_request_id_t);
   }
 
+  /// Handle a server response
+  /**
+    * \param[in] request_header used to check if the secuence number is valid
+    * \param[in] response message with the server response
+   */
   void
   handle_response(
     std::shared_ptr<rmw_request_id_t> request_header,
@@ -316,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/clock.hpp

@@ -89,6 +89,7 @@ public:
   bool
   ros_time_is_active();
 
+  /// Return the rcl_clock_t clock handle
   RCLCPP_PUBLIC
   rcl_clock_t *
   get_clock_handle() noexcept;
@@ -97,6 +98,7 @@ public:
   rcl_clock_type_t
   get_clock_type() const noexcept;
 
+  /// Get the clock's mutex
   RCLCPP_PUBLIC
   std::mutex &
   get_clock_mutex() noexcept;
@@ -113,9 +115,9 @@ public:
    *
    * Function is only applicable if the clock_type is `RCL_ROS_TIME`
    *
-   * \param pre_callback. Must be non-throwing
-   * \param post_callback. Must be non-throwing.
-   * \param threshold. Callbacks will be triggered if the time jump is greater
+   * \param pre_callback Must be non-throwing
+   * \param post_callback Must be non-throwing.
+   * \param threshold Callbacks will be triggered if the time jump is greater
    * then the threshold.
    * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
    * \throws std::bad_alloc if the allocation of the JumpHandler fails.

rclcpp/include/rclcpp/context.hpp

@@ -44,6 +44,9 @@ public:
   : std::runtime_error("context is already initialized") {}
 };
 
+/// Forward declare WeakContextsWrapper
+class WeakContextsWrapper;
+
 /// Context which encapsulates shared state between nodes and other similar entities.
 /**
  * A context also represents the lifecycle between init and shutdown of rclcpp.
@@ -100,6 +103,9 @@ public:
    * \param[in] argv argument array which may contain arguments intended for ROS
    * \param[in] init_options initialization options for rclcpp and underlying layers
    * \throw ContextAlreadyInitialized if called if init is called more than once
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
+   * \throws std::runtime_error if the global logging configure mutex is NULL
+   * \throws exceptions::UnknownROSArgsError if there are unknown ROS arguments
    */
   RCLCPP_PUBLIC
   virtual
@@ -133,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.
@@ -260,6 +271,7 @@ public:
    * \param[in] wait_set Pointer to the rcl_wait_set_t that will be using the
    *   resulting guard condition.
    * \return Pointer to the guard condition.
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
    */
   RCLCPP_PUBLIC
   rcl_guard_condition_t *
@@ -279,6 +291,8 @@ public:
    *
    * \param[in] wait_set Pointer to the rcl_wait_set_t that was using the
    *   resulting guard condition.
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
+   * \throws std::runtime_error if a nonexistent wait set is trying to release sigint guard condition.
    */
   RCLCPP_PUBLIC
   void
@@ -338,6 +352,9 @@ private:
   rclcpp::InitOptions init_options_;
   std::string shutdown_reason_;
 
+  // Keep shared ownership of the global logging mutex.
+  std::shared_ptr<std::recursive_mutex> logging_mutex_;
+
   std::unordered_map<std::type_index, std::shared_ptr<void>> sub_contexts_;
   // This mutex is recursive so that the constructor of a sub context may
   // attempt to acquire another sub context.
@@ -355,6 +372,9 @@ private:
   std::mutex interrupt_guard_cond_handles_mutex_;
   /// Guard conditions for interrupting of associated wait sets on interrupt_all_wait_sets().
   std::unordered_map<rcl_wait_set_t *, rcl_guard_condition_t> interrupt_guard_cond_handles_;
+
+  /// Keep shared ownership of global vector of weak contexts
+  std::shared_ptr<WeakContextsWrapper> weak_contexts_;
 };
 
 /// Return a copy of the list of context shared pointers.

rclcpp/include/rclcpp/create_subscription.hpp

@@ -15,15 +15,27 @@
 #ifndef RCLCPP__CREATE_SUBSCRIPTION_HPP_
 #define RCLCPP__CREATE_SUBSCRIPTION_HPP_
 
+#include <chrono>
+#include <functional>
 #include <memory>
+#include <stdexcept>
 #include <string>
 #include <utility>
 
+#include "rclcpp/detail/resolve_enable_topic_statistics.hpp"
+
+#include "rclcpp/node_interfaces/get_node_timers_interface.hpp"
 #include "rclcpp/node_interfaces/get_node_topics_interface.hpp"
+#include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
+
+#include "rclcpp/create_publisher.hpp"
+#include "rclcpp/create_timer.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/subscription_factory.hpp"
 #include "rclcpp/subscription_options.hpp"
-#include "rclcpp/qos.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp"
 #include "rmw/qos_profiles.h"
 
 namespace rclcpp
@@ -34,6 +46,23 @@ namespace rclcpp
  * The NodeT type only needs to have a method called get_node_topics_interface()
  * which returns a shared_ptr to a NodeTopicsInterface, or be a
  * NodeTopicsInterface pointer itself.
+ *
+ * \tparam MessageT
+ * \tparam CallbackT
+ * \tparam AllocatorT
+ * \tparam CallbackMessageT
+ * \tparam SubscriptionT
+ * \tparam MessageMemoryStrategyT
+ * \tparam NodeT
+ * \param node
+ * \param topic_name
+ * \param qos
+ * \param callback
+ * \param options
+ * \param msg_mem_strat
+ * \return the created subscription
+ * \throws std::invalid_argument if topic statistics is enabled and the publish period is
+ * less than or equal to zero.
  */
 template<
   typename MessageT,
@@ -64,10 +93,59 @@ create_subscription(
   using rclcpp::node_interfaces::get_node_topics_interface;
   auto node_topics = get_node_topics_interface(std::forward<NodeT>(node));
 
+  std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>>
+  subscription_topic_stats = nullptr;
+
+  if (rclcpp::detail::resolve_enable_topic_statistics(
+      options,
+      *node_topics->get_node_base_interface()))
+  {
+    if (options.topic_stats_options.publish_period <= std::chrono::milliseconds(0)) {
+      throw std::invalid_argument(
+              "topic_stats_options.publish_period must be greater than 0, specified value of " +
+              std::to_string(options.topic_stats_options.publish_period.count()) +
+              " ms");
+    }
+
+    std::shared_ptr<Publisher<statistics_msgs::msg::MetricsMessage>> publisher =
+      create_publisher<statistics_msgs::msg::MetricsMessage>(
+      node,
+      options.topic_stats_options.publish_topic,
+      qos);
+
+    subscription_topic_stats = std::make_shared<
+      rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>
+      >(node_topics->get_node_base_interface()->get_name(), publisher);
+
+    std::weak_ptr<
+      rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>
+    > weak_subscription_topic_stats(subscription_topic_stats);
+    auto sub_call_back = [weak_subscription_topic_stats]() {
+        auto subscription_topic_stats = weak_subscription_topic_stats.lock();
+        if (subscription_topic_stats) {
+          subscription_topic_stats->publish_message();
+        }
+      };
+
+    auto node_timer_interface = node_topics->get_node_timers_interface();
+
+    auto timer = create_wall_timer(
+      std::chrono::duration_cast<std::chrono::nanoseconds>(
+        options.topic_stats_options.publish_period),
+      sub_call_back,
+      options.callback_group,
+      node_topics->get_node_base_interface(),
+      node_timer_interface
+    );
+
+    subscription_topic_stats->set_publisher_timer(timer);
+  }
+
   auto factory = rclcpp::create_subscription_factory<MessageT>(
     std::forward<CallbackT>(callback),
     options,
-    msg_mem_strat
+    msg_mem_strat,
+    subscription_topic_stats
   );
 
   auto sub = node_topics->create_subscription(topic_name, factory, qos);

rclcpp/include/rclcpp/create_timer.hpp

@@ -15,6 +15,8 @@
 #ifndef RCLCPP__CREATE_TIMER_HPP_
 #define RCLCPP__CREATE_TIMER_HPP_
 
+#include <chrono>
+#include <exception>
 #include <memory>
 #include <string>
 #include <utility>
@@ -32,8 +34,8 @@ namespace rclcpp
 template<typename CallbackT>
 typename rclcpp::TimerBase::SharedPtr
 create_timer(
-  node_interfaces::NodeBaseInterface * node_base,
-  node_interfaces::NodeTimersInterface * node_timers,
+  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+  std::shared_ptr<node_interfaces::NodeTimersInterface> node_timers,
   rclcpp::Clock::SharedPtr clock,
   rclcpp::Duration period,
   CallbackT && callback,
@@ -68,6 +70,74 @@ create_timer(
     group);
 }
 
+/// Convenience method to create a timer with node resources.
+/**
+ *
+ * \tparam DurationRepT
+ * \tparam DurationT
+ * \tparam CallbackT
+ * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max()
+ * \param callback callback to execute via the timer period
+ * \param group
+ * \param node_base
+ * \param node_timers
+ * \return
+ * \throws std::invalid argument if either node_base or node_timers
+ * are null, or period is negative or too large
+ */
+template<typename DurationRepT, typename DurationT, typename CallbackT>
+typename rclcpp::WallTimer<CallbackT>::SharedPtr
+create_wall_timer(
+  std::chrono::duration<DurationRepT, DurationT> period,
+  CallbackT callback,
+  rclcpp::CallbackGroup::SharedPtr group,
+  node_interfaces::NodeBaseInterface * node_base,
+  node_interfaces::NodeTimersInterface * node_timers)
+{
+  if (node_base == nullptr) {
+    throw std::invalid_argument{"input node_base cannot be null"};
+  }
+
+  if (node_timers == nullptr) {
+    throw std::invalid_argument{"input node_timers cannot be null"};
+  }
+
+  if (period < std::chrono::duration<DurationRepT, DurationT>::zero()) {
+    throw std::invalid_argument{"timer period cannot be negative"};
+  }
+
+  // Casting to a double representation might lose precision and allow the check below to succeed
+  // but the actual cast to nanoseconds fail. Using 1 DurationT worth of nanoseconds less than max.
+  constexpr auto maximum_safe_cast_ns =
+    std::chrono::nanoseconds::max() - std::chrono::duration<DurationRepT, DurationT>(1);
+
+  // If period is greater than nanoseconds::max(), the duration_cast to nanoseconds will overflow
+  // a signed integer, which is undefined behavior. Checking whether any std::chrono::duration is
+  // greater than nanoseconds::max() is a difficult general problem. This is a more conservative
+  // version of Howard Hinnant's (the <chrono> guy>) response here:
+  // https://stackoverflow.com/a/44637334/2089061
+  // However, this doesn't solve the issue for all possible duration types of period.
+  // Follow-up issue: https://github.com/ros2/rclcpp/issues/1177
+  constexpr auto ns_max_as_double =
+    std::chrono::duration_cast<std::chrono::duration<double, std::chrono::nanoseconds::period>>(
+    maximum_safe_cast_ns);
+  if (period > ns_max_as_double) {
+    throw std::invalid_argument{
+            "timer period must be less than std::chrono::nanoseconds::max()"};
+  }
+
+  const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(period);
+  if (period_ns < std::chrono::nanoseconds::zero()) {
+    throw std::runtime_error{
+            "Casting timer period to nanoseconds resulted in integer overflow."};
+  }
+
+  auto timer = rclcpp::WallTimer<CallbackT>::make_shared(
+    period_ns, std::move(callback), node_base->get_context());
+  node_timers->add_timer(timer, group);
+  return timer;
+}
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__CREATE_TIMER_HPP_

rclcpp/include/rclcpp/detail/resolve_enable_topic_statistics.hpp

@@ -42,7 +42,6 @@ resolve_enable_topic_statistics(const OptionsT & options, const NodeBaseT & node
       break;
     default:
       throw std::runtime_error("Unrecognized EnableTopicStatistics value");
-      break;
   }
 
   return topic_stats_enabled;

rclcpp/include/rclcpp/duration.hpp

@@ -26,11 +26,26 @@ namespace rclcpp
 class RCLCPP_PUBLIC Duration
 {
 public:
+  /// Duration constructor.
+  /**
+   * Initializes the time values for seconds and nanoseconds individually.
+   * Large values for nsecs are wrapped automatically with the remainder added to secs.
+   * Both inputs must be integers.
+   * Seconds can be negative.
+   *
+   * \param seconds time in seconds
+   * \param nanoseconds time in nanoseconds
+   */
   Duration(int32_t seconds, uint32_t nanoseconds);
 
-  // This constructor matches any numeric value - ints or floats
+  /// Construct duration from the specified nanoseconds.
+  [[deprecated(
+    "Use Duration::from_nanoseconds instead or std::chrono_literals. For example:"
+    "rclcpp::Duration::from_nanoseconds(int64_variable);"
+    "rclcpp::Duration(0ns);")]]
   explicit Duration(rcl_duration_value_t nanoseconds);
 
+  /// Construct duration from the specified std::chrono::nanoseconds.
   explicit Duration(std::chrono::nanoseconds nanoseconds);
 
   // This constructor matches any std::chrono value other than nanoseconds
@@ -44,6 +59,10 @@ public:
   // cppcheck-suppress noExplicitConstructor
   Duration(const builtin_interfaces::msg::Duration & duration_msg);  // NOLINT(runtime/explicit)
 
+  /// Time constructor
+  /**
+   * \param duration rcl_duration_t structure to copy.
+   */
   explicit Duration(const rcl_duration_t & duration);
 
   Duration(const Duration & rhs);
@@ -57,11 +76,14 @@ public:
   operator=(const Duration & rhs);
 
   Duration &
-  operator=(const builtin_interfaces::msg::Duration & Duration_msg);
+  operator=(const builtin_interfaces::msg::Duration & duration_msg);
 
   bool
   operator==(const rclcpp::Duration & rhs) const;
 
+  bool
+  operator!=(const rclcpp::Duration & rhs) const;
+
   bool
   operator<(const rclcpp::Duration & rhs) const;
 
@@ -80,6 +102,10 @@ public:
   Duration
   operator-(const rclcpp::Duration & rhs) const;
 
+  /// Get the maximum representable value.
+  /**
+   * \return the maximum representable value
+   */
   static
   Duration
   max();
@@ -87,19 +113,31 @@ public:
   Duration
   operator*(double scale) const;
 
+  /// Get duration in nanosecods
+  /**
+   * \return the duration in nanoseconds as a rcl_duration_value_t.
+   */
   rcl_duration_value_t
   nanoseconds() const;
 
-  /// \return the duration in seconds as a floating point number.
-  /// \warning Depending on sizeof(double) there could be significant precision loss.
-  /// When an exact time is required use nanoseconds() instead.
+  /// Get duration in seconds
+  /**
+   * \warning Depending on sizeof(double) there could be significant precision loss.
+   *   When an exact time is required use nanoseconds() instead.
+   * \return the duration in seconds as a floating point number.
+   */
   double
   seconds() const;
 
-  // Create a duration object from a floating point number representing seconds
+  /// Create a duration object from a floating point number representing seconds
   static Duration
   from_seconds(double seconds);
 
+  /// Create a duration object from an integer number representing nanoseconds
+  static Duration
+  from_nanoseconds(rcl_duration_value_t nanoseconds);
+
+  /// Convert Duration into a std::chrono::Duration.
   template<class DurationT>
   DurationT
   to_chrono() const
@@ -107,11 +145,14 @@ public:
     return std::chrono::duration_cast<DurationT>(std::chrono::nanoseconds(this->nanoseconds()));
   }
 
+  /// Convert Duration into rmw_time_t.
   rmw_time_t
   to_rmw_time() const;
 
 private:
   rcl_duration_t rcl_duration_;
+
+  Duration() = default;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/event.hpp

@@ -29,17 +29,33 @@ class Event
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Event)
 
+  /// Default construct
+  /**
+   * Set the default value to false
+   */
   RCLCPP_PUBLIC
   Event();
 
+  /// Set the Event state value to true
+  /**
+   * \return The state value before the call.
+   */
   RCLCPP_PUBLIC
   bool
   set();
 
+  /// Get the state value of the Event
+  /**
+   * \return the Event state value
+   */
   RCLCPP_PUBLIC
   bool
   check();
 
+  /// Get the state value of the Event and set to false
+  /**
+   * \return the Event state value
+   */
   RCLCPP_PUBLIC
   bool
   check_and_clear();

rclcpp/include/rclcpp/exceptions/exceptions.hpp

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

rclcpp/include/rclcpp/executor.hpp

@@ -21,6 +21,7 @@
 #include <cstdlib>
 #include <iostream>
 #include <list>
+#include <map>
 #include <memory>
 #include <mutex>
 #include <string>
@@ -29,241 +30,216 @@
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
 
-#include "rclcpp/callback_group.hpp"
+#include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/executor_options.hpp"
-#include "rclcpp/executor_policies/timer_favoring_priority_queue.hpp"
 #include "rclcpp/future_return_code.hpp"
-#include "rclcpp/guard_condition.hpp"
 #include "rclcpp/memory_strategies.hpp"
 #include "rclcpp/memory_strategy.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
-#include "rclcpp/wait_set.hpp"
+#include "rclcpp/scope_exit.hpp"
 
 namespace rclcpp
 {
 
-/// Base class for Executor providing the common interface for adding items, spinning, etc.
-class ExecutorBase
+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;
+
+/// Coordinate the order and timing of available communication tasks.
+/**
+ * Executor provides spin functions (including spin_node_once and spin_some).
+ * It coordinates the nodes and callback groups by looking for available work and completing it,
+ * based on the threading or concurrency scheme provided by the subclass implementation.
+ * An example of available work is executing a subscription callback, or a timer callback.
+ * The executor structure allows for a decoupling of the communication graph and the execution
+ * model.
+ * See SingleThreadedExecutor and MultiThreadedExecutor for examples of execution paradigms.
+ */
+class Executor
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ExecutorBase)
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Executor)
 
   /// Default constructor.
   /**
-   * \param[in] options Options for the executor.
+   * \param[in] options Options used to configure the executor.
    */
   RCLCPP_PUBLIC
-  explicit ExecutorBase(const ExecutorOptions & options = ExecutorOptions());
+  explicit Executor(const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
 
   /// Default destructor.
   RCLCPP_PUBLIC
-  virtual ~ExecutorBase();
+  virtual ~Executor();
 
-  /// Execution loop which waits for work, executes work, and repeats until canceled.
-  /**
-   * This will block, continuing to wait for work and then execute it, until
-   * canceled, either by the cancel() method or by the associated context being
-   * shutdown, either explicitly or due to a SIGINT (perhaps due to ctrl-c).
-   */
-  virtual
-  void
+  /// Do work periodically as it becomes available to us. Blocking call, may block indefinitely.
+  // It is up to the implementation of Executor to implement spin.
+  virtual void
   spin() = 0;
 
-  /// Add all of a node's callback groups to the executor.
+  /// Add a callback group to an executor.
   /**
-   * Add all of the callback groups of a node to this 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.
    *
-   * If any callback groups are associated with another executor, this method
-   * will throw a std::runtime_error.
+   * Adding a callback group with this method does not associate its node with this executor
+   * in any way
    *
-   * It will also trigger the interrupt guard condition which will cause the
-   * executor to wake up and consider the changes, then go back to waiting.
-   * Unless the notify parameter is passed false, in which case it will not
-   * interrupt the executor, and the changes may not be considered immediately.
-   *
-   * \param[in] node_ptr Shared pointer to the node which will have callback groups added.
-   * \param[in] notify If true, notfiy the executor of changes, otherwise do not.
-   * \throws std::runtime_error if any callback groups are associated with another executor.
-   */
-  template<class NodeT>
-  void
-  add_node(const std::shared_ptr<NodeT> & node_ptr, bool notify = true)
-  {
-    this->add_node(
-      node_ptr->get_node_base_interface(),
-      notify,
-      false  // raise on encountering already associated callback groups
-    );
-  }
-
-  /// Overload that takes the NodeBaseInterface directly.
-  template<class NodeT>
-  void
-  add_node(
-    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & node_ptr,
-    bool notify = true)
-  {
-    this->add_node(
-      node_ptr,
-      notify,
-      false  // raise on encountering already associated callback groups
-    );
-  }
-
-  /// Add all unassociated callback groups of the given node to this executor.
-  /**
-   * Same as add_node(), but instead of throwing if a callback group is already
-   * associated with another exector (already added to it) it will just ignore
-   * it rather than throwing.
-   *
-   * \param[in] node_ptr Shared pointer to the node which will have callback groups added.
-   * \param[in] notify If true, notfiy the executor of changes, otherwise do not.
-   */
-  template<class NodeT>
-  void
-  add_unassociated_callback_groups_from_node(
-    const std::shared_ptr<NodeT> & node_ptr,
-    bool notify = true)
-  {
-    this->add_node(
-      node_ptr->get_node_base_interface(),
-      notify,
-      true  // ignore already associated callback groups
-    );
-  }
-
-  /// Overload that takes the NodeBaseInterface directly.
-  template<class NodeT>
-  void
-  add_unassociated_callback_groups_from_node(
-    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & node_ptr,
-    bool notify = true)
-  {
-    this->add_node(
-      node_ptr,
-      notify,
-      true  // ignore already associated callback groups
-    );
-  }
-
-  /// Remove all of a node's callback groups from the executor.
-  /**
-   * Remove all of the callback groups of a node from this executor.
-   *
-   * It will also trigger the interrupt guard condition which will cause the
-   * executor to wake up and consider the changes, then go back to waiting.
-   * Unless the notify parameter is passed false, in which case it will not
-   * interrupt the executor, and the changes may not be considered immediately.
-   *
-   * \param[in] node Node which will have callback groups removed.
-   * \param[in] notify If true, notfiy the executor of changes, otherwise do not.
-   */
-  template<class NodeT>
-  void
-  remove_node(const NodeT & node, bool notify = true)
-  {
-    this->remove_node(*node.get_node_base_interface(), notify);
-  }
-
-  /// Overload that takes a shared pointer to the node.
-  /**
-   * This is kept for backwards compatibility from when executors shared
-   * ownership of Nodes.
-   */
-  template<class NodeT>
-  void
-  remove_node(const std::shared_ptr<NodeT> & node_ptr, bool notify = true)
-  {
-    this->remove_node(*node_ptr->get_node_base_interface(), notify);
-  }
-
-  /// Placeholder used to indicate that a method overload should not notify the executor.
-  struct DoNotNotify {};
-
-  /// Add a callback group to this executor.
-  /**
-   * If the given callback group is already associated with another executor,
-   * this method will throw a std::runtime_error.
-   *
-   * This overload of add_callback_group() will notify the executor so it will
-   * wake up if waiting and consider the changes.
-   *
-   * Weak ownership of the callback group is kept by the executor all of the
-   * time, but while waiting the weak ownership is periodically elevated to
-   * shared ownership.
-   * Therefore, if you let the callback group shared pointer go out of scope
-   * then it will stay in scope until this executor is done using it, at which
-   * point the callback group will be destructed and automatically removed from
-   * this executor in the next pass.
-   *
-   * \param[in] callback_group_ptr The callback group to be added.
-   * \throws std::runtime_error if the callback group is associated with another
-   *   executor already.
-   * \throws std::invalid_argument if the callback group pointer is nullptr.
+   * \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 callback_group_ptr) = 0;
+  virtual void
+  add_callback_group(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    bool notify = true);
 
-  /// Add a callback group to this executor without notifying the executor.
+  /// Get callback groups that belong to executor.
   /**
-   * The same as the other overload of add_callback_group(), except it does not
-   * notify the executor, so it will not wake up and these changes may not be
-   * considered immediately.
+   * 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.
    *
-   * Note, a bool with a default value would be preferable for controlling the
-   * notify behavior, and we're using it in the add/remove node above, but
-   * in order to keep this function virtual, and to avoid using default values
-   * in conjunction with virtual methods, we use an overload instead, in the
-   * spirit of std::nothrow_t, e.g.:
-   *   https://en.cppreference.com/w/cpp/memory/new/nothrow
+   * \return a vector of weak pointers that point to callback groups that are associated with
+   * the executor
    */
   RCLCPP_PUBLIC
-  virtual
-  void
-  add_callback_group(rclcpp::CallbackGroup::SharedPtr callback_group_ptr, DoNotNotify) = 0;
+  virtual std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_all_callback_groups();
 
-  /// Remove a callback group from this executor.
+  /// Get callback groups that belong to executor.
   /**
-   * If the given callback group is not associated with this executor, this
-   * method will throw a std::runtime_error.
+   * 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`.
    *
-   * This overload of add_callback_group() will notify the executor so it will
-   * wake up if waiting and consider the changes.
-   *
-   * \param[in] callback_group The callback group to be removed.
-   * \throws std::runtime_error if the callback group is not associated with
-   *   this executor.
-   * \throws std::invalid_argument if the callback group pointer is nullptr.
+   * \return a vector of weak pointers that point to callback groups that are associated with
+   * the executor
    */
   RCLCPP_PUBLIC
-  virtual
-  void
-  remove_callback_group(const rclcpp::CallbackGroup & callback_group) = 0;
+  virtual std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_manually_added_callback_groups();
 
-  /// Remove a callback group from this executor without notifying the executor.
+  /// Get callback groups that belong to executor.
   /**
-   * The same as the other overload of remove_callback_group(), except it does not
-   * notify the executor, so it will not wake up and these changes may not be
-   * considered immediately.
+   * 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.
    *
-   * See add_callback_group() for a note about the use of DoNotNotify.
+   * \return a vector of weak pointers that point to callback groups from a node associated with
+   * the executor
    */
   RCLCPP_PUBLIC
-  virtual
-  void
-  remove_callback_group(const rclcpp::CallbackGroup & callback_group, DoNotNotify) = 0;
+  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.
+  /**
+   * 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
+  add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true);
+
+  /// Convenience function which takes Node and forwards NodeBaseInterface.
+  /**
+   * \see rclcpp::Executor::add_node
+   */
+  RCLCPP_PUBLIC
+  virtual void
+  add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true);
+
+  /// 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
+  remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true);
+
+  /// Convenience function which takes Node and forwards NodeBaseInterface.
+  /**
+   * \see rclcpp::Executor::remove_node
+   */
+  RCLCPP_PUBLIC
+  virtual void
+  remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true);
 
   /// Add a node to executor, execute the next available unit of work, and remove the node.
   /**
    * \param[in] node Shared pointer to the node to add.
-   * \param[in] timeout How long to wait for work to become available.
-   *   Negative values cause spin_node_once to block indefinitely (the default
-   *   behavior).
-   *   A timeout of 0 causes this function to be non-blocking.
+   * \param[in] timeout How long to wait for work to become available. Negative values cause
+   * spin_node_once to block indefinitely (the default behavior). A timeout of 0 causes this
+   * function to be non-blocking.
    */
   template<typename RepT = int64_t, typename T = std::milli>
   void
@@ -278,7 +254,7 @@ public:
   }
 
   /// Convenience function which takes Node and forwards NodeBaseInterface.
-  template<typename NodeT, typename RepT = int64_t, typename T = std::milli>
+  template<typename NodeT = rclcpp::Node, typename RepT = int64_t, typename T = std::milli>
   void
   spin_node_once(
     std::shared_ptr<NodeT> node,
@@ -292,16 +268,18 @@ public:
 
   /// Add a node, complete all immediately available work, and remove the node.
   /**
-   * \param[in] node Shared pointer to the node to spin some.
+   * \param[in] node Shared pointer to the node to add.
    */
-  template<class NodeT>
+  RCLCPP_PUBLIC
   void
-  spin_node_some(const std::shared_ptr<NodeT> & node)
-  {
-    this->spin_node_some(node->get_node_base_interface());
-  }
+  spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node);
 
-  /// Complete all available queued work without blocking.
+  /// Convenience function which takes Node and forwards NodeBaseInterface.
+  RCLCPP_PUBLIC
+  void
+  spin_node_some(std::shared_ptr<rclcpp::Node> node);
+
+  /// 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.
@@ -313,14 +291,29 @@ public:
    * been exceeded.
    */
   RCLCPP_PUBLIC
-  virtual
-  void
-  spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)) = 0;
+  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)) = 0;
+  virtual void
+  spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
   /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
   /**
@@ -332,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
@@ -356,9 +349,14 @@ public:
     }
     std::chrono::nanoseconds timeout_left = timeout_ns;
 
-    while (rclcpp::ok(this->context_)) {
+    if (spinning.exchange(true)) {
+      throw std::runtime_error("spin_until_future_complete() called while already spinning");
+    }
+    RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
+    while (rclcpp::ok(this->context_) && spinning.load()) {
       // Do one item of work.
-      spin_once(timeout_left);
+      spin_once_impl(timeout_left);
+
       // Check if the future is set, return SUCCESS if it is.
       status = future.wait_for(std::chrono::seconds(0));
       if (status == std::future_status::ready) {
@@ -383,40 +381,25 @@ public:
 
   /// Cancel any running spin* function, causing it to return.
   /**
-   * This function can be called asynchronously from any thread.
+   * This function can be called asynchonously from any thread.
+   * \throws std::runtime_error if there is an issue triggering the guard condition
    */
   RCLCPP_PUBLIC
-  virtual
   void
-  cancel() = 0;
+  cancel();
+
+  /// Support dynamic switching of the memory strategy.
+  /**
+   * Switching the memory strategy while the executor is spinning in another threading could have
+   * unintended consequences.
+   * \param[in] memory_strategy Shared pointer to the memory strategy to set.
+   * \throws std::runtime_error if memory_strategy is null
+   */
+  RCLCPP_PUBLIC
+  void
+  set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy);
 
 protected:
-  /// Implementation of add_node().
-  /**
-   * \param[in] node_ptr The node which will have its callback groups added.
-   * \param[in] notify If true, the executor is interrupted to consider the
-   *   changes, otherwise it is not interrupted.
-   * \param[in] ignore_associated_callback_groups If true, then when a callback
-   *   group which is already been added to another executor is encountered
-   *   it will be ignored, if false then std::runtime_error is thrown instead.
-   * \throws std::runtime_error if ignore_associated_callback_groups is false
-   *   and a callback group which is already associated with another executor
-   *   is encountered.
-   * \throws std::invalid_argument if node_ptr is nullptr.
-   */
-  RCLCPP_PUBLIC
-  virtual
-  void
-  add_node(
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    bool notify,
-    bool ignore_associated_callback_groups) = 0;
-
-  RCLCPP_PUBLIC
-  virtual
-  void
-  remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) = 0;
-
   RCLCPP_PUBLIC
   void
   spin_node_once_nanoseconds(
@@ -424,140 +407,167 @@ protected:
     std::chrono::nanoseconds timeout);
 
   RCLCPP_PUBLIC
-  virtual
   void
-  spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node);
+  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,
+  /**
+   * \param[in] any_exec Union structure that can hold any executable type (timer, subscription,
    * service, client).
+   * \throws std::runtime_error if there is an issue triggering the guard condition
    */
   RCLCPP_PUBLIC
   void
-  execute_any_executable(rclcpp::AnyExecutable & any_exec);
+  execute_any_executable(AnyExecutable & any_exec);
 
   RCLCPP_PUBLIC
-  static
-  void
+  static void
   execute_subscription(
     rclcpp::SubscriptionBase::SharedPtr subscription);
 
   RCLCPP_PUBLIC
-  static
-  void
+  static void
   execute_timer(rclcpp::TimerBase::SharedPtr timer);
 
   RCLCPP_PUBLIC
-  static
-  void
+  static void
   execute_service(rclcpp::ServiceBase::SharedPtr service);
 
   RCLCPP_PUBLIC
-  static
-  void
+  static void
   execute_client(rclcpp::ClientBase::SharedPtr client);
 
+  /**
+   * \throws std::runtime_error if the wait set can be cleared
+   */
   RCLCPP_PUBLIC
   void
   wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
+  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(rclcpp::AnyExecutable & any_executable);
+  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(
-    rclcpp::AnyExecutable & any_executable,
+    AnyExecutable & any_executable,
     std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
-  RCLCPP_DISABLE_COPY(ExecutorBase)
+  /// 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;
 
   /// Guard condition for signaling the rmw layer to wake up for special events.
-  rclcpp::GuardCondition interrupt_guard_condition_;
-  // rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
+  rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
 
-  // /// Wait set for managing entities that the rmw layer waits on.
-  // rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
+  /// Wait set for managing entities that the rmw layer waits on.
+  rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
 
-  // // Mutex to protect the subsequent memory_strategy_.
-  // std::mutex memory_strategy_mutex_;
+  // Mutex to protect the subsequent memory_strategy_.
+  std::mutex memory_strategy_mutex_;
 
-  // /// The memory strategy: an interface for handling user-defined memory allocation strategies.
-  // memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;
+  /// The memory strategy: an interface for handling user-defined memory allocation strategies.
+  memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;
 
   /// The context associated with this executor.
-  rclcpp::Context::SharedPtr context_;
+  std::shared_ptr<rclcpp::Context> context_;
 
-  // std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
-  // std::list<const rcl_guard_condition_t *> guard_conditions_;
-  std::vector<rclcpp::CallbackGroup::WeakPtr> weak_guard_conditions_;
+  RCLCPP_DISABLE_COPY(Executor)
 
-};
-
-/// Template class which serves as the foundation of actual executors.
-/**
- * This class combines the wait set, scheduling policy, and the ExecutorBase
- * class, and implements all of the pure virtual functions of ExecutorBase
- * making it a concrete class.
- */
-template<class WaitSetT, class SchedulingPolicy>
-class ExecutorTemplate : public ExecutorBase, public SchedulingPolicy
-{
-public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ExecutorTemplate)
-
-  /// Default constructor.
-  /**
-   * \param[in] options Options for the executor.
-   */
-  explicit ExecutorTemplate(const ExecutorOptions & options = ExecutorOptions())
-  : ExecutorBase(options), SchedulingPolicy(options), WaitSetT(options.context)
-  {}
-
-  /// Default virtual destructor.
   RCLCPP_PUBLIC
-  virtual
-  ~ExecutorTemplate() = default;
+  void
+  spin_once_impl(std::chrono::nanoseconds timeout);
 
-protected:
-  WaitSetT wait_set_;
+  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_;
 };
 
-/// Executor concept which waits for work and coordinates execution of user callbacks.
-/**
- * Executor provides spin functions (including spin_node_once and spin_some).
- * It coordinates the nodes and callback groups by looking for available work
- * and completing it, based on the threading or concurrency scheme provided by
- * the subclass implementation.
- * An example of available work is executing a subscription callback, or a
- * timer callback.
- * The executor structure allows for a decoupling of the communication graph
- * and the execution model.
- * See SingleThreadedExecutor and MultiThreadedExecutor for examples of
- * different execution paradigms.
- *
- * By default this alias provides a foundation based on specific wait set type
- * and a scheduling policy.
- * The wait set is expected to be dynamic, i.e. items can be added or removed
- * after creation, and thread-safe, i.e. items can be added or removed while
- * also waiting concurrently.
- */
-using Executor = ExecutorTemplate<
-  rclcpp::ThreadSafeWaitSet,
-  rclcpp::executor_policies::TimerFavoringPriorityQueue>;
-
 namespace executor
 {
 
-using Executor [[deprecated("use rclcpp::Executor instead")]] = Executor;
+using Executor [[deprecated("use rclcpp::Executor instead")]] = rclcpp::Executor;
 
 }  // namespace executor
 }  // namespace rclcpp

rclcpp/include/rclcpp/executor_options.hpp

@@ -1,4 +1,4 @@
-// Copyright 2014 Open Source Robotics Foundation, Inc.
+// Copyright 2014-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.
@@ -43,6 +43,14 @@ namespace executor
 
 using ExecutorArgs [[deprecated("use rclcpp::ExecutorOptions instead")]] = ExecutorOptions;
 
+[[deprecated("use rclcpp::ExecutorOptions() instead")]]
+inline
+rclcpp::ExecutorOptions
+create_default_executor_arguments()
+{
+  return rclcpp::ExecutorOptions();
+}
+
 }  // namespace executor
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/executor_policies/timer_favoring_priority_queue.hpp

@@ -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.
-
-#ifndef RCLCPP__EXECUTOR_POLICIES__TIMER_FAVORING_PRIORITY_QUEUE_HPP_
-#define RCLCPP__EXECUTOR_POLICIES__TIMER_FAVORING_PRIORITY_QUEUE_HPP_
-
-#include "rclcpp/any_executable.hpp"
-#include "rclcpp/executor_options.hpp"
-#include "rclcpp/executor_policies/scheduling_result.hpp"
-#include "rclcpp/visibility_control.hpp"
-#include "rclcpp/wait_result.hpp"
-
-namespace rclcpp
-{
-namespace executor_policies
-{
-
-/// A naive scheduling policy which selects Timers first, then Subscriptions and other items.
-/**
- * Items are executed in the order they were added, favoring Timers, then
- * Subscriptions, Service Servers, Service Clients, and finally Waitables.
- * All Timers are executed before any Subscriptions, and all Subscriptions
- * before any Service Servers, and so on.
- *
- * User guard conditions are not yet supported by the Executor and so all guard
- * condition are used by the executor itself and are handled before this policy
- * is consulted.
- * Therefore, guard conditions are ignored for the purposes of scheduling.
- *
- * This is a naive policy, but is the default until a better one is implemented.
- */
-class TimerFavoringPriorityQueue
-{
-public:
-  explicit TimerFavoringPriorityQueue(const rclcpp::ExecutorOptions &) {}
-
-  /// Select which item should next be executed, and indicate if waiting should resume.
-  /**
-   * Selects which item to be executed next, assign it to the any_executable, or
-   * assigning nullptr if no work should be done right now.
-   *
-   * This method is called by the executor after waiting on a wait set, in
-   * order to determine what to execute next based on the result.
-   *
-   * Additionally, if returning SchedulingResult::ContinueExecuting then the
-   * executor will call this function again without waiting on the wait set, or
-   * if returning SchedulingResult::WaitForWork then the executor will wait on
-   * the wait set again after executing the selected any_executable, or
-   * immediately if any_executable was assigned nullptr.
-   */
-  template<class WaitSetT>
-  rclcpp::executor_policies::SchedulingResult
-  schedule_next_any_executable(
-    const WaitResult<WaitSetT> & wait_result,
-    rclcpp::AnyExecutable & any_executable)
-  {
-    // Explicitly ignore guard conditions.
-    // Check Timers for being ready.
-
-  }
-};
-
-}  // namespace executor_policies
-}  // namespace rclcpp
-
-#endif  // RCLCPP__EXECUTOR_POLICIES__TIMER_FAVORING_PRIORITY_QUEUE_HPP_

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

@@ -49,6 +49,7 @@ public:
    * \param number_of_threads number of threads to have in the thread pool,
    *   the default 0 will use the number of cpu cores found instead
    * \param yield_before_execute if true std::this_thread::yield() is called
+   * \param timeout maximum time to wait
    */
   RCLCPP_PUBLIC
   MultiThreadedExecutor(
@@ -60,6 +61,10 @@ public:
   RCLCPP_PUBLIC
   virtual ~MultiThreadedExecutor();
 
+  /**
+   * \sa rclcpp::Executor:spin() for more details
+   * \throws std::runtime_error when spin() called while already spinning
+   */
   RCLCPP_PUBLIC
   void
   spin() override;

rclcpp/include/rclcpp/executors/single_threaded_executor.hpp

@@ -59,6 +59,7 @@ public:
    * the process until canceled.
    * It may be interrupt by a call to rclcpp::Executor::cancel() or by ctrl-c
    * if the associated context is configured to shutdown on SIGINT.
+   * \throws std::runtime_error when spin() called while already spinning
    */
   RCLCPP_PUBLIC
   void

rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp

@@ -17,7 +17,9 @@
 
 #include <chrono>
 #include <list>
+#include <map>
 #include <memory>
+#include <vector>
 
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
@@ -32,6 +34,9 @@ namespace rclcpp
 {
 namespace executors
 {
+typedef std::map<rclcpp::CallbackGroup::WeakPtr,
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+    std::owner_less<rclcpp::CallbackGroup::WeakPtr>> WeakCallbackGroupsToNodesMap;
 
 class StaticExecutorEntitiesCollector final
   : public rclcpp::Waitable,
@@ -45,38 +50,45 @@ public:
   StaticExecutorEntitiesCollector() = default;
 
   // Destructor
+  RCLCPP_PUBLIC
   ~StaticExecutorEntitiesCollector();
 
+  /// Initialize StaticExecutorEntitiesCollector
+  /**
+   * \param p_wait_set A reference to the wait set to be used in the executor
+   * \param memory_strategy Shared pointer to the memory strategy to set.
+   * \param executor_guard_condition executor's guard condition
+   * \throws std::runtime_error if memory strategy is null
+   */
   RCLCPP_PUBLIC
   void
   init(
     rcl_wait_set_t * p_wait_set,
-    rclcpp::memory_strategy::MemoryStrategy::SharedPtr & memory_strategy,
+    rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy,
     rcl_guard_condition_t * executor_guard_condition);
 
+  /// Finalize StaticExecutorEntitiesCollector to clear resources
+  RCLCPP_PUBLIC
+  void
+  fini();
+
   RCLCPP_PUBLIC
   void
   execute() override;
 
-  RCLCPP_PUBLIC
-  void
-  fill_memory_strategy();
-
-  RCLCPP_PUBLIC
-  void
-  fill_executable_list();
-
-  /// Function to reallocate space for entities in the wait set.
-  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.
+  /**
+   * 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, \see rclcpp::exceptions::throw_from_rcl_error()
+   */
   RCLCPP_PUBLIC
   void
   refresh_wait_set(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
+  /**
+   * \throws std::runtime_error if it couldn't add guard condition to wait set
+   */
   RCLCPP_PUBLIC
   bool
   add_to_wait_set(rcl_wait_set_t * wait_set) override;
@@ -85,16 +97,85 @@ public:
   size_t
   get_number_of_ready_guard_conditions() override;
 
+  /// Add a callback group to an executor.
+  /**
+   * \see rclcpp::Executor::add_callback_group
+   */
   RCLCPP_PUBLIC
-  void
+  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
+  bool
   add_node(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
 
+  /**
+   * \see rclcpp::Executor::remove_node()
+   * \throw std::runtime_error if no guard condition is associated with node.
+   */
   RCLCPP_PUBLIC
   bool
   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
@@ -105,53 +186,141 @@ public:
   bool
   is_ready(rcl_wait_set_t * wait_set) override;
 
+  /// Return number of timers
+  /**
+   * \return number of timers
+   */
   RCLCPP_PUBLIC
   size_t
   get_number_of_timers() {return exec_list_.number_of_timers;}
 
+  /// Return number of subscriptions
+  /**
+   * \return number of subscriptions
+   */
   RCLCPP_PUBLIC
   size_t
   get_number_of_subscriptions() {return exec_list_.number_of_subscriptions;}
 
+  /// Return number of services
+  /**
+   * \return number of services
+   */
   RCLCPP_PUBLIC
   size_t
   get_number_of_services() {return exec_list_.number_of_services;}
 
+  /// Return number of clients
+  /**
+   * \return number of clients
+   */
   RCLCPP_PUBLIC
   size_t
   get_number_of_clients() {return exec_list_.number_of_clients;}
 
+  /// Return number of waitables
+  /**
+   * \return number of waitables
+   */
   RCLCPP_PUBLIC
   size_t
   get_number_of_waitables() {return exec_list_.number_of_waitables;}
 
+  /** Return a SubscritionBase Sharedptr by index.
+   * \param[in] i The index of the SubscritionBase
+   * \return a SubscritionBase shared pointer
+   * \throws std::out_of_range if the argument is higher than the size of the structrue.
+   */
   RCLCPP_PUBLIC
   rclcpp::SubscriptionBase::SharedPtr
   get_subscription(size_t i) {return exec_list_.subscription[i];}
 
+  /** Return a TimerBase Sharedptr by index.
+   * \param[in] i The index of the TimerBase
+   * \return a TimerBase shared pointer
+   * \throws std::out_of_range if the argument is higher than the size.
+   */
   RCLCPP_PUBLIC
   rclcpp::TimerBase::SharedPtr
   get_timer(size_t i) {return exec_list_.timer[i];}
 
+  /** Return a ServiceBase Sharedptr by index.
+   * \param[in] i The index of the ServiceBase
+   * \return a ServiceBase shared pointer
+   * \throws std::out_of_range if the argument is higher than the size.
+   */
   RCLCPP_PUBLIC
   rclcpp::ServiceBase::SharedPtr
   get_service(size_t i) {return exec_list_.service[i];}
 
+  /** Return a ClientBase Sharedptr by index
+   * \param[in] i The index of the ClientBase
+   * \return a ClientBase shared pointer
+   * \throws std::out_of_range if the argument is higher than the size.
+   */
   RCLCPP_PUBLIC
   rclcpp::ClientBase::SharedPtr
   get_client(size_t i) {return exec_list_.client[i];}
 
+  /** Return a Waitable Sharedptr by index
+   * \param[in] i The index of the Waitable
+   * \return a Waitable shared pointer
+   * \throws std::out_of_range if the argument is higher than the size.
+   */
   RCLCPP_PUBLIC
   rclcpp::Waitable::SharedPtr
   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

@@ -69,19 +69,39 @@ public:
   virtual ~StaticSingleThreadedExecutor();
 
   /// Static executor implementation of spin.
-  // This function will block until work comes in, execute it, and keep blocking.
-  // It will only be interrupt by a CTRL-C (managed by the global signal handler).
+  /**
+   * This function will block until work comes in, execute it, and keep blocking.
+   * It will only be interrupted by a CTRL-C (managed by the global signal handler).
+   * \throws std::runtime_error when spin() called while already spinning
+   */
   RCLCPP_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.
+   * \sa rclcpp::Executor::add_node
    */
   RCLCPP_PUBLIC
   void
@@ -90,16 +110,16 @@ public:
     bool notify = true) override;
 
   /// Convenience function which takes Node and forwards NodeBaseInterface.
+  /**
+   * \sa rclcpp::StaticSingleThreadedExecutor::add_node
+   */
   RCLCPP_PUBLIC
   void
   add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
 
   /// 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.
+   * \sa rclcpp::Executor::remove_node
    */
   RCLCPP_PUBLIC
   void
@@ -108,10 +128,33 @@ public:
     bool notify = true) override;
 
   /// Convenience function which takes Node and forwards NodeBaseInterface.
+  /**
+   * \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
@@ -130,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));
@@ -149,8 +192,8 @@ public:
     }
     std::chrono::nanoseconds timeout_left = timeout_ns;
 
-    entities_collector_ = std::make_shared<StaticExecutorEntitiesCollector>();
     entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_);
+    RCLCPP_SCOPE_EXIT(entities_collector_->fini());
 
     while (rclcpp::ok(this->context_)) {
       // Do one set of work.
@@ -178,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/expand_topic_or_service_name.hpp

@@ -49,6 +49,8 @@ namespace rclcpp
  * \throws InvalidServiceNameError if name is invalid and is_service is true
  * \throws std::bad_alloc if memory cannot be allocated
  * \throws RCLError if an unexpect error occurs
+ * \throws std::runtime_error if the topic name is unexpectedly valid or,
+ *    if the rcl name is invalid or if the rcl namespace is invalid
  */
 RCLCPP_PUBLIC
 std::string

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

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

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

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

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

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

rclcpp/include/rclcpp/future_return_code.hpp

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

rclcpp/include/rclcpp/graph_listener.hpp

@@ -73,6 +73,8 @@ public:
    * This function is thread-safe.
    *
    * \throws GraphListenerShutdownError if the GraphListener is shutdown
+   * \throws std::runtime if the parent context was destroyed
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
    */
   RCLCPP_PUBLIC
   virtual

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"
@@ -30,11 +31,21 @@ public:
   /// If true, the context will be shutdown on SIGINT by the signal handler (if it was installed).
   bool shutdown_on_sigint = true;
 
-  /// Constructor which allows you to specify the allocator used within the init options.
+  /// Constructor
+  /**
+   * It allows you to specify the allocator used within the init options.
+   * \param[in] allocator used allocate memory within the init options
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
+   */
   RCLCPP_PUBLIC
   explicit InitOptions(rcl_allocator_t allocator = rcl_get_default_allocator());
 
   /// Constructor which is initialized by an existing init_options.
+  /**
+   * Initialized by an existing init_options.
+   * \param[in] init_options rcl_init_options_t to initialized
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
+   */
   RCLCPP_PUBLIC
   explicit InitOptions(const rcl_init_options_t & init_options);
 
@@ -42,6 +53,16 @@ public:
   RCLCPP_PUBLIC
   InitOptions(const InitOptions & other);
 
+  /// Return `true` if logging should be initialized when `rclcpp::Context::init` is called.
+  RCLCPP_PUBLIC
+  bool
+  auto_initialize_logging() const;
+
+  /// Set flag indicating if logging should be initialized or not.
+  RCLCPP_PUBLIC
+  InitOptions &
+  auto_initialize_logging(bool initialize_logging);
+
   /// Assignment operator.
   RCLCPP_PUBLIC
   InitOptions &
@@ -52,16 +73,40 @@ public:
   ~InitOptions();
 
   /// Return the rcl init options.
+  /**
+   * \return the rcl init options.
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
+   */
   RCLCPP_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};
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/loaned_message.hpp

@@ -52,8 +52,9 @@ public:
    * However, this user code is ought to be usable even when dynamically linked against
    * a middleware which doesn't support message loaning in which case the allocator will be used.
    *
-   * \param pub rclcpp::Publisher instance to which the memory belongs
-   * \param allocator Allocator instance in case middleware can not allocate messages
+   * \param[in] pub rclcpp::Publisher instance to which the memory belongs
+   * \param[in] allocator Allocator instance in case middleware can not allocate messages
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
    */
   LoanedMessage(
     const rclcpp::PublisherBase & pub,
@@ -65,7 +66,7 @@ public:
     if (pub_.can_loan_messages()) {
       void * message_ptr = nullptr;
       auto ret = rcl_borrow_loaned_message(
-        pub_.get_publisher_handle(),
+        pub_.get_publisher_handle().get(),
         rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>(),
         &message_ptr);
       if (RCL_RET_OK != ret) {
@@ -98,8 +99,9 @@ public:
    * However, this user code is ought to be usable even when dynamically linked against
    * a middleware which doesn't support message loaning in which case the allocator will be used.
    *
-   * \param pub rclcpp::Publisher instance to which the memory belongs
-   * \param allocator Allocator instance in case middleware can not allocate messages
+   * \param[in] pub rclcpp::Publisher instance to which the memory belongs
+   * \param[in] allocator Allocator instance in case middleware can not allocate messages
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
    */
   LoanedMessage(
     const rclcpp::PublisherBase * pub,
@@ -137,7 +139,7 @@ public:
     if (pub_.can_loan_messages()) {
       // return allocated memory to the middleware
       auto ret =
-        rcl_return_loaned_message_from_publisher(pub_.get_publisher_handle(), message_);
+        rcl_return_loaned_message_from_publisher(pub_.get_publisher_handle().get(), message_);
       if (ret != RCL_RET_OK) {
         RCLCPP_ERROR(
           error_logger, "rcl_deallocate_loaned_message failed: %s", rcl_get_error_string().str);

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

@@ -23,6 +23,10 @@ namespace rclcpp
 namespace memory_strategies
 {
 
+/// Create a MemoryStrategy sharedPtr
+/**
+ * \return a MemoryStrategy sharedPtr
+ */
 RCLCPP_PUBLIC
 memory_strategy::MemoryStrategy::SharedPtr
 create_default_strategy();

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

@@ -30,6 +30,9 @@ public:
   MessageInfo() = default;
 
   /// Conversion constructor, which is intentionally not marked as explicit.
+  /**
+   * \param[in] rmw_message_info message info to initialize the class
+   */
   // cppcheck-suppress noExplicitConstructor
   MessageInfo(const rmw_message_info_t & rmw_message_info);  // NOLINT(runtime/explicit)
 

rclcpp/include/rclcpp/message_memory_strategy.hpp

@@ -23,6 +23,7 @@
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/serialized_message.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 #include "rcutils/logging_macros.h"
@@ -46,10 +47,10 @@ public:
   using MessageAlloc = typename MessageAllocTraits::allocator_type;
   using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
 
-  using SerializedMessageAllocTraits = allocator::AllocRebind<rcl_serialized_message_t, Alloc>;
+  using SerializedMessageAllocTraits = allocator::AllocRebind<rclcpp::SerializedMessage, Alloc>;
   using SerializedMessageAlloc = typename SerializedMessageAllocTraits::allocator_type;
   using SerializedMessageDeleter =
-    allocator::Deleter<SerializedMessageAlloc, rcl_serialized_message_t>;
+    allocator::Deleter<SerializedMessageAlloc, rclcpp::SerializedMessage>;
 
   using BufferAllocTraits = allocator::AllocRebind<char, Alloc>;
   using BufferAlloc = typename BufferAllocTraits::allocator_type;
@@ -86,31 +87,12 @@ public:
     return std::allocate_shared<MessageT, MessageAlloc>(*message_allocator_.get());
   }
 
-  virtual std::shared_ptr<rcl_serialized_message_t> borrow_serialized_message(size_t capacity)
+  virtual std::shared_ptr<rclcpp::SerializedMessage> borrow_serialized_message(size_t capacity)
   {
-    auto msg = new rcl_serialized_message_t;
-    *msg = rmw_get_zero_initialized_serialized_message();
-    auto ret = rmw_serialized_message_init(msg, capacity, &rcutils_allocator_);
-    if (ret != RCL_RET_OK) {
-      rclcpp::exceptions::throw_from_rcl_error(ret);
-    }
-
-    auto serialized_msg = std::shared_ptr<rcl_serialized_message_t>(
-      msg,
-      [](rmw_serialized_message_t * msg) {
-        auto fini_ret = rmw_serialized_message_fini(msg);
-        delete msg;
-        if (fini_ret != RCL_RET_OK) {
-          RCUTILS_LOG_ERROR_NAMED(
-            "rclcpp",
-            "failed to destroy serialized message: %s", rcl_get_error_string().str);
-        }
-      });
-
-    return serialized_msg;
+    return std::make_shared<rclcpp::SerializedMessage>(capacity);
   }
 
-  virtual std::shared_ptr<rcl_serialized_message_t> borrow_serialized_message()
+  virtual std::shared_ptr<rclcpp::SerializedMessage> borrow_serialized_message()
   {
     return borrow_serialized_message(default_buffer_capacity_);
   }
@@ -127,7 +109,8 @@ public:
     msg.reset();
   }
 
-  virtual void return_serialized_message(std::shared_ptr<rcl_serialized_message_t> & serialized_msg)
+  virtual void return_serialized_message(
+    std::shared_ptr<rclcpp::SerializedMessage> & serialized_msg)
   {
     serialized_msg.reset();
   }

rclcpp/include/rclcpp/node.hpp

@@ -26,6 +26,8 @@
 #include <utility>
 #include <vector>
 
+#include "rcutils/macros.h"
+
 #include "rcl/error_handling.h"
 #include "rcl/node.h"
 
@@ -78,6 +80,7 @@ public:
   /**
    * \param[in] node_name Name of the node.
    * \param[in] options Additional options to control creation of the node.
+   * \throws InvalidNamespaceError if the namespace is invalid
    */
   RCLCPP_PUBLIC
   explicit Node(
@@ -89,6 +92,7 @@ public:
    * \param[in] node_name Name of the node.
    * \param[in] namespace_ Namespace of the node.
    * \param[in] options Additional options to control creation of the node.
+   * \throws InvalidNamespaceError if the namespace is invalid
    */
   RCLCPP_PUBLIC
   explicit Node(
@@ -122,6 +126,7 @@ public:
   /// Get the fully-qualified name of the node.
   /**
    * The fully-qualified name includes the local namespace and name of the node.
+   * \return fully-qualified name of the node.
    */
   RCLCPP_PUBLIC
   const char *
@@ -136,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
@@ -186,8 +193,8 @@ public:
   /// Create and return a Subscription.
   /**
    * \param[in] topic_name The topic to subscribe on.
+   * \param[in] qos QoS profile for Subcription.
    * \param[in] callback The user-defined callback function to receive a message
-   * \param[in] qos_history_depth The depth of the subscription's incoming message queue.
    * \param[in] options Additional options for the creation of the Subscription.
    * \param[in] msg_mem_strat The message memory strategy to use for allocating messages.
    * \return Shared pointer to the created subscription.
@@ -229,7 +236,13 @@ public:
     CallbackT callback,
     rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
-  /* Create and return a Client. */
+  /// Create and return a Client.
+  /**
+   * \param[in] service_name The topic to service on.
+   * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client.
+   * \param[in] group Callback group to call the service.
+   * \return Shared pointer to the created client.
+   */
   template<typename ServiceT>
   typename rclcpp::Client<ServiceT>::SharedPtr
   create_client(
@@ -237,7 +250,14 @@ public:
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default,
     rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
-  /* Create and return a Service. */
+  /// Create and return a Service.
+  /**
+   * \param[in] service_name The topic to service on.
+   * \param[in] callback User-defined callback function.
+   * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client.
+   * \param[in] group Callback group to call the service.
+   * \return Shared pointer to the created service.
+   */
   template<typename ServiceT, typename CallbackT>
   typename rclcpp::Service<ServiceT>::SharedPtr
   create_service(
@@ -264,7 +284,7 @@ public:
    * If `ignore_override` is `true`, the parameter override will be ignored.
    *
    * This method, if successful, will result in any callback registered with
-   * set_on_parameters_set_callback to be called.
+   * add_on_set_parameters_callback to be called.
    * If that callback prevents the initial value for the parameter from being
    * set then rclcpp::exceptions::InvalidParameterValueException is thrown.
    *
@@ -346,7 +366,7 @@ public:
    * by the function call will be ignored.
    *
    * This method, if successful, will result in any callback registered with
-   * set_on_parameters_set_callback to be called, once for each parameter.
+   * add_on_set_parameters_callback to be called, once for each parameter.
    * If that callback prevents the initial value for any parameter from being
    * set then rclcpp::exceptions::InvalidParameterValueException is thrown.
    *
@@ -388,7 +408,7 @@ public:
   /// Undeclare a previously declared parameter.
   /**
    * This method will not cause a callback registered with
-   * set_on_parameters_set_callback to be called.
+   * add_on_set_parameters_callback to be called.
    *
    * \param[in] name The name of the parameter to be undeclared.
    * \throws rclcpp::exceptions::ParameterNotDeclaredException if the parameter
@@ -422,7 +442,7 @@ public:
    * Parameter overrides are ignored by set_parameter.
    *
    * This method will result in any callback registered with
-   * set_on_parameters_set_callback to be called.
+   * add_on_set_parameters_callback to be called.
    * If the callback prevents the parameter from being set, then it will be
    * reflected in the SetParametersResult that is returned, but no exception
    * will be thrown.
@@ -463,7 +483,7 @@ public:
    * corresponding SetParametersResult in the vector returned by this function.
    *
    * This method will result in any callback registered with
-   * set_on_parameters_set_callback to be called, once for each parameter.
+   * add_on_set_parameters_callback to be called, once for each parameter.
    * If the callback prevents the parameter from being set, then, as mentioned
    * before, it will be reflected in the corresponding SetParametersResult
    * that is returned, but no exception will be thrown.
@@ -494,7 +514,7 @@ public:
    * If the exception is thrown then none of the parameters will have been set.
    *
    * This method will result in any callback registered with
-   * set_on_parameters_set_callback to be called, just one time.
+   * add_on_set_parameters_callback to be called, just one time.
    * If the callback prevents the parameters from being set, then it will be
    * reflected in the SetParametersResult which is returned, but no exception
    * will be thrown.
@@ -672,6 +692,7 @@ public:
    * \throws rclcpp::exceptions::ParameterNotDeclaredException if the
    *   parameter has not been declared and undeclared parameters are not
    *   allowed.
+   * \throws std::runtime_error if the number of described parameters is more than one
    */
   RCLCPP_PUBLIC
   rcl_interfaces::msg::ParameterDescriptor
@@ -694,6 +715,7 @@ public:
    * \throws rclcpp::exceptions::ParameterNotDeclaredException if any of the
    *   parameters have not been declared and undeclared parameters are not
    *   allowed.
+   * \throws std::runtime_error if the number of described parameters is more than one
    */
   RCLCPP_PUBLIC
   std::vector<rcl_interfaces::msg::ParameterDescriptor>
@@ -774,7 +796,7 @@ public:
    * of the {get,list,describe}_parameter() methods), but may *not* modify
    * other parameters (by calling any of the {set,declare}_parameter() methods)
    * or modify the registered callback itself (by calling the
-   * set_on_parameters_set_callback() method).  If a callback tries to do any
+   * add_on_set_parameters_callback() method).  If a callback tries to do any
    * of the latter things,
    * rclcpp::exceptions::ParameterModifiedInCallbackException will be thrown.
    *
@@ -794,6 +816,7 @@ public:
    * \throws std::bad_alloc if the allocation of the OnSetParametersCallbackHandle fails.
    */
   RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
   OnSetParametersCallbackHandle::SharedPtr
   add_on_set_parameters_callback(OnParametersSetCallbackType callback);
 
@@ -824,24 +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.
-  /**
-   * 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.
-   */
-  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.
@@ -851,18 +856,53 @@ public:
   std::vector<std::string>
   get_node_names() const;
 
+  /// Return a map of existing topic names to list of topic types.
+  /**
+   * \return a map of existing topic names to list of topic types.
+   * \throws std::runtime_error anything that rcl_error can throw
+   */
   RCLCPP_PUBLIC
   std::map<std::string, std::vector<std::string>>
   get_topic_names_and_types() const;
 
+  /// Return a map of existing service names to list of service types.
+  /**
+   * \return a map of existing service names to list of service types.
+   * \throws std::runtime_error anything that rcl_error can throw
+   */
   RCLCPP_PUBLIC
   std::map<std::string, std::vector<std::string>>
   get_service_names_and_types() const;
 
+  /// Return the number of publishers that are advertised on a given topic.
+  /**
+   * \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>>
+  get_service_names_and_types_by_node(
+    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 created for a given topic.
+  /**
+   * \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
   count_subscribers(const std::string & topic_name) const;
@@ -882,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.
@@ -908,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.
@@ -932,6 +972,9 @@ public:
   /**
    * The given Event must be acquire through the get_graph_event() method.
    *
+   * \param[in] event pointer to an Event to wait for
+   * \param[in] timeout nanoseconds to wait for the Event to change the state
+   *
    * \throws InvalidEventError if the given event is nullptr
    * \throws EventNotRegisteredError if the given event was not acquired with
    *   get_graph_event().
@@ -942,14 +985,26 @@ public:
     rclcpp::Event::SharedPtr event,
     std::chrono::nanoseconds timeout);
 
+  /// Get a clock as a non-const shared pointer which is managed by the node.
+  /**
+   * \sa rclcpp::node_interfaces::NodeClock::get_clock
+   */
   RCLCPP_PUBLIC
   rclcpp::Clock::SharedPtr
   get_clock();
 
+  /// Get a clock as a const shared pointer which is managed by the node.
+  /**
+   * \sa rclcpp::node_interfaces::NodeClock::get_clock
+   */
   RCLCPP_PUBLIC
   rclcpp::Clock::ConstSharedPtr
   get_clock() const;
 
+  /// Returns current time from the time source specified by clock_type.
+  /**
+   * \sa rclcpp::Clock::now
+   */
   RCLCPP_PUBLIC
   Time
   now() const;
@@ -999,7 +1054,7 @@ public:
   rclcpp::node_interfaces::NodeParametersInterface::SharedPtr
   get_node_parameters_interface();
 
-  /// Return the Node's internal NodeParametersInterface implementation.
+  /// Return the Node's internal NodeTimeSourceInterface implementation.
   RCLCPP_PUBLIC
   rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr
   get_node_time_source_interface();
@@ -1111,19 +1166,6 @@ public:
   const rclcpp::NodeOptions &
   get_node_options() const;
 
-  /// Manually assert that this Node is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE).
-  /**
-   * If the rmw Liveliness policy is set to RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE, the creator
-   * of this node may manually call `assert_liveliness` at some point in time to signal to the rest
-   * of the system that this Node is still alive.
-   *
-   * \return `true` if the liveliness was asserted successfully, otherwise `false`
-   */
-  RCLCPP_PUBLIC
-  RCUTILS_WARN_UNUSED
-  bool
-  assert_liveliness() const;
-
 protected:
   /// Construct a sub-node, which will extend the namespace of all entities created with it.
   /**
@@ -1140,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_impl.hpp

@@ -19,6 +19,7 @@
 #include <rmw/rmw.h>
 
 #include <algorithm>
+#include <chrono>
 #include <cstdlib>
 #include <iostream>
 #include <limits>
@@ -37,10 +38,12 @@
 #include "rclcpp/create_client.hpp"
 #include "rclcpp/create_publisher.hpp"
 #include "rclcpp/create_service.hpp"
+#include "rclcpp/create_timer.hpp"
 #include "rclcpp/create_subscription.hpp"
 #include "rclcpp/detail/resolve_enable_topic_statistics.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/qos.hpp"
+#include "rclcpp/timer.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -108,12 +111,12 @@ Node::create_wall_timer(
   CallbackT callback,
   rclcpp::CallbackGroup::SharedPtr group)
 {
-  auto timer = rclcpp::WallTimer<CallbackT>::make_shared(
-    std::chrono::duration_cast<std::chrono::nanoseconds>(period),
+  return rclcpp::create_wall_timer(
+    period,
     std::move(callback),
-    this->node_base_->get_context());
-  node_timers_->add_timer(timer, group);
-  return timer;
+    group,
+    this->node_base_.get(),
+    this->node_timers_.get());
 }
 
 template<typename ServiceT>

rclcpp/include/rclcpp/node_interfaces/get_node_base_interface.hpp

@@ -1,149 +0,0 @@
-// Copyright 2019 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__NODE_INTERFACES__GET_NODE_BASE_INTERFACE_HPP_
-#define RCLCPP__NODE_INTERFACES__GET_NODE_BASE_INTERFACE_HPP_
-
-#include <memory>
-#include <utility>
-#include <type_traits>
-
-#include "rclcpp/node_interfaces/node_base_interface.hpp"
-
-/// This header provides the get_node_base_interface() template function.
-/**
- * This function is useful for getting the NodeBaseInterface pointer from
- * various kinds of Node-like classes.
- *
- * It's able to get the NodeBaseInterface pointer so long as the class
- * has a method called ``get_node_base_interface()`` which returns
- * either a pointer (const or not) to a NodeBaseInterface or a
- * std::shared_ptr to a NodeBaseInterface.
- */
-
-namespace rclcpp
-{
-namespace node_interfaces
-{
-
-namespace detail
-{
-
-// This is a meta-programming checker for if a given Node-like object has a
-// getter called get_node_base_interface() which returns various types,
-// e.g. const pointer or a shared pointer.
-template<typename NodeType, typename ReturnType>
-struct has_get_node_base_interface
-{
-private:
-  template<typename T>
-  static constexpr
-  auto
-  check(T *)->typename std::is_same<
-    decltype(std::declval<T>().get_node_base_interface()),
-    ReturnType
-  >::type;
-
-  template<typename>
-  static constexpr
-  std::false_type
-  check(...);
-
-public:
-  using type = decltype(check<NodeType>(nullptr));
-  static constexpr bool value = type::value;
-};
-
-// If NodeType is a pointer to NodeBaseInterface already (just normal function overload).
-inline
-rclcpp::node_interfaces::NodeBaseInterface *
-get_node_base_interface_from_pointer(rclcpp::node_interfaces::NodeBaseInterface * pointer)
-{
-  return pointer;
-}
-
-// If NodeType has a method called get_node_base_interface() which returns a shared pointer.
-template<
-  typename NodeType,
-  typename std::enable_if<has_get_node_base_interface<
-    typename std::remove_pointer<NodeType>::type,
-    std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>
-  >::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeBaseInterface *
-get_node_base_interface_from_pointer(NodeType node_pointer)
-{
-  return node_pointer->get_node_base_interface().get();
-}
-
-// If NodeType has a method called get_node_base_interface() which returns a pointer.
-template<
-  typename NodeType,
-  typename std::enable_if<has_get_node_base_interface<
-    typename std::remove_pointer<NodeType>::type,
-    rclcpp::node_interfaces::NodeBaseInterface *
-  >::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeBaseInterface *
-get_node_base_interface_from_pointer(NodeType node_pointer)
-{
-  return node_pointer->get_node_base_interface();
-}
-
-// Forward shared_ptr's to const node pointer signatures.
-template<
-  typename NodeType,
-  typename std::enable_if<std::is_same<
-    NodeType,
-    typename std::shared_ptr<typename std::remove_pointer<NodeType>::type::element_type> *
-  >::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeBaseInterface *
-get_node_base_interface_from_pointer(NodeType node_shared_pointer)
-{
-  return get_node_base_interface_from_pointer(node_shared_pointer->get());
-}
-
-}  // namespace detail
-
-/// Get the NodeBaseInterface as a pointer from a pointer to a "Node like" object.
-template<
-  typename NodeType,
-  typename std::enable_if<std::is_pointer<NodeType>::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeBaseInterface *
-get_node_base_interface(NodeType node_pointer)
-{
-  // Forward pointers to detail implmentation directly.
-  return detail::get_node_base_interface_from_pointer(node_pointer);
-}
-
-/// Get the NodeBaseInterface as a pointer from a "Node like" object.
-template<
-  typename NodeType,
-  typename std::enable_if<
-    !std::is_pointer<typename std::remove_reference<NodeType>::type>::value, int
-  >::type = 0
->
-rclcpp::node_interfaces::NodeBaseInterface *
-get_node_base_interface(NodeType && node_reference)
-{
-  // Forward references to detail implmentation as a pointer.
-  return detail::get_node_base_interface_from_pointer(&node_reference);
-}
-
-}  // namespace node_interfaces
-}  // namespace rclcpp
-
-#endif  // RCLCPP__NODE_INTERFACES__GET_NODE_BASE_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/get_node_timers_interface.hpp

@@ -1,149 +0,0 @@
-// Copyright 2019 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__NODE_INTERFACES__GET_NODE_TIMERS_INTERFACE_HPP_
-#define RCLCPP__NODE_INTERFACES__GET_NODE_TIMERS_INTERFACE_HPP_
-
-#include <memory>
-#include <utility>
-#include <type_traits>
-
-#include "rclcpp/node_interfaces/node_timers_interface.hpp"
-
-/// This header provides the get_node_timers_interface() template function.
-/**
- * This function is useful for getting the NodeTimersInterface pointer from
- * various kinds of Node-like classes.
- *
- * It's able to get the NodeTimersInterface pointer so long as the class
- * has a method called ``get_node_timers_interface()`` which returns
- * either a pointer (const or not) to a NodeTimersInterface or a
- * std::shared_ptr to a NodeTimersInterface.
- */
-
-namespace rclcpp
-{
-namespace node_interfaces
-{
-
-namespace detail
-{
-
-// This is a meta-programming checker for if a given Node-like object has a
-// getter called get_node_timers_interface() which returns various types,
-// e.g. const pointer or a shared pointer.
-template<typename NodeType, typename ReturnType>
-struct has_get_node_timers_interface
-{
-private:
-  template<typename T>
-  static constexpr
-  auto
-  check(T *)->typename std::is_same<
-    decltype(std::declval<T>().get_node_timers_interface()),
-    ReturnType
-  >::type;
-
-  template<typename>
-  static constexpr
-  std::false_type
-  check(...);
-
-public:
-  using type = decltype(check<NodeType>(nullptr));
-  static constexpr bool value = type::value;
-};
-
-// If NodeType is a pointer to NodeTimersInterface already (just normal function overload).
-inline
-rclcpp::node_interfaces::NodeTimersInterface *
-get_node_timers_interface_from_pointer(rclcpp::node_interfaces::NodeTimersInterface * pointer)
-{
-  return pointer;
-}
-
-// If NodeType has a method called get_node_timers_interface() which returns a shared pointer.
-template<
-  typename NodeType,
-  typename std::enable_if<has_get_node_timers_interface<
-    typename std::remove_pointer<NodeType>::type,
-    std::shared_ptr<rclcpp::node_interfaces::NodeTimersInterface>
-  >::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeTimersInterface *
-get_node_timers_interface_from_pointer(NodeType node_pointer)
-{
-  return node_pointer->get_node_timers_interface().get();
-}
-
-// If NodeType has a method called get_node_timers_interface() which returns a pointer.
-template<
-  typename NodeType,
-  typename std::enable_if<has_get_node_timers_interface<
-    typename std::remove_pointer<NodeType>::type,
-    rclcpp::node_interfaces::NodeTimersInterface *
-  >::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeTimersInterface *
-get_node_timers_interface_from_pointer(NodeType node_pointer)
-{
-  return node_pointer->get_node_timers_interface();
-}
-
-// Forward shared_ptr's to const node pointer signatures.
-template<
-  typename NodeType,
-  typename std::enable_if<std::is_same<
-    NodeType,
-    typename std::shared_ptr<typename std::remove_pointer<NodeType>::type::element_type> *
-  >::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeTimersInterface *
-get_node_timers_interface_from_pointer(NodeType node_shared_pointer)
-{
-  return get_node_timers_interface_from_pointer(node_shared_pointer->get());
-}
-
-}  // namespace detail
-
-/// Get the NodeTimersInterface as a pointer from a pointer to a "Node like" object.
-template<
-  typename NodeType,
-  typename std::enable_if<std::is_pointer<NodeType>::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeTimersInterface *
-get_node_timers_interface(NodeType node_pointer)
-{
-  // Forward pointers to detail implmentation directly.
-  return detail::get_node_timers_interface_from_pointer(node_pointer);
-}
-
-/// Get the NodeTimersInterface as a pointer from a "Node like" object.
-template<
-  typename NodeType,
-  typename std::enable_if<
-    !std::is_pointer<typename std::remove_reference<NodeType>::type>::value, int
-  >::type = 0
->
-rclcpp::node_interfaces::NodeTimersInterface *
-get_node_timers_interface(NodeType && node_reference)
-{
-  // Forward references to detail implmentation as a pointer.
-  return detail::get_node_timers_interface_from_pointer(&node_reference);
-}
-
-}  // namespace node_interfaces
-}  // namespace rclcpp
-
-#endif  // RCLCPP__NODE_INTERFACES__GET_NODE_TIMERS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/get_node_topics_interface.hpp

@@ -1,149 +0,0 @@
-// Copyright 2019 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__NODE_INTERFACES__GET_NODE_TOPICS_INTERFACE_HPP_
-#define RCLCPP__NODE_INTERFACES__GET_NODE_TOPICS_INTERFACE_HPP_
-
-#include <memory>
-#include <utility>
-#include <type_traits>
-
-#include "rclcpp/node_interfaces/node_topics_interface.hpp"
-
-/// This header provides the get_node_topics_interface() template function.
-/**
- * This function is useful for getting the NodeTopicsInterface pointer from
- * various kinds of Node-like classes.
- *
- * It's able to get the NodeTopicsInterface pointer so long as the class
- * has a method called ``get_node_topics_interface()`` which returns
- * either a pointer (const or not) to a NodeTopicsInterface or a
- * std::shared_ptr to a NodeTopicsInterface.
- */
-
-namespace rclcpp
-{
-namespace node_interfaces
-{
-
-namespace detail
-{
-
-// This is a meta-programming checker for if a given Node-like object has a
-// getter called get_node_topics_interface() which returns various types,
-// e.g. const pointer or a shared pointer.
-template<typename NodeType, typename ReturnType>
-struct has_get_node_topics_interface
-{
-private:
-  template<typename T>
-  static constexpr
-  auto
-  check(T *)->typename std::is_same<
-    decltype(std::declval<T>().get_node_topics_interface()),
-    ReturnType
-  >::type;
-
-  template<typename>
-  static constexpr
-  std::false_type
-  check(...);
-
-public:
-  using type = decltype(check<NodeType>(nullptr));
-  static constexpr bool value = type::value;
-};
-
-// If NodeType is a pointer to NodeTopicsInterface already (just normal function overload).
-inline
-rclcpp::node_interfaces::NodeTopicsInterface *
-get_node_topics_interface_from_pointer(rclcpp::node_interfaces::NodeTopicsInterface * pointer)
-{
-  return pointer;
-}
-
-// If NodeType has a method called get_node_topics_interface() which returns a shared pointer.
-template<
-  typename NodeType,
-  typename std::enable_if<has_get_node_topics_interface<
-    typename std::remove_pointer<NodeType>::type,
-    std::shared_ptr<rclcpp::node_interfaces::NodeTopicsInterface>
-  >::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeTopicsInterface *
-get_node_topics_interface_from_pointer(NodeType node_pointer)
-{
-  return node_pointer->get_node_topics_interface().get();
-}
-
-// If NodeType has a method called get_node_topics_interface() which returns a pointer.
-template<
-  typename NodeType,
-  typename std::enable_if<has_get_node_topics_interface<
-    typename std::remove_pointer<NodeType>::type,
-    rclcpp::node_interfaces::NodeTopicsInterface *
-  >::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeTopicsInterface *
-get_node_topics_interface_from_pointer(NodeType node_pointer)
-{
-  return node_pointer->get_node_topics_interface();
-}
-
-// Forward shared_ptr's to const node pointer signatures.
-template<
-  typename NodeType,
-  typename std::enable_if<std::is_same<
-    NodeType,
-    typename std::shared_ptr<typename std::remove_pointer<NodeType>::type::element_type> *
-  >::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeTopicsInterface *
-get_node_topics_interface_from_pointer(NodeType node_shared_pointer)
-{
-  return get_node_topics_interface_from_pointer(node_shared_pointer->get());
-}
-
-}  // namespace detail
-
-/// Get the NodeTopicsInterface as a pointer from a pointer to a "Node like" object.
-template<
-  typename NodeType,
-  typename std::enable_if<std::is_pointer<NodeType>::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeTopicsInterface *
-get_node_topics_interface(NodeType node_pointer)
-{
-  // Forward pointers to detail implmentation directly.
-  return detail::get_node_topics_interface_from_pointer(node_pointer);
-}
-
-/// Get the NodeTopicsInterface as a pointer from a "Node like" object.
-template<
-  typename NodeType,
-  typename std::enable_if<
-    !std::is_pointer<typename std::remove_reference<NodeType>::type>::value, int
-  >::type = 0
->
-rclcpp::node_interfaces::NodeTopicsInterface *
-get_node_topics_interface(NodeType && node_reference)
-{
-  // Forward references to detail implmentation as a pointer.
-  return detail::get_node_topics_interface_from_pointer(&node_reference);
-}
-
-}  // namespace node_interfaces
-}  // namespace rclcpp
-
-#endif  // RCLCPP__NODE_INTERFACES__GET_NODE_TOPICS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -50,93 +50,78 @@ public:
   ~NodeBase();
 
   RCLCPP_PUBLIC
-
   const char *
   get_name() const override;
 
   RCLCPP_PUBLIC
-
   const char *
   get_namespace() const override;
 
   RCLCPP_PUBLIC
-
   const char *
   get_fully_qualified_name() const override;
 
   RCLCPP_PUBLIC
-
   rclcpp::Context::SharedPtr
   get_context() override;
 
   RCLCPP_PUBLIC
-
   rcl_node_t *
   get_rcl_node_handle() override;
 
   RCLCPP_PUBLIC
-
   const rcl_node_t *
   get_rcl_node_handle() const override;
 
   RCLCPP_PUBLIC
-
   std::shared_ptr<rcl_node_t>
   get_shared_rcl_node_handle() override;
 
   RCLCPP_PUBLIC
-
   std::shared_ptr<const rcl_node_t>
   get_shared_rcl_node_handle() const override;
 
   RCLCPP_PUBLIC
-
-  bool
-  assert_liveliness() const override;
-
-  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
   get_default_callback_group() override;
 
   RCLCPP_PUBLIC
-
   bool
   callback_group_in_node(rclcpp::CallbackGroup::SharedPtr group) override;
 
   RCLCPP_PUBLIC
-
   const std::vector<rclcpp::CallbackGroup::WeakPtr> &
   get_callback_groups() const override;
 
   RCLCPP_PUBLIC
-
   std::atomic_bool &
   get_associated_with_executor_atomic() override;
 
   RCLCPP_PUBLIC
-
   rcl_guard_condition_t *
   get_notify_guard_condition() override;
 
   RCLCPP_PUBLIC
-
   std::unique_lock<std::recursive_mutex>
   acquire_notify_guard_condition_lock() const override;
 
   RCLCPP_PUBLIC
-
   bool
   get_use_intra_process_default() const override;
 
   bool
   get_enable_topic_statistics_default() const override;
 
+  std::string
+  resolve_topic_or_service_name(
+    const std::string & name, bool is_service, bool only_expand = false) const override;
+
 private:
   RCLCPP_DISABLE_COPY(NodeBase)
 

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -102,17 +102,13 @@ public:
   std::shared_ptr<const rcl_node_t>
   get_shared_rcl_node_handle() const = 0;
 
-  /// Manually assert that this Node is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE).
-  RCLCPP_PUBLIC
-  virtual
-  bool
-  assert_liveliness() const = 0;
-
   /// Create and return a callback group.
   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
@@ -167,6 +163,13 @@ public:
   virtual
   bool
   get_enable_topic_statistics_default() const = 0;
+
+  /// Expand and remap a given topic or service name.
+  RCLCPP_PUBLIC
+  virtual
+  std::string
+  resolve_topic_or_service_name(
+    const std::string & name, bool is_service, bool only_expand = false) const = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

@@ -57,66 +57,60 @@ public:
   ~NodeGraph();
 
   RCLCPP_PUBLIC
-
   std::map<std::string, std::vector<std::string>>
   get_topic_names_and_types(bool no_demangle = false) const override;
 
   RCLCPP_PUBLIC
-
   std::map<std::string, std::vector<std::string>>
   get_service_names_and_types() const override;
 
   RCLCPP_PUBLIC
+  std::map<std::string, std::vector<std::string>>
+  get_service_names_and_types_by_node(
+    const std::string & node_name,
+    const std::string & namespace_) const override;
 
+  RCLCPP_PUBLIC
   std::vector<std::string>
   get_node_names() const override;
 
   RCLCPP_PUBLIC
-
   std::vector<std::pair<std::string, std::string>>
   get_node_names_and_namespaces() const override;
 
   RCLCPP_PUBLIC
-
   size_t
   count_publishers(const std::string & topic_name) const override;
 
   RCLCPP_PUBLIC
-
   size_t
   count_subscribers(const std::string & topic_name) const override;
 
   RCLCPP_PUBLIC
-
   const rcl_guard_condition_t *
   get_graph_guard_condition() const override;
 
   RCLCPP_PUBLIC
-
   void
   notify_graph_change() override;
 
   RCLCPP_PUBLIC
-
   void
   notify_shutdown() override;
 
   RCLCPP_PUBLIC
-
   rclcpp::Event::SharedPtr
   get_graph_event() override;
 
   RCLCPP_PUBLIC
-
   void
   wait_for_graph_change(
     rclcpp::Event::SharedPtr event,
     std::chrono::nanoseconds timeout) override;
 
   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,31 +160,59 @@ 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
   std::map<std::string, std::vector<std::string>>
   get_service_names_and_types() const = 0;
 
+  /// 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
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::map<std::string, std::vector<std::string>>
+  get_service_names_and_types_by_node(
+    const std::string & node_name,
+    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
@@ -249,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
@@ -262,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_logging.hpp

@@ -42,12 +42,10 @@ public:
   ~NodeLogging();
 
   RCLCPP_PUBLIC
-
   rclcpp::Logger
   get_logger() const override;
 
   RCLCPP_PUBLIC
-
   const char *
   get_logger_name() const override;
 

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -21,6 +21,8 @@
 #include <string>
 #include <vector>
 
+#include "rcutils/macros.h"
+
 #include "rcl_interfaces/msg/list_parameters_result.hpp"
 #include "rcl_interfaces/msg/parameter_descriptor.hpp"
 #include "rcl_interfaces/msg/parameter_event.hpp"
@@ -160,6 +162,7 @@ public:
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const override;
 
   RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
   OnSetParametersCallbackHandle::SharedPtr
   add_on_set_parameters_callback(OnParametersSetCallbackType callback) override;
 
@@ -167,10 +170,6 @@ public:
   void
   remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) override;
 
-  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,15 +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.
-  /**
-   * \sa rclcpp::Node::set_on_parameters_set_callback
-   */
-  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_services.hpp

@@ -15,6 +15,8 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_SERVICES_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_SERVICES_HPP_
 
+#include <string>
+
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/client.hpp"
 #include "rclcpp/macros.hpp"
@@ -42,19 +44,21 @@ public:
   ~NodeServices();
 
   RCLCPP_PUBLIC
-
   void
   add_client(
     rclcpp::ClientBase::SharedPtr client_base_ptr,
     rclcpp::CallbackGroup::SharedPtr group) override;
 
   RCLCPP_PUBLIC
-
   void
   add_service(
     rclcpp::ServiceBase::SharedPtr service_base_ptr,
     rclcpp::CallbackGroup::SharedPtr group) override;
 
+  RCLCPP_PUBLIC
+  std::string
+  resolve_service_name(const std::string & name, bool only_expand = false) const override;
+
 private:
   RCLCPP_DISABLE_COPY(NodeServices)
 

rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_timers.hpp

@@ -42,7 +42,6 @@ public:
 
   /// Add a timer to the node.
   RCLCPP_PUBLIC
-
   void
   add_timer(
     rclcpp::TimerBase::SharedPtr timer,

rclcpp/include/rclcpp/node_interfaces/node_topics.hpp

@@ -22,6 +22,7 @@
 
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
 #include "rclcpp/publisher.hpp"
 #include "rclcpp/publisher_factory.hpp"
@@ -39,7 +40,9 @@ public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTopicsInterface)
 
   RCLCPP_PUBLIC
-  explicit NodeTopics(rclcpp::node_interfaces::NodeBaseInterface * node_base);
+  NodeTopics(
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    rclcpp::node_interfaces::NodeTimersInterface * node_timers);
 
   RCLCPP_PUBLIC
   ~NodeTopics() override;
@@ -74,10 +77,19 @@ public:
   rclcpp::node_interfaces::NodeBaseInterface *
   get_node_base_interface() const override;
 
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeTimersInterface *
+  get_node_timers_interface() const override;
+
+  RCLCPP_PUBLIC
+  std::string
+  resolve_topic_name(const std::string & name, bool only_expand = false) const override;
+
 private:
   RCLCPP_DISABLE_COPY(NodeTopics)
 
   rclcpp::node_interfaces::NodeBaseInterface * node_base_;
+  rclcpp::node_interfaces::NodeTimersInterface * node_timers_;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp

@@ -25,6 +25,7 @@
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/publisher.hpp"
 #include "rclcpp/publisher_factory.hpp"
 #include "rclcpp/subscription.hpp"
@@ -80,6 +81,17 @@ public:
   virtual
   rclcpp::node_interfaces::NodeBaseInterface *
   get_node_base_interface() const = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::node_interfaces::NodeTimersInterface *
+  get_node_timers_interface() const = 0;
+
+  /// Get a remapped and expanded topic name given an input name.
+  RCLCPP_PUBLIC
+  virtual
+  std::string
+  resolve_topic_name(const std::string & name, bool only_expand = false) const = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp

@@ -41,14 +41,12 @@ public:
   ~NodeWaitables();
 
   RCLCPP_PUBLIC
-
   void
   add_waitable(
     rclcpp::Waitable::SharedPtr waitable_base_ptr,
     rclcpp::CallbackGroup::SharedPtr group) override;
 
   RCLCPP_PUBLIC
-
   void
   remove_waitable(
     rclcpp::Waitable::SharedPtr waitable_ptr,

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
@@ -77,6 +79,9 @@ public:
    * This data structure is created lazily, on the first call to this function.
    * Repeated calls will not regenerate it unless one of the input settings
    * changed, like arguments, use_global_arguments, or the rcl allocator.
+   *
+   * \return a const rcl_node_options_t structure used by the node
+   * \throws exceptions::UnknownROSArgsError if there are unknown ROS arguments
    */
   RCLCPP_PUBLIC
   const rcl_node_options_t *
@@ -240,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 &
@@ -253,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 &
@@ -324,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.
@@ -356,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.hpp

@@ -83,18 +83,22 @@ public:
   bool
   operator!=(const Parameter & rhs) const;
 
+  /// Get the type of the parameter
   RCLCPP_PUBLIC
   ParameterType
   get_type() const;
 
+  /// Get the type name of the parameter
   RCLCPP_PUBLIC
   std::string
   get_type_name() const;
 
+  /// Get the name of the parameter
   RCLCPP_PUBLIC
   const std::string &
   get_name() const;
 
+  /// Get value of parameter as a parameter message.
   RCLCPP_PUBLIC
   rcl_interfaces::msg::ParameterValue
   get_value_message() const;
@@ -105,6 +109,9 @@ public:
   get_parameter_value() const;
 
   /// Get value of parameter using rclcpp::ParameterType as template argument.
+  /**
+   * \throws rclcpp::exceptions::InvalidParameterTypeException if the type doesn't match
+   */
   template<ParameterType ParamT>
   decltype(auto)
   get_value() const
@@ -117,50 +124,89 @@ public:
   decltype(auto)
   get_value() const;
 
+  /// Get value of parameter as boolean.
+  /**
+   * \throws rclcpp::ParameterTypeException if the type doesn't match
+   */
   RCLCPP_PUBLIC
   bool
   as_bool() const;
 
+  /// Get value of parameter as integer.
+  /**
+   * \throws rclcpp::ParameterTypeException if the type doesn't match
+   */
   RCLCPP_PUBLIC
   int64_t
   as_int() const;
 
+  /// Get value of parameter as double.
+  /**
+   * \throws rclcpp::ParameterTypeException if the type doesn't match
+   */
   RCLCPP_PUBLIC
   double
   as_double() const;
 
+  /// Get value of parameter as string.
+  /**
+   * \throws rclcpp::ParameterTypeException if the type doesn't match
+   */
   RCLCPP_PUBLIC
   const std::string &
   as_string() const;
 
+  /// Get value of parameter as byte array (vector<uint8_t>).
+  /**
+   * \throws rclcpp::ParameterTypeException if the type doesn't match
+   */
   RCLCPP_PUBLIC
   const std::vector<uint8_t> &
   as_byte_array() const;
 
+  /// Get value of parameter as bool array (vector<bool>).
+  /**
+   * \throws rclcpp::ParameterTypeException if the type doesn't match
+   */
   RCLCPP_PUBLIC
   const std::vector<bool> &
   as_bool_array() const;
 
+  /// Get value of parameter as integer array (vector<int64_t>).
+  /**
+   * \throws rclcpp::ParameterTypeException if the type doesn't match
+   */
   RCLCPP_PUBLIC
   const std::vector<int64_t> &
   as_integer_array() const;
 
+  /// Get value of parameter as double array (vector<double>).
+  /**
+   * \throws rclcpp::ParameterTypeException if the type doesn't match
+   */
   RCLCPP_PUBLIC
   const std::vector<double> &
   as_double_array() const;
 
+  /// Get value of parameter as string array (vector<std::string>).
+  /**
+   * \throws rclcpp::ParameterTypeException if the type doesn't match
+   */
   RCLCPP_PUBLIC
   const std::vector<std::string> &
   as_string_array() const;
 
+  /// Convert a parameter message in a Parameter class object.
   RCLCPP_PUBLIC
   static Parameter
   from_parameter_msg(const rcl_interfaces::msg::Parameter & parameter);
 
+  /// Convert the class in a parameter message.
   RCLCPP_PUBLIC
   rcl_interfaces::msg::Parameter
   to_parameter_msg() const;
 
+  /// Get value of parameter as a string.
   RCLCPP_PUBLIC
   std::string
   value_to_string() const;

rclcpp/include/rclcpp/parameter_client.hpp

@@ -46,6 +46,16 @@ class AsyncParametersClient
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(AsyncParametersClient)
 
+  /// Create an async parameters client.
+  /**
+   * \param[in] node_base_interface The node base interface of the corresponding node.
+   * \param[in] node_topics_interface Node topic base interface.
+   * \param[in] node_graph_interface The node graph interface of the corresponding node.
+   * \param[in] node_services_interface Node service interface.
+   * \param[in] remote_node_name (optional) name of the remote node
+   * \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
   AsyncParametersClient(
     const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
@@ -56,19 +66,51 @@ public:
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters,
     rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
-  RCLCPP_PUBLIC
+  /// Constructor
+  /**
+   * \param[in] node The async paramters client will be added to this node.
+   * \param[in] remote_node_name (optional) name of the remote node
+   * \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.
+   */
+  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)
+  {}
 
-  RCLCPP_PUBLIC
+  /// Constructor
+  /**
+   * \param[in] node The  async paramters client will be added to this node.
+   * \param[in] remote_node_name (optional) name of the remote node
+   * \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.
+   */
+  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>>
@@ -153,16 +195,32 @@ public:
   {
     return rclcpp::create_subscription<rcl_interfaces::msg::ParameterEvent>(
       node,
-      "parameter_events",
+      "/parameter_events",
       qos,
       std::forward<CallbackT>(callback),
       options);
   }
 
+  /// Return if the parameter services are ready.
+  /**
+   * This method checks the following services:
+   *  - get parameter
+   *  - get parameter
+   *  - set parameters
+   *  - list parameters
+   *  - describe parameters
+   *
+   * \return `true` if the service is ready, `false` otherwise
+   */
   RCLCPP_PUBLIC
   bool
   service_is_ready() const;
 
+  /// Wait for the services to be ready.
+  /**
+   * \param timeout maximum time to wait
+   * \return `true` if the services are ready and the timeout is not over, `false` otherwise
+   */
   template<typename RepT = int64_t, typename RatioT = std::milli>
   bool
   wait_for_service(
@@ -197,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(
@@ -231,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

@@ -59,6 +59,17 @@ public:
 
   RCLCPP_SMART_PTR_DEFINITIONS(Publisher<MessageT, AllocatorT>)
 
+  /// Default constructor.
+  /**
+   * The constructor for a Publisher is almost never called directly.
+   * Instead, subscriptions should be instantiated through the function
+   * rclcpp::create_publisher().
+   *
+   * \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.
+   */
   Publisher(
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
     const std::string & topic,
@@ -217,6 +228,12 @@ public:
     return this->do_serialized_publish(&serialized_msg);
   }
 
+  void
+  publish(const SerializedMessage & serialized_msg)
+  {
+    return this->do_serialized_publish(&serialized_msg.get_rcl_serialized_message());
+  }
+
   /// Publish an instance of a LoanedMessage.
   /**
    * When publishing a loaned message, the memory for this ROS message will be deallocated
@@ -262,12 +279,12 @@ protected:
   void
   do_inter_process_publish(const MessageT & msg)
   {
-    auto status = rcl_publish(&publisher_handle_, &msg, nullptr);
+    auto status = rcl_publish(publisher_handle_.get(), &msg, nullptr);
 
     if (RCL_RET_PUBLISHER_INVALID == status) {
       rcl_reset_error();  // next call will reset error message if not context
-      if (rcl_publisher_is_valid_except_context(&publisher_handle_)) {
-        rcl_context_t * context = rcl_publisher_get_context(&publisher_handle_);
+      if (rcl_publisher_is_valid_except_context(publisher_handle_.get())) {
+        rcl_context_t * context = rcl_publisher_get_context(publisher_handle_.get());
         if (nullptr != context && !rcl_context_is_valid(context)) {
           // publisher is invalid due to context being shutdown
           return;
@@ -286,7 +303,7 @@ protected:
       // TODO(Karsten1987): support serialized message passed by intraprocess
       throw std::runtime_error("storing serialized messages in intra process is not supported yet");
     }
-    auto status = rcl_publish_serialized_message(&publisher_handle_, serialized_msg, nullptr);
+    auto status = rcl_publish_serialized_message(publisher_handle_.get(), serialized_msg, nullptr);
     if (RCL_RET_OK != status) {
       rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish serialized message");
     }
@@ -295,12 +312,12 @@ protected:
   void
   do_loaned_message_publish(MessageT * msg)
   {
-    auto status = rcl_publish_loaned_message(&publisher_handle_, msg, nullptr);
+    auto status = rcl_publish_loaned_message(publisher_handle_.get(), msg, nullptr);
 
     if (RCL_RET_PUBLISHER_INVALID == status) {
       rcl_reset_error();  // next call will reset error message if not context
-      if (rcl_publisher_is_valid_except_context(&publisher_handle_)) {
-        rcl_context_t * context = rcl_publisher_get_context(&publisher_handle_);
+      if (rcl_publisher_is_valid_except_context(publisher_handle_.get())) {
+        rcl_context_t * context = rcl_publisher_get_context(publisher_handle_.get());
         if (nullptr != context && !rcl_context_is_valid(context)) {
           // publisher is invalid due to context being shutdown
           return;

rclcpp/include/rclcpp/publisher_base.hpp

@@ -99,13 +99,13 @@ public:
   /// Get the rcl publisher handle.
   /** \return The rcl publisher handle. */
   RCLCPP_PUBLIC
-  rcl_publisher_t *
+  std::shared_ptr<rcl_publisher_t>
   get_publisher_handle();
 
   /// Get the rcl publisher handle.
   /** \return The rcl publisher handle. */
   RCLCPP_PUBLIC
-  const rcl_publisher_t *
+  std::shared_ptr<const rcl_publisher_t>
   get_publisher_handle() const;
 
   /// Get all the QoS event handlers associated with this publisher.
@@ -200,10 +200,11 @@ protected:
     const EventCallbackT & callback,
     const rcl_publisher_event_type_t event_type)
   {
-    auto handler = std::make_shared<QOSEventHandler<EventCallbackT>>(
+    auto handler = std::make_shared<QOSEventHandler<EventCallbackT,
+        std::shared_ptr<rcl_publisher_t>>>(
       callback,
       rcl_publisher_event_init,
-      &publisher_handle_,
+      publisher_handle_,
       event_type);
     event_handlers_.emplace_back(handler);
   }
@@ -213,7 +214,7 @@ protected:
 
   std::shared_ptr<rcl_node_t> rcl_node_handle_;
 
-  rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher();
+  std::shared_ptr<rcl_publisher_t> publisher_handle_;
 
   std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
 

rclcpp/include/rclcpp/publisher_options.hpp

@@ -71,7 +71,7 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
   rcl_publisher_options_t
   to_rcl_publisher_options(const rclcpp::QoS & qos) const
   {
-    rcl_publisher_options_t result;
+    rcl_publisher_options_t result = rcl_publisher_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());

rclcpp/include/rclcpp/qos.hpp

@@ -19,6 +19,7 @@
 
 #include "rclcpp/duration.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rcl/logging_rosout.h"
 #include "rmw/incompatible_qos_events_statuses.h"
 #include "rmw/qos_profiles.h"
 #include "rmw/types.h"
@@ -26,6 +27,7 @@
 namespace rclcpp
 {
 
+RCLCPP_PUBLIC
 std::string qos_policy_name_from_kind(rmw_qos_policy_kind_t policy_kind);
 
 /// QoS initialization values, cannot be created directly, use KeepAll or KeepLast instead.
@@ -161,6 +163,38 @@ 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,
+ *    - Depth: 5,
+ *    - Reliability: Best effort,
+ *    - Durability: Volatile,
+ *    - Deadline: Default,
+ *    - Lifespan: Default,
+ *    - Liveliness: System default,
+ *    - Liveliness lease duration: default,
+ *    - avoid ros namespace conventions: false
+ */
 class RCLCPP_PUBLIC SensorDataQoS : public QoS
 {
 public:
@@ -171,6 +205,18 @@ public:
   ));
 };
 
+/**
+ * Parameters QoS class
+ *    - History: Keep last,
+ *    - Depth: 1000,
+ *    - Reliability: Reliable,
+ *    - Durability: Volatile,
+ *    - Deadline: Default,
+ *    - Lifespan: Default,
+ *    - Liveliness: System default,
+ *    - Liveliness lease duration: default,
+ *    - Avoid ros namespace conventions: false
+ */
 class RCLCPP_PUBLIC ParametersQoS : public QoS
 {
 public:
@@ -181,6 +227,18 @@ public:
   ));
 };
 
+/**
+ * Services QoS class
+ *    - History: Keep last,
+ *    - Depth: 10,
+ *    - Reliability: Reliable,
+ *    - Durability: Volatile,
+ *    - Deadline: Default,
+ *    - Lifespan: Default,
+ *    - Liveliness: System default,
+ *    - Liveliness lease duration: default,
+ *    - Avoid ros namespace conventions: false
+ */
 class RCLCPP_PUBLIC ServicesQoS : public QoS
 {
 public:
@@ -191,6 +249,18 @@ public:
   ));
 };
 
+/**
+ * Parameter events QoS class
+ *    - History: Keep last,
+ *    - Depth: 1000,
+ *    - Reliability: Reliable,
+ *    - Durability: Volatile,
+ *    - Deadline: Default,
+ *    - Lifespan: Default,
+ *    - Liveliness: System default,
+ *    - Liveliness lease duration: default,
+ *    - Avoid ros namespace conventions: false
+ */
 class RCLCPP_PUBLIC ParameterEventsQoS : public QoS
 {
 public:
@@ -201,6 +271,40 @@ 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,
+ *    - Depth: System default,
+ *    - Reliability: System default,
+ *    - Durability: System default,
+ *    - Deadline: Default,
+ *    - Lifespan: Default,
+ *    - Liveliness: System default,
+ *    - Liveliness lease duration: System default,
+ *    - Avoid ros namespace conventions: false
+ */
 class RCLCPP_PUBLIC SystemDefaultsQoS : public QoS
 {
 public:

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
@@ -102,11 +105,11 @@ protected:
   size_t wait_set_event_index_;
 };
 
-template<typename EventCallbackT>
+template<typename EventCallbackT, typename ParentHandleT>
 class QOSEventHandler : public QOSEventHandlerBase
 {
 public:
-  template<typename InitFuncT, typename ParentHandleT, typename EventTypeEnum>
+  template<typename InitFuncT, typename EventTypeEnum>
   QOSEventHandler(
     const EventCallbackT & callback,
     InitFuncT init_func,
@@ -114,8 +117,9 @@ public:
     EventTypeEnum event_type)
   : event_callback_(callback)
   {
+    parent_handle_ = parent_handle;
     event_handle_ = rcl_get_zero_initialized_event();
-    rcl_ret_t ret = init_func(&event_handle_, parent_handle, event_type);
+    rcl_ret_t ret = init_func(&event_handle_, parent_handle.get(), event_type);
     if (ret != RCL_RET_OK) {
       if (ret == RCL_RET_UNSUPPORTED) {
         UnsupportedEventTypeException exc(ret, rcl_get_error_state(), "Failed to initialize event");
@@ -148,6 +152,7 @@ private:
   using EventCallbackInfoT = typename std::remove_reference<typename
       rclcpp::function_traits::function_traits<EventCallbackT>::template argument_type<0>>::type;
 
+  ParentHandleT parent_handle_;
   EventCallbackT event_callback_;
 };
 

rclcpp/include/rclcpp/rate.hpp

@@ -31,6 +31,7 @@ class RateBase
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(RateBase)
 
+  virtual ~RateBase() {}
   virtual bool sleep() = 0;
   virtual bool is_steady() const = 0;
   virtual void reset() = 0;

rclcpp/include/rclcpp/rclcpp.hpp

@@ -81,7 +81,7 @@
  *   - rclcpp/executors/multi_threaded_executor.hpp
  * - CallbackGroups (mechanism for enforcing concurrency rules for callbacks):
  *   - rclcpp::Node::create_callback_group()
- *   - rclcpp::callback_group::CallbackGroup
+ *   - rclcpp::CallbackGroup
  *   - rclcpp/callback_group.hpp
  *
  * Additionally, there are some methods for introspecting the ROS graph:
@@ -128,6 +128,7 @@
  *   - rclcpp/context.hpp
  *   - rclcpp/contexts/default_context.hpp
  * - Various utilities:
+ *   - rclcpp/duration.hpp
  *   - rclcpp/function_traits.hpp
  *   - rclcpp/macros.hpp
  *   - rclcpp/scope_exit.hpp

rclcpp/include/rclcpp/serialization.hpp

@@ -39,10 +39,6 @@ template<typename T>
 struct is_serialized_message_class : std::false_type
 {};
 
-template<>
-struct is_serialized_message_class<rcl_serialized_message_t>: std::true_type
-{};
-
 template<>
 struct is_serialized_message_class<SerializedMessage>: std::true_type
 {};
@@ -64,7 +60,7 @@ public:
 
   /// Serialize a ROS2 message to a serialized stream
   /**
-   * \param[in] message The ROS2 message which is read and serialized by rmw.
+   * \param[in] ros_message The ROS2 message which is read and serialized by rmw.
    * \param[out] serialized_message The serialized message.
    */
   void serialize_message(
@@ -73,7 +69,7 @@ public:
   /// Deserialize a serialized stream to a ROS message
   /**
    * \param[in] serialized_message The serialized message to be converted to ROS2 by rmw.
-   * \param[out] message The deserialized ROS2 message.
+   * \param[out] ros_message The deserialized ROS2 message.
    */
   void deserialize_message(
     const SerializedMessage * serialized_message, void * ros_message) const;

rclcpp/include/rclcpp/serialized_message.hpp

@@ -98,6 +98,20 @@ public:
    */
   size_t capacity() const;
 
+  /// Allocate memory in the data buffer
+  /**
+   * The data buffer of the underlying rcl_serialized_message_t will be resized.
+   * This might change the data layout and invalidates all pointers to the data.
+   */
+  void reserve(size_t capacity);
+
+  /// Release the underlying rcl_serialized_message_t
+  /**
+   * The memory (i.e. the data buffer) of the serialized message will no longer
+   * be managed by this instance and the memory won't be deallocated on destruction.
+   */
+  rcl_serialized_message_t release_rcl_serialized_message();
+
 private:
   rcl_serialized_message_t serialized_message_;
 };

rclcpp/include/rclcpp/service.hpp

@@ -50,14 +50,26 @@ public:
   RCLCPP_PUBLIC
   virtual ~ServiceBase();
 
+  /// Return the name of the service.
+  /** \return The name of the service. */
   RCLCPP_PUBLIC
   const char *
   get_service_name();
 
+  /// Return the rcl_service_t service handle in a std::shared_ptr.
+  /**
+   * This handle remains valid after the Service is destroyed.
+   * The actual rcl service is not finalized until it is out of scope everywhere.
+   */
   RCLCPP_PUBLIC
   std::shared_ptr<rcl_service_t>
   get_service_handle();
 
+  /// Return the rcl_service_t service handle in a std::shared_ptr.
+  /**
+   * This handle remains valid after the Service is destroyed.
+   * The actual rcl service is not finalized until it is out of scope everywhere.
+   */
   RCLCPP_PUBLIC
   std::shared_ptr<const rcl_service_t>
   get_service_handle() const;
@@ -144,6 +156,17 @@ public:
       std::shared_ptr<typename ServiceT::Response>)>;
   RCLCPP_SMART_PTR_DEFINITIONS(Service)
 
+  /// Default constructor.
+  /**
+   * The constructor for a Service is almost never called directly.
+   * Instead, services should be instantiated through the function
+   * rclcpp::create_service().
+   *
+   * \param[in] node_handle NodeBaseInterface pointer that is used in part of the setup.
+   * \param[in] service_name Name of the topic to publish to.
+   * \param[in] any_callback User defined callback to call when a client request is received.
+   * \param[in] service_options options for the subscription.
+   */
   Service(
     std::shared_ptr<rcl_node_t> node_handle,
     const std::string & service_name,
@@ -200,13 +223,23 @@ 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
   }
 
+  /// Default constructor.
+  /**
+   * The constructor for a Service is almost never called directly.
+   * Instead, services should be instantiated through the function
+   * rclcpp::create_service().
+   *
+   * \param[in] node_handle NodeBaseInterface pointer that is used in part of the setup.
+   * \param[in] service_handle service handle.
+   * \param[in] any_callback User defined callback to call when a client request is received.
+   */
   Service(
     std::shared_ptr<rcl_node_t> node_handle,
     std::shared_ptr<rcl_service_t> service_handle,
@@ -225,13 +258,23 @@ 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
   }
 
+  /// Default constructor.
+  /**
+   * The constructor for a Service is almost never called directly.
+   * Instead, services should be instantiated through the function
+   * rclcpp::create_service().
+   *
+   * \param[in] node_handle NodeBaseInterface pointer that is used in part of the setup.
+   * \param[in] service_handle service handle.
+   * \param[in] any_callback User defined callback to call when a client request is received.
+   */
   Service(
     std::shared_ptr<rcl_node_t> node_handle,
     rcl_service_t * service_handle,
@@ -252,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

@@ -18,6 +18,7 @@
 #include <rmw/error_handling.h>
 #include <rmw/rmw.h>
 
+#include <chrono>
 #include <functional>
 #include <iostream>
 #include <memory>
@@ -25,7 +26,6 @@
 #include <string>
 #include <utility>
 
-
 #include "rcl/error_handling.h"
 #include "rcl/subscription.h"
 
@@ -47,6 +47,7 @@
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rclcpp/waitable.hpp"
+#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp"
 #include "tracetools/tracetools.h"
 
 namespace rclcpp
@@ -75,6 +76,8 @@ public:
   using MessageDeleter = allocator::Deleter<MessageAllocator, CallbackMessageT>;
   using ConstMessageSharedPtr = std::shared_ptr<const CallbackMessageT>;
   using MessageUniquePtr = std::unique_ptr<CallbackMessageT, MessageDeleter>;
+  using SubscriptionTopicStatisticsSharedPtr =
+    std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>>;
 
   RCLCPP_SMART_PTR_DEFINITIONS(Subscription)
 
@@ -87,9 +90,14 @@ public:
    * \param[in] node_base NodeBaseInterface pointer that is used in part 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] 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
+   *   of the following conditions are true: qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_ALL,
+   *   qos_profile.depth == 0 or qos_profile.durability != RMW_QOS_POLICY_DURABILITY_VOLATILE).
    */
   Subscription(
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
@@ -98,7 +106,8 @@ public:
     const rclcpp::QoS & qos,
     AnySubscriptionCallback<CallbackMessageT, AllocatorT> callback,
     const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
-    typename MessageMemoryStrategyT::SharedPtr message_memory_strategy)
+    typename MessageMemoryStrategyT::SharedPtr message_memory_strategy,
+    SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics = nullptr)
   : SubscriptionBase(
       node_base,
       type_support_handle,
@@ -135,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)) {
@@ -170,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;
@@ -180,14 +194,18 @@ public:
       this->setup_intra_process(intra_process_subscription_id, ipm);
     }
 
+    if (subscription_topic_statistics != nullptr) {
+      this->subscription_topic_statistics_ = std::move(subscription_topic_statistics);
+    }
+
     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.
@@ -242,7 +260,7 @@ public:
     return message_memory_strategy_->borrow_message();
   }
 
-  std::shared_ptr<rcl_serialized_message_t>
+  std::shared_ptr<rclcpp::SerializedMessage>
   create_serialized_message() override
   {
     return message_memory_strategy_->borrow_serialized_message();
@@ -260,6 +278,13 @@ public:
     }
     auto typed_message = std::static_pointer_cast<CallbackMessageT>(message);
     any_callback_.dispatch(typed_message, message_info);
+
+    if (subscription_topic_statistics_) {
+      const auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(
+        std::chrono::system_clock::now());
+      const auto time = rclcpp::Time(nanos.time_since_epoch().count());
+      subscription_topic_statistics_->handle_message(*typed_message, time);
+    }
   }
 
   void
@@ -275,7 +300,9 @@ public:
   }
 
   /// Return the borrowed message.
-  /** \param message message to be returned */
+  /**
+   * \param[inout] message message to be returned
+   */
   void
   return_message(std::shared_ptr<void> & message) override
   {
@@ -283,8 +310,12 @@ public:
     message_memory_strategy_->return_message(typed_message);
   }
 
+  /// Return the borrowed serialized message.
+  /**
+   * \param[inout] message serialized message to be returned
+   */
   void
-  return_serialized_message(std::shared_ptr<rcl_serialized_message_t> & message) override
+  return_serialized_message(std::shared_ptr<rclcpp::SerializedMessage> & message) override
   {
     message_memory_strategy_->return_serialized_message(message);
   }
@@ -307,6 +338,8 @@ private:
   const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> options_;
   typename message_memory_strategy::MessageMemoryStrategy<CallbackMessageT, AllocatorT>::SharedPtr
     message_memory_strategy_;
+  /// Component which computes and publishes topic statistics for this subscriber
+  SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics_{nullptr};
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/subscription_base.hpp

@@ -33,6 +33,7 @@
 #include "rclcpp/message_info.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/qos_event.hpp"
+#include "rclcpp/serialized_message.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -65,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
@@ -90,7 +91,7 @@ public:
   get_subscription_handle();
 
   RCLCPP_PUBLIC
-  const std::shared_ptr<rcl_subscription_t>
+  std::shared_ptr<const rcl_subscription_t>
   get_subscription_handle() const;
 
   /// Get all the QoS event handlers associated with this subscription.
@@ -109,6 +110,7 @@ public:
    * May throw runtime_error when an unexpected error occurs.
    *
    * \return The actual qos settings.
+   * \throws std::runtime_error if failed to get qos settings
    */
   RCLCPP_PUBLIC
   rclcpp::QoS
@@ -151,7 +153,7 @@ public:
    */
   RCLCPP_PUBLIC
   bool
-  take_serialized(rcl_serialized_message_t & message_out, rclcpp::MessageInfo & message_info_out);
+  take_serialized(rclcpp::SerializedMessage & message_out, rclcpp::MessageInfo & message_info_out);
 
   /// Borrow a new message.
   /** \return Shared pointer to the fresh message. */
@@ -164,7 +166,7 @@ public:
   /** \return Shared pointer to a rcl_message_serialized_t. */
   RCLCPP_PUBLIC
   virtual
-  std::shared_ptr<rcl_serialized_message_t>
+  std::shared_ptr<rclcpp::SerializedMessage>
   create_serialized_message() = 0;
 
   /// Check if we need to handle the message, and execute the callback if we do.
@@ -194,12 +196,16 @@ public:
   RCLCPP_PUBLIC
   virtual
   void
-  return_serialized_message(std::shared_ptr<rcl_serialized_message_t> & message) = 0;
+  return_serialized_message(std::shared_ptr<rclcpp::SerializedMessage> & message) = 0;
 
   RCLCPP_PUBLIC
   const rosidl_message_type_support_t &
   get_message_type_support_handle() const;
 
+  /// Return if the subscription is serialized
+  /**
+   * \return `true` if the subscription is serialized, `false` otherwise
+   */
   RCLCPP_PUBLIC
   bool
   is_serialized() const;
@@ -231,7 +237,11 @@ public:
     uint64_t intra_process_subscription_id,
     IntraProcessManagerWeakPtr weak_ipm);
 
-  /// Return the waitable for intra-process, or nullptr if intra-process is not setup.
+  /// Return the waitable for intra-process
+  /**
+   * \return the waitable sharedpointer for intra-process, or nullptr if intra-process is not setup.
+   * \throws std::runtime_error if the intra process manager is destroyed
+   */
   RCLCPP_PUBLIC
   rclcpp::Waitable::SharedPtr
   get_intra_process_waitable() const;
@@ -260,10 +270,11 @@ protected:
     const EventCallbackT & callback,
     const rcl_subscription_event_type_t event_type)
   {
-    auto handler = std::make_shared<QOSEventHandler<EventCallbackT>>(
+    auto handler = std::make_shared<QOSEventHandler<EventCallbackT,
+        std::shared_ptr<rcl_subscription_t>>>(
       callback,
       rcl_subscription_event_init,
-      get_subscription_handle().get(),
+      get_subscription_handle(),
       event_type);
     qos_events_in_use_by_wait_set_.insert(std::make_pair(handler.get(), false));
     event_handlers_.emplace_back(handler);

rclcpp/include/rclcpp/subscription_factory.hpp

@@ -32,6 +32,7 @@
 #include "rclcpp/subscription_options.hpp"
 #include "rclcpp/subscription_traits.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp"
 
 namespace rclcpp
 {
@@ -63,6 +64,12 @@ struct SubscriptionFactory
 };
 
 /// Return a SubscriptionFactory setup to create a SubscriptionT<MessageT, AllocatorT>.
+/**
+ * \param[in] callback The user-defined callback function to receive a message
+ * \param[in] options Additional options for the creation of the Subscription.
+ * \param[in] msg_mem_strat The message memory strategy to use for allocating messages.
+ * \param[in] subscription_topic_stats Optional stats callback for topic_statistics
+ */
 template<
   typename MessageT,
   typename CallbackT,
@@ -78,7 +85,10 @@ SubscriptionFactory
 create_subscription_factory(
   CallbackT && callback,
   const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
-  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat)
+  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat,
+  std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>>
+  subscription_topic_stats = nullptr
+)
 {
   auto allocator = options.get_allocator();
 
@@ -88,7 +98,7 @@ create_subscription_factory(
 
   SubscriptionFactory factory {
     // factory function that creates a MessageT specific SubscriptionT
-    [options, msg_mem_strat, any_subscription_callback](
+    [options, msg_mem_strat, any_subscription_callback, subscription_topic_stats](
       rclcpp::node_interfaces::NodeBaseInterface * node_base,
       const std::string & topic_name,
       const rclcpp::QoS & qos
@@ -104,7 +114,8 @@ create_subscription_factory(
         qos,
         any_subscription_callback,
         options,
-        msg_mem_strat);
+        msg_mem_strat,
+        subscription_topic_stats);
       // This is used for setting up things like intra process comms which
       // require this->shared_from_this() which cannot be called from
       // the constructor.

rclcpp/include/rclcpp/subscription_options.hpp

@@ -66,7 +66,8 @@ struct SubscriptionOptionsBase
     // Topic to which topic statistics get published when enabled. Defaults to /statistics.
     std::string publish_topic = "/statistics";
 
-    // Topic statistics publication period in ms. Defaults to one minute.
+    // Topic statistics publication period in ms. Defaults to one second.
+    // Only values greater than zero are allowed.
     std::chrono::milliseconds publish_period{std::chrono::seconds(1)};
   };
 
@@ -89,6 +90,10 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
   {}
 
   /// Convert this class, with a rclcpp::QoS, into an rcl_subscription_options_t.
+  /**
+   * \param qos QoS profile for subcription.
+   * \return rcl_subscription_options_t structure based on the rclcpp::QoS
+   */
   template<typename MessageT>
   rcl_subscription_options_t
   to_rcl_subscription_options(const rclcpp::QoS & qos) const

rclcpp/include/rclcpp/subscription_traits.hpp

@@ -42,15 +42,6 @@ template<typename T>
 struct is_serialized_subscription_argument : std::false_type
 {};
 
-template<>
-struct is_serialized_subscription_argument<rcl_serialized_message_t>: std::true_type
-{};
-
-template<>
-struct is_serialized_subscription_argument<std::shared_ptr<rcl_serialized_message_t>>
-  : std::true_type
-{};
-
 template<>
 struct is_serialized_subscription_argument<SerializedMessage>: std::true_type
 {};

rclcpp/include/rclcpp/time.hpp

@@ -31,37 +31,78 @@ class Clock;
 class Time
 {
 public:
+  /// Time constructor
+  /**
+   * Initializes the time values for seconds and nanoseconds individually.
+   * Large values for nanoseconds are wrapped automatically with the remainder added to seconds.
+   * Both inputs must be integers.
+   *
+   * \param seconds part of the time in seconds since time epoch
+   * \param nanoseconds part of the time in nanoseconds since time epoch
+   * \param clock_type clock type
+   * \throws std::runtime_error if seconds are negative
+   */
   RCLCPP_PUBLIC
   Time(int32_t seconds, uint32_t nanoseconds, rcl_clock_type_t clock_type = RCL_SYSTEM_TIME);
 
+  /// Time constructor
+  /**
+   * \param nanoseconds since time epoch
+   * \param clock_type clock type
+   */
   RCLCPP_PUBLIC
-  explicit Time(int64_t nanoseconds = 0, rcl_clock_type_t clock = RCL_SYSTEM_TIME);
+  explicit Time(int64_t nanoseconds = 0, rcl_clock_type_t clock_type = RCL_SYSTEM_TIME);
 
+  /// Copy constructor
   RCLCPP_PUBLIC
   Time(const Time & rhs);
 
+  /// Time constructor
+  /**
+   * \param time_msg builtin_interfaces time message to copy
+   * \param clock_type clock type
+   * \throws std::runtime_error if seconds are negative
+   */
   RCLCPP_PUBLIC
   Time(
     const builtin_interfaces::msg::Time & time_msg,
-    rcl_clock_type_t ros_time = RCL_ROS_TIME);
+    rcl_clock_type_t clock_type = RCL_ROS_TIME);
 
+  /// Time constructor
+  /**
+   * \param time_point rcl_time_point_t structure to copy
+   */
   RCLCPP_PUBLIC
   explicit Time(const rcl_time_point_t & time_point);
 
+  /// Time destructor
   RCLCPP_PUBLIC
   virtual ~Time();
 
+  /// Return a builtin_interfaces::msg::Time object based
   RCLCPP_PUBLIC
   operator builtin_interfaces::msg::Time() const;
 
+  /**
+   * \throws std::runtime_error if seconds are negative
+   */
   RCLCPP_PUBLIC
   Time &
   operator=(const Time & rhs);
 
+  /**
+   * Assign Time from a builtin_interfaces::msg::Time instance.
+   * The clock_type will be reset to RCL_ROS_TIME.
+   * Equivalent to *this = Time(time_msg, RCL_ROS_TIME).
+   * \throws std::runtime_error if seconds are negative
+   */
   RCLCPP_PUBLIC
   Time &
   operator=(const builtin_interfaces::msg::Time & time_msg);
 
+  /**
+   * \throws std::runtime_error if the time sources are different
+   */
   RCLCPP_PUBLIC
   bool
   operator==(const rclcpp::Time & rhs) const;
@@ -70,57 +111,101 @@ public:
   bool
   operator!=(const rclcpp::Time & rhs) const;
 
+  /**
+   * \throws std::runtime_error if the time sources are different
+   */
   RCLCPP_PUBLIC
   bool
   operator<(const rclcpp::Time & rhs) const;
 
+  /**
+   * \throws std::runtime_error if the time sources are different
+   */
   RCLCPP_PUBLIC
   bool
   operator<=(const rclcpp::Time & rhs) const;
 
+  /**
+   * \throws std::runtime_error if the time sources are different
+   */
   RCLCPP_PUBLIC
   bool
   operator>=(const rclcpp::Time & rhs) const;
 
+  /**
+   * \throws std::runtime_error if the time sources are different
+   */
   RCLCPP_PUBLIC
   bool
   operator>(const rclcpp::Time & rhs) const;
 
+  /**
+   * \throws std::overflow_error if addition leads to overflow
+   */
   RCLCPP_PUBLIC
   Time
   operator+(const rclcpp::Duration & rhs) const;
 
+  /**
+   * \throws std::runtime_error if the time sources are different
+   * \throws std::overflow_error if addition leads to overflow
+   */
   RCLCPP_PUBLIC
   Duration
   operator-(const rclcpp::Time & rhs) const;
 
+  /**
+   * \throws std::overflow_error if addition leads to overflow
+   */
   RCLCPP_PUBLIC
   Time
   operator-(const rclcpp::Duration & rhs) const;
 
+  /**
+   * \throws std::overflow_error if addition leads to overflow
+   */
   RCLCPP_PUBLIC
   Time &
   operator+=(const rclcpp::Duration & rhs);
 
+  /**
+   * \throws std::overflow_error if addition leads to overflow
+   */
   RCLCPP_PUBLIC
   Time &
   operator-=(const rclcpp::Duration & rhs);
 
+  /// Get the nanoseconds since epoch
+  /**
+   * \return the nanoseconds since epoch as a rcl_time_point_value_t structure.
+   */
   RCLCPP_PUBLIC
   rcl_time_point_value_t
   nanoseconds() const;
 
+  /// Get the maximum representable value.
+  /**
+   * \return the maximum representable value
+   */
   RCLCPP_PUBLIC
   static Time
   max();
 
-  /// \return the seconds since epoch as a floating point number.
-  /// \warning Depending on sizeof(double) there could be significant precision loss.
-  /// When an exact time is required use nanoseconds() instead.
+  /// Get the seconds since epoch
+  /**
+   * \warning Depending on sizeof(double) there could be significant precision loss.
+   * When an exact time is required use nanoseconds() instead.
+   *
+   * \return the seconds since epoch as a floating point number.
+   */
   RCLCPP_PUBLIC
   double
   seconds() const;
 
+  /// Get the clock type
+  /**
+   * \return the clock type
+   */
   RCLCPP_PUBLIC
   rcl_clock_type_t
   get_clock_type() const;
@@ -130,6 +215,10 @@ private:
   friend Clock;  // Allow clock to manipulate internal data
 };
 
+/**
+ * \throws std::overflow_error if addition leads to overflow
+ */
+RCLCPP_PUBLIC
 Time
 operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs);
 

rclcpp/include/rclcpp/time_source.hpp

@@ -35,15 +35,45 @@ class Clock;
 class TimeSource
 {
 public:
+  /// Constructor
+  /**
+   * 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.
+  /**
+   * \param node std::shared pointer to a initialized node
+   */
   RCLCPP_PUBLIC
   void attachNode(rclcpp::Node::SharedPtr node);
 
+  /// Attack node to the time source.
+  /**
+   * If the parameter `use_sim_time` is `true` then the source time is the simulation time,
+   * otherwise the source time is defined by the system.
+   *
+   * \param node_base_interface Node base interface.
+   * \param node_topics_interface Node topic base interface.
+   * \param node_graph_interface Node graph interface.
+   * \param node_services_interface Node service interface.
+   * \param node_logging_interface Node logging interface.
+   * \param node_clock_interface Node clock interface.
+   * \param node_parameters_interface Node parameters interface.
+   */
   RCLCPP_PUBLIC
   void attachNode(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
@@ -54,19 +84,23 @@ public:
     rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface,
     rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface);
 
+  /// Detach the node from the time source
   RCLCPP_PUBLIC
   void detachNode();
 
   /// Attach a clock to the time source to be updated
   /**
-   * \throws std::invalid_argument if node is nullptr
+   * \param[in] clock to attach to the time source
+   * \throws std::invalid_argument the time source must be a RCL_ROS_TIME otherwise throws an exception
    */
   RCLCPP_PUBLIC
   void attachClock(rclcpp::Clock::SharedPtr clock);
 
+  /// Detach a clock to the time source
   RCLCPP_PUBLIC
   void detachClock(rclcpp::Clock::SharedPtr clock);
 
+  /// TimeSource Destructor
   RCLCPP_PUBLIC
   ~TimeSource();
 
@@ -83,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
@@ -117,8 +154,6 @@ private:
   void disable_ros_time();
 
   // Internal helper functions used inside iterators
-  static void enable_ros_time(rclcpp::Clock::SharedPtr clock);
-  static void disable_ros_time(rclcpp::Clock::SharedPtr clock);
   static void set_clock(
     const builtin_interfaces::msg::Time::SharedPtr msg,
     bool set_ros_time_enabled,
@@ -126,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_;
 
@@ -135,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

@@ -48,15 +48,27 @@ class TimerBase
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase)
 
+  /// TimerBase constructor
+  /**
+   * \param clock A clock to use for time and sleeping
+   * \param period The interval at which the timer fires
+   * \param context node context
+   */
   RCLCPP_PUBLIC
   explicit TimerBase(
     Clock::SharedPtr clock,
     std::chrono::nanoseconds period,
     rclcpp::Context::SharedPtr context);
 
+  /// TimerBase destructor
   RCLCPP_PUBLIC
+  virtual
   ~TimerBase();
 
+  /// Cancel the timer.
+  /**
+   * \throws std::runtime_error if the rcl_timer_cancel returns a failure
+   */
   RCLCPP_PUBLIC
   void
   cancel();
@@ -71,10 +83,15 @@ public:
   bool
   is_canceled();
 
+  /// Reset the timer.
+  /**
+   * \throws std::runtime_error if the rcl_timer_reset returns a failure
+   */
   RCLCPP_PUBLIC
   void
   reset();
 
+  /// Call the callback function when the timer signal is emitted.
   RCLCPP_PUBLIC
   virtual void
   execute_callback() = 0;
@@ -84,7 +101,10 @@ public:
   get_timer_handle();
 
   /// Check how long the timer has until its next scheduled callback.
-  /** \return A std::chrono::duration representing the relative time until the next callback. */
+  /**
+   * \return A std::chrono::duration representing the relative time until the next callback.
+   * \throws std::runtime_error if the rcl_timer_get_time_until_next_call returns a failure
+   */
   RCLCPP_PUBLIC
   std::chrono::nanoseconds
   time_until_trigger();
@@ -98,6 +118,7 @@ public:
    * This function expects its caller to immediately trigger the callback after this function,
    * since it maintains the last time the callback was triggered.
    * \return True if the timer needs to trigger.
+   * \throws std::runtime_error if it failed to check timer
    */
   RCLCPP_PUBLIC
   bool is_ready();
@@ -145,6 +166,7 @@ public:
    * \param[in] clock The clock providing the current time.
    * \param[in] period The interval at which the timer fires.
    * \param[in] callback User-specified callback function.
+   * \param[in] context custom context to be used.
    */
   explicit GenericTimer(
     Clock::SharedPtr clock, std::chrono::nanoseconds period, FunctorT && callback,
@@ -154,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_));
   }
 
@@ -169,6 +191,10 @@ public:
     cancel();
   }
 
+  /**
+   * \sa rclcpp::TimerBase::execute_callback
+   * \throws std::runtime_error if it failed to notify timer that callback occurred
+   */
   void
   execute_callback() override
   {
@@ -179,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
@@ -209,6 +235,8 @@ public:
     callback_(*this);
   }
 
+  /// Is the clock steady (i.e. is the time between ticks constant?)
+  /** \return True if the clock used by this timer is steady. */
   bool
   is_steady() override
   {
@@ -233,6 +261,12 @@ class WallTimer : public GenericTimer<FunctorT>
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(WallTimer)
 
+  /// Wall timer constructor
+  /**
+   * \param period The interval at which the timer fires
+   * \param callback The callback function to execute every interval
+   * \param context node context
+   */
   WallTimer(
     std::chrono::nanoseconds period,
     FunctorT && callback,

rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp

@@ -23,6 +23,7 @@
 #include "libstatistics_collector/collector/generate_statistics_message.hpp"
 #include "libstatistics_collector/moving_average_statistics/types.hpp"
 #include "libstatistics_collector/topic_statistics_collector/constants.hpp"
+#include "libstatistics_collector/topic_statistics_collector/received_message_age.hpp"
 #include "libstatistics_collector/topic_statistics_collector/received_message_period.hpp"
 
 #include "rcl/time.h"
@@ -56,6 +57,9 @@ class SubscriptionTopicStatistics
   using TopicStatsCollector =
     libstatistics_collector::topic_statistics_collector::TopicStatisticsCollector<
     CallbackMessageT>;
+  using ReceivedMessageAge =
+    libstatistics_collector::topic_statistics_collector::ReceivedMessageAgeCollector<
+    CallbackMessageT>;
   using ReceivedMessagePeriod =
     libstatistics_collector::topic_statistics_collector::ReceivedMessagePeriodCollector<
     CallbackMessageT>;
@@ -71,6 +75,7 @@ public:
    * topic source
    * \param publisher instance constructed by the node in order to publish statistics data.
    * This class owns the publisher.
+   * \throws std::invalid_argument if publisher pointer is nullptr
    */
   SubscriptionTopicStatistics(
     const std::string & node_name,
@@ -94,6 +99,8 @@ public:
 
   /// Handle a message received by the subscription to collect statistics.
   /**
+   * This method acquires a lock to prevent race conditions to collectors list.
+   *
    * \param received_message the message received by the subscription
    * \param now_nanoseconds current time in nanoseconds
    */
@@ -101,6 +108,7 @@ public:
     const CallbackMessageT & received_message,
     const rclcpp::Time now_nanoseconds) const
   {
+    std::lock_guard<std::mutex> lock(mutex_);
     for (const auto & collector : subscriber_statistics_collectors_) {
       collector->OnMessageReceived(received_message, now_nanoseconds.nanoseconds());
     }
@@ -108,7 +116,7 @@ public:
 
   /// Set the timer used to publish statistics messages.
   /**
-   * \param measurement_timer the timer to fire the publisher, created by the node
+   * \param publisher_timer the timer to fire the publisher, created by the node
    */
   void set_publisher_timer(rclcpp::TimerBase::SharedPtr publisher_timer)
   {
@@ -116,21 +124,32 @@ public:
   }
 
   /// Publish a populated MetricsStatisticsMessage.
+  /**
+   * This method acquires a lock to prevent race conditions to collectors list.
+   */
   virtual void publish_message()
   {
+    std::vector<MetricsMessage> msgs;
     rclcpp::Time window_end{get_current_nanoseconds_since_epoch()};
 
-    for (auto & collector : subscriber_statistics_collectors_) {
-      const auto collected_stats = collector->GetStatisticsResults();
+    {
+      std::lock_guard<std::mutex> lock(mutex_);
+      for (auto & collector : subscriber_statistics_collectors_) {
+        const auto collected_stats = collector->GetStatisticsResults();
 
-      auto message = libstatistics_collector::collector::GenerateStatisticMessage(
-        node_name_,
-        collector->GetMetricName(),
-        collector->GetMetricUnit(),
-        window_start_,
-        window_end,
-        collected_stats);
-      publisher_->publish(message);
+        auto message = libstatistics_collector::collector::GenerateStatisticMessage(
+          node_name_,
+          collector->GetMetricName(),
+          collector->GetMetricUnit(),
+          window_start_,
+          window_end,
+          collected_stats);
+        msgs.push_back(message);
+      }
+    }
+
+    for (auto & msg : msgs) {
+      publisher_->publish(msg);
     }
     window_start_ = window_end;
   }
@@ -138,11 +157,14 @@ public:
 protected:
   /// Return a vector of all the currently collected data.
   /**
+   * This method acquires a lock to prevent race conditions to collectors list.
+   *
    * \return a vector of all the collected data
    */
   std::vector<StatisticData> get_current_collector_data() const
   {
     std::vector<StatisticData> data;
+    std::lock_guard<std::mutex> lock(mutex_);
     for (const auto & collector : subscriber_statistics_collectors_) {
       data.push_back(collector->GetStatisticsResults());
     }
@@ -151,23 +173,39 @@ protected:
 
 private:
   /// Construct and start all collectors and set window_start_.
+  /**
+   * This method acquires a lock to prevent race conditions to collectors list.
+   */
   void bring_up()
   {
+    auto received_message_age = std::make_unique<ReceivedMessageAge>();
+    received_message_age->Start();
+    subscriber_statistics_collectors_.emplace_back(std::move(received_message_age));
+
     auto received_message_period = std::make_unique<ReceivedMessagePeriod>();
     received_message_period->Start();
-    subscriber_statistics_collectors_.emplace_back(std::move(received_message_period));
+    {
+      std::lock_guard<std::mutex> lock(mutex_);
+      subscriber_statistics_collectors_.emplace_back(std::move(received_message_period));
+    }
 
     window_start_ = rclcpp::Time(get_current_nanoseconds_since_epoch());
   }
 
   /// Stop all collectors, clear measurements, stop publishing timer, and reset publisher.
+  /**
+   * This method acquires a lock to prevent race conditions to collectors list.
+   */
   void tear_down()
   {
-    for (auto & collector : subscriber_statistics_collectors_) {
-      collector->Stop();
-    }
+    {
+      std::lock_guard<std::mutex> lock(mutex_);
+      for (auto & collector : subscriber_statistics_collectors_) {
+        collector->Stop();
+      }
 
-    subscriber_statistics_collectors_.clear();
+      subscriber_statistics_collectors_.clear();
+    }
 
     if (publisher_timer_) {
       publisher_timer_->cancel();
@@ -187,6 +225,8 @@ private:
     return std::chrono::duration_cast<std::chrono::nanoseconds>(now.time_since_epoch()).count();
   }
 
+  /// Mutex to protect the subsequence vectors
+  mutable std::mutex mutex_;
   /// Collection of statistics collectors
   std::vector<std::unique_ptr<TopicStatsCollector>> subscriber_statistics_collectors_{};
   /// Node name used to generate topic statistics messages to be published

rclcpp/include/rclcpp/utilities.hpp

@@ -136,7 +136,7 @@ remove_ros_arguments(int argc, char const * const argv[]);
  * If nullptr is given for the context, then the global context is used, i.e.
  * the context initialized by rclcpp::init().
  *
- * \param[in] context Check for shutdown of this Context.
+ * \param[in] context Optional check for shutdown of this Context.
  * \return true if shutdown has been called, false otherwise
  */
 RCLCPP_PUBLIC
@@ -150,7 +150,7 @@ ok(rclcpp::Context::SharedPtr context = nullptr);
  *
  * Deprecated, as it is no longer different from rcl_ok().
  *
- * \param[in] context Check for initialization of this Context.
+ * \param[in] context Optional check for initialization of this Context.
  * \return true if the context is initialized, and false otherwise
  */
 [[deprecated("use the function ok() instead, which has the same usage.")]]
@@ -168,7 +168,8 @@ is_initialized(rclcpp::Context::SharedPtr context = nullptr);
  * This will also cause the "on_shutdown" callbacks to be called.
  *
  * \sa rclcpp::Context::shutdown()
- * \param[in] context to be shutdown
+ * \param[in] context Optional to be shutdown
+ * \param[in] reason Optional string passed to the context shutdown method
  * \return true if shutdown was successful, false if context was already shutdown
  */
 RCLCPP_PUBLIC
@@ -206,7 +207,7 @@ on_shutdown(std::function<void()> callback, rclcpp::Context::SharedPtr context =
  * the context initialized by rclcpp::init().
  *
  * \param[in] nanoseconds A std::chrono::duration representing how long to sleep for.
- * \param[in] context which may interrupt this sleep
+ * \param[in] context Optional which may interrupt this sleep
  * \return true if the condition variable did not timeout.
  */
 RCLCPP_PUBLIC

rclcpp/include/rclcpp/wait_result.hpp

@@ -58,6 +58,7 @@ public:
   /**
    * \param[in] wait_set A reference to the wait set, which this class
    *   will keep for the duration of its lifetime.
+   * \return a WaitResult from a "ready" result.
    */
   static
   WaitResult
@@ -90,6 +91,10 @@ public:
   }
 
   /// Return the rcl wait set.
+  /**
+   * \return const rcl wait set.
+   * \throws std::runtime_error if the class cannot access wait set when the result was not ready
+   */
   const WaitSetT &
   get_wait_set() const
   {
@@ -102,6 +107,10 @@ public:
   }
 
   /// Return the rcl wait set.
+  /**
+   * \return rcl wait set.
+   * \throws std::runtime_error if the class cannot access wait set when the result was not ready
+   */
   WaitSetT &
   get_wait_set()
   {

rclcpp/include/rclcpp/wait_set_algorithms.hpp

@@ -1,48 +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.
-
-#ifndef RCLCPP__WAIT_SET_ALGORITHMS_HPP_
-#define RCLCPP__WAIT_SET_ALGORITHMS_HPP_
-
-#include <algorithm>
-
-#include "rclcpp/visibility_control.hpp"
-#include "rclcpp/wait_result.hpp"
-
-namespace rclcpp
-{
-
-/// Find next ready entity out of the given sequence from start to last.
-template<class InputIt>
-InputIt
-find_next_ready_wait_set_entity(InputIt start, InputIt last)
-{
-  return std::find_if(start, last, [](const auto & entity) {return nullptr != entity;});
-}
-
-/// Find next ready guard condition given a WaitResult.
-template<class WaitSetT>
-typename WaitSetT::StoragePolicy::GuardConditionsIterable::const_iterator
-find_next_ready_guard_condition(
-  const typename rclcpp::WaitResult<WaitSetT> & wait_result,
-  typename WaitSetT::StoragePolicy::GuardConditionsIterable::const_iterator start = (
-    wait_result.get_wait_set()))
-{
-  return
-}
-
-
-}  // namespace rclcpp
-
-#endif  // RCLCPP__WAIT_SET_ALGORITHMS_HPP_

rclcpp/include/rclcpp/wait_set_policies/detail/entry_types.hpp

@@ -1,123 +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.
-
-#ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__ENTRY_TYPES_HPP_
-#define RCLCPP__WAIT_SET_POLICIES__DETAIL__ENTRY_TYPES_HPP_
-
-#include <limits>
-#include <memory>
-
-#include "rclcpp/client.hpp"
-#include "rclcpp/guard_condition.hpp"
-#include "rclcpp/service.hpp"
-#include "rclcpp/subscription_base.hpp"
-#include "rclcpp/subscription_wait_set_mask.hpp"
-#include "rclcpp/timer.hpp"
-#include "rclcpp/visibility_control.hpp"
-#include "rclcpp/waitable.hpp"
-
-namespace rclcpp
-{
-namespace wait_set_policies
-{
-namespace detail
-{
-
-constexpr const size_t not_in_wait_set = (std::numeric_limits<std::size_t>::max)();
-
-/// Templated ownership entries used for storage.
-template<class PointerT>
-class EntryTemplate : public PointerT
-{
-public:
-  size_t rcl_wait_set_index;
-  constexpr static const size_t not_in_wait_set = not_in_wait_set;
-
-  /// Conversion constructor, which is intentionally not marked explicit.
-  EntryTemplate(
-    const PointerT & entity_in = nullptr,
-    size_t rcl_wait_set_index_in = not_in_wait_set) noexcept
-  : PointerT(entity_in),
-    rcl_wait_set_index(rcl_wait_set_index_in)
-  {}
-};
-
-/// Templated ownership subscription entries used for storage.
-template<class PointerT>
-class SubscriptionEntryTemplate : public EntryTemplate<PointerT>
-{
-public:
-  rclcpp::SubscriptionWaitSetMask mask;
-
-  /// Conversion constructor, which is intentionally not marked explicit.
-  SubscriptionEntryTemplate(
-    const PointerT & entity_in = nullptr,
-    const rclcpp::SubscriptionWaitSetMask & mask_in = {},
-    size_t rcl_wait_set_index_in = not_in_wait_set) noexcept
-  : EntryTemplate<PointerT>(entity_in, rcl_wait_set_index_in),
-    mask(mask_in)
-  {}
-};
-
-/// Templated ownership entries with associated entity used for storage.
-template<class PointerT>
-class EntryWithAssociatedEntityTemplate : public PointerT
-{
-public:
-  std::shared_ptr<void> associated_entity;
-
-  /// Conversion constructor, which is intentionally not marked explicit.
-  EntryWithAssociatedEntityTemplate(
-    const PointerT & entity_in = nullptr,
-    const std::shared_ptr<void> & associated_entity_in = nullptr) noexcept
-  : PointerT(entity_in),
-    associated_entity(associated_entity_in)
-  {}
-};
-
-/// Shared ownership SubscriptionBase entry for storage.
-using SubscriptionEntry = SubscriptionEntryTemplate<std::shared_ptr<rclcpp::SubscriptionBase>>;
-/// Weak ownership SubscriptionBase entry for storage.
-using WeakSubscriptionEntry = SubscriptionEntryTemplate<std::weak_ptr<rclcpp::SubscriptionBase>>;
-
-/// Shared ownership GuardCondition entry for storage.
-using GuardConditionEntry = EntryTemplate<std::shared_ptr<rclcpp::GuardCondition>>;
-/// Weak ownership GuardCondition entry for storage.
-using WeakGuardConditionEntry = EntryTemplate<std::weak_ptr<rclcpp::GuardCondition>>;
-
-/// Shared ownership Timer entry for storage.
-using TimerEntry = EntryTemplate<std::shared_ptr<rclcpp::TimerBase>>;
-/// Weak ownership Timer entry for storage.
-using WeakTimerEntry = EntryTemplate<std::weak_ptr<rclcpp::TimerBase>>;
-
-/// Shared ownership Client entry for storage.
-using ClientEntry = EntryTemplate<std::shared_ptr<rclcpp::ClientBase>>;
-/// Weak ownership Client entry for storage.
-using WeakClientEntry = EntryTemplate<std::weak_ptr<rclcpp::ClientBase>>;
-
-/// Shared ownership Service entry for storage.
-using ServiceEntry = EntryTemplate<std::shared_ptr<rclcpp::ServiceBase>>;
-/// Weak ownership Service entry for storage.
-using WeakServiceEntry = EntryTemplate<std::weak_ptr<rclcpp::ServiceBase>>;
-
-/// Shared ownership Waitable entry for storage.
-using WaitableEntry = EntryWithAssociatedEntityTemplate<std::shared_ptr<rclcpp::Waitable>>;
-/// Weak ownership Waitable entry for storage.
-using WeakWaitableEntry = EntryWithAssociatedEntityTemplate<std::weak_ptr<rclcpp::Waitable>>;
-
-}  // namespace detail
-}  // namespace wait_set_policies
-}  // namespace rclcpp
-
-#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__ENTRY_TYPES_HPP_

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

@@ -59,9 +59,7 @@ protected:
     const WaitablesIterable & waitables,
     rclcpp::Context::SharedPtr context
   )
-  : rcl_wait_set_(rcl_get_zero_initialized_wait_set()),
-    context_(context),
-    previous_size_of_extra_guard_conditions_(extra_guard_conditions.size())
+  : rcl_wait_set_(rcl_get_zero_initialized_wait_set()), context_(context)
   {
     // Check context is not nullptr.
     if (nullptr == context) {
@@ -75,7 +73,17 @@ protected:
     size_t services_from_waitables = 0;
     size_t events_from_waitables = 0;
     for (const auto & waitable_entry : waitables) {
-      rclcpp::Waitable & waitable = *waitable_entry.waitable.get();
+      auto waitable_ptr_pair = get_raw_pointer_from_smart_pointer(waitable_entry.waitable);
+      if (nullptr == waitable_ptr_pair.second) {
+        if (HasStrongOwnership) {
+          throw std::runtime_error("unexpected condition, fixed storage policy needs pruning");
+        }
+        // Flag for pruning.
+        needs_pruning_ = true;
+        continue;
+      }
+
+      rclcpp::Waitable & waitable = *waitable_ptr_pair.second;
       subscriptions_from_waitables += waitable.get_number_of_ready_subscriptions();
       guard_conditions_from_waitables += waitable.get_number_of_ready_guard_conditions();
       timers_from_waitables += waitable.get_number_of_ready_timers();
@@ -168,11 +176,6 @@ protected:
   )
   {
     bool was_resized = false;
-    // Check if the extra_guard_conditions has changed.
-    if (previous_size_of_extra_guard_conditions_ != extra_guard_conditions.size()) {
-      previous_size_of_extra_guard_conditions_ = extra_guard_conditions.size();
-      needs_resize_ = true;
-    }
     // Resize the wait set, but only if it needs to be.
     if (needs_resize_) {
       // Resizing with rcl_wait_set_resize() is a no-op if nothing has changed,
@@ -242,7 +245,7 @@ protected:
     // Add subscriptions.
     for (const auto & subscription_entry : subscriptions) {
       auto subscription_ptr_pair =
-        get_raw_pointer_from_smart_pointer(subscription_entry);
+        get_raw_pointer_from_smart_pointer(subscription_entry.subscription);
       if (nullptr == subscription_ptr_pair.second) {
         // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
         if (HasStrongOwnership) {
@@ -257,7 +260,7 @@ protected:
       rcl_ret_t ret = rcl_wait_set_add_subscription(
         &rcl_wait_set_,
         subscription_ptr_pair.second->get_subscription_handle().get(),
-        &subscription_entry.rcl_wait_set_index);
+        nullptr);
       if (RCL_RET_OK != ret) {
         rclcpp::exceptions::throw_from_rcl_error(ret);
       }
@@ -283,7 +286,7 @@ protected:
           rcl_ret_t ret = rcl_wait_set_add_guard_condition(
             &rcl_wait_set_,
             &guard_condition_ptr_pair.second->get_rcl_guard_condition(),
-            &guard_condition.rcl_wait_set_index);
+            nullptr);
           if (RCL_RET_OK != ret) {
             rclcpp::exceptions::throw_from_rcl_error(ret);
           }
@@ -313,7 +316,7 @@ protected:
       rcl_ret_t ret = rcl_wait_set_add_timer(
         &rcl_wait_set_,
         timer_ptr_pair.second->get_timer_handle().get(),
-        &timer.rcl_wait_set_index);
+        nullptr);
       if (RCL_RET_OK != ret) {
         rclcpp::exceptions::throw_from_rcl_error(ret);
       }
@@ -336,7 +339,7 @@ protected:
       rcl_ret_t ret = rcl_wait_set_add_client(
         &rcl_wait_set_,
         client_ptr_pair.second->get_client_handle().get(),
-        &clients.rcl_wait_set_index);
+        nullptr);
       if (RCL_RET_OK != ret) {
         rclcpp::exceptions::throw_from_rcl_error(ret);
       }
@@ -359,7 +362,7 @@ protected:
       rcl_ret_t ret = rcl_wait_set_add_service(
         &rcl_wait_set_,
         service_ptr_pair.second->get_service_handle().get(),
-        &service.rcl_wait_set_index);
+        nullptr);
       if (RCL_RET_OK != ret) {
         rclcpp::exceptions::throw_from_rcl_error(ret);
       }
@@ -367,7 +370,7 @@ protected:
 
     // Add waitables.
     for (auto & waitable_entry : waitables) {
-      auto waitable_ptr_pair = get_raw_pointer_from_smart_pointer(waitable_entry);
+      auto waitable_ptr_pair = get_raw_pointer_from_smart_pointer(waitable_entry.waitable);
       if (nullptr == waitable_ptr_pair.second) {
         // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
         if (HasStrongOwnership) {
@@ -410,7 +413,6 @@ protected:
 
   bool needs_pruning_ = false;
   bool needs_resize_ = false;
-  size_t previous_size_of_extra_guard_conditions_ = 0;
 };
 
 }  // namespace detail

rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp

@@ -20,9 +20,14 @@
 #include <utility>
 #include <vector>
 
+#include "rclcpp/client.hpp"
+#include "rclcpp/guard_condition.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/subscription_wait_set_mask.hpp"
+#include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
-#include "rclcpp/wait_set_policies/detail/entry_types.hpp"
 #include "rclcpp/wait_set_policies/detail/storage_policy_common.hpp"
 #include "rclcpp/waitable.hpp"
 
@@ -37,25 +42,126 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo
 protected:
   using is_mutable = std::true_type;
 
-  using SubscriptionEntry = detail::SubscriptionEntry;
-  using SequenceOfWeakSubscriptions = std::vector<detail::WeakSubscriptionEntry>;
-  using SubscriptionsIterable = std::vector<detail::SubscriptionEntry>;
+  class SubscriptionEntry
+  {
+    // (wjwwood): indent of 'public:' is weird, I know. uncrustify is dumb.
 
-  using SequenceOfWeakGuardConditions = std::vector<detail::WeakGuardConditionEntry>;
-  using GuardConditionsIterable = std::vector<detail::GuardConditionEntry>;
+public:
+    std::shared_ptr<rclcpp::SubscriptionBase> subscription;
+    rclcpp::SubscriptionWaitSetMask mask;
 
-  using SequenceOfWeakTimers = std::vector<detail::WeakTimerEntry>;
-  using TimersIterable = std::vector<detail::TimerEntry>;
+    /// Conversion constructor, which is intentionally not marked explicit.
+    SubscriptionEntry(
+      std::shared_ptr<rclcpp::SubscriptionBase> subscription_in = nullptr,
+      const rclcpp::SubscriptionWaitSetMask & mask_in = {})
+    : subscription(std::move(subscription_in)),
+      mask(mask_in)
+    {}
 
-  using SequenceOfWeakClients = std::vector<detail::WeakClientEntry>;
-  using ClientsIterable = std::vector<detail::ClientEntry>;
+    void
+    reset() noexcept
+    {
+      subscription.reset();
+    }
+  };
+  class WeakSubscriptionEntry
+  {
+public:
+    std::weak_ptr<rclcpp::SubscriptionBase> subscription;
+    rclcpp::SubscriptionWaitSetMask mask;
 
-  using SequenceOfWeakServices = std::vector<detail::WeakServiceEntry>;
-  using ServicesIterable = std::vector<detail::ServiceEntry>;
+    explicit WeakSubscriptionEntry(
+      const std::shared_ptr<rclcpp::SubscriptionBase> & subscription_in,
+      const rclcpp::SubscriptionWaitSetMask & mask_in) noexcept
+    : subscription(subscription_in),
+      mask(mask_in)
+    {}
 
-  using WaitableEntry = detail::WaitableEntry;
-  using SequenceOfWeakWaitables = std::vector<detail::WeakWaitableEntry>;
-  using WaitablesIterable = std::vector<detail::WaitableEntry>;
+    explicit WeakSubscriptionEntry(const SubscriptionEntry & other)
+    : subscription(other.subscription),
+      mask(other.mask)
+    {}
+
+    std::shared_ptr<rclcpp::SubscriptionBase>
+    lock() const
+    {
+      return subscription.lock();
+    }
+
+    bool
+    expired() const noexcept
+    {
+      return subscription.expired();
+    }
+  };
+  using SequenceOfWeakSubscriptions = std::vector<WeakSubscriptionEntry>;
+  using SubscriptionsIterable = std::vector<SubscriptionEntry>;
+
+  using SequenceOfWeakGuardConditions = std::vector<std::weak_ptr<rclcpp::GuardCondition>>;
+  using GuardConditionsIterable = std::vector<std::shared_ptr<rclcpp::GuardCondition>>;
+
+  using SequenceOfWeakTimers = std::vector<std::weak_ptr<rclcpp::TimerBase>>;
+  using TimersIterable = std::vector<std::shared_ptr<rclcpp::TimerBase>>;
+
+  using SequenceOfWeakClients = std::vector<std::weak_ptr<rclcpp::ClientBase>>;
+  using ClientsIterable = std::vector<std::shared_ptr<rclcpp::ClientBase>>;
+
+  using SequenceOfWeakServices = std::vector<std::weak_ptr<rclcpp::ServiceBase>>;
+  using ServicesIterable = std::vector<std::shared_ptr<rclcpp::ServiceBase>>;
+
+  class WaitableEntry
+  {
+public:
+    std::shared_ptr<rclcpp::Waitable> waitable;
+    std::shared_ptr<void> associated_entity;
+
+    /// Conversion constructor, which is intentionally not marked explicit.
+    WaitableEntry(
+      std::shared_ptr<rclcpp::Waitable> waitable_in = nullptr,
+      std::shared_ptr<void> associated_entity_in = nullptr) noexcept
+    : waitable(std::move(waitable_in)),
+      associated_entity(std::move(associated_entity_in))
+    {}
+
+    void
+    reset() noexcept
+    {
+      waitable.reset();
+      associated_entity.reset();
+    }
+  };
+  class WeakWaitableEntry
+  {
+public:
+    std::weak_ptr<rclcpp::Waitable> waitable;
+    std::weak_ptr<void> associated_entity;
+
+    explicit WeakWaitableEntry(
+      const std::shared_ptr<rclcpp::Waitable> & waitable_in,
+      const std::shared_ptr<void> & associated_entity_in) noexcept
+    : waitable(waitable_in),
+      associated_entity(associated_entity_in)
+    {}
+
+    explicit WeakWaitableEntry(const WaitableEntry & other)
+    : waitable(other.waitable),
+      associated_entity(other.associated_entity)
+    {}
+
+    std::shared_ptr<rclcpp::Waitable>
+    lock() const
+    {
+      return waitable.lock();
+    }
+
+    bool
+    expired() const noexcept
+    {
+      return waitable.expired();
+    }
+  };
+  using SequenceOfWeakWaitables = std::vector<WeakWaitableEntry>;
+  using WaitablesIterable = std::vector<WaitableEntry>;
 
   template<class ArrayOfExtraGuardConditions>
   explicit
@@ -99,13 +205,13 @@ protected:
   storage_rebuild_rcl_wait_set(const ArrayOfExtraGuardConditions & extra_guard_conditions)
   {
     this->storage_rebuild_rcl_wait_set_with_sets(
-      shared_subscriptions_,
-      shared_guard_conditions_,
+      subscriptions_,
+      guard_conditions_,
       extra_guard_conditions,
-      shared_timers_,
-      shared_clients_,
-      shared_services_,
-      shared_waitables_
+      timers_,
+      clients_,
+      services_,
+      waitables_
     );
   }
 
@@ -137,7 +243,7 @@ protected:
     if (this->storage_has_entity(*subscription, subscriptions_)) {
       throw std::runtime_error("subscription already in wait set");
     }
-    detail::WeakSubscriptionEntry weak_entry{std::move(subscription), {}, detail::not_in_wait_set};
+    WeakSubscriptionEntry weak_entry{std::move(subscription), {}};
     subscriptions_.push_back(std::move(weak_entry));
     this->storage_flag_for_resize();
   }
@@ -159,7 +265,7 @@ protected:
     if (this->storage_has_entity(*guard_condition, guard_conditions_)) {
       throw std::runtime_error("guard_condition already in wait set");
     }
-    guard_conditions_.push_back({std::move(guard_condition), detail::not_in_wait_set});
+    guard_conditions_.push_back(std::move(guard_condition));
     this->storage_flag_for_resize();
   }
 
@@ -180,7 +286,7 @@ protected:
     if (this->storage_has_entity(*timer, timers_)) {
       throw std::runtime_error("timer already in wait set");
     }
-    timers_.push_back({std::move(timer), detail::not_in_wait_set});
+    timers_.push_back(std::move(timer));
     this->storage_flag_for_resize();
   }
 
@@ -201,7 +307,7 @@ protected:
     if (this->storage_has_entity(*client, clients_)) {
       throw std::runtime_error("client already in wait set");
     }
-    clients_.push_back({std::move(client), detail::not_in_wait_set});
+    clients_.push_back(std::move(client));
     this->storage_flag_for_resize();
   }
 
@@ -222,7 +328,7 @@ protected:
     if (this->storage_has_entity(*service, services_)) {
       throw std::runtime_error("service already in wait set");
     }
-    services_.push_back({std::move(service), detail::not_in_wait_set});
+    services_.push_back(std::move(service));
     this->storage_flag_for_resize();
   }
 
@@ -245,10 +351,7 @@ protected:
     if (this->storage_has_entity(*waitable, waitables_)) {
       throw std::runtime_error("waitable already in wait set");
     }
-    detail::WeakWaitableEntry weak_entry(
-      std::move(waitable),
-      std::move(associated_entity),
-      detail::not_in_wait_set);
+    WeakWaitableEntry weak_entry(std::move(waitable), std::move(associated_entity));
     waitables_.push_back(std::move(weak_entry));
     this->storage_flag_for_resize();
   }
@@ -279,12 +382,13 @@ protected:
         return weak_ptr.expired();
       };
     // remove guard conditions which have been deleted
-    subscriptions_.erase(std::remove_if(subscriptions_.begin(), subscriptions_.end(), p));
-    guard_conditions_.erase(std::remove_if(guard_conditions_.begin(), guard_conditions_.end(), p));
-    timers_.erase(std::remove_if(timers_.begin(), timers_.end(), p));
-    clients_.erase(std::remove_if(clients_.begin(), clients_.end(), p));
-    services_.erase(std::remove_if(services_.begin(), services_.end(), p));
-    waitables_.erase(std::remove_if(waitables_.begin(), waitables_.end(), p));
+    guard_conditions_.erase(
+      std::remove_if(guard_conditions_.begin(), guard_conditions_.end(), p),
+      guard_conditions_.end());
+    timers_.erase(std::remove_if(timers_.begin(), timers_.end(), p), timers_.end());
+    clients_.erase(std::remove_if(clients_.begin(), clients_.end(), p), clients_.end());
+    services_.erase(std::remove_if(services_.begin(), services_.end(), p), services_.end());
+    waitables_.erase(std::remove_if(waitables_.begin(), waitables_.end(), p), waitables_.end());
   }
 
   void
@@ -303,7 +407,6 @@ protected:
         }
       };
     // Lock all the weak pointers and hold them until released.
-    lock_all(subscriptions_, shared_subscriptions_);
     lock_all(guard_conditions_, shared_guard_conditions_);
     lock_all(timers_, shared_timers_);
     lock_all(clients_, shared_clients_);
@@ -314,9 +417,9 @@ protected:
         shared_ptrs.resize(weak_ptrs.size());
         size_t index = 0;
         for (const auto & weak_ptr : weak_ptrs) {
-          shared_ptrs[index++] = detail::WaitableEntry{
-            weak_ptr.lock(),
-            weak_ptr.associated_entity};
+          shared_ptrs[index++] = WaitableEntry{
+            weak_ptr.waitable.lock(),
+            weak_ptr.associated_entity.lock()};
         }
       };
     lock_all_waitables(waitables_, shared_waitables_);
@@ -335,7 +438,6 @@ protected:
           shared_ptr.reset();
         }
       };
-    reset_all(shared_subscriptions_);
     reset_all(shared_guard_conditions_);
     reset_all(shared_timers_);
     reset_all(shared_clients_);

rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp

@@ -17,6 +17,7 @@
 
 #include <array>
 #include <memory>
+#include <utility>
 
 #include "rclcpp/client.hpp"
 #include "rclcpp/guard_condition.hpp"
@@ -26,7 +27,6 @@
 #include "rclcpp/subscription_wait_set_mask.hpp"
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
-#include "rclcpp/wait_set_policies/detail/entry_types.hpp"
 #include "rclcpp/wait_set_policies/detail/storage_policy_common.hpp"
 #include "rclcpp/waitable.hpp"
 
@@ -53,38 +53,65 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom
 protected:
   using is_mutable = std::false_type;
 
+  class SubscriptionEntry
+  {
+public:
+    std::shared_ptr<rclcpp::SubscriptionBase> subscription;
+    rclcpp::SubscriptionWaitSetMask mask;
+
+    /// Conversion constructor, which is intentionally not marked explicit.
+    SubscriptionEntry(
+      std::shared_ptr<rclcpp::SubscriptionBase> subscription_in = nullptr,
+      rclcpp::SubscriptionWaitSetMask mask_in = {})
+    : subscription(std::move(subscription_in)),
+      mask(mask_in)
+    {}
+  };
   using ArrayOfSubscriptions = std::array<
-    detail::SubscriptionEntry,
+    SubscriptionEntry,
     NumberOfSubscriptions
   >;
   using SubscriptionsIterable = ArrayOfSubscriptions;
 
   using ArrayOfGuardConditions = std::array<
-    detail::GuardConditionEntry,
+    std::shared_ptr<rclcpp::GuardCondition>,
     NumberOfGuardCondtions
   >;
   using GuardConditionsIterable = ArrayOfGuardConditions;
 
   using ArrayOfTimers = std::array<
-    detail::TimerEntry,
+    std::shared_ptr<rclcpp::TimerBase>,
     NumberOfTimers
   >;
   using TimersIterable = ArrayOfTimers;
 
   using ArrayOfClients = std::array<
-    detail::ClientEntry,
+    std::shared_ptr<rclcpp::ClientBase>,
     NumberOfClients
   >;
   using ClientsIterable = ArrayOfClients;
 
   using ArrayOfServices = std::array<
-    detail::ServiceEntry,
+    std::shared_ptr<rclcpp::ServiceBase>,
     NumberOfServices
   >;
   using ServicesIterable = ArrayOfServices;
 
+  struct WaitableEntry
+  {
+    /// Conversion constructor, which is intentionally not marked explicit.
+    WaitableEntry(
+      std::shared_ptr<rclcpp::Waitable> waitable_in = nullptr,
+      std::shared_ptr<void> associated_entity_in = nullptr) noexcept
+    : waitable(std::move(waitable_in)),
+      associated_entity(std::move(associated_entity_in))
+    {}
+
+    std::shared_ptr<rclcpp::Waitable> waitable;
+    std::shared_ptr<void> associated_entity;
+  };
   using ArrayOfWaitables = std::array<
-    detail::WaitableEntry,
+    WaitableEntry,
     NumberOfWaitables
   >;
   using WaitablesIterable = ArrayOfWaitables;

rclcpp/include/rclcpp/wait_set_template.hpp

@@ -61,6 +61,8 @@ public:
    * \param[in] subscriptions Vector of subscriptions to be added.
    * \param[in] guard_conditions Vector of guard conditions to be added.
    * \param[in] timers Vector of timers to be added.
+   * \param[in] clients Vector of clients and their associated entity to be added.
+   * \param[in] services Vector of services and their associated entity to be added.
    * \param[in] waitables Vector of waitables and their associated entity to be added.
    * \param[in] context Custom context to be used, defaults to global default.
    * \throws std::invalid_argument If context is nullptr.
@@ -73,8 +75,8 @@ public:
     const typename StoragePolicy::ClientsIterable & clients = {},
     const typename StoragePolicy::ServicesIterable & services = {},
     const typename StoragePolicy::WaitablesIterable & waitables = {},
-    rclcpp::Context::SharedPtr context = (
-      rclcpp::contexts::get_global_default_context()))
+    rclcpp::Context::SharedPtr context =
+    rclcpp::contexts::get_global_default_context())
   : SynchronizationPolicy(context),
     StoragePolicy(
       subscriptions,
@@ -231,9 +233,9 @@ public:
         }
         if (mask.include_intra_process_waitable) {
           auto local_waitable = inner_subscription->get_intra_process_waitable();
-          inner_subscription->exchange_in_use_by_wait_set_state(local_waitable.get(), false);
           if (nullptr != local_waitable) {
-            // This is the case when intra process is disabled for the subscription.
+            // This is the case when intra process is enabled for the subscription.
+            inner_subscription->exchange_in_use_by_wait_set_state(local_waitable.get(), false);
             this->storage_remove_waitable(std::move(local_waitable));
           }
         }
@@ -249,7 +251,7 @@ public:
    *
    * Except in the case of a fixed sized storage, where changes to the wait set
    * cannot occur after construction, in which case it holds shared ownership
-   * at all times until the wait set is destroyed, but this method also does not
+   * at all times until the wait set is destroy, but this method also does not
    * exist on a fixed sized wait set.
    *
    * This function may be thread-safe depending on the SynchronizationPolicy
@@ -643,7 +645,8 @@ public:
    *   when time_to_wait is < 0, or
    * \returns Empty if the wait set is empty, avoiding the possibility of
    *   waiting indefinitely on an empty wait set.
-   * \throws rclcpp::exceptions::RCLError on unhandled rcl errors
+   * \throws rclcpp::exceptions::RCLError on unhandled rcl errors or,
+   * \throws std::runtime_error if unknown WaitResultKind
    */
   template<class Rep = int64_t, class Period = std::milli>
   RCUTILS_WARN_UNUSED

rclcpp/include/rclcpp/waitable.hpp

@@ -30,6 +30,9 @@ class Waitable
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Waitable)
 
+  RCLCPP_PUBLIC
+  virtual ~Waitable() = default;
+
   /// Get the number of ready subscriptions
   /**
    * Returns a value of 0 by default.
@@ -120,7 +123,7 @@ public:
   RCLCPP_PUBLIC
   virtual
   bool
-  is_ready(rcl_wait_set_t *) = 0;
+  is_ready(rcl_wait_set_t * wait_set) = 0;
 
   /// Execute any entities of the Waitable that are ready.
   /**

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>0.8.3</version>
+  <version>5.1.0</version>
   <description>The ROS client library in C++.</description>
-  <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
+  <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
+  <maintainer email="mabel@openrobotics.org">Mabel Zhang</maintainer>
+  <maintainer email="william@openrobotics.org">William Woodall</maintainer>
   <license>Apache License 2.0</license>
+  <author email="dthomas@openrobotics.org">Dirk Thomas</author>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
@@ -35,8 +38,11 @@
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>mimick_vendor</test_depend>
+  <test_depend>performance_test_fixture</test_depend>
   <test_depend>rmw</test_depend>
   <test_depend>rmw_implementation_cmake</test_depend>
+  <test_depend>rosidl_default_generators</test_depend>
   <test_depend>test_msgs</test_depend>
 
   <export>

rclcpp/resource/get_interface.hpp.em

@@ -0,0 +1,111 @@
+// 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.
+
+@{
+uppercase_interface_name = interface_name.upper()
+}@
+
+#ifndef RCLCPP__NODE_INTERFACES__GET_@(uppercase_interface_name)_HPP_
+#define RCLCPP__NODE_INTERFACES__GET_@(uppercase_interface_name)_HPP_
+
+#include <memory>
+#include <utility>
+#include <type_traits>
+
+#include "rcpputils/pointer_traits.hpp"
+
+#include "rclcpp/node_interfaces/@(interface_name).hpp"
+#include "rclcpp/node_interfaces/@(interface_name)_traits.hpp"
+
+@{
+interface_typename = ''.join([part.capitalize() for part in interface_name.split('_')])
+}@
+
+/// This header provides the get_@(interface_name)() template function.
+/**
+ * This function is useful for getting the @(interface_typename) pointer from
+ * various kinds of Node-like classes.
+ *
+ * It's able to get a std::shared_ptr to a @(interface_typename) so long as the class
+ * has a method called ``get_@(interface_name)()`` which returns one.
+ */
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+namespace detail
+{
+
+// If NodeType has a method called get_@(interface_name)() which returns a shared pointer.
+template<
+  typename NodeType,
+  typename std::enable_if<has_@(interface_name)<
+    typename rcpputils::remove_pointer<NodeType>::type
+  >::value, int>::type = 0
+>
+std::shared_ptr<rclcpp::node_interfaces::@(interface_typename)>
+get_@(interface_name)_from_pointer(NodeType node_pointer)
+{
+  if (!node_pointer) {
+    throw std::invalid_argument("node cannot be nullptr");
+  }
+  return node_pointer->get_@(interface_name)();
+}
+
+}  // namespace detail
+
+/// Get the @(interface_typename) as a shared pointer from a pointer to a "Node like" object.
+template<
+  typename NodeType,
+  typename std::enable_if<
+    rcpputils::is_pointer<NodeType>::value, int
+  >::type = 0
+>
+inline
+std::shared_ptr<rclcpp::node_interfaces::@(interface_typename)>
+get_@(interface_name)(NodeType && node)
+{
+  // Forward pointers to detail implementation directly.
+  return detail::get_@(interface_name)_from_pointer(node);
+}
+
+/// Get the @(interface_typename) as a shared pointer from a "Node like" object.
+template<
+  typename NodeType,
+  typename std::enable_if<
+    !rcpputils::is_pointer<NodeType>::value, int
+  >::type = 0
+>
+inline
+std::shared_ptr<rclcpp::node_interfaces::@(interface_typename)>
+get_@(interface_name)(NodeType && node)
+{
+  // Forward references to detail implementation as a pointer.
+  return detail::get_@(interface_name)_from_pointer(&node);
+}
+
+/// Keep the @(interface_typename) a shared pointer.
+inline
+std::shared_ptr<rclcpp::node_interfaces::@(interface_typename)>
+get_@(interface_name)(
+  std::shared_ptr<rclcpp::node_interfaces::@(interface_typename)> & node_interface)
+{
+  return node_interface;
+}
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__GET_@(uppercase_interface_name)_HPP_

rclcpp/resource/interface_traits.hpp.em

@@ -0,0 +1,47 @@
+// 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.
+@{
+uppercase_interface_name = interface_name.upper()
+interface_typename = ''.join([part.capitalize() for part in interface_name.split('_')])
+}@
+
+#ifndef RCLCPP__NODE_INTERFACES__@(uppercase_interface_name)_TRAITS_HPP_
+#define RCLCPP__NODE_INTERFACES__@(uppercase_interface_name)_TRAITS_HPP_
+
+#include <functional>
+#include <type_traits>
+
+#include "rclcpp/node_interfaces/@(interface_name).hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+template<class T, typename = void>
+struct has_@(interface_name) : std::false_type
+{};
+
+template<class T>
+struct has_@(interface_name)<
+  T, typename std::enable_if<
+    std::is_same<
+      std::shared_ptr<rclcpp::node_interfaces::@(interface_typename)>,
+      decltype(std::declval<T>().get_@(interface_name)())>::value>::type> : std::true_type
+{};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__@(uppercase_interface_name)_TRAITS_HPP_

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/clock.cpp

@@ -32,7 +32,7 @@ public:
   {
     rcl_ret_t ret = rcl_clock_init(clock_type, &rcl_clock_, &allocator_);
     if (ret != RCL_RET_OK) {
-      exceptions::throw_from_rcl_error(ret, "could not get current time stamp");
+      exceptions::throw_from_rcl_error(ret, "failed to initialize rcl clock");
     }
   }
 

rclcpp/src/rclcpp/context.cpp

@@ -1,4 +1,4 @@
-// Copyright 2015 Open Source Robotics Foundation, Inc.
+// Copyright 2015-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.
@@ -22,20 +22,132 @@
 #include <utility>
 
 #include "rcl/init.h"
+#include "rcl/logging.h"
+
 #include "rclcpp/detail/utilities.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/logging.hpp"
+
+#include "rcutils/error_handling.h"
+#include "rcutils/macros.h"
+
 #include "rmw/impl/cpp/demangle.hpp"
 
-/// Mutex to protect initialized contexts.
-static std::mutex g_contexts_mutex;
-/// Weak list of context to be shutdown by the signal handler.
-static std::vector<std::weak_ptr<rclcpp::Context>> g_contexts;
+#include "./logging_mutex.hpp"
 
 using rclcpp::Context;
 
+namespace rclcpp
+{
+/// Class to manage vector of weak pointers to all created contexts
+class WeakContextsWrapper
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(WeakContextsWrapper)
+
+  void
+  add_context(const Context::SharedPtr & context)
+  {
+    std::lock_guard<std::mutex> guard(mutex_);
+    weak_contexts_.push_back(context);
+  }
+
+  void
+  remove_context(const Context * context)
+  {
+    std::lock_guard<std::mutex> guard(mutex_);
+    weak_contexts_.erase(
+      std::remove_if(
+        weak_contexts_.begin(),
+        weak_contexts_.end(),
+        [context](const Context::WeakPtr weak_context) {
+          auto locked_context = weak_context.lock();
+          if (!locked_context) {
+            // take advantage and removed expired contexts
+            return true;
+          }
+          return locked_context.get() == context;
+        }
+      ),
+      weak_contexts_.end());
+  }
+
+  std::vector<Context::SharedPtr>
+  get_contexts()
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    std::vector<Context::SharedPtr> shared_contexts;
+    for (auto it = weak_contexts_.begin(); it != weak_contexts_.end(); /* noop */) {
+      auto context_ptr = it->lock();
+      if (!context_ptr) {
+        // remove invalid weak context pointers
+        it = weak_contexts_.erase(it);
+      } else {
+        ++it;
+        shared_contexts.push_back(context_ptr);
+      }
+    }
+    return shared_contexts;
+  }
+
+private:
+  std::vector<std::weak_ptr<rclcpp::Context>> weak_contexts_;
+  std::mutex mutex_;
+};
+}  // namespace rclcpp
+
+using rclcpp::WeakContextsWrapper;
+
+/// Global vector of weak pointers to all contexts
+static
+WeakContextsWrapper::SharedPtr
+get_weak_contexts()
+{
+  static WeakContextsWrapper::SharedPtr weak_contexts = WeakContextsWrapper::make_shared();
+  if (!weak_contexts) {
+    throw std::runtime_error("weak contexts vector is not valid");
+  }
+  return weak_contexts;
+}
+
+/// Count of contexts that wanted to initialize the logging system.
+static
+size_t &
+get_logging_reference_count()
+{
+  static size_t ref_count = 0;
+  return ref_count;
+}
+
+extern "C"
+{
+static
+void
+rclcpp_logging_output_handler(
+  const rcutils_log_location_t * location,
+  int severity, const char * name, rcutils_time_point_value_t timestamp,
+  const char * format, va_list * args)
+{
+  try {
+    std::shared_ptr<std::recursive_mutex> logging_mutex;
+    logging_mutex = get_global_logging_mutex();
+    std::lock_guard<std::recursive_mutex> guard(*logging_mutex);
+    return rcl_logging_multiple_output_handler(
+      location, severity, name, timestamp, format, args);
+  } catch (std::exception & ex) {
+    RCUTILS_SAFE_FWRITE_TO_STDERR(ex.what());
+    RCUTILS_SAFE_FWRITE_TO_STDERR("\n");
+  } catch (...) {
+    RCUTILS_SAFE_FWRITE_TO_STDERR("failed to take global rclcpp logging mutex\n");
+  }
+}
+}  // extern "C"
+
 Context::Context()
-: rcl_context_(nullptr), shutdown_reason_("") {}
+: rcl_context_(nullptr),
+  shutdown_reason_(""),
+  logging_mutex_(nullptr)
+{}
 
 Context::~Context()
 {
@@ -87,13 +199,39 @@ 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();
+    std::lock_guard<std::recursive_mutex> guard(*logging_mutex_);
+    size_t & count = get_logging_reference_count();
+    if (0u == count) {
+      ret = rcl_logging_configure_with_output_handler(
+        &rcl_context_->global_arguments,
+        rcl_init_options_get_allocator(init_options_.get_rcl_init_options()),
+        rclcpp_logging_output_handler);
+      if (RCL_RET_OK != ret) {
+        rcl_context_.reset();
+        rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure logging");
+      }
+    } else {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "logging was initialized more than once");
+    }
+    ++count;
+  }
+
   try {
     std::vector<std::string> unparsed_ros_arguments = detail::get_unparsed_ros_arguments(
       argc, argv, &(rcl_context_->global_arguments), rcl_get_default_allocator());
@@ -103,8 +241,8 @@ Context::init(
 
     init_options_ = init_options;
 
-    std::lock_guard<std::mutex> lock(g_contexts_mutex);
-    g_contexts.push_back(this->shared_from_this());
+    weak_contexts_ = get_weak_contexts();
+    weak_contexts_->add_context(this->shared_from_this());
   } catch (const std::exception & e) {
     ret = rcl_shutdown(rcl_context_.get());
     rcl_context_.reset();
@@ -141,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()
 {
@@ -173,14 +322,21 @@ Context::shutdown(const std::string & reason)
   this->interrupt_all_sleep_for();
   this->interrupt_all_wait_sets();
   // remove self from the global contexts
-  std::lock_guard<std::mutex> context_lock(g_contexts_mutex);
-  for (auto it = g_contexts.begin(); it != g_contexts.end(); ) {
-    auto shared_context = it->lock();
-    if (shared_context.get() == this) {
-      it = g_contexts.erase(it);
-      break;
-    } else {
-      ++it;
+  weak_contexts_->remove_context(this);
+  // shutdown logger
+  if (logging_mutex_) {
+    // logging was initialized by this context
+    std::lock_guard<std::recursive_mutex> guard(*logging_mutex_);
+    size_t & count = get_logging_reference_count();
+    if (0u == --count) {
+      rcl_ret_t rcl_ret = rcl_logging_fini();
+      if (RCL_RET_OK != rcl_ret) {
+        RCUTILS_SAFE_FWRITE_TO_STDERR(
+          RCUTILS_STRINGIFY(__file__) ":"
+          RCUTILS_STRINGIFY(__LINE__)
+          " failed to fini logging");
+        rcl_reset_error();
+      }
     }
   }
   return true;
@@ -315,17 +471,6 @@ Context::clean_up()
 std::vector<Context::SharedPtr>
 rclcpp::get_contexts()
 {
-  std::lock_guard<std::mutex> lock(g_contexts_mutex);
-  std::vector<Context::SharedPtr> shared_contexts;
-  for (auto it = g_contexts.begin(); it != g_contexts.end(); /* noop */) {
-    auto context_ptr = it->lock();
-    if (!context_ptr) {
-      // remove invalid weak context pointers
-      it = g_contexts.erase(it);
-    } else {
-      ++it;
-      shared_contexts.push_back(context_ptr);
-    }
-  }
-  return shared_contexts;
+  WeakContextsWrapper::SharedPtr weak_contexts = get_weak_contexts();
+  return weak_contexts->get_contexts();
 }

rclcpp/src/rclcpp/duration.cpp

@@ -37,7 +37,7 @@ Duration::Duration(int32_t seconds, uint32_t nanoseconds)
   rcl_duration_.nanoseconds += nanoseconds;
 }
 
-Duration::Duration(int64_t nanoseconds)
+Duration::Duration(rcl_duration_value_t nanoseconds)
 {
   rcl_duration_.nanoseconds = nanoseconds;
 }
@@ -47,17 +47,14 @@ Duration::Duration(std::chrono::nanoseconds nanoseconds)
   rcl_duration_.nanoseconds = nanoseconds.count();
 }
 
-Duration::Duration(const Duration & rhs)
-{
-  rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds;
-}
+Duration::Duration(const Duration & rhs) = default;
 
 Duration::Duration(
   const builtin_interfaces::msg::Duration & duration_msg)
 {
   rcl_duration_.nanoseconds =
-    static_cast<rcl_duration_value_t>(RCL_S_TO_NS(duration_msg.sec));
-  rcl_duration_.nanoseconds += duration_msg.nanosec;
+    RCL_S_TO_NS(static_cast<rcl_duration_value_t>(duration_msg.sec));
+  rcl_duration_.nanoseconds += static_cast<rcl_duration_value_t>(duration_msg.nanosec);
 }
 
 Duration::Duration(const rcl_duration_t & duration)
@@ -69,24 +66,25 @@ Duration::Duration(const rcl_duration_t & duration)
 Duration::operator builtin_interfaces::msg::Duration() const
 {
   builtin_interfaces::msg::Duration msg_duration;
-  msg_duration.sec = static_cast<std::int32_t>(RCL_NS_TO_S(rcl_duration_.nanoseconds));
-  msg_duration.nanosec =
-    static_cast<std::uint32_t>(rcl_duration_.nanoseconds % (1000 * 1000 * 1000));
+  constexpr rcl_duration_value_t kDivisor = RCL_S_TO_NS(1);
+  const auto result = std::div(rcl_duration_.nanoseconds, kDivisor);
+  if (result.rem >= 0) {
+    msg_duration.sec = static_cast<std::int32_t>(result.quot);
+    msg_duration.nanosec = static_cast<std::uint32_t>(result.rem);
+  } else {
+    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;
 }
 
 Duration &
-Duration::operator=(const Duration & rhs)
-{
-  rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds;
-  return *this;
-}
+Duration::operator=(const Duration & rhs) = default;
 
 Duration &
 Duration::operator=(const builtin_interfaces::msg::Duration & duration_msg)
 {
-  rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast<int64_t>(duration_msg.sec));
-  rcl_duration_.nanoseconds += duration_msg.nanosec;
+  *this = Duration(duration_msg);
   return *this;
 }
 
@@ -96,6 +94,12 @@ Duration::operator==(const rclcpp::Duration & rhs) const
   return rcl_duration_.nanoseconds == rhs.rcl_duration_.nanoseconds;
 }
 
+bool
+Duration::operator!=(const rclcpp::Duration & rhs) const
+{
+  return rcl_duration_.nanoseconds != rhs.rcl_duration_.nanoseconds;
+}
+
 bool
 Duration::operator<(const rclcpp::Duration & rhs) const
 {
@@ -144,7 +148,7 @@ Duration::operator+(const rclcpp::Duration & rhs) const
     this->rcl_duration_.nanoseconds,
     rhs.rcl_duration_.nanoseconds,
     std::numeric_limits<rcl_duration_value_t>::max());
-  return Duration(
+  return Duration::from_nanoseconds(
     rcl_duration_.nanoseconds + rhs.rcl_duration_.nanoseconds);
 }
 
@@ -173,7 +177,7 @@ Duration::operator-(const rclcpp::Duration & rhs) const
     rhs.rcl_duration_.nanoseconds,
     std::numeric_limits<rcl_duration_value_t>::max());
 
-  return Duration(
+  return Duration::from_nanoseconds(
     rcl_duration_.nanoseconds - rhs.rcl_duration_.nanoseconds);
 }
 
@@ -204,7 +208,7 @@ Duration::operator*(double scale) const
     scale,
     std::numeric_limits<rcl_duration_value_t>::max());
   long double scale_ld = static_cast<long double>(scale);
-  return Duration(
+  return Duration::from_nanoseconds(
     static_cast<rcl_duration_value_t>(
       static_cast<long double>(rcl_duration_.nanoseconds) * scale_ld));
 }
@@ -230,6 +234,10 @@ Duration::seconds() const
 rmw_time_t
 Duration::to_rmw_time() const
 {
+  if (rcl_duration_.nanoseconds < 0) {
+    throw std::runtime_error("rmw_time_t cannot be negative");
+  }
+
   // reuse conversion logic from msg creation
   builtin_interfaces::msg::Duration msg = *this;
   rmw_time_t result;
@@ -241,7 +249,17 @@ Duration::to_rmw_time() const
 Duration
 Duration::from_seconds(double seconds)
 {
-  return Duration(static_cast<int64_t>(RCL_S_TO_NS(seconds)));
+  Duration ret;
+  ret.rcl_duration_.nanoseconds = static_cast<int64_t>(RCL_S_TO_NS(seconds));
+  return ret;
+}
+
+Duration
+Duration::from_nanoseconds(rcl_duration_value_t nanoseconds)
+{
+  Duration ret;
+  ret.rcl_duration_.nanoseconds = nanoseconds;
+  return ret;
 }
 
 }  // namespace rclcpp

rclcpp/src/rclcpp/executor.cpp

@@ -14,8 +14,11 @@
 
 #include <algorithm>
 #include <memory>
+#include <map>
 #include <string>
 #include <type_traits>
+#include <utility>
+#include <vector>
 
 #include "rcl/allocator.h"
 #include "rcl/error_handling.h"
@@ -28,32 +31,87 @@
 
 #include "rcutils/logging_macros.h"
 
+using namespace std::chrono_literals;
+
 using rclcpp::exceptions::throw_from_rcl_error;
 using rclcpp::AnyExecutable;
 using rclcpp::Executor;
 using rclcpp::ExecutorOptions;
 using rclcpp::FutureReturnCode;
 
-Executor::Executor(const ExecutorOptions & options)
-: spinning(false), interrupt_guard_condition_(options.context)
+Executor::Executor(const rclcpp::ExecutorOptions & options)
+: spinning(false),
+  memory_strategy_(options.memory_strategy)
 {
+  rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options();
+  rcl_ret_t ret = rcl_guard_condition_init(
+    &interrupt_guard_condition_, options.context->get_rcl_context().get(), guard_condition_options);
+  if (RCL_RET_OK != ret) {
+    throw_from_rcl_error(ret, "Failed to create interrupt guard condition in Executor constructor");
+  }
+
+  // The number of guard conditions is always at least 2: 1 for the ctrl-c guard cond,
+  // and one for the executor's guard cond (interrupt_guard_condition_)
+
+  // Put the global ctrl-c guard condition in
+  memory_strategy_->add_guard_condition(options.context->get_interrupt_guard_condition(&wait_set_));
+
+  // Put the executor's guard condition in
+  memory_strategy_->add_guard_condition(&interrupt_guard_condition_);
+  rcl_allocator_t allocator = memory_strategy_->get_allocator();
+
+  // Store the context for later use.
+  context_ = options.context;
+
+  ret = rcl_wait_set_init(
+    &wait_set_,
+    0, 2, 0, 0, 0, 0,
+    context_->get_rcl_context().get(),
+    allocator);
+  if (RCL_RET_OK != ret) {
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "failed to create wait set: %s", rcl_get_error_string().str);
+    rcl_reset_error();
+    if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "failed to destroy guard condition: %s", rcl_get_error_string().str);
+      rcl_reset_error();
+    }
+    throw_from_rcl_error(ret, "Failed to create wait set in Executor constructor");
+  }
 }
 
 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) {
@@ -74,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)
 {
@@ -82,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
@@ -112,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
@@ -168,7 +393,26 @@ Executor::spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr n
 }
 
 void
-Executor::spin_some(std::chrono::nanoseconds max_duration)
+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)
+{
+  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]() {
@@ -187,18 +431,33 @@ 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());
-  while (spinning.load() && max_duration_not_elapsed()) {
+  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;
     }
   }
 }
 
+void
+Executor::spin_once_impl(std::chrono::nanoseconds timeout)
+{
+  AnyExecutable any_exec;
+  if (get_next_executable(any_exec, timeout)) {
+    execute_any_executable(any_exec);
+  }
+}
+
 void
 Executor::spin_once(std::chrono::nanoseconds timeout)
 {
@@ -206,21 +465,28 @@ Executor::spin_once(std::chrono::nanoseconds timeout)
     throw std::runtime_error("spin_once() called while already spinning");
   }
   RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
-  AnyExecutable any_exec;
-  if (get_next_executable(any_exec, timeout)) {
-    execute_any_executable(any_exec);
-  }
+  spin_once_impl(timeout);
 }
 
 void
 Executor::cancel()
 {
   spinning.store(false);
-  if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-    throw std::runtime_error(rcl_get_error_string().str);
+  rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
+  if (ret != RCL_RET_OK) {
+    throw_from_rcl_error(ret, "Failed to trigger guard condition in cancel");
   }
 }
 
+void
+Executor::set_memory_strategy(rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy)
+{
+  if (memory_strategy == nullptr) {
+    throw std::runtime_error("Received NULL memory strategy in executor.");
+  }
+  memory_strategy_ = memory_strategy;
+}
+
 void
 Executor::execute_any_executable(AnyExecutable & any_exec)
 {
@@ -246,8 +512,9 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
   any_exec.callback_group->can_be_taken_from().store(true);
   // Wake the wait, because it may need to be recalculated or work that
   // was previously blocked is now available.
-  if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-    throw std::runtime_error(rcl_get_error_string().str);
+  rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
+  if (ret != RCL_RET_OK) {
+    throw_from_rcl_error(ret, "Failed to trigger guard condition from execute_any_executable");
   }
 }
 
@@ -296,8 +563,7 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription)
   if (subscription->is_serialized()) {
     // This is the case where a copy of the serialized message is taken from
     // the middleware via inter-process communication.
-    std::shared_ptr<rcl_serialized_message_t> serialized_msg =
-      subscription->create_serialized_message();
+    std::shared_ptr<SerializedMessage> serialized_msg = subscription->create_serialized_message();
     take_and_do_error_handling(
       "taking a serialized message from topic",
       subscription->get_topic_name(),
@@ -393,45 +659,67 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
   {
     std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
 
+    // Check weak_nodes_ to find any callback group that is not owned
+    // by an executor and add it to the list of callbackgroups for
+    // collect entities. Also exchange to false so it is not
+    // allowed to add to another executor
+    add_callback_groups_from_nodes_associated_to_executor();
+
     // Collect the subscriptions and timers to be waited on
     memory_strategy_->clear_handles();
-    bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_);
+    bool has_invalid_weak_groups_or_nodes =
+      memory_strategy_->collect_entities(weak_groups_to_nodes_);
 
-    // Clean up any invalid nodes, if they were detected
-    if (has_invalid_weak_nodes) {
-      auto node_it = weak_nodes_.begin();
-      auto gc_it = guard_conditions_.begin();
-      while (node_it != weak_nodes_.end()) {
-        if (node_it->expired()) {
-          node_it = weak_nodes_.erase(node_it);
-          memory_strategy_->remove_guard_condition(*gc_it);
-          gc_it = guard_conditions_.erase(gc_it);
-        } else {
-          ++node_it;
-          ++gc_it;
+    if (has_invalid_weak_groups_or_nodes) {
+      std::vector<rclcpp::CallbackGroup::WeakPtr> invalid_group_ptrs;
+      for (auto pair : weak_groups_to_nodes_) {
+        auto weak_group_ptr = pair.first;
+        auto weak_node_ptr = pair.second;
+        if (weak_group_ptr.expired() || weak_node_ptr.expired()) {
+          invalid_group_ptrs.push_back(weak_group_ptr);
+          auto node_guard_pair = weak_nodes_to_guard_conditions_.find(weak_node_ptr);
+          weak_nodes_to_guard_conditions_.erase(weak_node_ptr);
+          memory_strategy_->remove_guard_condition(node_guard_pair->second);
         }
       }
+      std::for_each(
+        invalid_group_ptrs.begin(), invalid_group_ptrs.end(),
+        [this](rclcpp::CallbackGroup::WeakPtr group_ptr) {
+          if (weak_groups_to_nodes_associated_with_executor_.find(group_ptr) !=
+          weak_groups_to_nodes_associated_with_executor_.end())
+          {
+            weak_groups_to_nodes_associated_with_executor_.erase(group_ptr);
+          }
+          if (weak_groups_associated_with_executor_to_nodes_.find(group_ptr) !=
+          weak_groups_associated_with_executor_to_nodes_.end())
+          {
+            weak_groups_associated_with_executor_to_nodes_.erase(group_ptr);
+          }
+          weak_groups_to_nodes_.erase(group_ptr);
+        });
     }
+
     // clear wait set
-    if (rcl_wait_set_clear(&wait_set_) != RCL_RET_OK) {
-      throw std::runtime_error("Couldn't clear wait set");
+    rcl_ret_t ret = rcl_wait_set_clear(&wait_set_);
+    if (ret != RCL_RET_OK) {
+      throw_from_rcl_error(ret, "Couldn't clear wait set");
     }
 
     // The size of waitables are accounted for in size of the other entities
-    rcl_ret_t ret = rcl_wait_set_resize(
+    ret = rcl_wait_set_resize(
       &wait_set_, memory_strategy_->number_of_ready_subscriptions(),
       memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(),
       memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(),
       memory_strategy_->number_of_ready_events());
     if (RCL_RET_OK != ret) {
-      throw std::runtime_error(
-              std::string("Couldn't resize the wait set : ") + rcl_get_error_string().str);
+      throw_from_rcl_error(ret, "Couldn't resize the wait set");
     }
 
     if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) {
       throw std::runtime_error("Couldn't fill wait set");
     }
   }
+
   rcl_ret_t status =
     rcl_wait(&wait_set_, std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count());
   if (status == RCL_RET_WAIT_SET_EMPTY) {
@@ -449,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;
 }
@@ -472,84 +756,107 @@ 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);
     }
   }
-  // If there is no ready executable, return a null ptr
+  // If there is no ready executable, return false
   return success;
 }
 
@@ -572,3 +879,18 @@ Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanos
   }
   return success;
 }
+
+// Returns true iff the weak_groups_to_nodes map has node_ptr as the value in any of its entry.
+bool
+Executor::has_node(
+  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes) const
+{
+  return std::find_if(
+    weak_groups_to_nodes.begin(),
+    weak_groups_to_nodes.end(),
+    [&](const WeakCallbackGroupsToNodesMap::value_type & other) -> bool {
+      auto other_ptr = other.second.lock();
+      return other_ptr == node_ptr;
+    }) != weak_groups_to_nodes.end();
+}

rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp

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

rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp

@@ -15,6 +15,7 @@
 #include "rclcpp/executors/static_single_threaded_executor.hpp"
 
 #include <memory>
+#include <vector>
 
 #include "rclcpp/scope_exit.hpp"
 
@@ -41,6 +42,7 @@ StaticSingleThreadedExecutor::spin()
   // Set memory_strategy_ and exec_list_ based on weak_nodes_
   // Prepare wait_set_ based on memory_strategy_
   entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
 
   while (rclcpp::ok(this->context_) && spinning.load()) {
     // Refresh wait set and wait for work
@@ -50,23 +52,31 @@ StaticSingleThreadedExecutor::spin()
 }
 
 void
-StaticSingleThreadedExecutor::add_node(
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
+StaticSingleThreadedExecutor::add_callback_group(
+  rclcpp::CallbackGroup::SharedPtr group_ptr,
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  bool notify)
 {
-  // If the node already has an executor
-  std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
-  if (has_executor.exchange(true)) {
-    throw std::runtime_error("Node has already been added to an executor.");
-  }
-
-  if (notify) {
+  bool is_new_node = entities_collector_->add_callback_group(group_ptr, node_ptr);
+  if (is_new_node && notify) {
     // Interrupt waiting to handle new node
     if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
       throw std::runtime_error(rcl_get_error_string().str);
     }
   }
+}
 
-  entities_collector_->add_node(node_ptr);
+void
+StaticSingleThreadedExecutor::add_node(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
+{
+  bool is_new_node = entities_collector_->add_node(node_ptr);
+  if (is_new_node && notify) {
+    // Interrupt waiting to handle new node
+    if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+      throw std::runtime_error(rcl_get_error_string().str);
+    }
+  }
 }
 
 void
@@ -75,23 +85,51 @@ StaticSingleThreadedExecutor::add_node(std::shared_ptr<rclcpp::Node> node_ptr, b
   this->add_node(node_ptr->get_node_base_interface(), notify);
 }
 
+void
+StaticSingleThreadedExecutor::remove_callback_group(
+  rclcpp::CallbackGroup::SharedPtr group_ptr, bool notify)
+{
+  bool node_removed = entities_collector_->remove_callback_group(group_ptr);
+  // If the node was matched and removed, interrupt waiting
+  if (node_removed && notify) {
+    if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+      throw std::runtime_error(rcl_get_error_string().str);
+    }
+  }
+}
+
 void
 StaticSingleThreadedExecutor::remove_node(
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
 {
   bool node_removed = entities_collector_->remove_node(node_ptr);
-
+  if (!node_removed) {
+    throw std::runtime_error("Node needs to be associated with this executor.");
+  }
+  // If the node was matched and removed, interrupt waiting
   if (notify) {
-    // If the node was matched and removed, interrupt waiting
-    if (node_removed) {
-      if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-        throw std::runtime_error(rcl_get_error_string().str);
-      }
+    if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+      throw std::runtime_error(rcl_get_error_string().str);
     }
   }
+}
 
-  std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
-  has_executor.store(false);
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+StaticSingleThreadedExecutor::get_all_callback_groups()
+{
+  return entities_collector_->get_all_callback_groups();
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+StaticSingleThreadedExecutor::get_manually_added_callback_groups()
+{
+  return entities_collector_->get_manually_added_callback_groups();
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+StaticSingleThreadedExecutor::get_automatically_added_callback_groups_from_nodes()
+{
+  return entities_collector_->get_automatically_added_callback_groups_from_nodes();
 }
 
 void