6.3.0

Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>

rclcpp/CHANGELOG.rst

@@ -2,6 +2,200 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+6.3.0 (2021-01-25)
+------------------
+* Add instrumentation for linking a timer to a node (`#1500 <https://github.com/ros2/rclcpp/issues/1500>`_)
+* Fix error when using IPC with StaticSingleThreadExecutor (`#1520 <https://github.com/ros2/rclcpp/issues/1520>`_)
+* Change to using unique_ptrs for DummyExecutor. (`#1517 <https://github.com/ros2/rclcpp/issues/1517>`_)
+* Allow reconfiguring 'clock' topic qos (`#1512 <https://github.com/ros2/rclcpp/issues/1512>`_)
+* Allow to add/remove nodes thread safely in rclcpp::Executor  (`#1505 <https://github.com/ros2/rclcpp/issues/1505>`_)
+* Call rclcpp::shutdown in test_node for clean shutdown on Windows (`#1515 <https://github.com/ros2/rclcpp/issues/1515>`_)
+* Reapply "Add get_logging_directory method to rclcpp::Logger (`#1509 <https://github.com/ros2/rclcpp/issues/1509>`_)" (`#1513 <https://github.com/ros2/rclcpp/issues/1513>`_)
+* use describe_parameters of parameter client for test (`#1499 <https://github.com/ros2/rclcpp/issues/1499>`_)
+* Revert "Add get_logging_directory method to rclcpp::Logger (`#1509 <https://github.com/ros2/rclcpp/issues/1509>`_)" (`#1511 <https://github.com/ros2/rclcpp/issues/1511>`_)
+* Add get_logging_directory method to rclcpp::Logger (`#1509 <https://github.com/ros2/rclcpp/issues/1509>`_)
+* Contributors: Chris Lalancette, Christophe Bedard, Ivan Santiago Paunovic, eboasson, mauropasse, tomoya
+
+6.2.0 (2021-01-08)
+------------------
+* Better documentation for the QoS class (`#1508 <https://github.com/ros2/rclcpp/issues/1508>`_)
+* Modify excluding callback duration from topic statistics (`#1492 <https://github.com/ros2/rclcpp/issues/1492>`_)
+* Make the test of graph users more robust. (`#1504 <https://github.com/ros2/rclcpp/issues/1504>`_)
+* Make sure to wait for graph change events in test_node_graph. (`#1503 <https://github.com/ros2/rclcpp/issues/1503>`_)
+* add timeout to SyncParametersClient methods (`#1493 <https://github.com/ros2/rclcpp/issues/1493>`_)
+* Fix wrong test expectations (`#1497 <https://github.com/ros2/rclcpp/issues/1497>`_)
+* Update create_publisher/subscription documentation, clarifying when a parameters interface is required (`#1494 <https://github.com/ros2/rclcpp/issues/1494>`_)
+* Fix string literal warnings (`#1442 <https://github.com/ros2/rclcpp/issues/1442>`_)
+* support describe_parameters methods to parameter client. (`#1453 <https://github.com/ros2/rclcpp/issues/1453>`_)
+* Contributors: Audrow Nash, Chris Lalancette, Ivan Santiago Paunovic, Nikolai Morin, hsgwa, tomoya
+
+6.1.0 (2020-12-10)
+------------------
+* Add getters to rclcpp::qos and rclcpp::Policy enum classes (`#1467 <https://github.com/ros2/rclcpp/issues/1467>`_)
+* Change nullptr checks to use ASSERT_TRUE. (`#1486 <https://github.com/ros2/rclcpp/issues/1486>`_)
+* Adjust logic around finding and erasing guard_condition (`#1474 <https://github.com/ros2/rclcpp/issues/1474>`_)
+* Update QDs to QL 1 (`#1477 <https://github.com/ros2/rclcpp/issues/1477>`_)
+* Add performance tests for parameter transport (`#1463 <https://github.com/ros2/rclcpp/issues/1463>`_)
+* Contributors: Chris Lalancette, Ivan Santiago Paunovic, Scott K Logan, Stephen Brawner
+
+6.0.0 (2020-11-18)
+------------------
+* Move ownership of shutdown_guard_condition to executors/graph_listener (`#1404 <https://github.com/ros2/rclcpp/issues/1404>`_)
+* Add options to automatically declare qos parameters when creating a publisher/subscription (`#1465 <https://github.com/ros2/rclcpp/issues/1465>`_)
+* Add `take_data` to `Waitable` and `data` to `AnyExecutable` (`#1241 <https://github.com/ros2/rclcpp/issues/1241>`_)
+* Add benchmarks for node parameters interface (`#1444 <https://github.com/ros2/rclcpp/issues/1444>`_)
+* Remove allocation from executor::remove_node() (`#1448 <https://github.com/ros2/rclcpp/issues/1448>`_)
+* Fix test crashes on CentOS 7 (`#1449 <https://github.com/ros2/rclcpp/issues/1449>`_)
+* Bump rclcpp packages to Quality Level 2 (`#1445 <https://github.com/ros2/rclcpp/issues/1445>`_)
+* Added executor benchmark tests (`#1413 <https://github.com/ros2/rclcpp/issues/1413>`_)
+* Add fully-qualified namespace to WeakCallbackGroupsToNodesMap (`#1435 <https://github.com/ros2/rclcpp/issues/1435>`_)
+* Contributors: Alejandro Hernández Cordero, Audrow Nash, Chris Lalancette, Ivan Santiago Paunovic, Louise Poubel, Scott K Logan, brawner
+
+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>`_)

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,7 +25,11 @@ 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 -Wnon-virtual-dtor -Woverloaded-virtual)
+  # 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()
 
 set(${PROJECT_NAME}_SRCS
@@ -79,6 +85,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/publisher_base.cpp
   src/rclcpp/qos.cpp
   src/rclcpp/qos_event.cpp
+  src/rclcpp/qos_overriding_options.cpp
   src/rclcpp/serialization.cpp
   src/rclcpp/serialized_message.cpp
   src/rclcpp/service.cpp
@@ -166,6 +173,7 @@ 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"
@@ -212,442 +220,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(rosidl_default_generators REQUIRED)
-
-  find_package(test_msgs REQUIRED)
-
-  include(cmake/rclcpp_add_build_failure_test.cmake)
-
-  add_definitions(-DTEST_RESOURCES_DIRECTORY="${CMAKE_CURRENT_BINARY_DIR}/test/resources")
-
-  rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs
-    test/msg/Header.msg
-    test/msg/MessageWithHeader.msg
-    DEPENDENCIES builtin_interfaces
-    LIBRARY_NAME ${PROJECT_NAME}
-    SKIP_INSTALL
-  )
-
-  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)
-    target_include_directories(test_function_traits PUBLIC include)
-    ament_target_dependencies(test_function_traits
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-  endif()
-  ament_add_gmock(test_intra_process_manager 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_interface_traits test/test_interface_traits.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_interface_traits)
-    ament_target_dependencies(test_interface_traits
-      "rcl")
-    target_link_libraries(test_interface_traits ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_multi_threaded_executor 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
-    APPEND_LIBRARY_DIRS "${append_library_dirs}"
-  )
-  if(TARGET test_subscription_topic_statistics)
-    ament_target_dependencies(test_subscription_topic_statistics
-      "builtin_interfaces"
-      "libstatistics_collector"
-      "rcl_interfaces"
-      "rcutils"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-      "statistics_msgs"
-      "test_msgs")
-    rosidl_target_interfaces(test_subscription_topic_statistics ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp")
-    target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_subscription_options test/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

@@ -1,8 +1,8 @@
 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
+# rclcpp Quality Declaration
 
-The package `rclcpp` claims to be in the **Quality Level 4** category.
+The package `rclcpp` claims to be in the **Quality Level 1** category when it is used with a **Quality Level 1** middleware.
 
 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.
 
@@ -10,7 +10,7 @@ Below are the rationales, notes, and caveats for this claim, organized by each r
 
 ### 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)
+`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]
 
@@ -76,19 +76,19 @@ All pull requests must resolve related documentation changes before merging.
 
 ### Public API Documentation [3.ii]
 
-The API is publicly available in the [ROS 2 API documentation](http://docs.ros2.org/latest/api/rclcpp/).
+The API is publicly available in its [ROS 2 API documentation](http://docs.ros2.org/latest/api/rclcpp/).
 
 ### License [3.iii]
 
 The license for `rclcpp` is Apache 2.0, and a summary is in each source file, the type is declared in the [`package.xml`](./package.xml) manifest file, and a full copy of the license is in the [`LICENSE`](../LICENSE) file.
 
-There is an automated test which runs a linter that ensures each file has a license statement. [Here](http://build.ros2.org/view/Epr/job/Epr__rclcpp__ubuntu_bionic_amd64/lastBuild/testReport/rclcpp/) can be found a list with the latest results of the various linters being run on the package.
+There is an automated test which runs a linter that ensures each file has a license statement. [Here](http://build.ros2.org/view/Rpr/job/Rpr__rclcpp__ubuntu_focal_amd64/lastCompletedBuild/testReport/rclcpp/) can be found a list with the latest results of the various linters being run on the package.
 
 ### 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/Epr/job/Epr__rclcpp__ubuntu_bionic_amd64/lastBuild/testReport/rclcpp/copyright/).
+There is an automated test which runs a linter that ensures each file has at least one copyright statement. Latest linter result report can be seen [here](http://build.ros2.org/view/Rpr/job/Rpr__rclcpp__ubuntu_focal_amd64/lastCompletedBuild/testReport/rclcpp/copyright/).
 
 ## Testing [4]
 
@@ -121,9 +121,21 @@ This includes:
 
 Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers.
 
+Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_src_rclcpp/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://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.
+`rclcpp` follows the recommendations for performance testing of C/C++ code in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#performance), and opts to do performance analysis on each release rather than each change.
+
+The performance tests of `rclcpp` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/master/rclcpp/test/benchmark).
+
+System level performance benchmarks that cover features of `rclcpp` can be found at:
+* [Benchmarks](http://build.ros2.org/view/Rci/job/Rci__benchmark_ubuntu_focal_amd64/BenchmarkTable/)
+* [Performance](http://build.ros2.org/view/Rci/job/Rci__nightly-performance_ubuntu_focal_amd64/lastCompletedBuild/)
+
+Changes that introduce regressions in performance must be adequately justified in order to be accepted and merged.
 
 ### Linters and Static Analysis [4.v]
 
@@ -151,49 +163,49 @@ It also has several test dependencies, which do not affect the resulting quality
 
 The `libstatistics_collector` package provides lightweight aggregation utilities to collect statistics and measure message metrics.
 
-It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros-tooling/libstatistics_collector/tree/master/QUALITY_DECLARATION.md).
+It is **Quality Level 1**, 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 4**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl/QUALITY_DECLARATION.md).
+It is **Quality Level 1**, 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 4**, see its [Quality Declaration document](https://github.com/ros2/rcl/tree/master/rcl_yaml_param_parser/QUALITY_DECLARATION.md).
+It is **Quality Level 1**, 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 4**, see its [Quality Declaration document](https://github.com/ros2/rcpputils/blob/master/QUALITY_DECLARATION.md).
+It is **Quality Level 1**, 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 4**, see its [Quality Declaration document](https://github.com/ros2/rcutils/blob/master/QUALITY_DECLARATION.md).
+It is **Quality Level 1**, 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 4**, see its [Quality Declaration document](https://github.com/ros2/rmw/blob/master/rmw/QUALITY_DECLARATION.md).
+It is **Quality Level 1**, 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 4**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/statistics_msgs/QUALITY_DECLARATION.md).
+It is **Quality Level 1**, 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 `rcl` so that it may be traced for debugging and performance analysis.
+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 4**, see its [Quality Declaration document](https://gitlab.com/micro-ROS/ros_tracing/ros2_tracing/-/blob/master/tracetools/QUALITY_DECLARATION.md).
+It is **Quality Level 1**, see its [Quality Declaration document](https://gitlab.com/ros-tracing/ros2_tracing/-/blob/master/tracetools/QUALITY_DECLARATION.md).
 
 ### Direct Runtime non-ROS Dependency [5.iii]
 

rclcpp/README.md

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

rclcpp/include/rclcpp/any_executable.hpp

@@ -47,6 +47,7 @@ struct AnyExecutable
   // These are used to keep the scope on the containing items
   rclcpp::CallbackGroup::SharedPtr callback_group;
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base;
+  std::shared_ptr<void> data;
 };
 
 namespace executor

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

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

rclcpp/include/rclcpp/clock.hpp

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

@@ -139,6 +139,11 @@ public:
   rclcpp::InitOptions
   get_init_options();
 
+  /// Return actual domain id.
+  RCLCPP_PUBLIC
+  size_t
+  get_domain_id() const;
+
   /// Return the shutdown reason, or empty string if not shutdown.
   /**
    * This function is thread-safe.
@@ -243,67 +248,6 @@ public:
   void
   interrupt_all_sleep_for();
 
-  /// Get a handle to the guard condition which is triggered when interrupted.
-  /**
-   * This guard condition is triggered any time interrupt_all_wait_sets() is
-   * called, which may be called by the user, or shutdown().
-   * And in turn, shutdown() may be called by the user, the destructor of this
-   * context, or the signal handler if installed and shutdown_on_sigint is true
-   * for this context.
-   *
-   * The first time that this function is called for a given wait set a new guard
-   * condition will be created and returned; thereafter the same guard condition
-   * will be returned for the same wait set.
-   * This mechanism is designed to ensure that the same guard condition is not
-   * reused across wait sets (e.g., when using multiple executors in the same
-   * process).
-   * This method will throw an exception if initialization of the guard
-   * condition fails.
-   *
-   * The returned guard condition needs to be released with the
-   * release_interrupt_guard_condition() method in order to reclaim resources.
-   *
-   * \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 *
-  get_interrupt_guard_condition(rcl_wait_set_t * wait_set);
-
-  /// Release the previously allocated guard condition which is triggered when interrupted.
-  /**
-   * If you previously called get_interrupt_guard_condition() for a given wait
-   * set to get a interrupt guard condition, then you should call
-   * release_interrupt_guard_condition() when you're done, to free that
-   * condition.
-   * Will throw an exception if get_interrupt_guard_condition() wasn't
-   * previously called for the given wait set.
-   *
-   * After calling this, the pointer returned by get_interrupt_guard_condition()
-   * for the given wait_set is invalid.
-   *
-   * \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
-  release_interrupt_guard_condition(rcl_wait_set_t * wait_set);
-
-  /// Nothrow version of release_interrupt_guard_condition(), logs to RCLCPP_ERROR instead.
-  RCLCPP_PUBLIC
-  void
-  release_interrupt_guard_condition(rcl_wait_set_t * wait_set, const std::nothrow_t &) noexcept;
-
-  /// Interrupt any blocking executors, or wait sets associated with this context.
-  RCLCPP_PUBLIC
-  virtual
-  void
-  interrupt_all_wait_sets();
-
   /// Return a singleton instance for the SubContext type, constructing one if necessary.
   template<typename SubContext, typename ... Args>
   std::shared_ptr<SubContext>
@@ -363,11 +307,6 @@ private:
   /// Mutex for protecting the global condition variable.
   std::mutex interrupt_mutex_;
 
-  /// Mutex to protect sigint_guard_cond_handles_.
-  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_;
 };

rclcpp/include/rclcpp/create_publisher.hpp

@@ -17,6 +17,7 @@
 
 #include <memory>
 #include <string>
+#include <utility>
 
 #include "rclcpp/node_interfaces/get_node_topics_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
@@ -24,15 +25,65 @@
 #include "rclcpp/publisher_factory.hpp"
 #include "rclcpp/publisher_options.hpp"
 #include "rclcpp/qos.hpp"
+#include "rclcpp/qos_overriding_options.hpp"
+#include "rclcpp/detail/qos_parameters.hpp"
+
 #include "rmw/qos_profiles.h"
 
 namespace rclcpp
 {
 
+namespace detail
+{
+/// Create and return a publisher of the given MessageT type.
+template<
+  typename MessageT,
+  typename AllocatorT = std::allocator<void>,
+  typename PublisherT = rclcpp::Publisher<MessageT, AllocatorT>,
+  typename NodeParametersT,
+  typename NodeTopicsT>
+std::shared_ptr<PublisherT>
+create_publisher(
+  NodeParametersT & node_parameters,
+  NodeTopicsT & node_topics,
+  const std::string & topic_name,
+  const rclcpp::QoS & qos,
+  const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options = (
+    rclcpp::PublisherOptionsWithAllocator<AllocatorT>()
+  )
+)
+{
+  auto node_topics_interface = rclcpp::node_interfaces::get_node_topics_interface(node_topics);
+  const rclcpp::QoS & actual_qos = options.qos_overriding_options.get_policy_kinds().size() ?
+    rclcpp::detail::declare_qos_parameters(
+    options.qos_overriding_options, node_parameters,
+    node_topics_interface->resolve_topic_name(topic_name),
+    qos, rclcpp::detail::PublisherQosParametersTraits{}) :
+    qos;
+
+  // Create the publisher.
+  auto pub = node_topics_interface->create_publisher(
+    topic_name,
+    rclcpp::create_publisher_factory<MessageT, AllocatorT, PublisherT>(options),
+    actual_qos
+  );
+
+  // Add the publisher to the node topics interface.
+  node_topics_interface->add_publisher(pub, options.callback_group);
+
+  return std::dynamic_pointer_cast<PublisherT>(pub);
+}
+}  // namespace detail
+
+
 /// Create and return a publisher of the given MessageT type.
 /**
  * The NodeT type only needs to have a method called get_node_topics_interface()
  * which returns a shared_ptr to a NodeTopicsInterface.
+ *
+ * In case `options.qos_overriding_options` is enabling qos parameter overrides,
+ * NodeT must also have a method called get_node_parameters_interface()
+ * which returns a shared_ptr to a NodeParametersInterface.
  */
 template<
   typename MessageT,
@@ -49,21 +100,28 @@ create_publisher(
   )
 )
 {
-  // Extract the NodeTopicsInterface from the NodeT.
-  using rclcpp::node_interfaces::get_node_topics_interface;
-  auto node_topics = get_node_topics_interface(node);
+  return detail::create_publisher<MessageT, AllocatorT, PublisherT>(
+    node, node, topic_name, qos, options);
+}
 
-  // Create the publisher.
-  auto pub = node_topics->create_publisher(
-    topic_name,
-    rclcpp::create_publisher_factory<MessageT, AllocatorT, PublisherT>(options),
-    qos
-  );
-
-  // Add the publisher to the node topics interface.
-  node_topics->add_publisher(pub, options.callback_group);
-
-  return std::dynamic_pointer_cast<PublisherT>(pub);
+/// Create and return a publisher of the given MessageT type.
+template<
+  typename MessageT,
+  typename AllocatorT = std::allocator<void>,
+  typename PublisherT = rclcpp::Publisher<MessageT, AllocatorT>>
+std::shared_ptr<PublisherT>
+create_publisher(
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters,
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr & node_topics,
+  const std::string & topic_name,
+  const rclcpp::QoS & qos,
+  const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options = (
+    rclcpp::PublisherOptionsWithAllocator<AllocatorT>()
+  )
+)
+{
+  return detail::create_publisher<MessageT, AllocatorT, PublisherT>(
+    node_parameters, node_topics, topic_name, qos, options);
 }
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/create_subscription.hpp

@@ -18,6 +18,7 @@
 #include <chrono>
 #include <functional>
 #include <memory>
+#include <stdexcept>
 #include <string>
 #include <utility>
 
@@ -28,6 +29,7 @@
 #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"
@@ -39,11 +41,131 @@
 namespace rclcpp
 {
 
+namespace detail
+{
+template<
+  typename MessageT,
+  typename CallbackT,
+  typename AllocatorT,
+  typename CallbackMessageT,
+  typename SubscriptionT,
+  typename MessageMemoryStrategyT,
+  typename NodeParametersT,
+  typename NodeTopicsT>
+typename std::shared_ptr<SubscriptionT>
+create_subscription(
+  NodeParametersT & node_parameters,
+  NodeTopicsT & node_topics,
+  const std::string & topic_name,
+  const rclcpp::QoS & qos,
+  CallbackT && callback,
+  const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
+    rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
+  ),
+  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = (
+    MessageMemoryStrategyT::create_default()
+  )
+)
+{
+  using rclcpp::node_interfaces::get_node_topics_interface;
+  auto node_topics_interface = get_node_topics_interface(node_topics);
+
+  std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>>
+  subscription_topic_stats = nullptr;
+
+  if (rclcpp::detail::resolve_enable_topic_statistics(
+      options,
+      *node_topics_interface->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 = rclcpp::detail::create_publisher<statistics_msgs::msg::MetricsMessage>(
+      node_parameters,
+      node_topics_interface,
+      options.topic_stats_options.publish_topic,
+      qos);
+
+    subscription_topic_stats = std::make_shared<
+      rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>
+      >(node_topics_interface->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_interface->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_interface->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,
+    subscription_topic_stats
+  );
+
+  const rclcpp::QoS & actual_qos = options.qos_overriding_options.get_policy_kinds().size() ?
+    rclcpp::detail::declare_qos_parameters(
+    options.qos_overriding_options, node_parameters,
+    node_topics_interface->resolve_topic_name(topic_name),
+    qos, rclcpp::detail::SubscriptionQosParametersTraits{}) :
+    qos;
+
+  auto sub = node_topics_interface->create_subscription(topic_name, factory, actual_qos);
+  node_topics_interface->add_subscription(sub, options.callback_group);
+
+  return std::dynamic_pointer_cast<SubscriptionT>(sub);
+}
+}  // namespace detail
+
 /// Create and return a subscription of the given MessageT type.
 /**
  * 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.
+ *
+ * In case `options.qos_overriding_options` is enabling qos parameter overrides,
+ * NodeT must also have a method called get_node_parameters_interface()
+ * which returns a shared_ptr to a NodeParametersInterface.
+ *
+ * \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,
@@ -59,7 +181,7 @@ template<
   typename NodeT>
 typename std::shared_ptr<SubscriptionT>
 create_subscription(
-  NodeT && node,
+  NodeT & node,
   const std::string & topic_name,
   const rclcpp::QoS & qos,
   CallbackT && callback,
@@ -71,55 +193,45 @@ create_subscription(
   )
 )
 {
-  using rclcpp::node_interfaces::get_node_topics_interface;
-  auto node_topics = get_node_topics_interface(std::forward<NodeT>(node));
+  return rclcpp::detail::create_subscription<
+    MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT, MessageMemoryStrategyT>(
+    node, node, topic_name, qos, std::forward<CallbackT>(callback), options, msg_mem_strat);
+}
 
-  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()))
-  {
-    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);
-
-    auto sub_call_back = [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,
-    subscription_topic_stats
-  );
-
-  auto sub = node_topics->create_subscription(topic_name, factory, qos);
-  node_topics->add_subscription(sub, options.callback_group);
-
-  return std::dynamic_pointer_cast<SubscriptionT>(sub);
+/// Create and return a subscription of the given MessageT type.
+/**
+ * See \ref create_subscription().
+ */
+template<
+  typename MessageT,
+  typename CallbackT,
+  typename AllocatorT = std::allocator<void>,
+  typename CallbackMessageT =
+  typename rclcpp::subscription_traits::has_message_type<CallbackT>::type,
+  typename SubscriptionT = rclcpp::Subscription<CallbackMessageT, AllocatorT>,
+  typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy<
+    CallbackMessageT,
+    AllocatorT
+  >>
+typename std::shared_ptr<SubscriptionT>
+create_subscription(
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters,
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr & node_topics,
+  const std::string & topic_name,
+  const rclcpp::QoS & qos,
+  CallbackT && callback,
+  const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
+    rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
+  ),
+  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = (
+    MessageMemoryStrategyT::create_default()
+  )
+)
+{
+  return rclcpp::detail::create_subscription<
+    MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT, MessageMemoryStrategyT>(
+    node_parameters, node_topics, topic_name, qos,
+    std::forward<CallbackT>(callback), options, msg_mem_strat);
 }
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/create_timer.hpp

@@ -76,14 +76,14 @@ create_timer(
  * \tparam DurationRepT
  * \tparam DurationT
  * \tparam CallbackT
- * \param period period to exectute callback
+ * \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
+ * are null, or period is negative or too large
  */
 template<typename DurationRepT, typename DurationT, typename CallbackT>
 typename rclcpp::WallTimer<CallbackT>::SharedPtr
@@ -102,10 +102,38 @@ create_wall_timer(
     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(
-    std::chrono::duration_cast<std::chrono::nanoseconds>(period),
-    std::move(callback),
-    node_base->get_context());
+    period_ns, std::move(callback), node_base->get_context());
   node_timers->add_timer(timer, group);
   return timer;
 }

rclcpp/include/rclcpp/detail/qos_parameters.hpp

@@ -0,0 +1,326 @@
+// 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__DETAIL__QOS_PARAMETERS_HPP_
+#define RCLCPP__DETAIL__QOS_PARAMETERS_HPP_
+
+#include <algorithm>
+#include <array>
+#include <functional>
+#include <initializer_list>
+#include <map>
+#include <string>
+#include <type_traits>
+#include <vector>
+
+#include "rcl_interfaces/msg/parameter_descriptor.hpp"
+#include "rcpputils/pointer_traits.hpp"
+#include "rmw/qos_string_conversions.h"
+
+#include "rclcpp/duration.hpp"
+#include "rclcpp/node_interfaces/get_node_parameters_interface.hpp"
+#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
+#include "rclcpp/qos_overriding_options.hpp"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+/// \internal Trait used to specialize `declare_qos_parameters()` for publishers.
+struct PublisherQosParametersTraits
+{
+  static constexpr const char * entity_type() {return "publisher";}
+  static constexpr auto allowed_policies()
+  {
+    return std::array<::rclcpp::QosPolicyKind, 9> {
+      QosPolicyKind::AvoidRosNamespaceConventions,
+      QosPolicyKind::Deadline,
+      QosPolicyKind::Durability,
+      QosPolicyKind::History,
+      QosPolicyKind::Depth,
+      QosPolicyKind::Lifespan,
+      QosPolicyKind::Liveliness,
+      QosPolicyKind::LivelinessLeaseDuration,
+      QosPolicyKind::Reliability,
+    };
+  }
+};
+
+/// \internal Trait used to specialize `declare_qos_parameters()` for subscriptions.
+struct SubscriptionQosParametersTraits
+{
+  static constexpr const char * entity_type() {return "subscription";}
+  static constexpr auto allowed_policies()
+  {
+    return std::array<::rclcpp::QosPolicyKind, 8> {
+      QosPolicyKind::AvoidRosNamespaceConventions,
+      QosPolicyKind::Deadline,
+      QosPolicyKind::Durability,
+      QosPolicyKind::History,
+      QosPolicyKind::Depth,
+      QosPolicyKind::Liveliness,
+      QosPolicyKind::LivelinessLeaseDuration,
+      QosPolicyKind::Reliability,
+    };
+  }
+};
+
+/// \internal Returns the given `policy` of the profile `qos` converted to a parameter value.
+inline
+::rclcpp::ParameterValue
+get_default_qos_param_value(rclcpp::QosPolicyKind policy, const rclcpp::QoS & qos);
+
+/// \internal Modify the given `policy` in `qos` to be `value`.
+inline
+void
+apply_qos_override(
+  rclcpp::QosPolicyKind policy, rclcpp::ParameterValue value, rclcpp::QoS & qos);
+
+inline
+rclcpp::ParameterValue
+declare_parameter_or_get(
+  rclcpp::node_interfaces::NodeParametersInterface & parameters_interface,
+  const std::string & param_name,
+  rclcpp::ParameterValue param_value,
+  rcl_interfaces::msg::ParameterDescriptor descriptor)
+{
+  try {
+    return parameters_interface.declare_parameter(
+      param_name, param_value, descriptor);
+  } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) {
+    return parameters_interface.get_parameter(param_name).get_parameter_value();
+  }
+}
+
+/// \internal Declare QoS parameters for the given entity.
+/**
+ * \tparam NodeT Node pointer or reference type.
+ * \tparam EntityQosParametersTraits A class with two static methods: `entity_type()` and
+ *  `allowed_policies()`. See `PublisherQosParametersTraits` and `SubscriptionQosParametersTraits`.
+ * \param options User provided options that indicate if QoS parameter overrides should be
+ *  declared or not, which policy can have overrides, and optionally a callback to validate the profile.
+ * \param node Parameters will be declared using this node.
+ * \param topic_name Name of the topic of the entity.
+ * \param default_qos User provided qos. It will be used as a default for the parameters declared.
+ * \return qos profile based on the user provided parameter overrides.
+ */
+template<typename NodeT, typename EntityQosParametersTraits>
+std::enable_if_t<
+  rclcpp::node_interfaces::has_node_parameters_interface<
+    decltype(std::declval<typename rcpputils::remove_pointer<NodeT>::type>())>::value ||
+  std::is_same<typename std::decay_t<NodeT>,
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr>::value,
+  rclcpp::QoS>
+declare_qos_parameters(
+  const ::rclcpp::QosOverridingOptions & options,
+  NodeT & node,
+  const std::string & topic_name,
+  const ::rclcpp::QoS & default_qos,
+  EntityQosParametersTraits)
+{
+  auto & parameters_interface = *rclcpp::node_interfaces::get_node_parameters_interface(node);
+  std::string param_prefix;
+  const auto & id = options.get_id();
+  {
+    std::ostringstream oss{"qos_overrides.", std::ios::ate};
+    oss << topic_name << "." << EntityQosParametersTraits::entity_type();
+    if (!id.empty()) {
+      oss << "_" << id;
+    }
+    oss << ".";
+    param_prefix = oss.str();
+  }
+  std::string param_description_suffix;
+  {
+    std::ostringstream oss{"} for ", std::ios::ate};
+    oss << EntityQosParametersTraits::entity_type() << " {" << topic_name << "}";
+    if (!id.empty()) {
+      oss << " with id {" << id << "}";
+    }
+    param_description_suffix = oss.str();
+  }
+  rclcpp::QoS qos = default_qos;
+  for (auto policy : EntityQosParametersTraits::allowed_policies()) {
+    if (
+      std::count(options.get_policy_kinds().begin(), options.get_policy_kinds().end(), policy))
+    {
+      std::ostringstream param_name{param_prefix, std::ios::ate};
+      param_name << qos_policy_kind_to_cstr(policy);
+      std::ostringstream param_desciption{"qos policy {", std::ios::ate};
+      param_desciption << qos_policy_kind_to_cstr(policy) << param_description_suffix;
+      rcl_interfaces::msg::ParameterDescriptor descriptor{};
+      descriptor.description = param_desciption.str();
+      descriptor.read_only = true;
+      auto value = declare_parameter_or_get(
+        parameters_interface, param_name.str(),
+        get_default_qos_param_value(policy, qos), descriptor);
+      ::rclcpp::detail::apply_qos_override(policy, value, qos);
+    }
+  }
+  const auto & validation_callback = options.get_validation_callback();
+  if (validation_callback) {
+    auto result = validation_callback(qos);
+    if (!result.successful) {
+      throw rclcpp::exceptions::InvalidQosOverridesException{
+              "validation callback failed: " + result.reason};
+    }
+  }
+  return qos;
+}
+
+// TODO(ivanpauno): This overload cannot declare the QoS parameters, as a node parameters interface
+// was not provided.
+template<typename NodeT, typename EntityQosParametersTraits>
+std::enable_if_t<
+  !(rclcpp::node_interfaces::has_node_parameters_interface<
+    decltype(std::declval<typename rcpputils::remove_pointer<NodeT>::type>())>::value ||
+  std::is_same<typename std::decay_t<NodeT>,
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr>::value),
+  rclcpp::QoS>
+declare_qos_parameters(
+  const ::rclcpp::QosOverridingOptions & options,
+  NodeT &,
+  const std::string &,
+  const ::rclcpp::QoS & default_qos,
+  EntityQosParametersTraits)
+{
+  if (options.get_policy_kinds().size()) {
+    std::runtime_error exc{
+      "passed non-default qos overriding options without providing a parameters interface"};
+    throw exc;
+  }
+  return default_qos;
+}
+
+/// \internal Helper function to get a rmw qos policy value from a string.
+#define RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING( \
+    kind_lower, kind_upper, parameter_value, rclcpp_qos) \
+  do { \
+    auto policy_string = (parameter_value).get<std::string>(); \
+    auto policy_value = rmw_qos_ ## kind_lower ## _policy_from_str(policy_string.c_str()); \
+    if (RMW_QOS_POLICY_ ## kind_upper ## _UNKNOWN == policy_value) { \
+      throw std::invalid_argument{"unknown QoS policy " #kind_lower " value: " + policy_string}; \
+    } \
+    ((rclcpp_qos).kind_lower)(policy_value); \
+  } while (0)
+
+inline
+void
+apply_qos_override(
+  rclcpp::QosPolicyKind policy, rclcpp::ParameterValue value, rclcpp::QoS & qos)
+{
+  switch (policy) {
+    case QosPolicyKind::AvoidRosNamespaceConventions:
+      qos.avoid_ros_namespace_conventions(value.get<bool>());
+      break;
+    case QosPolicyKind::Deadline:
+      qos.deadline(::rclcpp::Duration::from_nanoseconds(value.get<int64_t>()));
+      break;
+    case QosPolicyKind::Durability:
+      RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING(
+        durability, DURABILITY, value, qos);
+      break;
+    case QosPolicyKind::History:
+      RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING(
+        history, HISTORY, value, qos);
+      break;
+    case QosPolicyKind::Depth:
+      qos.get_rmw_qos_profile().depth = static_cast<size_t>(value.get<int64_t>());
+      break;
+    case QosPolicyKind::Lifespan:
+      qos.lifespan(::rclcpp::Duration::from_nanoseconds(value.get<int64_t>()));
+      break;
+    case QosPolicyKind::Liveliness:
+      RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING(
+        liveliness, LIVELINESS, value, qos);
+      break;
+    case QosPolicyKind::LivelinessLeaseDuration:
+      qos.liveliness_lease_duration(::rclcpp::Duration::from_nanoseconds(value.get<int64_t>()));
+      break;
+    case QosPolicyKind::Reliability:
+      RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING(
+        reliability, RELIABILITY, value, qos);
+      break;
+    default:
+      throw std::invalid_argument{"unknown QosPolicyKind"};
+  }
+}
+
+/// Convert `rmw_time_t` to `int64_t` that can be used as a parameter value.
+inline
+int64_t
+rmw_duration_to_int64_t(rmw_time_t rmw_duration)
+{
+  return ::rclcpp::Duration(
+    static_cast<int32_t>(rmw_duration.sec),
+    static_cast<uint32_t>(rmw_duration.nsec)
+  ).nanoseconds();
+}
+
+/// \internal Throw an exception if `policy_value_stringified` is NULL.
+inline
+const char *
+check_if_stringified_policy_is_null(const char * policy_value_stringified, QosPolicyKind kind)
+{
+  if (!policy_value_stringified) {
+    std::ostringstream oss{"unknown value for policy kind {", std::ios::ate};
+    oss << kind << "}";
+    throw std::invalid_argument{oss.str()};
+  }
+  return policy_value_stringified;
+}
+
+inline
+::rclcpp::ParameterValue
+get_default_qos_param_value(rclcpp::QosPolicyKind kind, const rclcpp::QoS & qos)
+{
+  using ParameterValue = ::rclcpp::ParameterValue;
+  const auto & rmw_qos = qos.get_rmw_qos_profile();
+  switch (kind) {
+    case QosPolicyKind::AvoidRosNamespaceConventions:
+      return ParameterValue(rmw_qos.avoid_ros_namespace_conventions);
+    case QosPolicyKind::Deadline:
+      return ParameterValue(rmw_duration_to_int64_t(rmw_qos.deadline));
+    case QosPolicyKind::Durability:
+      return ParameterValue(
+        check_if_stringified_policy_is_null(
+          rmw_qos_durability_policy_to_str(rmw_qos.durability), kind));
+    case QosPolicyKind::History:
+      return ParameterValue(
+        check_if_stringified_policy_is_null(
+          rmw_qos_history_policy_to_str(rmw_qos.history), kind));
+    case QosPolicyKind::Depth:
+      return ParameterValue(static_cast<int64_t>(rmw_qos.depth));
+    case QosPolicyKind::Lifespan:
+      return ParameterValue(rmw_duration_to_int64_t(rmw_qos.lifespan));
+    case QosPolicyKind::Liveliness:
+      return ParameterValue(
+        check_if_stringified_policy_is_null(
+          rmw_qos_liveliness_policy_to_str(rmw_qos.liveliness), kind));
+    case QosPolicyKind::LivelinessLeaseDuration:
+      return ParameterValue(rmw_duration_to_int64_t(rmw_qos.liveliness_lease_duration));
+    case QosPolicyKind::Reliability:
+      return ParameterValue(
+        check_if_stringified_policy_is_null(
+          rmw_qos_reliability_policy_to_str(rmw_qos.reliability), kind));
+    default:
+      throw std::invalid_argument{"unknown QoS policy kind"};
+  }
+}
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__QOS_PARAMETERS_HPP_

rclcpp/include/rclcpp/duration.hpp

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

rclcpp/include/rclcpp/exceptions/exceptions.hpp

@@ -100,6 +100,15 @@ public:
   {}
 };
 
+class UnimplementedError : public std::runtime_error
+{
+public:
+  UnimplementedError()
+  : std::runtime_error("This code is unimplemented.") {}
+  explicit UnimplementedError(const std::string & msg)
+  : std::runtime_error(msg) {}
+};
+
 /// Throw a C++ std::exception which was created based on an rcl error.
 /**
  * Passing nullptr for reset_error is safe and will avoid calling any function
@@ -273,6 +282,13 @@ class ParameterModifiedInCallbackException : public std::runtime_error
   using std::runtime_error::runtime_error;
 };
 
+/// Thrown if the QoS overrides provided aren't valid.
+class InvalidQosOverridesException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
 }  // namespace exceptions
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/executor.hpp

@@ -21,6 +21,7 @@
 #include <cstdlib>
 #include <iostream>
 #include <list>
+#include <map>
 #include <memory>
 #include <mutex>
 #include <string>
@@ -30,6 +31,7 @@
 #include "rcl/wait.h"
 
 #include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/guard_condition.hpp"
 #include "rclcpp/executor_options.hpp"
 #include "rclcpp/future_return_code.hpp"
 #include "rclcpp/memory_strategies.hpp"
@@ -37,10 +39,15 @@
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/scope_exit.hpp"
 
 namespace rclcpp
 {
 
+typedef std::map<rclcpp::CallbackGroup::WeakPtr,
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+    std::owner_less<rclcpp::CallbackGroup::WeakPtr>> WeakCallbackGroupsToNodesMap;
+
 // Forward declaration is used in convenience method signature.
 class Node;
 
@@ -75,13 +82,118 @@ public:
   virtual void
   spin() = 0;
 
+  /// Add a callback group to an executor.
+  /**
+   * An executor can have zero or more callback groups which provide work during `spin` functions.
+   * When an executor attempts to add a callback group, the executor checks to see if it is already
+   * associated with another executor, and if it has been, then an exception is thrown.
+   * Otherwise, the callback group is added to the executor.
+   *
+   * Adding a callback group with this method does not associate its node with this executor
+   * in any way
+   *
+   * \param[in] group_ptr a shared ptr that points to a callback group
+   * \param[in] node_ptr a shared pointer that points to a node base interface
+   * \param[in] notify True to trigger the interrupt guard condition during this function. If
+   * the executor is blocked at the rmw layer while waiting for work and it is notified that a new
+   * callback group was added, it will wake up.
+   * \throw std::runtime_error if the callback group is associated to an executor
+   */
+  RCLCPP_PUBLIC
+  virtual void
+  add_callback_group(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    bool notify = true);
+
+  /// Get callback groups that belong to executor.
+  /**
+   * This function returns a vector of weak pointers that point to callback groups that were
+   * associated with the executor.
+   * The callback groups associated with this executor may have been added with
+   * `add_callback_group`, or added when a node was added to the executor with `add_node`, or
+   * automatically added when it created by a node already associated with this executor and the
+   * automatically_add_to_executor_with_node parameter was true.
+   *
+   * \return a vector of weak pointers that point to callback groups that are associated with
+   * the executor
+   */
+  RCLCPP_PUBLIC
+  virtual std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_all_callback_groups();
+
+  /// Get callback groups that belong to executor.
+  /**
+   * This function returns a vector of weak pointers that point to callback groups that were
+   * associated with the executor.
+   * The callback groups associated with this executor have been added with
+   * `add_callback_group`.
+   *
+   * \return a vector of weak pointers that point to callback groups that are associated with
+   * the executor
+   */
+  RCLCPP_PUBLIC
+  virtual std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_manually_added_callback_groups();
+
+  /// Get callback groups that belong to executor.
+  /**
+   * This function returns a vector of weak pointers that point to callback groups that were
+   * added from a node that is associated with the executor.
+   * The callback groups are added when a node is added to the executor with `add_node`, or
+   * automatically if they are created in the future by that node and have the
+   * automatically_add_to_executor_with_node argument set to true.
+   *
+   * \return a vector of weak pointers that point to callback groups from a node associated with
+   * the executor
+   */
+  RCLCPP_PUBLIC
+  virtual std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_automatically_added_callback_groups_from_nodes();
+
+  /// Remove a callback group from the executor.
+  /**
+   * The callback group is removed from and disassociated with the executor.
+   * If the callback group removed was the last callback group from the node
+   * that is associated with the executor, the interrupt guard condition
+   * is triggered and node's guard condition is removed from the executor.
+   *
+   * This function only removes a callback group that was manually added with
+   * rclcpp::Executor::add_callback_group.
+   * To remove callback groups that were added from a node using
+   * rclcpp::Executor::add_node, use rclcpp::Executor::remove_node instead.
+   *
+   * \param[in] group_ptr Shared pointer to the callback group to be added.
+   * \param[in] notify True to trigger the interrupt guard condition during this function. If
+   * the executor is blocked at the rmw layer while waiting for work and it is notified that a
+   * callback group was removed, it will wake up.
+   * \throw std::runtime_error if node is deleted before callback group
+   * \throw std::runtime_error if the callback group is not associated with the executor
+   */
+  RCLCPP_PUBLIC
+  virtual void
+  remove_callback_group(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    bool notify = true);
+
   /// Add a node to the executor.
   /**
-   * An executor can have zero or more nodes which provide work during `spin` functions.
+   * Nodes have associated callback groups, and this method adds any of those callback groups
+   * to this executor which have their automatically_add_to_executor_with_node parameter true.
+   * The node is also associated with the executor so that future callback groups which are
+   * created on the node with the automatically_add_to_executor_with_node parameter set to true
+   * are also automatically associated with this executor.
+   *
+   * Callback groups with the automatically_add_to_executor_with_node parameter set to false must
+   * be manually added to an executor using the rclcpp::Executor::add_callback_group method.
+   *
+   * If a node is already associated with an executor, this method throws an exception.
+   *
    * \param[in] node_ptr Shared pointer to the node to be added.
    * \param[in] notify True to trigger the interrupt guard condition during this function. If
    * the executor is blocked at the rmw layer while waiting for work and it is notified that a new
    * node was added, it will wake up.
+   * \throw std::runtime_error if a node is already associated to an executor
    */
   RCLCPP_PUBLIC
   virtual void
@@ -97,10 +209,19 @@ public:
 
   /// Remove a node from the executor.
   /**
+   * Any callback groups automatically added when this node was added with
+   * rclcpp::Executor::add_node are automatically removed, and the node is no longer associated
+   * with this executor.
+   *
+   * This also means that future callback groups created by the given node are no longer
+   * automatically added to this executor.
+   *
    * \param[in] node_ptr Shared pointer to the node to remove.
    * \param[in] notify True to trigger the interrupt guard condition and wake up the executor.
    * This is useful if the last node was removed from the executor while the executor was blocked
    * waiting for work in another thread, because otherwise the executor would never be notified.
+   * \throw std::runtime_error if the node is not associated with an executor.
+   * \throw std::runtime_error if the node is not associated with this executor.
    */
   RCLCPP_PUBLIC
   virtual void
@@ -159,7 +280,7 @@ public:
   void
   spin_node_some(std::shared_ptr<rclcpp::Node> node);
 
-  /// Complete all available queued work without blocking.
+  /// Collect work once and execute all available work, optionally within a duration.
   /**
    * This function can be overridden. The default implementation is suitable for a
    * single-threaded model of execution.
@@ -174,6 +295,23 @@ public:
   virtual void
   spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0));
 
+  /// Collect and execute work repeatedly within a duration or until no more work is available.
+  /**
+   * This function can be overridden. The default implementation is suitable for a
+   * single-threaded model of execution.
+   * Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function
+   * to block (which may have unintended consequences).
+   * If the time that waitables take to be executed is longer than the period on which new waitables
+   * become ready, this method will execute work repeatedly until `max_duration` has elapsed.
+   *
+   * \param[in] max_duration The maximum amount of time to spend executing work. Must be positive.
+   * Note that spin_all() may take longer than this time as it only returns once max_duration has
+   * been exceeded.
+   */
+  RCLCPP_PUBLIC
+  virtual void
+  spin_all(std::chrono::nanoseconds max_duration);
+
   RCLCPP_PUBLIC
   virtual void
   spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
@@ -188,10 +326,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
@@ -212,9 +350,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) {
@@ -264,6 +407,10 @@ protected:
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
     std::chrono::nanoseconds timeout);
 
+  RCLCPP_PUBLIC
+  void
+  spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive);
+
   /// Find the next available executable and do the work associated with it.
   /**
    * \param[in] any_exec Union structure that can hold any executable type (timer, subscription,
@@ -300,44 +447,130 @@ protected:
 
   RCLCPP_PUBLIC
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
-  get_node_by_group(rclcpp::CallbackGroup::SharedPtr group);
+  get_node_by_group(
+    const 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,
+    const 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) RCPPUTILS_TSA_REQUIRES(mutex_);
+
+  /// 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) RCPPUTILS_TSA_REQUIRES(mutex_);
+
   RCLCPP_PUBLIC
   bool
   get_next_ready_executable(AnyExecutable & any_executable);
 
+  RCLCPP_PUBLIC
+  bool
+  get_next_ready_executable_from_map(
+    AnyExecutable & any_executable,
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
+
   RCLCPP_PUBLIC
   bool
   get_next_executable(
     AnyExecutable & any_executable,
     std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
+  /// Add all callback groups that can be automatically added from associated nodes.
+  /**
+   * The executor, before collecting entities, verifies if any callback group from
+   * nodes associated with the executor, which is not already associated to an executor,
+   * can be automatically added to this executor.
+   * This takes care of any callback group that has been added to a node but not explicitly added
+   * to the executor.
+   * It is important to note that in order for the callback groups to be automatically added to an
+   * executor through this function, the node of the callback groups needs to have been added
+   * through the `add_node` method.
+   */
+  RCLCPP_PUBLIC
+  virtual void
+  add_callback_groups_from_nodes_associated_to_executor() RCPPUTILS_TSA_REQUIRES(mutex_);
+
   /// 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.
   rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
 
+  std::shared_ptr<rclcpp::GuardCondition> shutdown_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();
 
   // Mutex to protect the subsequent memory_strategy_.
-  std::mutex memory_strategy_mutex_;
+  mutable std::mutex mutex_;
 
   /// The memory strategy: an interface for handling user-defined memory allocation strategies.
-  memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;
+  memory_strategy::MemoryStrategy::SharedPtr
+  memory_strategy_ RCPPUTILS_TSA_PT_GUARDED_BY(mutex_);
 
   /// The context associated with this executor.
   std::shared_ptr<rclcpp::Context> context_;
 
   RCLCPP_DISABLE_COPY(Executor)
 
-  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
-  std::list<const rcl_guard_condition_t *> guard_conditions_;
+  RCLCPP_PUBLIC
+  void
+  spin_once_impl(std::chrono::nanoseconds timeout);
+
+  typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+      const rcl_guard_condition_t *,
+      std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
+    WeakNodesToGuardConditionsMap;
+
+  /// maps nodes to guard conditions
+  WeakNodesToGuardConditionsMap
+  weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
+  /// maps callback groups associated to nodes
+  WeakCallbackGroupsToNodesMap
+  weak_groups_associated_with_executor_to_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
+  /// maps callback groups to nodes associated with executor
+  WeakCallbackGroupsToNodesMap
+  weak_groups_to_nodes_associated_with_executor_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
+  /// maps all callback groups to nodes
+  WeakCallbackGroupsToNodesMap
+  weak_groups_to_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
+  /// nodes that are associated with the executor
+  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>
+  weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
 };
 
 namespace executor

rclcpp/include/rclcpp/executors.hpp

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

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

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

rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp

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

rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp

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

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

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

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

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

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -18,7 +18,9 @@
 #include <rmw/rmw.h>
 
 #include <functional>
+#include <map>
 #include <memory>
+#include <stdexcept>
 #include <string>
 #include <utility>
 
@@ -92,8 +94,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,16 +104,44 @@ 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)
   {
-    (void)wait_set;
+    (void) wait_set;
     return buffer_->has_data();
   }
 
-  void execute()
+  std::shared_ptr<void>
+  take_data()
   {
-    execute_impl<CallbackMessageT>();
+    ConstMessageSharedPtr shared_msg;
+    MessageUniquePtr unique_msg;
+
+    if (any_callback_.use_take_shared_method()) {
+      shared_msg = buffer_->consume_shared();
+    } else {
+      unique_msg = buffer_->consume_unique();
+    }
+    return std::static_pointer_cast<void>(
+      std::make_shared<std::pair<ConstMessageSharedPtr, MessageUniquePtr>>(
+        std::pair<ConstMessageSharedPtr, MessageUniquePtr>(
+          shared_msg, std::move(unique_msg)))
+    );
+  }
+
+  void execute(std::shared_ptr<void> & data)
+  {
+    execute_impl<CallbackMessageT>(data);
   }
 
   void
@@ -144,26 +174,35 @@ private:
 
   template<typename T>
   typename std::enable_if<std::is_same<T, rcl_serialized_message_t>::value, void>::type
-  execute_impl()
+  execute_impl(std::shared_ptr<void> & data)
   {
+    (void)data;
     throw std::runtime_error("Subscription intra-process can't handle serialized messages");
   }
 
   template<class T>
   typename std::enable_if<!std::is_same<T, rcl_serialized_message_t>::value, void>::type
-  execute_impl()
+  execute_impl(std::shared_ptr<void> & data)
   {
+    if (!data) {
+      throw std::runtime_error("'data' is empty");
+    }
+
     rmw_message_info_t msg_info;
     msg_info.publisher_gid = {0, {0}};
     msg_info.from_intra_process = true;
 
+    auto shared_ptr = std::static_pointer_cast<std::pair<ConstMessageSharedPtr, MessageUniquePtr>>(
+      data);
+
     if (any_callback_.use_take_shared_method()) {
-      ConstMessageSharedPtr msg = buffer_->consume_shared();
-      any_callback_.dispatch_intra_process(msg, msg_info);
+      ConstMessageSharedPtr shared_msg = shared_ptr->first;
+      any_callback_.dispatch_intra_process(shared_msg, msg_info);
     } else {
-      MessageUniquePtr msg = buffer_->consume_unique();
-      any_callback_.dispatch_intra_process(std::move(msg), msg_info);
+      MessageUniquePtr unique_msg = std::move(shared_ptr->second);
+      any_callback_.dispatch_intra_process(std::move(unique_msg), msg_info);
     }
+    shared_ptr.reset();
   }
 
   AnySubscriptionCallback<CallbackMessageT, Alloc> any_callback_;

rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp

@@ -56,8 +56,12 @@ public:
   virtual bool
   is_ready(rcl_wait_set_t * wait_set) = 0;
 
+  virtual
+  std::shared_ptr<void>
+  take_data() = 0;
+
   virtual void
-  execute() = 0;
+  execute(std::shared_ptr<void> & data) = 0;
 
   virtual bool
   use_take_shared_method() const = 0;

rclcpp/include/rclcpp/future_return_code.hpp

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

rclcpp/include/rclcpp/graph_listener.hpp

@@ -63,7 +63,7 @@ class GraphListener : public std::enable_shared_from_this<GraphListener>
 {
 public:
   RCLCPP_PUBLIC
-  explicit GraphListener(std::shared_ptr<rclcpp::Context> parent_context);
+  explicit GraphListener(const rclcpp::Context::SharedPtr & parent_context);
 
   RCLCPP_PUBLIC
   virtual ~GraphListener();
@@ -160,14 +160,23 @@ protected:
   void
   run_loop();
 
+  RCLCPP_PUBLIC
+  void
+  init_wait_set();
+
+  RCLCPP_PUBLIC
+  void
+  cleanup_wait_set();
+
 private:
   RCLCPP_DISABLE_COPY(GraphListener)
 
   /** \internal */
   void
-  __shutdown(bool);
+  __shutdown();
 
-  rclcpp::Context::WeakPtr parent_context_;
+  std::weak_ptr<rclcpp::Context> weak_parent_context_;
+  std::shared_ptr<rcl_context_t> rcl_parent_context_;
 
   std::thread listener_thread_;
   bool is_started_;
@@ -179,7 +188,6 @@ private:
   std::vector<rclcpp::node_interfaces::NodeGraphInterface *> node_graph_interfaces_;
 
   rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
-  rcl_guard_condition_t * shutdown_guard_condition_;
   rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
 };
 

rclcpp/include/rclcpp/init_options.hpp

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

rclcpp/include/rclcpp/logger.hpp

@@ -21,6 +21,8 @@
 #include "rclcpp/visibility_control.hpp"
 
 #include "rcl/node.h"
+#include "rcutils/logging.h"
+#include "rcpputils/filesystem_helper.hpp"
 
 /**
  * \def RCLCPP_LOGGING_ENABLED
@@ -74,8 +76,32 @@ RCLCPP_PUBLIC
 Logger
 get_node_logger(const rcl_node_t * node);
 
+/// Get the current logging directory.
+/**
+ * For more details of how the logging directory is determined,
+ * see \ref rcl_logging_get_logging_directory.
+ *
+ * \returns the logging directory being used.
+ * \throws rclcpp::exceptions::RCLError if an unexpected error occurs.
+ */
+RCLCPP_PUBLIC
+rcpputils::fs::path
+get_logging_directory();
+
 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 +164,16 @@ public:
     }
     return Logger(*name_ + "." + suffix);
   }
+
+  /// Set level for current logger.
+  /**
+   * \param[in] level the logger's level
+   * \throws rclcpp::exceptions::RCLInvalidArgument if level is invalid.
+   * \throws rclcpp::exceptions::RCLError if other error happens.
+   */
+  RCLCPP_PUBLIC
+  void
+  set_level(Level level);
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/memory_strategy.hpp

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

rclcpp/include/rclcpp/node.hpp

@@ -26,6 +26,8 @@
 #include <utility>
 #include <vector>
 
+#include "rcutils/macros.h"
+
 #include "rcl/error_handling.h"
 #include "rcl/node.h"
 
@@ -139,7 +141,9 @@ public:
   /// Create and return a callback group.
   RCLCPP_PUBLIC
   rclcpp::CallbackGroup::SharedPtr
-  create_callback_group(rclcpp::CallbackGroupType group_type);
+  create_callback_group(
+    rclcpp::CallbackGroupType group_type,
+    bool automatically_add_to_executor_with_node = true);
 
   /// Return the list of callback groups in the node.
   RCLCPP_PUBLIC
@@ -235,7 +239,7 @@ public:
   /// Create and return a Client.
   /**
    * \param[in] service_name The topic to service on.
-   * \param[in] rmw_qos_profile_t Quality of service profile for client.
+   * \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.
    */
@@ -250,7 +254,7 @@ public:
   /**
    * \param[in] service_name The topic to service on.
    * \param[in] callback User-defined callback function.
-   * \param[in] rmw_qos_profile_t Quality of service profile for client.
+   * \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.
    */
@@ -812,6 +816,7 @@ public:
    * \throws std::bad_alloc if the allocation of the OnSetParametersCallbackHandle fails.
    */
   RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
   OnSetParametersCallbackHandle::SharedPtr
   add_on_set_parameters_callback(OnParametersSetCallbackType callback);
 
@@ -842,26 +847,6 @@ public:
   void
   remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler);
 
-  /// Register a callback to be called anytime a parameter is about to be changed.
-  /**
-   * \deprecated Use add_on_set_parameters_callback instead.
-   * With this method, only one callback can be set at a time. The callback that was previously
-   * set by this method is returned or `nullptr` if no callback was previously set.
-   *
-   * The callbacks added with `add_on_set_parameters_callback` are stored in a different place.
-   * `remove_on_set_parameters_callback` can't be used with the callbacks registered with this
-   * method. For removing it, use `set_on_parameters_set_callback(nullptr)`.
-   *
-   * \param[in] callback The callback to be called when the value for a
-   *   parameter is about to be set.
-   * \return The previous callback that was registered, if there was one,
-   *   otherwise nullptr.
-   */
-  [[deprecated("use add_on_set_parameters_callback(OnParametersSetCallbackType callback) instead")]]
-  RCLCPP_PUBLIC
-  OnParametersSetCallbackType
-  set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback);
-
   /// Get the fully-qualified names of all available nodes.
   /**
    * The fully-qualified name includes the local namespace and name of the node.
@@ -891,7 +876,8 @@ public:
 
   /// Return the number of publishers that are advertised on a given topic.
   /**
-   * \param[in] topic_name the topic_name on which to count the publishers.
+   * \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
    */
@@ -901,15 +887,21 @@ public:
     const std::string & node_name,
     const std::string & namespace_) const;
 
+  /// Return the number of publishers created for a given topic.
+  /**
+   * \param[in] topic_name the actual topic name used; it will not be automatically remapped.
+   * \return number of publishers that have been created for the given topic.
+   * \throws std::runtime_error if publishers could not be counted
+   */
   RCLCPP_PUBLIC
   size_t
   count_publishers(const std::string & topic_name) const;
 
-  /// Return the number of subscribers who have created a subscription for a given topic.
+  /// Return the number of subscribers created for a given topic.
   /**
-   * \param[in] topic_name the topic_name on which to count the subscribers.
-   * \return number of subscribers who have created a subscription for a given topic.
-   * \throws std::runtime_error if publishers could not be counted
+   * \param[in] topic_name the actual topic name used; it will not be automatically remapped.
+   * \return number of subscribers that have been created for the given topic.
+   * \throws std::runtime_error if subscribers could not be counted
    */
   RCLCPP_PUBLIC
   size_t
@@ -930,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.
@@ -956,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.
@@ -1062,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();
@@ -1190,10 +1182,6 @@ protected:
 private:
   RCLCPP_DISABLE_COPY(Node)
 
-  RCLCPP_PUBLIC
-  bool
-  group_in_node(CallbackGroup::SharedPtr group);
-
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
   rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -83,7 +83,9 @@ public:
 
   RCLCPP_PUBLIC
   rclcpp::CallbackGroup::SharedPtr
-  create_callback_group(rclcpp::CallbackGroupType group_type) override;
+  create_callback_group(
+    rclcpp::CallbackGroupType group_type,
+    bool automatically_add_to_executor_with_node = true) override;
 
   RCLCPP_PUBLIC
   rclcpp::CallbackGroup::SharedPtr
@@ -116,6 +118,10 @@ public:
   bool
   get_enable_topic_statistics_default() const override;
 
+  std::string
+  resolve_topic_or_service_name(
+    const std::string & name, bool is_service, bool only_expand = false) const override;
+
 private:
   RCLCPP_DISABLE_COPY(NodeBase)
 

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -106,7 +106,9 @@ public:
   RCLCPP_PUBLIC
   virtual
   rclcpp::CallbackGroup::SharedPtr
-  create_callback_group(rclcpp::CallbackGroupType group_type) = 0;
+  create_callback_group(
+    rclcpp::CallbackGroupType group_type,
+    bool automatically_add_to_executor_with_node = true) = 0;
 
   /// Return the default callback group.
   RCLCPP_PUBLIC
@@ -161,6 +163,13 @@ public:
   virtual
   bool
   get_enable_topic_statistics_default() const = 0;
+
+  /// Expand and remap a given topic or service name.
+  RCLCPP_PUBLIC
+  virtual
+  std::string
+  resolve_topic_or_service_name(
+    const std::string & name, bool is_service, bool only_expand = false) const = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -21,6 +21,8 @@
 #include <string>
 #include <vector>
 
+#include "rcutils/macros.h"
+
 #include "rcl_interfaces/msg/list_parameters_result.hpp"
 #include "rcl_interfaces/msg/parameter_descriptor.hpp"
 #include "rcl_interfaces/msg/parameter_event.hpp"
@@ -160,6 +162,7 @@ public:
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const override;
 
   RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
   OnSetParametersCallbackHandle::SharedPtr
   add_on_set_parameters_callback(OnParametersSetCallbackType callback) override;
 
@@ -167,11 +170,6 @@ public:
   void
   remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) override;
 
-  [[deprecated("use add_on_set_parameters_callback(OnParametersSetCallbackType callback) instead")]]
-  RCLCPP_PUBLIC
-  OnParametersSetCallbackType
-  set_on_parameters_set_callback(OnParametersSetCallbackType callback) override;
-
   RCLCPP_PUBLIC
   const std::map<std::string, rclcpp::ParameterValue> &
   get_parameter_overrides() const override;

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_services.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_topics.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp

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

rclcpp/include/rclcpp/node_options.hpp

@@ -46,6 +46,8 @@ public:
    *   - enable_topic_statistics = false
    *   - start_parameter_services = true
    *   - start_parameter_event_publisher = true
+   *   - clock_qos = rclcpp::ClockQoS()
+   *   - rosout_qos = rclcpp::RosoutQoS()
    *   - parameter_event_qos = rclcpp::ParameterEventQoS
    *     - with history setting and depth from rmw_qos_profile_parameter_events
    *   - parameter_event_publisher_options = rclcpp::PublisherOptionsBase
@@ -243,6 +245,19 @@ public:
   NodeOptions &
   start_parameter_event_publisher(bool start_parameter_event_publisher);
 
+  /// Return a reference to the clock QoS.
+  RCLCPP_PUBLIC
+  const rclcpp::QoS &
+  clock_qos() const;
+
+  /// Set the clock QoS.
+  /**
+   * The QoS settings to be used for the publisher on /clock topic, if enabled.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  clock_qos(const rclcpp::QoS & clock_qos);
+
   /// Return a reference to the parameter_event_qos QoS.
   RCLCPP_PUBLIC
   const rclcpp::QoS &
@@ -256,6 +271,19 @@ public:
   NodeOptions &
   parameter_event_qos(const rclcpp::QoS & parameter_event_qos);
 
+  /// Return a reference to the rosout QoS.
+  RCLCPP_PUBLIC
+  const rclcpp::QoS &
+  rosout_qos() const;
+
+  /// Set the rosout QoS.
+  /**
+   * The QoS settings to be used for the publisher on /rosout topic, if enabled.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  rosout_qos(const rclcpp::QoS & rosout_qos);
+
   /// Return a reference to the parameter_event_publisher_options.
   RCLCPP_PUBLIC
   const rclcpp::PublisherOptionsBase &
@@ -327,11 +355,6 @@ public:
   NodeOptions &
   allocator(rcl_allocator_t allocator);
 
-protected:
-  /// Retrieve the ROS_DOMAIN_ID environment variable and populate options.
-  size_t
-  get_domain_id_from_env() const;
-
 private:
   // This is mutable to allow for a const accessor which lazily creates the node options instance.
   /// Underlying rcl_node_options structure.
@@ -359,10 +382,14 @@ private:
 
   bool start_parameter_event_publisher_ {true};
 
+  rclcpp::QoS clock_qos_ = rclcpp::ClockQoS();
+
   rclcpp::QoS parameter_event_qos_ = rclcpp::ParameterEventsQoS(
     rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)
   );
 
+  rclcpp::QoS rosout_qos_ = rclcpp::RosoutQoS();
+
   rclcpp::PublisherOptionsBase parameter_event_publisher_options_ = rclcpp::PublisherOptionsBase();
 
   bool allow_undeclared_parameters_ {false};

rclcpp/include/rclcpp/parameter_client.hpp

@@ -15,6 +15,8 @@
 #ifndef RCLCPP__PARAMETER_CLIENT_HPP_
 #define RCLCPP__PARAMETER_CLIENT_HPP_
 
+#include <functional>
+#include <future>
 #include <memory>
 #include <string>
 #include <utility>
@@ -48,13 +50,13 @@ public:
 
   /// Create an async parameters client.
   /**
-   * \param node_base_interface[in] The node base interface of the corresponding node.
-   * \param node_topics_interface[in] Node topic base interface.
-   * \param node_graph_interface[in] The node graph interface of the corresponding node.
-   * \param node_services_interface[in] Node service interface.
-   * \param remote_node_name[in] (optional) name of the remote node
-   * \param qos_profile[in] (optional) The rmw qos profile to use to subscribe
-   * \param group[in] (optional)  The async parameter client will be added to this callback group.
+   * \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(
@@ -68,31 +70,49 @@ public:
 
   /// Constructor
   /**
-   * \param node[in] The  async paramters client will be added to this node.
-   * \param remote_node_name[in] (optional) name of the remote node
-   * \param qos_profile[in] (optional) The rmw qos profile to use to subscribe
-   * \param group[in] (optional)  The async parameter client will be added to this callback group.
+   * \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.
    */
-  RCLCPP_PUBLIC
+  template<typename NodeT>
   AsyncParametersClient(
-    const rclcpp::Node::SharedPtr node,
+    const std::shared_ptr<NodeT> node,
     const std::string & remote_node_name = "",
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters,
-    rclcpp::CallbackGroup::SharedPtr group = nullptr);
+    rclcpp::CallbackGroup::SharedPtr group = nullptr)
+  : AsyncParametersClient(
+      node->get_node_base_interface(),
+      node->get_node_topics_interface(),
+      node->get_node_graph_interface(),
+      node->get_node_services_interface(),
+      remote_node_name,
+      qos_profile,
+      group)
+  {}
 
   /// Constructor
   /**
-   * \param node[in] The  async paramters client will be added to this node.
-   * \param remote_node_name[in] (optional) name of the remote node
-   * \param qos_profile[in] (optional) The rmw qos profile to use to subscribe
-   * \param group[in] (optional)  The async parameter client will be added to this callback group.
+   * \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.
    */
-  RCLCPP_PUBLIC
+  template<typename NodeT>
   AsyncParametersClient(
-    rclcpp::Node * node,
+    NodeT * node,
     const std::string & remote_node_name = "",
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters,
-    rclcpp::CallbackGroup::SharedPtr group = nullptr);
+    rclcpp::CallbackGroup::SharedPtr group = nullptr)
+  : AsyncParametersClient(
+      node->get_node_base_interface(),
+      node->get_node_topics_interface(),
+      node->get_node_graph_interface(),
+      node->get_node_services_interface(),
+      remote_node_name,
+      qos_profile,
+      group)
+  {}
 
   RCLCPP_PUBLIC
   std::shared_future<std::vector<rclcpp::Parameter>>
@@ -102,6 +122,14 @@ public:
       void(std::shared_future<std::vector<rclcpp::Parameter>>)
     > callback = nullptr);
 
+  RCLCPP_PUBLIC
+  std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>>
+  describe_parameters(
+    const std::vector<std::string> & names,
+    std::function<
+      void(std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>>)
+    > callback = nullptr);
+
   RCLCPP_PUBLIC
   std::shared_future<std::vector<rclcpp::ParameterType>>
   get_parameter_types(
@@ -177,7 +205,7 @@ public:
   {
     return rclcpp::create_subscription<rcl_interfaces::msg::ParameterEvent>(
       node,
-      "parameter_events",
+      "/parameter_events",
       qos,
       std::forward<CallbackT>(callback),
       options);
@@ -237,31 +265,61 @@ class SyncParametersClient
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient)
 
-  RCLCPP_PUBLIC
+  template<typename NodeT>
   explicit SyncParametersClient(
-    rclcpp::Node::SharedPtr node,
+    std::shared_ptr<NodeT> node,
     const std::string & remote_node_name = "",
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters)
+  : SyncParametersClient(
+      std::make_shared<rclcpp::executors::SingleThreadedExecutor>(),
+      node,
+      remote_node_name,
+      qos_profile)
+  {}
 
-  RCLCPP_PUBLIC
+  template<typename NodeT>
   SyncParametersClient(
     rclcpp::Executor::SharedPtr executor,
-    rclcpp::Node::SharedPtr node,
+    std::shared_ptr<NodeT> node,
     const std::string & remote_node_name = "",
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters)
+  : SyncParametersClient(
+      executor,
+      node->get_node_base_interface(),
+      node->get_node_topics_interface(),
+      node->get_node_graph_interface(),
+      node->get_node_services_interface(),
+      remote_node_name,
+      qos_profile)
+  {}
 
-  RCLCPP_PUBLIC
-  explicit SyncParametersClient(
-    rclcpp::Node * node,
+  template<typename NodeT>
+  SyncParametersClient(
+    NodeT * node,
     const std::string & remote_node_name = "",
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters)
+  : SyncParametersClient(
+      std::make_shared<rclcpp::executors::SingleThreadedExecutor>(),
+      node,
+      remote_node_name,
+      qos_profile)
+  {}
 
-  RCLCPP_PUBLIC
+  template<typename NodeT>
   SyncParametersClient(
     rclcpp::Executor::SharedPtr executor,
-    rclcpp::Node * node,
+    NodeT * node,
     const std::string & remote_node_name = "",
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters)
+  : SyncParametersClient(
+      executor,
+      node->get_node_base_interface(),
+      node->get_node_topics_interface(),
+      node->get_node_graph_interface(),
+      node->get_node_services_interface(),
+      remote_node_name,
+      qos_profile)
+  {}
 
   RCLCPP_PUBLIC
   SyncParametersClient(
@@ -271,11 +329,30 @@ 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
+  template<typename RepT = int64_t, typename RatioT = std::milli>
   std::vector<rclcpp::Parameter>
-  get_parameters(const std::vector<std::string> & parameter_names);
+  get_parameters(
+    const std::vector<std::string> & parameter_names,
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
+  {
+    return get_parameters(
+      parameter_names,
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
+  }
 
   RCLCPP_PUBLIC
   bool
@@ -319,23 +396,67 @@ public:
     );
   }
 
-  RCLCPP_PUBLIC
+  template<typename RepT = int64_t, typename RatioT = std::milli>
+  std::vector<rcl_interfaces::msg::ParameterDescriptor>
+  describe_parameters(
+    const std::vector<std::string> & parameter_names,
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
+  {
+    return describe_parameters(
+      parameter_names,
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
+  }
+
+  template<typename RepT = int64_t, typename RatioT = std::milli>
   std::vector<rclcpp::ParameterType>
-  get_parameter_types(const std::vector<std::string> & parameter_names);
+  get_parameter_types(
+    const std::vector<std::string> & parameter_names,
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
+  {
+    return get_parameter_types(
+      parameter_names,
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
+  }
 
-  RCLCPP_PUBLIC
+  template<typename RepT = int64_t, typename RatioT = std::milli>
   std::vector<rcl_interfaces::msg::SetParametersResult>
-  set_parameters(const std::vector<rclcpp::Parameter> & parameters);
+  set_parameters(
+    const std::vector<rclcpp::Parameter> & parameters,
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
+  {
+    return set_parameters(
+      parameters,
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
+  }
 
-  RCLCPP_PUBLIC
+  template<typename RepT = int64_t, typename RatioT = std::milli>
   rcl_interfaces::msg::SetParametersResult
-  set_parameters_atomically(const std::vector<rclcpp::Parameter> & parameters);
+  set_parameters_atomically(
+    const std::vector<rclcpp::Parameter> & parameters,
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
+  {
+    return set_parameters_atomically(
+      parameters,
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
+  }
 
-  RCLCPP_PUBLIC
+  template<typename RepT = int64_t, typename RatioT = std::milli>
   rcl_interfaces::msg::ListParametersResult
   list_parameters(
     const std::vector<std::string> & parameter_prefixes,
-    uint64_t depth);
+    uint64_t depth,
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
+  {
+    return list_parameters(
+      parameter_prefixes,
+      depth,
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
+  }
 
   template<typename CallbackT>
   typename rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
@@ -378,6 +499,44 @@ public:
     return async_parameters_client_->wait_for_service(timeout);
   }
 
+protected:
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::Parameter>
+  get_parameters(
+    const std::vector<std::string> & parameter_names,
+    std::chrono::nanoseconds timeout);
+
+  RCLCPP_PUBLIC
+  std::vector<rcl_interfaces::msg::ParameterDescriptor>
+  describe_parameters(
+    const std::vector<std::string> & parameter_names,
+    std::chrono::nanoseconds timeout);
+
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::ParameterType>
+  get_parameter_types(
+    const std::vector<std::string> & parameter_names,
+    std::chrono::nanoseconds timeout);
+
+  RCLCPP_PUBLIC
+  std::vector<rcl_interfaces::msg::SetParametersResult>
+  set_parameters(
+    const std::vector<rclcpp::Parameter> & parameters,
+    std::chrono::nanoseconds timeout);
+
+  RCLCPP_PUBLIC
+  rcl_interfaces::msg::SetParametersResult
+  set_parameters_atomically(
+    const std::vector<rclcpp::Parameter> & parameters,
+    std::chrono::nanoseconds timeout);
+
+  RCLCPP_PUBLIC
+  rcl_interfaces::msg::ListParametersResult
+  list_parameters(
+    const std::vector<std::string> & parameter_prefixes,
+    uint64_t depth,
+    std::chrono::nanoseconds timeout);
+
 private:
   rclcpp::Executor::SharedPtr executor_;
   const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_;

rclcpp/include/rclcpp/publisher.hpp

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

rclcpp/include/rclcpp/publisher_options.hpp

@@ -26,6 +26,7 @@
 #include "rclcpp/intra_process_setting.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/qos_event.hpp"
+#include "rclcpp/qos_overriding_options.hpp"
 
 namespace rclcpp
 {
@@ -50,6 +51,8 @@ struct PublisherOptionsBase
   /// Optional RMW implementation specific payload to be used during creation of the publisher.
   std::shared_ptr<rclcpp::detail::RMWImplementationSpecificPublisherPayload>
   rmw_implementation_payload = nullptr;
+
+  QosOverridingOptions qos_overriding_options;
 };
 
 /// Structure containing optional configuration for Publishers.

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,8 +27,41 @@
 namespace rclcpp
 {
 
+RCLCPP_PUBLIC
 std::string qos_policy_name_from_kind(rmw_qos_policy_kind_t policy_kind);
 
+enum class HistoryPolicy
+{
+  KeepLast = RMW_QOS_POLICY_HISTORY_KEEP_LAST,
+  KeepAll = RMW_QOS_POLICY_HISTORY_KEEP_ALL,
+  SystemDefault = RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT,
+  Unknown = RMW_QOS_POLICY_HISTORY_UNKNOWN,
+};
+
+enum class ReliabilityPolicy
+{
+  BestEffort = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
+  Reliable = RMW_QOS_POLICY_RELIABILITY_RELIABLE,
+  SystemDefault = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT,
+  Unknown = RMW_QOS_POLICY_RELIABILITY_UNKNOWN,
+};
+
+enum class DurabilityPolicy
+{
+  Volatile = RMW_QOS_POLICY_DURABILITY_VOLATILE,
+  TransientLocal = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
+  SystemDefault = RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT,
+  Unknown = RMW_QOS_POLICY_DURABILITY_UNKNOWN,
+};
+
+enum class LivelinessPolicy
+{
+  Automatic = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC,
+  ManualByTopic = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC,
+  SystemDefault = RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
+  Unknown = RMW_QOS_POLICY_LIVELINESS_UNKNOWN,
+};
+
 /// QoS initialization values, cannot be created directly, use KeepAll or KeepLast instead.
 struct RCLCPP_PUBLIC QoSInitialization
 {
@@ -56,10 +90,32 @@ struct RCLCPP_PUBLIC KeepLast : public rclcpp::QoSInitialization
 };
 
 /// Encapsulation of Quality of Service settings.
+/**
+ * Quality of Service settings control the behavior of publishers, subscriptions,
+ * and other entities, and includes things like how data is sent or resent,
+ * how data is buffered on the publishing and subscribing side, and other things.
+ * See:
+ *   <a href="https://index.ros.org/doc/ros2/Concepts/About-Quality-of-Service-Settings">
+ *     https://index.ros.org/doc/ros2/Concepts/About-Quality-of-Service-Settings
+ *   </a>
+ */
 class RCLCPP_PUBLIC QoS
 {
 public:
-  /// Constructor which allows you to construct a QoS by giving the only required settings.
+  /// Create a QoS by specifying only the history policy and history depth.
+  /**
+   * When using the default initial profile, the defaults will include:
+   *
+   *   - \link rclcpp::ReliabilityPolicy::Reliable ReliabilityPolicy::Reliable\endlink
+   *   - \link rclcpp::DurabilityPolicy::Volatile DurabilityPolicy::Volatile\endlink
+   *
+   * See rmw_qos_profile_default for a full list of default settings.
+   * If some other rmw_qos_profile_t is passed to initial_profile, then the defaults will derive from
+   * that profile instead.
+   *
+   * \param[in] qos_initialization Specifies history policy and history depth.
+   * \param[in] initial_profile The rmw_qos_profile_t instance on which to base the default settings.
+   */
   explicit
   QoS(
     const QoSInitialization & qos_initialization,
@@ -67,7 +123,11 @@ public:
 
   /// Conversion constructor to ease construction in the common case of just specifying depth.
   /**
-   * Convenience constructor, equivalent to QoS(KeepLast(history_depth)).
+   * This is a convenience constructor that calls QoS(KeepLast(history_depth)).
+   *
+   * \param[in] history_depth How many messages can be queued when publishing
+   *   with a Publisher, or how many messages can be queued before being replaced
+   *   by a Subscription.
    */
   // cppcheck-suppress noExplicitConstructor
   QoS(size_t history_depth);  // NOLINT(runtime/explicit): conversion constructor
@@ -80,6 +140,10 @@ public:
   const rmw_qos_profile_t &
   get_rmw_qos_profile() const;
 
+  /// Set the history policy.
+  QoS &
+  history(HistoryPolicy history);
+
   /// Set the history policy.
   QoS &
   history(rmw_qos_history_policy_t history);
@@ -96,6 +160,10 @@ public:
   QoS &
   reliability(rmw_qos_reliability_policy_t reliability);
 
+  /// Set the reliability setting.
+  QoS &
+  reliability(ReliabilityPolicy reliability);
+
   /// Set the reliability setting to reliable.
   QoS &
   reliable();
@@ -108,6 +176,10 @@ public:
   QoS &
   durability(rmw_qos_durability_policy_t durability);
 
+  /// Set the durability setting.
+  QoS &
+  durability(DurabilityPolicy durability);
+
   /// Set the durability setting to volatile.
   /**
     * Note that this cannot be named `volatile` because it is a C++ keyword.
@@ -139,6 +211,10 @@ public:
   QoS &
   liveliness(rmw_qos_liveliness_policy_t liveliness);
 
+  /// Set the liveliness setting.
+  QoS &
+  liveliness(LivelinessPolicy liveliness);
+
   /// Set the liveliness_lease_duration setting.
   QoS &
   liveliness_lease_duration(rmw_time_t liveliness_lease_duration);
@@ -151,6 +227,42 @@ public:
   QoS &
   avoid_ros_namespace_conventions(bool avoid_ros_namespace_conventions);
 
+  /// Get the history qos policy.
+  HistoryPolicy
+  history() const;
+
+  /// Get the history depth.
+  size_t
+  depth() const;
+
+  /// Get the reliability policy.
+  ReliabilityPolicy
+  reliability() const;
+
+  /// Get the durability policy.
+  DurabilityPolicy
+  durability() const;
+
+  /// Get the deadline duration setting.
+  rclcpp::Duration
+  deadline() const;
+
+  /// Get the lifespan duration setting.
+  rclcpp::Duration
+  lifespan() const;
+
+  /// Get the liveliness policy.
+  LivelinessPolicy
+  liveliness() const;
+
+  /// Get the liveliness lease duration setting.
+  rclcpp::Duration
+  liveliness_lease_duration() const;
+
+  /// Get the `avoid ros namespace convention` setting.
+  bool
+  avoid_ros_namespace_conventions() const;
+
 private:
   rmw_qos_profile_t rmw_qos_profile_;
 };
@@ -161,6 +273,26 @@ bool operator==(const QoS & left, const QoS & right);
 RCLCPP_PUBLIC
 bool operator!=(const QoS & left, const QoS & right);
 
+/**
+ * Clock QoS class
+ *    - History: Keep last,
+ *    - Depth: 1,
+ *    - Reliability: Best effort,
+ *    - Durability: Volatile,
+ *    - Deadline: Default,
+ *    - Lifespan: Default,
+ *    - Liveliness: System default,
+ *    - Liveliness lease duration: default,
+ *    - avoid ros namespace conventions: false
+ */
+class RCLCPP_PUBLIC ClockQoS : public QoS
+{
+public:
+  explicit
+  ClockQoS(
+    const QoSInitialization & qos_initialization = KeepLast(1));
+};
+
 /**
  * Sensor Data QoS class
  *    - History: Keep last,
@@ -249,6 +381,28 @@ public:
   ));
 };
 
+/**
+ * Rosout QoS class
+ *    - History: Keep last,
+ *    - Depth: 1000,
+ *    - Reliability: Reliable,
+ *    - Durability: TRANSIENT_LOCAL,
+ *    - Deadline: Default,
+ *    - Lifespan: {10, 0},
+ *    - Liveliness: System default,
+ *    - Liveliness lease duration: default,
+ *    - Avoid ros namespace conventions: false
+ */
+class RCLCPP_PUBLIC RosoutQoS : public QoS
+{
+public:
+  explicit
+  RosoutQoS(
+    const QoSInitialization & rosout_qos_initialization = (
+      QoSInitialization::from_rmw(rcl_qos_profile_rosout_default)
+  ));
+};
+
 /**
  * System defaults QoS class
  *    - History: System default,

rclcpp/include/rclcpp/qos_event.hpp

@@ -16,6 +16,8 @@
 #define RCLCPP__QOS_EVENT_HPP_
 
 #include <functional>
+#include <memory>
+#include <stdexcept>
 #include <string>
 
 #include "rcl/error_handling.h"
@@ -34,6 +36,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 +44,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 +63,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
@@ -128,21 +133,31 @@ public:
     }
   }
 
-  /// Execute any entities of the Waitable that are ready.
-  void
-  execute() override
+  /// Take data so that the callback cannot be scheduled again
+  std::shared_ptr<void>
+  take_data() override
   {
     EventCallbackInfoT callback_info;
-
     rcl_ret_t ret = rcl_take_event(&event_handle_, &callback_info);
     if (ret != RCL_RET_OK) {
       RCUTILS_LOG_ERROR_NAMED(
         "rclcpp",
         "Couldn't take event info: %s", rcl_get_error_string().str);
-      return;
+      return nullptr;
     }
+    return std::static_pointer_cast<void>(std::make_shared<EventCallbackInfoT>(callback_info));
+  }
 
-    event_callback_(callback_info);
+  /// Execute any entities of the Waitable that are ready.
+  void
+  execute(std::shared_ptr<void> & data) override
+  {
+    if (!data) {
+      throw std::runtime_error("'data' is empty");
+    }
+    auto callback_ptr = std::static_pointer_cast<EventCallbackInfoT>(data);
+    event_callback_(*callback_ptr);
+    callback_ptr.reset();
   }
 
 private:

rclcpp/include/rclcpp/qos_overriding_options.hpp

@@ -0,0 +1,154 @@
+// 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__QOS_OVERRIDING_OPTIONS_HPP_
+#define RCLCPP__QOS_OVERRIDING_OPTIONS_HPP_
+
+#include <functional>
+#include <initializer_list>
+#include <ostream>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "rclcpp/qos.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
+#include "rmw/qos_policy_kind.h"
+
+namespace rclcpp
+{
+
+enum class RCLCPP_PUBLIC_TYPE QosPolicyKind
+{
+  AvoidRosNamespaceConventions = RMW_QOS_POLICY_AVOID_ROS_NAMESPACE_CONVENTIONS,
+  Deadline = RMW_QOS_POLICY_DEADLINE,
+  Depth = RMW_QOS_POLICY_DEPTH,
+  Durability = RMW_QOS_POLICY_DURABILITY,
+  History = RMW_QOS_POLICY_HISTORY,
+  Lifespan = RMW_QOS_POLICY_LIFESPAN,
+  Liveliness = RMW_QOS_POLICY_LIVELINESS,
+  LivelinessLeaseDuration = RMW_QOS_POLICY_LIVELINESS_LEASE_DURATION,
+  Reliability = RMW_QOS_POLICY_RELIABILITY,
+  Invalid = RMW_QOS_POLICY_INVALID,
+};
+
+RCLCPP_PUBLIC
+const char *
+qos_policy_kind_to_cstr(const QosPolicyKind & qpk);
+
+RCLCPP_PUBLIC
+std::ostream &
+operator<<(std::ostream & os, const QosPolicyKind & qpk);
+
+using QosCallbackResult = rcl_interfaces::msg::SetParametersResult;
+using QosCallback = std::function<QosCallbackResult(const rclcpp::QoS &)>;
+
+namespace detail
+{
+// forward declare
+template<typename T>
+class QosParameters;
+}
+
+/// Options that are passed in subscription/publisher constructor to specify QoSConfigurability.
+/**
+ * This options struct allows configuring:
+ * - Which policy kinds will have declared parameters.
+ * - An optional callback, that will be called to validate the final qos profile.
+ * - An optional id. In the case that different qos are desired for two publishers/subscriptions in
+ *   the same topic, this id will allow disambiguating them.
+ *
+ * Example parameter file:
+ *
+ * ```yaml
+ * my_node_name:
+ *  ros__parameters:
+ *    qos_overrides:
+ *      /my/topic/name:
+ *        publisher:  # publisher without provided id
+ *          reliability: reliable
+ *          depth: 100
+ *        publisher_my_id:  # publisher with `id="my_id"
+ *          reliability: reliable
+ *          depth: 10
+ * ```
+ */
+class QosOverridingOptions
+{
+public:
+  /// Default constructor, no overrides allowed.
+  RCLCPP_PUBLIC
+  QosOverridingOptions() = default;
+
+  /// Construct passing a list of QoS policies and a verification callback.
+  /**
+   * This constructor is implicit, e.g.:
+   * ```cpp
+   * node->create_publisher(
+   *   "topic_name",
+   *   default_qos_profile,
+   *   {
+   *     {QosPolicyKind::Reliability},
+   *     [] (auto && qos) {return check_qos_validity(qos)},
+   *     "my_id"
+   *   });
+   * ```
+   * \param policy_kinds list of policy kinds that will be reconfigurable.
+   * \param validation_callback callbak that will be called to validate the validity of
+   *   the qos profile set by the user.
+   * \param id id of the entity.
+   */
+  RCLCPP_PUBLIC
+  QosOverridingOptions(
+    std::initializer_list<QosPolicyKind> policy_kinds,
+    QosCallback validation_callback = nullptr,
+    std::string id = {});
+
+  RCLCPP_PUBLIC
+  const std::string &
+  get_id() const;
+
+  RCLCPP_PUBLIC
+  const std::vector<QosPolicyKind> &
+  get_policy_kinds() const;
+
+  RCLCPP_PUBLIC
+  const QosCallback &
+  get_validation_callback() const;
+
+  /// Construct passing a list of QoS policies and a verification callback.
+  /**
+   * Same as `QosOverridingOptions` constructor, but only declares the default policies:
+   *
+   * History, Depth, Reliability.
+   */
+  RCLCPP_PUBLIC
+  static
+  QosOverridingOptions
+  with_default_policies(QosCallback validation_callback = nullptr, std::string id = {});
+
+private:
+  /// \internal Id of the entity requesting to create parameters.
+  std::string id_;
+  /// \internal Policy kinds that are allowed to be reconfigured.
+  std::vector<QosPolicyKind> policy_kinds_;
+  /// \internal Validation callback that will be called to verify the profile.
+  QosCallback validation_callback_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__QOS_OVERRIDING_OPTIONS_HPP_

rclcpp/include/rclcpp/serialization.hpp

@@ -60,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(
@@ -69,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/service.hpp

@@ -223,8 +223,8 @@ public:
     }
     TRACEPOINT(
       rclcpp_service_callback_added,
-      (const void *)get_service_handle().get(),
-      (const void *)&any_callback_);
+      static_cast<const void *>(get_service_handle().get()),
+      static_cast<const void *>(&any_callback_));
 #ifndef TRACETOOLS_DISABLED
     any_callback_.register_callback_for_tracing();
 #endif
@@ -237,7 +237,7 @@ public:
    * 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] service_handle service handle.
    * \param[in] any_callback User defined callback to call when a client request is received.
    */
   Service(
@@ -258,8 +258,8 @@ public:
     service_handle_ = service_handle;
     TRACEPOINT(
       rclcpp_service_callback_added,
-      (const void *)get_service_handle().get(),
-      (const void *)&any_callback_);
+      static_cast<const void *>(get_service_handle().get()),
+      static_cast<const void *>(&any_callback_));
 #ifndef TRACETOOLS_DISABLED
     any_callback_.register_callback_for_tracing();
 #endif
@@ -272,7 +272,7 @@ public:
    * 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] service_handle service handle.
    * \param[in] any_callback User defined callback to call when a client request is received.
    */
   Service(
@@ -295,8 +295,8 @@ public:
     service_handle_->impl = service_handle->impl;
     TRACEPOINT(
       rclcpp_service_callback_added,
-      (const void *)get_service_handle().get(),
-      (const void *)&any_callback_);
+      static_cast<const void *>(get_service_handle().get()),
+      static_cast<const void *>(&any_callback_));
 #ifndef TRACETOOLS_DISABLED
     any_callback_.register_callback_for_tracing();
 #endif

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

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

rclcpp/include/rclcpp/subscription.hpp

@@ -92,7 +92,7 @@ public:
    * \param[in] topic_name Name of the topic to subscribe to.
    * \param[in] qos QoS profile for Subcription.
    * \param[in] callback User defined callback to call when a message is received.
-   * \param[in] options options for the subscription.
+   * \param[in] options Options for the subscription.
    * \param[in] message_memory_strategy The memory strategy to be used for managing message memory.
    * \param[in] subscription_topic_statistics Optional pointer to a topic statistics subcription.
    * \throws std::invalid_argument if the QoS is uncompatible with intra-process (if one
@@ -144,6 +144,11 @@ public:
         // pass
       }
     }
+    if (options.event_callbacks.message_lost_callback) {
+      this->add_event_handler(
+        options.event_callbacks.message_lost_callback,
+        RCL_SUBSCRIPTION_MESSAGE_LOST);
+    }
 
     // Setup intra process publishing if requested.
     if (rclcpp::detail::resolve_use_intra_process(options, *node_base)) {
@@ -179,8 +184,8 @@ public:
         resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback));
       TRACEPOINT(
         rclcpp_subscription_init,
-        (const void *)get_subscription_handle().get(),
-        (const void *)subscription_intra_process.get());
+        static_cast<const void *>(get_subscription_handle().get()),
+        static_cast<const void *>(subscription_intra_process.get()));
 
       // Add it to the intra process manager.
       using rclcpp::experimental::IntraProcessManager;
@@ -195,12 +200,12 @@ public:
 
     TRACEPOINT(
       rclcpp_subscription_init,
-      (const void *)get_subscription_handle().get(),
-      (const void *)this);
+      static_cast<const void *>(get_subscription_handle().get()),
+      static_cast<const void *>(this));
     TRACEPOINT(
       rclcpp_subscription_callback_added,
-      (const void *)this,
-      (const void *)&any_callback_);
+      static_cast<const void *>(this),
+      static_cast<const void *>(&any_callback_));
     // The callback object gets copied, so if registration is done too early/before this point
     // (e.g. in `AnySubscriptionCallback::set()`), its address won't match any address used later
     // in subsequent tracepoints.
@@ -272,11 +277,18 @@ public:
       return;
     }
     auto typed_message = std::static_pointer_cast<CallbackMessageT>(message);
+
+    std::chrono::time_point<std::chrono::system_clock> now;
+    if (subscription_topic_statistics_) {
+      // get current time before executing callback to
+      // exclude callback duration from topic statistics result.
+      now = std::chrono::system_clock::now();
+    }
+
     any_callback_.dispatch(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 nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(now);
       const auto time = rclcpp::Time(nanos.time_since_epoch().count());
       subscription_topic_statistics_->handle_message(*typed_message, time);
     }

rclcpp/include/rclcpp/subscription_base.hpp

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

rclcpp/include/rclcpp/subscription_factory.hpp

@@ -68,7 +68,7 @@ struct SubscriptionFactory
  * \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_Optinal stats callback for topic_statistics
+ * \param[in] subscription_topic_stats Optional stats callback for topic_statistics
  */
 template<
   typename MessageT,

rclcpp/include/rclcpp/subscription_options.hpp

@@ -26,6 +26,7 @@
 #include "rclcpp/intra_process_setting.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/qos_event.hpp"
+#include "rclcpp/qos_overriding_options.hpp"
 #include "rclcpp/topic_statistics_state.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -66,11 +67,14 @@ 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)};
   };
 
   TopicStatisticsOptions topic_stats_options;
+
+  QosOverridingOptions qos_overriding_options;
 };
 
 /// Structure containing optional configuration for Subscriptions.

rclcpp/include/rclcpp/time.hpp

@@ -51,7 +51,7 @@ public:
    * \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
@@ -66,12 +66,11 @@ public:
   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
-   * \param clock_type clock type
    */
   RCLCPP_PUBLIC
   explicit Time(const rcl_time_point_t & time_point);
@@ -91,6 +90,12 @@ 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);
@@ -213,6 +218,7 @@ private:
 /**
  * \throws std::overflow_error if addition leads to overflow
  */
+RCLCPP_PUBLIC
 Time
 operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs);
 

rclcpp/include/rclcpp/time_source.hpp

@@ -32,6 +32,20 @@ namespace rclcpp
 {
 class Clock;
 
+/**
+ * Time source that will drive the attached clocks.
+ *
+ * If the attached node `use_sim_time` parameter is `true`, the attached clocks will
+ * be updated based on messages received.
+ *
+ * The subscription to the clock topic created by the time source can have it's qos reconfigured
+ * using parameter overrides, particularly the following ones are accepted:
+ *
+ * - qos_overrides./clock.depth
+ * - qos_overrides./clock.durability
+ * - qos_overrides./clock.history
+ * - qos_overrides./clock.reliability
+ */
 class TimeSource
 {
 public:
@@ -40,16 +54,19 @@ public:
    * The node will be attached to the time source.
    *
    * \param node std::shared pointer to a initialized node
+   * \param qos QoS that will be used when creating a `/clock` subscription.
    */
   RCLCPP_PUBLIC
-  explicit TimeSource(rclcpp::Node::SharedPtr node);
+  explicit TimeSource(rclcpp::Node::SharedPtr node, const rclcpp::QoS & qos = rclcpp::ClockQoS());
 
   /// Empty constructor
   /**
    * An Empty TimeSource class
+   *
+   * \param qos QoS that will be used when creating a `/clock` subscription.
    */
   RCLCPP_PUBLIC
-  TimeSource();
+  explicit TimeSource(const rclcpp::QoS & qos = rclcpp::ClockQoS());
 
   /// Attack node to the time source.
   /**
@@ -114,11 +131,14 @@ private:
   // Store (and update on node attach) logger for logging.
   Logger logger_;
 
+  // QoS of the clock subscription.
+  rclcpp::QoS qos_;
+
   // The subscription for the clock callback
   using MessageT = rosgraph_msgs::msg::Clock;
   using Alloc = std::allocator<void>;
   using SubscriptionT = rclcpp::Subscription<MessageT, Alloc>;
-  std::shared_ptr<SubscriptionT> clock_subscription_;
+  std::shared_ptr<SubscriptionT> clock_subscription_{nullptr};
   std::mutex clock_sub_lock_;
 
   // The clock callback itself
@@ -148,8 +168,6 @@ private:
   void disable_ros_time();
 
   // Internal helper functions used inside iterators
-  static void enable_ros_time(rclcpp::Clock::SharedPtr clock);
-  static void disable_ros_time(rclcpp::Clock::SharedPtr clock);
   static void set_clock(
     const builtin_interfaces::msg::Time::SharedPtr msg,
     bool set_ros_time_enabled,
@@ -157,7 +175,7 @@ private:
 
   // Local storage of validity of ROS time
   // This is needed when new clocks are added.
-  bool ros_time_active_;
+  bool ros_time_active_{false};
   // Last set message to be passed to newly registered clocks
   rosgraph_msgs::msg::Clock::SharedPtr last_msg_set_;
 
@@ -166,7 +184,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

@@ -166,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,
@@ -175,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_));
   }
 
@@ -204,9 +205,9 @@ public:
     if (ret != RCL_RET_OK) {
       throw std::runtime_error("Failed to notify timer that callback occurred");
     }
-    TRACEPOINT(callback_start, (const void *)&callback_, false);
+    TRACEPOINT(callback_start, static_cast<const void *>(&callback_), false);
     execute_callback_delegate<>();
-    TRACEPOINT(callback_end, (const void *)&callback_);
+    TRACEPOINT(callback_end, static_cast<const void *>(&callback_));
   }
 
   // void specialization

rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp

@@ -116,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)
   {

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

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

rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp

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

rclcpp/include/rclcpp/wait_set_template.hpp

@@ -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.
@@ -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));
           }
         }

rclcpp/include/rclcpp/waitable.hpp

@@ -16,6 +16,7 @@
 #define RCLCPP__WAITABLE_HPP_
 
 #include <atomic>
+#include <memory>
 
 #include "rclcpp/macros.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -123,10 +124,19 @@ 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.
+  /// Take the data so that it can be consumed with `execute`.
   /**
+   * NOTE: take_data is a partial fix to a larger design issue with the
+   * multithreaded executor. This method is likely to be removed when
+   * a more permanent fix is implemented. A longterm fix is currently
+   * being discussed here: https://github.com/ros2/rclcpp/pull/1276
+   *
+   * This method takes the data from the underlying data structure and
+   * writes it to the void shared pointer `data` that is passed into the
+   * method. The `data` can then be executed with the `execute` method.
+   *
    * Before calling this method, the Waitable should be added to a wait set,
    * waited on, and then updated.
    *
@@ -143,13 +153,41 @@ public:
    * // Update the Waitable
    * waitable.update(wait_set);
    * // Execute any entities of the Waitable that may be ready
-   * waitable.execute();
+   * std::shared_ptr<void> data = waitable.take_data();
+   * ```
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::shared_ptr<void>
+  take_data() = 0;
+
+  /// Execute data that is passed in.
+  /**
+   * Before calling this method, the Waitable should be added to a wait set,
+   * waited on, and then updated - and the `take_data` method should be
+   * called.
+   *
+   * Example usage:
+   *
+   * ```cpp
+   * // ... create a wait set and a Waitable
+   * // Add the Waitable to the wait set
+   * bool add_ret = waitable.add_to_wait_set(wait_set);
+   * // ... error handling
+   * // Wait
+   * rcl_ret_t wait_ret = rcl_wait(wait_set);
+   * // ... error handling
+   * // Update the Waitable
+   * waitable.update(wait_set);
+   * // Execute any entities of the Waitable that may be ready
+   * std::shared_ptr<void> data = waitable.take_data();
+   * waitable.execute(data);
    * ```
    */
   RCLCPP_PUBLIC
   virtual
   void
-  execute() = 0;
+  execute(std::shared_ptr<void> & data) = 0;
 
   /// Exchange the "in use by wait set" state for this timer.
   /**

rclcpp/package.xml

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

rclcpp/resource/logging.hpp.em

@@ -151,10 +151,9 @@ def get_rclcpp_suffix_from_features(features):
 @[ end if]@
       logger.get_name(), \
 @[ if 'stream' not in feature_combination]@
-      rclcpp::get_c_string(RCLCPP_FIRST_ARG(__VA_ARGS__, "")), \
-        RCLCPP_ALL_BUT_FIRST_ARGS(__VA_ARGS__,"")); \
+      __VA_ARGS__); \
 @[ else]@
-      "%s", rclcpp::get_c_string(ss.str())); \
+      "%s", ss.str().c_str()); \
 @[ end if]@
   } while (0)
 

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

@@ -199,13 +199,17 @@ Context::init(
     throw rclcpp::ContextAlreadyInitialized();
   }
   this->clean_up();
-  rcl_context_.reset(new rcl_context_t, __delete_context);
-  *rcl_context_.get() = rcl_get_zero_initialized_context();
-  rcl_ret_t ret = rcl_init(argc, argv, init_options.get_rcl_init_options(), rcl_context_.get());
+  rcl_context_t * context = new rcl_context_t;
+  if (!context) {
+    throw std::runtime_error("failed to allocate memory for rcl context");
+  }
+  *context = rcl_get_zero_initialized_context();
+  rcl_ret_t ret = rcl_init(argc, argv, init_options.get_rcl_init_options(), context);
   if (RCL_RET_OK != ret) {
-    rcl_context_.reset();
+    delete context;
     rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl");
   }
+  rcl_context_.reset(context, __delete_context);
 
   if (init_options.auto_initialize_logging()) {
     logging_mutex_ = get_global_logging_mutex();
@@ -275,6 +279,17 @@ Context::get_init_options()
   return init_options_;
 }
 
+size_t
+Context::get_domain_id() const
+{
+  size_t domain_id;
+  rcl_ret_t ret = rcl_context_get_domain_id(rcl_context_.get(), &domain_id);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get domain id from context");
+  }
+  return domain_id;
+}
+
 std::string
 Context::shutdown_reason()
 {
@@ -305,7 +320,6 @@ Context::shutdown(const std::string & reason)
   }
   // interrupt all blocking sleep_for() and all blocking executors or wait sets
   this->interrupt_all_sleep_for();
-  this->interrupt_all_wait_sets();
   // remove self from the global contexts
   weak_contexts_->remove_context(this);
   // shutdown logger
@@ -376,75 +390,6 @@ Context::interrupt_all_sleep_for()
   interrupt_condition_variable_.notify_all();
 }
 
-rcl_guard_condition_t *
-Context::get_interrupt_guard_condition(rcl_wait_set_t * wait_set)
-{
-  std::lock_guard<std::mutex> lock(interrupt_guard_cond_handles_mutex_);
-  auto kv = interrupt_guard_cond_handles_.find(wait_set);
-  if (kv != interrupt_guard_cond_handles_.end()) {
-    return &kv->second;
-  } else {
-    rcl_guard_condition_t handle = rcl_get_zero_initialized_guard_condition();
-    rcl_guard_condition_options_t options = rcl_guard_condition_get_default_options();
-    auto ret = rcl_guard_condition_init(&handle, this->get_rcl_context().get(), options);
-    if (RCL_RET_OK != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't initialize guard condition");
-    }
-    interrupt_guard_cond_handles_.emplace(wait_set, handle);
-    return &interrupt_guard_cond_handles_[wait_set];
-  }
-}
-
-void
-Context::release_interrupt_guard_condition(rcl_wait_set_t * wait_set)
-{
-  std::lock_guard<std::mutex> lock(interrupt_guard_cond_handles_mutex_);
-  auto kv = interrupt_guard_cond_handles_.find(wait_set);
-  if (kv != interrupt_guard_cond_handles_.end()) {
-    rcl_ret_t ret = rcl_guard_condition_fini(&kv->second);
-    if (RCL_RET_OK != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to destroy sigint guard condition");
-    }
-    interrupt_guard_cond_handles_.erase(kv);
-  } else {
-    throw std::runtime_error("Tried to release sigint guard condition for nonexistent wait set");
-  }
-}
-
-void
-Context::release_interrupt_guard_condition(
-  rcl_wait_set_t * wait_set,
-  const std::nothrow_t &) noexcept
-{
-  try {
-    this->release_interrupt_guard_condition(wait_set);
-  } catch (const std::exception & exc) {
-    RCLCPP_ERROR(
-      rclcpp::get_logger("rclcpp"),
-      "caught %s exception when releasing interrupt guard condition: %s",
-      rmw::impl::cpp::demangle(exc).c_str(), exc.what());
-  } catch (...) {
-    RCLCPP_ERROR(
-      rclcpp::get_logger("rclcpp"),
-      "caught unknown exception when releasing interrupt guard condition");
-  }
-}
-
-void
-Context::interrupt_all_wait_sets()
-{
-  std::lock_guard<std::mutex> lock(interrupt_guard_cond_handles_mutex_);
-  for (auto & kv : interrupt_guard_cond_handles_) {
-    rcl_ret_t status = rcl_trigger_guard_condition(&(kv.second));
-    if (status != RCL_RET_OK) {
-      RCUTILS_LOG_ERROR_NAMED(
-        "rclcpp",
-        "failed to trigger guard condition in Context::interrupt_all_wait_sets(): %s",
-        rcl_get_error_string().str);
-    }
-  }
-}
-
 void
 Context::clean_up()
 {

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;
@@ -238,10 +246,41 @@ Duration::to_rmw_time() const
   return result;
 }
 
+Duration
+Duration::from_rmw_time(rmw_time_t duration)
+{
+  Duration ret;
+  constexpr rcl_duration_value_t limit_ns = std::numeric_limits<rcl_duration_value_t>::max();
+  constexpr rcl_duration_value_t limit_sec = RCL_NS_TO_S(limit_ns);
+  if (duration.sec > limit_sec || duration.nsec > limit_ns) {
+    // saturate if will overflow
+    ret.rcl_duration_.nanoseconds = limit_ns;
+    return ret;
+  }
+  uint64_t total_ns = RCL_S_TO_NS(duration.sec) + duration.nsec;
+  if (total_ns > limit_ns) {
+    // saturate if will overflow
+    ret.rcl_duration_.nanoseconds = limit_ns;
+    return ret;
+  }
+  ret.rcl_duration_.nanoseconds = static_cast<rcl_duration_value_t>(total_ns);
+  return ret;
+}
+
 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,20 +14,27 @@
 
 #include <algorithm>
 #include <memory>
+#include <map>
 #include <string>
 #include <type_traits>
+#include <utility>
+#include <vector>
 
 #include "rcl/allocator.h"
 #include "rcl/error_handling.h"
 
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/executor.hpp"
+#include "rclcpp/guard_condition.hpp"
+#include "rclcpp/memory_strategy.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/scope_exit.hpp"
 #include "rclcpp/utilities.hpp"
 
 #include "rcutils/logging_macros.h"
 
+using namespace std::chrono_literals;
+
 using rclcpp::exceptions::throw_from_rcl_error;
 using rclcpp::AnyExecutable;
 using rclcpp::Executor;
@@ -36,28 +43,35 @@ using rclcpp::FutureReturnCode;
 
 Executor::Executor(const rclcpp::ExecutorOptions & options)
 : spinning(false),
+  shutdown_guard_condition_(std::make_shared<rclcpp::GuardCondition>(options.context)),
   memory_strategy_(options.memory_strategy)
 {
+  // Store the context for later use.
+  context_ = options.context;
+
   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);
+    &interrupt_guard_condition_, 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");
   }
 
+  context_->on_shutdown(
+    [weak_gc = std::weak_ptr<rclcpp::GuardCondition>{shutdown_guard_condition_}]() {
+      auto strong_gc = weak_gc.lock();
+      if (strong_gc) {
+        strong_gc->trigger();
+      }
+    });
+
   // 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_));
+  memory_strategy_->add_guard_condition(&shutdown_guard_condition_->get_rcl_guard_condition());
 
   // 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,
@@ -74,25 +88,39 @@ Executor::Executor(const rclcpp::ExecutorOptions & options)
         "failed to destroy guard condition: %s", rcl_get_error_string().str);
       rcl_reset_error();
     }
-    throw std::runtime_error("Failed to create wait set in Executor constructor");
+    throw_from_rcl_error(ret, "Failed to create wait set in Executor constructor");
   }
 }
 
 Executor::~Executor()
 {
-  // Disassocate all nodes
-  for (auto & weak_node : weak_nodes_) {
-    auto node = weak_node.lock();
-    if (node) {
-      std::atomic_bool & has_executor = node->get_associated_with_executor_atomic();
+  // Disassociate all callback groups.
+  for (auto & pair : weak_groups_to_nodes_) {
+    auto group = pair.first.lock();
+    if (group) {
+      std::atomic_bool & has_executor = group->get_associated_with_executor_atomic();
       has_executor.store(false);
     }
   }
+  // Disassociate all nodes.
+  std::for_each(
+    weak_nodes_.begin(), weak_nodes_.end(), []
+      (rclcpp::node_interfaces::NodeBaseInterface::WeakPtr weak_node_ptr) {
+      auto shared_node_ptr = weak_node_ptr.lock();
+      if (shared_node_ptr) {
+        std::atomic_bool & has_executor = shared_node_ptr->get_associated_with_executor_atomic();
+        has_executor.store(false);
+      }
+    });
   weak_nodes_.clear();
-  for (auto & guard_condition : guard_conditions_) {
+  weak_groups_associated_with_executor_to_nodes_.clear();
+  weak_groups_to_nodes_associated_with_executor_.clear();
+  weak_groups_to_nodes_.clear();
+  for (const auto & pair : weak_nodes_to_guard_conditions_) {
+    auto & guard_condition = pair.second;
     memory_strategy_->remove_guard_condition(guard_condition);
   }
-  guard_conditions_.clear();
+  weak_nodes_to_guard_conditions_.clear();
 
   // Finalize the wait set.
   if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) {
@@ -109,8 +137,121 @@ Executor::~Executor()
     rcl_reset_error();
   }
   // Remove and release the sigint guard condition
-  memory_strategy_->remove_guard_condition(context_->get_interrupt_guard_condition(&wait_set_));
-  context_->release_interrupt_guard_condition(&wait_set_, std::nothrow);
+  memory_strategy_->remove_guard_condition(&shutdown_guard_condition_->get_rcl_guard_condition());
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+Executor::get_all_callback_groups()
+{
+  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
+  std::lock_guard<std::mutex> guard{mutex_};
+  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;
+  std::lock_guard<std::mutex> guard{mutex_};
+  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;
+  std::lock_guard<std::mutex> guard{mutex_};
+  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,
+  rclcpp::memory_strategy::MemoryStrategy::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
+    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)
+{
+  std::lock_guard<std::mutex> guard{mutex_};
+  this->add_callback_group_to_map(
+    group_ptr,
+    node_ptr,
+    weak_groups_associated_with_executor_to_nodes_,
+    notify);
 }
 
 void
@@ -121,25 +262,69 @@ 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.");
+  std::lock_guard<std::mutex> guard{mutex_};
+  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,
+  rclcpp::memory_strategy::MemoryStrategy::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");
+      }
+    }
+    memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition());
+  }
+}
+
+void
+Executor::remove_callback_group(
+  rclcpp::CallbackGroup::SharedPtr group_ptr,
+  bool notify)
+{
+  std::lock_guard<std::mutex> guard{mutex_};
+  this->remove_callback_group_from_map(
+    group_ptr,
+    weak_groups_associated_with_executor_to_nodes_,
+    notify);
 }
 
 void
@@ -151,34 +336,45 @@ 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.");
+  }
+
+  std::lock_guard<std::mutex> guard{mutex_};
+  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.");
+  }
+
+  for (auto it = weak_groups_to_nodes_associated_with_executor_.begin();
+    it != weak_groups_to_nodes_associated_with_executor_.end(); )
+  {
+    auto weak_node_ptr = it->second;
+    auto shared_node_ptr = weak_node_ptr.lock();
+    auto group_ptr = it->first.lock();
+
+    // Increment iterator before removing in case it's invalidated
+    it++;
+    if (shared_node_ptr == node_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
@@ -212,8 +408,21 @@ 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(std::chrono::nanoseconds max_duration)
+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]() {
@@ -232,18 +441,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)
 {
@@ -251,18 +475,16 @@ 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");
   }
 }
 
@@ -272,6 +494,7 @@ Executor::set_memory_strategy(rclcpp::memory_strategy::MemoryStrategy::SharedPtr
   if (memory_strategy == nullptr) {
     throw std::runtime_error("Received NULL memory strategy in executor.");
   }
+  std::lock_guard<std::mutex> guard{mutex_};
   memory_strategy_ = memory_strategy;
 }
 
@@ -294,14 +517,15 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
     execute_client(any_exec.client);
   }
   if (any_exec.waitable) {
-    any_exec.waitable->execute();
+    any_exec.waitable->execute(any_exec.data);
   }
   // Reset the callback_group, regardless of type
   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");
   }
 }
 
@@ -444,47 +668,72 @@ void
 Executor::wait_for_work(std::chrono::nanoseconds timeout)
 {
   {
-    std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
+    std::lock_guard<std::mutex> guard(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);
+          if (node_guard_pair != weak_nodes_to_guard_conditions_.end()) {
+            auto guard_condition = node_guard_pair->second;
+            weak_nodes_to_guard_conditions_.erase(weak_node_ptr);
+            memory_strategy_->remove_guard_condition(guard_condition);
+          }
         }
       }
+      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) {
@@ -498,26 +747,24 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
 
   // check the null handles in the wait set and remove them from the handles in memory strategy
   // for callback-based entities
+  std::lock_guard<std::mutex> guard(mutex_);
   memory_strategy_->remove_null_handles(&wait_set_);
 }
 
 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
-Executor::get_node_by_group(rclcpp::CallbackGroup::SharedPtr group)
+Executor::get_node_by_group(
+  const rclcpp::memory_strategy::MemoryStrategy::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;
 }
@@ -525,80 +772,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) {
+  std::lock_guard<std::mutex> guard{mutex_};
+  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,
+  const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap &
+  weak_groups_to_nodes)
 {
   bool success = false;
+  std::lock_guard<std::mutex> guard{mutex_};
   // 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) {
+      any_executable.data = any_executable.waitable->take_data();
       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);
     }
   }
@@ -625,3 +899,19 @@ 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,
+  const rclcpp::memory_strategy::MemoryStrategy::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,15 +75,29 @@ 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();
+  std::shared_ptr<void> shared_ptr;
+  execute(shared_ptr);
 }
 
 void
-StaticExecutorEntitiesCollector::execute()
+StaticExecutorEntitiesCollector::fini()
 {
+  memory_strategy_->clear_handles();
+  exec_list_.clear();
+}
+
+std::shared_ptr<void>
+StaticExecutorEntitiesCollector::take_data()
+{
+  return nullptr;
+}
+
+void
+StaticExecutorEntitiesCollector::execute(std::shared_ptr<void> & data)
+{
+  (void) data;
   // Fill memory strategy with entities coming from weak_nodes_
   fill_memory_strategy();
   // Fill exec_list_ with entities coming from weak_nodes_ (same as memory strategy)
@@ -77,18 +110,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 +155,60 @@ 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 rclcpp::memory_strategy::MemoryStrategy::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 +227,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 +261,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 +273,151 @@ 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,
+  rclcpp::memory_strategy::MemoryStrategy::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) {
+    weak_nodes_to_guard_conditions_[node_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,
+  rclcpp::memory_strategy::MemoryStrategy::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 +426,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 +440,77 @@ 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 rclcpp::memory_strategy::MemoryStrategy::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
@@ -137,8 +175,10 @@ StaticSingleThreadedExecutor::execute_ready_executables()
   }
   // Execute all the ready waitables
   for (size_t i = 0; i < entities_collector_->get_number_of_waitables(); ++i) {
-    if (entities_collector_->get_waitable(i)->is_ready(&wait_set_)) {
-      entities_collector_->get_waitable(i)->execute();
+    auto waitable = entities_collector_->get_waitable(i);
+    if (waitable->is_ready(&wait_set_)) {
+      auto data = waitable->take_data();
+      waitable->execute(data);
     }
   }
 }

rclcpp/src/rclcpp/graph_listener.cpp

@@ -36,11 +36,11 @@ namespace rclcpp
 namespace graph_listener
 {
 
-GraphListener::GraphListener(std::shared_ptr<rclcpp::Context> parent_context)
-: parent_context_(parent_context),
+GraphListener::GraphListener(const std::shared_ptr<Context> & parent_context)
+: weak_parent_context_(parent_context),
+  rcl_parent_context_(parent_context->get_rcl_context()),
   is_started_(false),
-  is_shutdown_(false),
-  shutdown_guard_condition_(nullptr)
+  is_shutdown_(false)
 {
   // TODO(wjwwood): make a guard condition class in rclcpp so this can be tracked
   //   automatically with the rcl guard condition
@@ -48,13 +48,11 @@ GraphListener::GraphListener(std::shared_ptr<rclcpp::Context> parent_context)
   // guard condition is using it.
   rcl_ret_t ret = rcl_guard_condition_init(
     &interrupt_guard_condition_,
-    parent_context->get_rcl_context().get(),
+    rcl_parent_context_.get(),
     rcl_guard_condition_get_default_options());
   if (RCL_RET_OK != ret) {
     throw_from_rcl_error(ret, "failed to create interrupt guard condition");
   }
-
-  shutdown_guard_condition_ = parent_context->get_interrupt_guard_condition(&wait_set_);
 }
 
 GraphListener::~GraphListener()
@@ -62,6 +60,23 @@ GraphListener::~GraphListener()
   this->shutdown(std::nothrow);
 }
 
+void GraphListener::init_wait_set()
+{
+  rcl_ret_t ret = rcl_wait_set_init(
+    &wait_set_,
+    0,  // number_of_subscriptions
+    2,  // number_of_guard_conditions
+    0,  // number_of_timers
+    0,  // number_of_clients
+    0,  // number_of_services
+    0,  // number_of_events
+    rcl_parent_context_.get(),
+    rcl_get_default_allocator());
+  if (RCL_RET_OK != ret) {
+    throw_from_rcl_error(ret, "failed to initialize wait set");
+  }
+}
+
 void
 GraphListener::start_if_not_started()
 {
@@ -69,30 +84,13 @@ GraphListener::start_if_not_started()
   if (is_shutdown_.load()) {
     throw GraphListenerShutdownError();
   }
-  if (!is_started_) {
-    // Initialize the wait set before starting.
-    auto parent_context = parent_context_.lock();
-    if (!parent_context) {
-      throw std::runtime_error("parent context was destroyed");
-    }
-    rcl_ret_t ret = rcl_wait_set_init(
-      &wait_set_,
-      0,  // number_of_subscriptions
-      2,  // number_of_guard_conditions
-      0,  // number_of_timers
-      0,  // number_of_clients
-      0,  // number_of_services
-      0,  // number_of_events
-      parent_context->get_rcl_context().get(),
-      rcl_get_default_allocator());
-    if (RCL_RET_OK != ret) {
-      throw_from_rcl_error(ret, "failed to initialize wait set");
-    }
+  auto parent_context = weak_parent_context_.lock();
+  if (!is_started_ && parent_context) {
     // Register an on_shutdown hook to shtudown the graph listener.
     // This is important to ensure that the wait set is finalized before
     // destruction of static objects occurs.
     std::weak_ptr<GraphListener> weak_this = shared_from_this();
-    rclcpp::on_shutdown(
+    parent_context->on_shutdown(
       [weak_this]() {
         auto shared_this = weak_this.lock();
         if (shared_this) {
@@ -100,6 +98,8 @@ GraphListener::start_if_not_started()
           shared_this->shutdown(std::nothrow);
         }
       });
+    // Initialize the wait set before starting.
+    init_wait_set();
     // Start the listener thread.
     listener_thread_ = std::thread(&GraphListener::run, this);
     is_started_ = true;
@@ -144,14 +144,6 @@ GraphListener::run_loop()
     }
     // This lock is released when the loop continues or exits.
     std::lock_guard<std::mutex> nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock);
-    // Ensure that the context doesn't go out of scope.
-    auto parent_context = parent_context_.lock();
-    if (!parent_context) {
-      // The parent context may be destroyed before this loop is stopped.
-      // In that case, the loop is broken and the function just returns silently.
-      return;
-    }
-
     // Resize the wait set if necessary.
     const size_t node_graph_interfaces_size = node_graph_interfaces_.size();
     // Add 2 for the interrupt and shutdown guard conditions
@@ -171,13 +163,7 @@ GraphListener::run_loop()
     if (RCL_RET_OK != ret) {
       throw_from_rcl_error(ret, "failed to add interrupt guard condition to wait set");
     }
-    // Put the shutdown guard condition in the wait set.
-    size_t shutdown_guard_condition_index = 0u;
-    ret = rcl_wait_set_add_guard_condition(
-      &wait_set_, shutdown_guard_condition_, &shutdown_guard_condition_index);
-    if (RCL_RET_OK != ret) {
-      throw_from_rcl_error(ret, "failed to add shutdown guard condition to wait set");
-    }
+
     // Put graph guard conditions for each node into the wait set.
     std::vector<size_t> graph_gc_indexes(node_graph_interfaces_size, 0u);
     for (size_t i = 0u; i < node_graph_interfaces_size; ++i) {
@@ -206,9 +192,6 @@ GraphListener::run_loop()
       throw_from_rcl_error(ret, "failed to wait on wait set");
     }
 
-    // Check to see if the shutdown guard condition has been triggered.
-    bool shutdown_guard_condition_triggered =
-      (shutdown_guard_condition_ == wait_set_.guard_conditions[shutdown_guard_condition_index]);
     // Notify nodes who's guard conditions are set (triggered).
     for (size_t i = 0u; i < node_graph_interfaces_size; ++i) {
       const auto node_ptr = node_graph_interfaces_[i];
@@ -219,7 +202,7 @@ GraphListener::run_loop()
       if (graph_gc == wait_set_.guard_conditions[graph_gc_indexes[i]]) {
         node_ptr->notify_graph_change();
       }
-      if (shutdown_guard_condition_triggered) {
+      if (is_shutdown_) {
         // If shutdown, then notify the node of this as well.
         node_ptr->notify_shutdown();
       }
@@ -351,7 +334,16 @@ GraphListener::remove_node(rclcpp::node_interfaces::NodeGraphInterface * node_gr
 }
 
 void
-GraphListener::__shutdown(bool should_throw)
+GraphListener::cleanup_wait_set()
+{
+  rcl_ret_t ret = rcl_wait_set_fini(&wait_set_);
+  if (RCL_RET_OK != ret) {
+    throw_from_rcl_error(ret, "failed to finalize wait set");
+  }
+}
+
+void
+GraphListener::__shutdown()
 {
   std::lock_guard<std::mutex> shutdown_lock(shutdown_mutex_);
   if (!is_shutdown_.exchange(true)) {
@@ -363,33 +355,8 @@ GraphListener::__shutdown(bool should_throw)
     if (RCL_RET_OK != ret) {
       throw_from_rcl_error(ret, "failed to finalize interrupt guard condition");
     }
-    if (shutdown_guard_condition_) {
-      auto parent_context_ptr = parent_context_.lock();
-      if (parent_context_ptr) {
-        if (should_throw) {
-          parent_context_ptr->release_interrupt_guard_condition(&wait_set_);
-        } else {
-          parent_context_ptr->release_interrupt_guard_condition(&wait_set_, std::nothrow);
-        }
-      } else {
-        ret = rcl_guard_condition_fini(shutdown_guard_condition_);
-        if (RCL_RET_OK != ret) {
-          if (should_throw) {
-            throw_from_rcl_error(ret, "failed to finalize shutdown guard condition");
-          } else {
-            RCLCPP_ERROR(
-              rclcpp::get_logger("rclcpp"),
-              "failed to finalize shutdown guard condition");
-          }
-        }
-      }
-      shutdown_guard_condition_ = nullptr;
-    }
     if (is_started_) {
-      ret = rcl_wait_set_fini(&wait_set_);
-      if (RCL_RET_OK != ret) {
-        throw_from_rcl_error(ret, "failed to finalize wait set");
-      }
+      cleanup_wait_set();
     }
   }
 }
@@ -397,14 +364,14 @@ GraphListener::__shutdown(bool should_throw)
 void
 GraphListener::shutdown()
 {
-  this->__shutdown(true);
+  this->__shutdown();
 }
 
 void
 GraphListener::shutdown(const std::nothrow_t &) noexcept
 {
   try {
-    this->__shutdown(false);
+    this->__shutdown();
   } catch (const std::exception & exc) {
     RCLCPP_ERROR(
       rclcpp::get_logger("rclcpp"),

rclcpp/src/rclcpp/init_options.cpp

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

rclcpp/src/rclcpp/logger.cpp

@@ -14,8 +14,10 @@
 
 #include <string>
 
-#include "rclcpp/logger.hpp"
+#include "rcl_logging_interface/rcl_logging_interface.h"
 
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/logger.hpp"
 #include "rclcpp/logging.hpp"
 
 namespace rclcpp
@@ -38,10 +40,44 @@ get_node_logger(const rcl_node_t * node)
   const char * logger_name = rcl_node_get_logger_name(node);
   if (nullptr == logger_name) {
     auto logger = rclcpp::get_logger("rclcpp");
-    RCLCPP_ERROR(logger, "failed to get logger name from node at address %p", node);
+    RCLCPP_ERROR(
+      logger, "failed to get logger name from node at address %p",
+      static_cast<void *>(const_cast<rcl_node_t *>(node)));
     return logger;
   }
   return rclcpp::get_logger(logger_name);
 }
 
+rcpputils::fs::path
+get_logging_directory()
+{
+  char * log_dir = NULL;
+  auto allocator = rcutils_get_default_allocator();
+  rcl_logging_ret_t ret = rcl_logging_get_logging_directory(allocator, &log_dir);
+  if (RCL_LOGGING_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+  std::string path{log_dir};
+  allocator.deallocate(log_dir, allocator.state);
+  return path;
+}
+
+void
+Logger::set_level(Level level)
+{
+  rcutils_ret_t rcutils_ret = rcutils_logging_set_logger_level(
+    get_name(),
+    static_cast<RCUTILS_LOG_SEVERITY>(level));
+  if (rcutils_ret != RCUTILS_RET_OK) {
+    if (rcutils_ret == RCUTILS_RET_INVALID_ARGUMENT) {
+      exceptions::throw_from_rcl_error(
+        RCL_RET_INVALID_ARGUMENT, "Invalid parameter",
+        rcutils_get_error_state(), rcutils_reset_error);
+    }
+    exceptions::throw_from_rcl_error(
+      RCL_RET_ERROR, "Couldn't set logger level",
+      rcutils_get_error_state(), rcutils_reset_error);
+  }
+}
+
 }  // namespace rclcpp

rclcpp/src/rclcpp/memory_strategy.cpp

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

rclcpp/src/rclcpp/node.cpp

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

rclcpp/src/rclcpp/node_interfaces/node_base.cpp

@@ -70,6 +70,7 @@ NodeBase::NodeBase(
   std::shared_ptr<std::recursive_mutex> logging_mutex = get_global_logging_mutex();
   {
     std::lock_guard<std::recursive_mutex> guard(*logging_mutex);
+    // TODO(ivanpauno): /rosout Qos should be reconfigurable.
     // TODO(ivanpauno): Instead of mutually excluding rcl_node_init with the global logger mutex,
     // rcl_logging_rosout_init_publisher_for_node could be decoupled from there and be called
     // here directly.
@@ -216,11 +217,16 @@ NodeBase::get_shared_rcl_node_handle() const
 }
 
 rclcpp::CallbackGroup::SharedPtr
-NodeBase::create_callback_group(rclcpp::CallbackGroupType group_type)
+NodeBase::create_callback_group(
+  rclcpp::CallbackGroupType group_type,
+  bool automatically_add_to_executor_with_node)
 {
   using rclcpp::CallbackGroup;
   using rclcpp::CallbackGroupType;
-  auto group = CallbackGroup::SharedPtr(new CallbackGroup(group_type));
+  auto group = CallbackGroup::SharedPtr(
+    new CallbackGroup(
+      group_type,
+      automatically_add_to_executor_with_node));
   callback_groups_.push_back(group);
   return group;
 }
@@ -283,3 +289,24 @@ NodeBase::get_enable_topic_statistics_default() const
 {
   return enable_topic_statistics_default_;
 }
+
+std::string
+NodeBase::resolve_topic_or_service_name(
+  const std::string & name, bool is_service, bool only_expand) const
+{
+  char * output_cstr = NULL;
+  auto allocator = rcl_get_default_allocator();
+  rcl_ret_t ret = rcl_node_resolve_name(
+    node_handle_.get(),
+    name.c_str(),
+    allocator,
+    is_service,
+    only_expand,
+    &output_cstr);
+  if (RCL_RET_OK != ret) {
+    throw_from_rcl_error(ret, "failed to resolve name", rcl_get_error_state());
+  }
+  std::string output{output_cstr};
+  allocator.deallocate(output_cstr, allocator.state);
+  return output;
+}

rclcpp/src/rclcpp/node_interfaces/node_graph.cpp

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

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -67,6 +67,7 @@ NodeParameters::NodeParameters(
   }
 
   if (start_parameter_event_publisher) {
+    // TODO(ivanpauno): Qos of the `/parameters_event` topic should be somehow overridable.
     events_publisher_ = rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
       node_topics,
       "/parameter_events",
@@ -159,7 +160,7 @@ __lockless_has_parameter(
 // see https://en.cppreference.com/w/cpp/types/numeric_limits/epsilon
 RCLCPP_LOCAL
 bool
-__are_doubles_equal(double x, double y, size_t ulp = 100)
+__are_doubles_equal(double x, double y, double ulp = 100.0)
 {
   return std::abs(x - y) <= std::numeric_limits<double>::epsilon() * std::abs(x + y) * ulp;
 }
@@ -874,18 +875,6 @@ NodeParameters::add_on_set_parameters_callback(OnParametersSetCallbackType callb
   return handle;
 }
 
-NodeParameters::OnParametersSetCallbackType
-NodeParameters::set_on_parameters_set_callback(OnParametersSetCallbackType callback)
-{
-  std::lock_guard<std::recursive_mutex> lock(mutex_);
-
-  ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
-
-  auto existing_callback = on_parameters_set_callback_;
-  on_parameters_set_callback_ = callback;
-  return existing_callback;
-}
-
 const std::map<std::string, rclcpp::ParameterValue> &
 NodeParameters::get_parameter_overrides() const
 {

rclcpp/src/rclcpp/node_interfaces/node_services.cpp

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

rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp

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

rclcpp/src/rclcpp/node_interfaces/node_timers.cpp

@@ -16,6 +16,8 @@
 
 #include <string>
 
+#include "tracetools/tracetools.h"
+
 using rclcpp::node_interfaces::NodeTimers;
 
 NodeTimers::NodeTimers(rclcpp::node_interfaces::NodeBaseInterface * node_base)
@@ -44,4 +46,8 @@ NodeTimers::add_timer(
             std::string("Failed to notify wait set on timer creation: ") +
             rmw_get_error_string().str);
   }
+  TRACEPOINT(
+    rclcpp_timer_link_node,
+    static_cast<const void *>(timer->get_timer_handle().get()),
+    static_cast<const void *>(node_base_->get_rcl_node_handle()));
 }

rclcpp/src/rclcpp/node_interfaces/node_topics.cpp

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

rclcpp/src/rclcpp/node_options.cpp

@@ -67,6 +67,7 @@ NodeOptions &
 NodeOptions::operator=(const NodeOptions & other)
 {
   if (this != &other) {
+    this->node_options_.reset();
     this->context_ = other.context_;
     this->arguments_ = other.arguments_;
     this->parameter_overrides_ = other.parameter_overrides_;
@@ -75,10 +76,15 @@ NodeOptions::operator=(const NodeOptions & other)
     this->use_intra_process_comms_ = other.use_intra_process_comms_;
     this->enable_topic_statistics_ = other.enable_topic_statistics_;
     this->start_parameter_services_ = other.start_parameter_services_;
-    this->allocator_ = other.allocator_;
+    this->start_parameter_event_publisher_ = other.start_parameter_event_publisher_;
+    this->clock_qos_ = other.clock_qos_;
+    this->parameter_event_qos_ = other.parameter_event_qos_;
+    this->rosout_qos_ = other.rosout_qos_;
+    this->parameter_event_publisher_options_ = other.parameter_event_publisher_options_;
     this->allow_undeclared_parameters_ = other.allow_undeclared_parameters_;
     this->automatically_declare_parameters_from_overrides_ =
       other.automatically_declare_parameters_from_overrides_;
+    this->allocator_ = other.allocator_;
   }
   return *this;
 }
@@ -92,8 +98,8 @@ NodeOptions::get_rcl_node_options() const
     *node_options_ = rcl_node_get_default_options();
     node_options_->allocator = this->allocator_;
     node_options_->use_global_arguments = this->use_global_arguments_;
-    node_options_->domain_id = this->get_domain_id_from_env();
     node_options_->enable_rosout = this->enable_rosout_;
+    node_options_->rosout_qos = this->rosout_qos_.get_rmw_qos_profile();
 
     int c_argc = 0;
     std::unique_ptr<const char *[]> c_argv;
@@ -176,7 +182,7 @@ NodeOptions::parameter_overrides(const std::vector<rclcpp::Parameter> & paramete
 bool
 NodeOptions::use_global_arguments() const
 {
-  return this->node_options_->use_global_arguments;
+  return this->use_global_arguments_;
 }
 
 NodeOptions &
@@ -253,6 +259,19 @@ NodeOptions::start_parameter_event_publisher(bool start_parameter_event_publishe
   return *this;
 }
 
+const rclcpp::QoS &
+NodeOptions::clock_qos() const
+{
+  return this->clock_qos_;
+}
+
+NodeOptions &
+NodeOptions::clock_qos(const rclcpp::QoS & clock_qos)
+{
+  this->clock_qos_ = clock_qos;
+  return *this;
+}
+
 const rclcpp::QoS &
 NodeOptions::parameter_event_qos() const
 {
@@ -266,6 +285,20 @@ NodeOptions::parameter_event_qos(const rclcpp::QoS & parameter_event_qos)
   return *this;
 }
 
+const rclcpp::QoS &
+NodeOptions::rosout_qos() const
+{
+  return this->rosout_qos_;
+}
+
+NodeOptions &
+NodeOptions::rosout_qos(const rclcpp::QoS & rosout_qos)
+{
+  this->node_options_.reset();
+  this->rosout_qos_ = rosout_qos;
+  return *this;
+}
+
 const rclcpp::PublisherOptionsBase &
 NodeOptions::parameter_event_publisher_options() const
 {
@@ -322,36 +355,4 @@ NodeOptions::allocator(rcl_allocator_t allocator)
   return *this;
 }
 
-// TODO(wjwwood): reuse rcutils_get_env() to avoid code duplication.
-//   See also: https://github.com/ros2/rcl/issues/119
-size_t
-NodeOptions::get_domain_id_from_env() const
-{
-  // Determine the domain id based on the options and the ROS_DOMAIN_ID env variable.
-  size_t domain_id = std::numeric_limits<size_t>::max();
-  char * ros_domain_id = nullptr;
-  const char * env_var = "ROS_DOMAIN_ID";
-#ifndef _WIN32
-  ros_domain_id = getenv(env_var);
-#else
-  size_t ros_domain_id_size;
-  _dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var);
-#endif
-  if (ros_domain_id) {
-    uint32_t number = strtoul(ros_domain_id, NULL, 0);
-    if (number == (std::numeric_limits<uint32_t>::max)()) {
-#ifdef _WIN32
-      // free the ros_domain_id before throwing, if getenv was used on Windows
-      free(ros_domain_id);
-#endif
-      throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number");
-    }
-    domain_id = static_cast<size_t>(number);
-#ifdef _WIN32
-    free(ros_domain_id);
-#endif
-  }
-  return domain_id;
-}
-
 }  // namespace rclcpp

rclcpp/src/rclcpp/parameter_client.cpp

@@ -15,7 +15,12 @@
 #include "rclcpp/parameter_client.hpp"
 
 #include <algorithm>
+#include <chrono>
+#include <functional>
+#include <future>
+#include <iterator>
 #include <memory>
+#include <stdexcept>
 #include <string>
 #include <vector>
 
@@ -99,36 +104,6 @@ AsyncParametersClient::AsyncParametersClient(
   node_services_interface->add_client(describe_parameters_base, group);
 }
 
-AsyncParametersClient::AsyncParametersClient(
-  const rclcpp::Node::SharedPtr node,
-  const std::string & remote_node_name,
-  const rmw_qos_profile_t & qos_profile,
-  rclcpp::CallbackGroup::SharedPtr group)
-: AsyncParametersClient(
-    node->get_node_base_interface(),
-    node->get_node_topics_interface(),
-    node->get_node_graph_interface(),
-    node->get_node_services_interface(),
-    remote_node_name,
-    qos_profile,
-    group)
-{}
-
-AsyncParametersClient::AsyncParametersClient(
-  rclcpp::Node * node,
-  const std::string & remote_node_name,
-  const rmw_qos_profile_t & qos_profile,
-  rclcpp::CallbackGroup::SharedPtr group)
-: AsyncParametersClient(
-    node->get_node_base_interface(),
-    node->get_node_topics_interface(),
-    node->get_node_graph_interface(),
-    node->get_node_services_interface(),
-    remote_node_name,
-    qos_profile,
-    group)
-{}
-
 std::shared_future<std::vector<rclcpp::Parameter>>
 AsyncParametersClient::get_parameters(
   const std::vector<std::string> & names,
@@ -169,6 +144,35 @@ AsyncParametersClient::get_parameters(
   return future_result;
 }
 
+std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>>
+AsyncParametersClient::describe_parameters(
+  const std::vector<std::string> & names,
+  std::function<
+    void(std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>>)
+  > callback)
+{
+  auto promise_result =
+    std::make_shared<std::promise<std::vector<rcl_interfaces::msg::ParameterDescriptor>>>();
+  auto future_result = promise_result->get_future().share();
+
+  auto request = std::make_shared<rcl_interfaces::srv::DescribeParameters::Request>();
+  request->names = names;
+
+  describe_parameters_client_->async_send_request(
+    request,
+    [promise_result, future_result, callback](
+      rclcpp::Client<rcl_interfaces::srv::DescribeParameters>::SharedFuture cb_f)
+    {
+      promise_result->set_value(cb_f.get()->descriptors);
+      if (callback != nullptr) {
+        callback(future_result);
+      }
+    }
+  );
+
+  return future_result;
+}
+
 std::shared_future<std::vector<rclcpp::ParameterType>>
 AsyncParametersClient::get_parameter_types(
   const std::vector<std::string> & names,
@@ -337,87 +341,17 @@ AsyncParametersClient::wait_for_service_nanoseconds(std::chrono::nanoseconds tim
   return true;
 }
 
-SyncParametersClient::SyncParametersClient(
-  rclcpp::Node::SharedPtr node,
-  const std::string & remote_node_name,
-  const rmw_qos_profile_t & qos_profile)
-: SyncParametersClient(
-    std::make_shared<rclcpp::executors::SingleThreadedExecutor>(),
-    node,
-    remote_node_name,
-    qos_profile)
-{}
-
-SyncParametersClient::SyncParametersClient(
-  rclcpp::Executor::SharedPtr executor,
-  rclcpp::Node::SharedPtr node,
-  const std::string & remote_node_name,
-  const rmw_qos_profile_t & qos_profile)
-: SyncParametersClient(
-    executor,
-    node->get_node_base_interface(),
-    node->get_node_topics_interface(),
-    node->get_node_graph_interface(),
-    node->get_node_services_interface(),
-    remote_node_name,
-    qos_profile)
-{}
-
-SyncParametersClient::SyncParametersClient(
-  rclcpp::Node * node,
-  const std::string & remote_node_name,
-  const rmw_qos_profile_t & qos_profile)
-: SyncParametersClient(
-    std::make_shared<rclcpp::executors::SingleThreadedExecutor>(),
-    node,
-    remote_node_name,
-    qos_profile)
-{}
-
-SyncParametersClient::SyncParametersClient(
-  rclcpp::Executor::SharedPtr executor,
-  rclcpp::Node * node,
-  const std::string & remote_node_name,
-  const rmw_qos_profile_t & qos_profile)
-: SyncParametersClient(
-    executor,
-    node->get_node_base_interface(),
-    node->get_node_topics_interface(),
-    node->get_node_graph_interface(),
-    node->get_node_services_interface(),
-    remote_node_name,
-    qos_profile)
-{}
-
-SyncParametersClient::SyncParametersClient(
-  rclcpp::Executor::SharedPtr executor,
-  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
-  const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
-  const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
-  const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
-  const std::string & remote_node_name,
-  const rmw_qos_profile_t & qos_profile)
-: executor_(executor), node_base_interface_(node_base_interface)
-{
-  async_parameters_client_ =
-    std::make_shared<AsyncParametersClient>(
-    node_base_interface,
-    node_topics_interface,
-    node_graph_interface,
-    node_services_interface,
-    remote_node_name,
-    qos_profile);
-}
-
 std::vector<rclcpp::Parameter>
-SyncParametersClient::get_parameters(const std::vector<std::string> & parameter_names)
+SyncParametersClient::get_parameters(
+  const std::vector<std::string> & parameter_names,
+  std::chrono::nanoseconds timeout)
 {
   auto f = async_parameters_client_->get_parameters(parameter_names);
   using rclcpp::executors::spin_node_until_future_complete;
   if (
     spin_node_until_future_complete(
-      *executor_, node_base_interface_,
-      f) == rclcpp::FutureReturnCode::SUCCESS)
+      *executor_, node_base_interface_, f,
+      timeout) == rclcpp::FutureReturnCode::SUCCESS)
   {
     return f.get();
   }
@@ -434,16 +368,34 @@ SyncParametersClient::has_parameter(const std::string & parameter_name)
   return vars.names.size() > 0;
 }
 
+std::vector<rcl_interfaces::msg::ParameterDescriptor>
+SyncParametersClient::describe_parameters(
+  const std::vector<std::string> & parameter_names,
+  std::chrono::nanoseconds timeout)
+{
+  auto f = async_parameters_client_->describe_parameters(parameter_names);
+
+  using rclcpp::executors::spin_node_until_future_complete;
+  rclcpp::FutureReturnCode future =
+    spin_node_until_future_complete(*executor_, node_base_interface_, f, timeout);
+  if (future == rclcpp::FutureReturnCode::SUCCESS) {
+    return f.get();
+  }
+  return std::vector<rcl_interfaces::msg::ParameterDescriptor>();
+}
+
 std::vector<rclcpp::ParameterType>
-SyncParametersClient::get_parameter_types(const std::vector<std::string> & parameter_names)
+SyncParametersClient::get_parameter_types(
+  const std::vector<std::string> & parameter_names,
+  std::chrono::nanoseconds timeout)
 {
   auto f = async_parameters_client_->get_parameter_types(parameter_names);
 
   using rclcpp::executors::spin_node_until_future_complete;
   if (
     spin_node_until_future_complete(
-      *executor_, node_base_interface_,
-      f) == rclcpp::FutureReturnCode::SUCCESS)
+      *executor_, node_base_interface_, f,
+      timeout) == rclcpp::FutureReturnCode::SUCCESS)
   {
     return f.get();
   }
@@ -452,15 +404,16 @@ SyncParametersClient::get_parameter_types(const std::vector<std::string> & param
 
 std::vector<rcl_interfaces::msg::SetParametersResult>
 SyncParametersClient::set_parameters(
-  const std::vector<rclcpp::Parameter> & parameters)
+  const std::vector<rclcpp::Parameter> & parameters,
+  std::chrono::nanoseconds timeout)
 {
   auto f = async_parameters_client_->set_parameters(parameters);
 
   using rclcpp::executors::spin_node_until_future_complete;
   if (
     spin_node_until_future_complete(
-      *executor_, node_base_interface_,
-      f) == rclcpp::FutureReturnCode::SUCCESS)
+      *executor_, node_base_interface_, f,
+      timeout) == rclcpp::FutureReturnCode::SUCCESS)
   {
     return f.get();
   }
@@ -469,15 +422,16 @@ SyncParametersClient::set_parameters(
 
 rcl_interfaces::msg::SetParametersResult
 SyncParametersClient::set_parameters_atomically(
-  const std::vector<rclcpp::Parameter> & parameters)
+  const std::vector<rclcpp::Parameter> & parameters,
+  std::chrono::nanoseconds timeout)
 {
   auto f = async_parameters_client_->set_parameters_atomically(parameters);
 
   using rclcpp::executors::spin_node_until_future_complete;
   if (
     spin_node_until_future_complete(
-      *executor_, node_base_interface_,
-      f) == rclcpp::FutureReturnCode::SUCCESS)
+      *executor_, node_base_interface_, f,
+      timeout) == rclcpp::FutureReturnCode::SUCCESS)
   {
     return f.get();
   }
@@ -488,15 +442,16 @@ SyncParametersClient::set_parameters_atomically(
 rcl_interfaces::msg::ListParametersResult
 SyncParametersClient::list_parameters(
   const std::vector<std::string> & parameter_prefixes,
-  uint64_t depth)
+  uint64_t depth,
+  std::chrono::nanoseconds timeout)
 {
   auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth);
 
   using rclcpp::executors::spin_node_until_future_complete;
   if (
     spin_node_until_future_complete(
-      *executor_, node_base_interface_,
-      f) == rclcpp::FutureReturnCode::SUCCESS)
+      *executor_, node_base_interface_, f,
+      timeout) == rclcpp::FutureReturnCode::SUCCESS)
   {
     return f.get();
   }

rclcpp/src/rclcpp/parameter_service.cpp

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

rclcpp/src/rclcpp/publisher_base.cpp

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

rclcpp/src/rclcpp/qos.cpp

@@ -99,6 +99,13 @@ QoS::history(rmw_qos_history_policy_t history)
   return *this;
 }
 
+QoS &
+QoS::history(HistoryPolicy history)
+{
+  rmw_qos_profile_.history = static_cast<rmw_qos_history_policy_t>(history);
+  return *this;
+}
+
 QoS &
 QoS::keep_last(size_t depth)
 {
@@ -122,6 +129,13 @@ QoS::reliability(rmw_qos_reliability_policy_t reliability)
   return *this;
 }
 
+QoS &
+QoS::reliability(ReliabilityPolicy reliability)
+{
+  rmw_qos_profile_.reliability = static_cast<rmw_qos_reliability_policy_t>(reliability);
+  return *this;
+}
+
 QoS &
 QoS::reliable()
 {
@@ -141,6 +155,13 @@ QoS::durability(rmw_qos_durability_policy_t durability)
   return *this;
 }
 
+QoS &
+QoS::durability(DurabilityPolicy durability)
+{
+  rmw_qos_profile_.durability = static_cast<rmw_qos_durability_policy_t>(durability);
+  return *this;
+}
+
 QoS &
 QoS::durability_volatile()
 {
@@ -186,6 +207,14 @@ QoS::liveliness(rmw_qos_liveliness_policy_t liveliness)
   return *this;
 }
 
+QoS &
+QoS::liveliness(LivelinessPolicy liveliness)
+{
+  rmw_qos_profile_.liveliness = static_cast<rmw_qos_liveliness_policy_t>(liveliness);
+  return *this;
+}
+
+
 QoS &
 QoS::liveliness_lease_duration(rmw_time_t liveliness_lease_duration)
 {
@@ -206,6 +235,51 @@ QoS::avoid_ros_namespace_conventions(bool avoid_ros_namespace_conventions)
   return *this;
 }
 
+HistoryPolicy
+QoS::history() const
+{
+  return static_cast<HistoryPolicy>(rmw_qos_profile_.history);
+}
+
+size_t
+QoS::depth() const {return rmw_qos_profile_.depth;}
+
+ReliabilityPolicy
+QoS::reliability() const
+{
+  return static_cast<ReliabilityPolicy>(rmw_qos_profile_.reliability);
+}
+
+DurabilityPolicy
+QoS::durability() const
+{
+  return static_cast<DurabilityPolicy>(rmw_qos_profile_.durability);
+}
+
+Duration
+QoS::deadline() const {return Duration::from_rmw_time(rmw_qos_profile_.deadline);}
+
+Duration
+QoS::lifespan() const {return Duration::from_rmw_time(rmw_qos_profile_.lifespan);}
+
+LivelinessPolicy
+QoS::liveliness() const
+{
+  return static_cast<LivelinessPolicy>(rmw_qos_profile_.liveliness);
+}
+
+Duration
+QoS::liveliness_lease_duration() const
+{
+  return Duration::from_rmw_time(rmw_qos_profile_.liveliness_lease_duration);
+}
+
+bool
+QoS::avoid_ros_namespace_conventions() const
+{
+  return rmw_qos_profile_.avoid_ros_namespace_conventions;
+}
+
 namespace
 {
 /// Check if two rmw_time_t have the same values.
@@ -235,6 +309,12 @@ bool operator!=(const QoS & left, const QoS & right)
   return !(left == right);
 }
 
+ClockQoS::ClockQoS(const QoSInitialization & qos_initialization)
+// Using `rmw_qos_profile_sensor_data` intentionally.
+// It's best effort and `qos_initialization` is overriding the depth to 1.
+: QoS(qos_initialization, rmw_qos_profile_sensor_data)
+{}
+
 SensorDataQoS::SensorDataQoS(const QoSInitialization & qos_initialization)
 : QoS(qos_initialization, rmw_qos_profile_sensor_data)
 {}
@@ -251,6 +331,10 @@ ParameterEventsQoS::ParameterEventsQoS(const QoSInitialization & qos_initializat
 : QoS(qos_initialization, rmw_qos_profile_parameter_events)
 {}
 
+RosoutQoS::RosoutQoS(const QoSInitialization & rosout_initialization)
+: QoS(rosout_initialization, rcl_qos_profile_rosout_default)
+{}
+
 SystemDefaultsQoS::SystemDefaultsQoS(const QoSInitialization & qos_initialization)
 : QoS(qos_initialization, rmw_qos_profile_system_default)
 {}

rclcpp/src/rclcpp/qos_overriding_options.cpp

@@ -0,0 +1,84 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/qos_overriding_options.hpp"
+
+#include <initializer_list>
+#include <ostream>
+#include <stdexcept>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "rmw/qos_policy_kind.h"
+#include "rmw/qos_string_conversions.h"
+
+namespace rclcpp
+{
+
+const char *
+qos_policy_kind_to_cstr(const QosPolicyKind & qpk)
+{
+  const char * ret = rmw_qos_policy_kind_to_str(static_cast<rmw_qos_policy_kind_t>(qpk));
+  if (!ret) {
+    throw std::invalid_argument{"unknown QoS policy kind"};
+  }
+  return ret;
+}
+
+std::ostream &
+operator<<(std::ostream & oss, const QosPolicyKind & qpk)
+{
+  return oss << qos_policy_kind_to_cstr(qpk);
+}
+
+static std::initializer_list<QosPolicyKind> kDefaultPolicies =
+{QosPolicyKind::History, QosPolicyKind::Depth, QosPolicyKind::Reliability};
+
+QosOverridingOptions::QosOverridingOptions(
+  std::initializer_list<QosPolicyKind> policy_kinds,
+  QosCallback validation_callback,
+  std::string id)
+: id_{std::move(id)},
+  policy_kinds_{policy_kinds},
+  validation_callback_{std::move(validation_callback)}
+{}
+
+QosOverridingOptions
+QosOverridingOptions::with_default_policies(
+  QosCallback validation_callback,
+  std::string id)
+{
+  return QosOverridingOptions{kDefaultPolicies, validation_callback, id};
+}
+
+const std::string &
+QosOverridingOptions::get_id() const
+{
+  return id_;
+}
+
+const std::vector<QosPolicyKind> &
+QosOverridingOptions::get_policy_kinds() const
+{
+  return policy_kinds_;
+}
+
+const QosCallback &
+QosOverridingOptions::get_validation_callback() const
+{
+  return validation_callback_;
+}
+
+}  // namespace rclcpp

rclcpp/src/rclcpp/signal_handler.cpp

@@ -243,7 +243,7 @@ SignalHandler::deferred_signal_handler()
             get_logger(),
             "deferred_signal_handler(): "
             "shutting down rclcpp::Context @ %p, because it had shutdown_on_sigint == true",
-            context_ptr.get());
+            static_cast<void *>(context_ptr.get()));
           context_ptr->shutdown("signal handler");
         }
       }

rclcpp/src/rclcpp/subscription_base.cpp

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

rclcpp/src/rclcpp/time.cpp

@@ -63,17 +63,13 @@ Time::Time(int64_t nanoseconds, rcl_clock_type_t clock_type)
   rcl_time_.nanoseconds = nanoseconds;
 }
 
-Time::Time(const Time & rhs)
-: rcl_time_(rhs.rcl_time_)
-{
-  rcl_time_.nanoseconds = rhs.rcl_time_.nanoseconds;
-}
+Time::Time(const Time & rhs) = default;
 
 Time::Time(
   const builtin_interfaces::msg::Time & time_msg,
-  rcl_clock_type_t ros_time)
+  rcl_clock_type_t clock_type)
+: rcl_time_(init_time_point(clock_type))
 {
-  rcl_time_ = init_time_point(ros_time);
   if (time_msg.sec < 0) {
     throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
   }
@@ -95,31 +91,25 @@ Time::~Time()
 Time::operator builtin_interfaces::msg::Time() const
 {
   builtin_interfaces::msg::Time msg_time;
-  msg_time.sec = static_cast<std::int32_t>(RCL_NS_TO_S(rcl_time_.nanoseconds));
-  msg_time.nanosec = static_cast<std::uint32_t>(rcl_time_.nanoseconds % (1000 * 1000 * 1000));
+  constexpr rcl_time_point_value_t kRemainder = RCL_S_TO_NS(1);
+  const auto result = std::div(rcl_time_.nanoseconds, kRemainder);
+  if (result.rem >= 0) {
+    msg_time.sec = static_cast<std::int32_t>(result.quot);
+    msg_time.nanosec = static_cast<std::uint32_t>(result.rem);
+  } else {
+    msg_time.sec = static_cast<std::int32_t>(result.quot - 1);
+    msg_time.nanosec = static_cast<std::uint32_t>(kRemainder + result.rem);
+  }
   return msg_time;
 }
 
 Time &
-Time::operator=(const Time & rhs)
-{
-  rcl_time_ = rhs.rcl_time_;
-  return *this;
-}
+Time::operator=(const Time & rhs) = default;
 
 Time &
 Time::operator=(const builtin_interfaces::msg::Time & time_msg)
 {
-  if (time_msg.sec < 0) {
-    throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
-  }
-
-
-  rcl_clock_type_t ros_time = RCL_ROS_TIME;
-  rcl_time_ = init_time_point(ros_time);  // TODO(tfoote) hard coded ROS here
-
-  rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast<int64_t>(time_msg.sec));
-  rcl_time_.nanoseconds += time_msg.nanosec;
+  *this = Time(time_msg);
   return *this;
 }
 
@@ -209,7 +199,7 @@ Time::operator-(const rclcpp::Time & rhs) const
     throw std::underflow_error("time subtraction leads to int64_t underflow");
   }
 
-  return Duration(rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds);
+  return Duration::from_nanoseconds(rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds);
 }
 
 Time

rclcpp/src/rclcpp/time_source.cpp

@@ -33,17 +33,16 @@
 namespace rclcpp
 {
 
-TimeSource::TimeSource(std::shared_ptr<rclcpp::Node> node)
+TimeSource::TimeSource(std::shared_ptr<rclcpp::Node> node, const rclcpp::QoS & qos)
 : logger_(rclcpp::get_logger("rclcpp")),
-  clock_subscription_(nullptr),
-  ros_time_active_(false)
+  qos_(qos)
 {
   this->attachNode(node);
 }
 
-TimeSource::TimeSource()
+TimeSource::TimeSource(const rclcpp::QoS & qos)
 : logger_(rclcpp::get_logger("rclcpp")),
-  ros_time_active_(false)
+  qos_(qos)
 {
 }
 
@@ -185,11 +184,21 @@ void TimeSource::set_clock(
   const builtin_interfaces::msg::Time::SharedPtr msg, bool set_ros_time_enabled,
   std::shared_ptr<rclcpp::Clock> clock)
 {
+  std::lock_guard<std::mutex> clock_guard(clock->get_clock_mutex());
+
   // Do change
   if (!set_ros_time_enabled && clock->ros_time_is_active()) {
-    disable_ros_time(clock);
+    auto ret = rcl_disable_ros_time_override(clock->get_clock_handle());
+    if (ret != RCL_RET_OK) {
+      rclcpp::exceptions::throw_from_rcl_error(
+        ret, "Failed to disable ros_time_override_status");
+    }
   } else if (set_ros_time_enabled && !clock->ros_time_is_active()) {
-    enable_ros_time(clock);
+    auto ret = rcl_enable_ros_time_override(clock->get_clock_handle());
+    if (ret != RCL_RET_OK) {
+      rclcpp::exceptions::throw_from_rcl_error(
+        ret, "Failed to enable ros_time_override_status");
+    }
   }
 
   auto ret = rcl_set_ros_time_override(clock->get_clock_handle(), rclcpp::Time(*msg).nanoseconds());
@@ -224,11 +233,22 @@ void TimeSource::create_clock_sub()
     return;
   }
 
+  rclcpp::SubscriptionOptions options;
+  options.qos_overriding_options = rclcpp::QosOverridingOptions(
+    {
+      rclcpp::QosPolicyKind::Depth,
+      rclcpp::QosPolicyKind::Durability,
+      rclcpp::QosPolicyKind::History,
+      rclcpp::QosPolicyKind::Reliability,
+    });
+
   clock_subscription_ = rclcpp::create_subscription<rosgraph_msgs::msg::Clock>(
+    node_parameters_,
     node_topics_,
     "/clock",
-    rclcpp::QoS(QoSInitialization::from_rmw(rmw_qos_profile_default)),
-    std::bind(&TimeSource::clock_cb, this, std::placeholders::_1)
+    rclcpp::QoS(KeepLast(1)).best_effort(),
+    std::bind(&TimeSource::clock_cb, this, std::placeholders::_1),
+    options
   );
 }
 
@@ -273,24 +293,6 @@ void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::S
   }
 }
 
-void TimeSource::enable_ros_time(std::shared_ptr<rclcpp::Clock> clock)
-{
-  auto ret = rcl_enable_ros_time_override(clock->get_clock_handle());
-  if (ret != RCL_RET_OK) {
-    rclcpp::exceptions::throw_from_rcl_error(
-      ret, "Failed to enable ros_time_override_status");
-  }
-}
-
-void TimeSource::disable_ros_time(std::shared_ptr<rclcpp::Clock> clock)
-{
-  auto ret = rcl_disable_ros_time_override(clock->get_clock_handle());
-  if (ret != RCL_RET_OK) {
-    rclcpp::exceptions::throw_from_rcl_error(
-      ret, "Failed to enable ros_time_override_status");
-  }
-}
-
 void TimeSource::enable_ros_time()
 {
   if (ros_time_active_) {

rclcpp/src/rclcpp/timer.cpp

@@ -61,15 +61,11 @@ TimerBase::TimerBase(
   rcl_clock_t * clock_handle = clock_->get_clock_handle();
   {
     std::lock_guard<std::mutex> clock_guard(clock_->get_clock_mutex());
-    if (
-      rcl_timer_init(
-        timer_handle_.get(), clock_handle, rcl_context.get(), period.count(), nullptr,
-        rcl_get_default_allocator()) != RCL_RET_OK)
-    {
-      RCUTILS_LOG_ERROR_NAMED(
-        "rclcpp",
-        "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string().str);
-      rcl_reset_error();
+    rcl_ret_t ret = rcl_timer_init(
+      timer_handle_.get(), clock_handle, rcl_context.get(), period.count(), nullptr,
+      rcl_get_default_allocator());
+    if (ret != RCL_RET_OK) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't initialize rcl timer handle");
     }
   }
 }
@@ -80,8 +76,9 @@ TimerBase::~TimerBase()
 void
 TimerBase::cancel()
 {
-  if (rcl_timer_cancel(timer_handle_.get()) != RCL_RET_OK) {
-    throw std::runtime_error(std::string("Couldn't cancel timer: ") + rcl_get_error_string().str);
+  rcl_ret_t ret = rcl_timer_cancel(timer_handle_.get());
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't cancel timer");
   }
 }
 
@@ -99,8 +96,9 @@ TimerBase::is_canceled()
 void
 TimerBase::reset()
 {
-  if (rcl_timer_reset(timer_handle_.get()) != RCL_RET_OK) {
-    throw std::runtime_error(std::string("Couldn't reset timer: ") + rcl_get_error_string().str);
+  rcl_ret_t ret = rcl_timer_reset(timer_handle_.get());
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't reset timer");
   }
 }
 
@@ -108,8 +106,9 @@ bool
 TimerBase::is_ready()
 {
   bool ready = false;
-  if (rcl_timer_is_ready(timer_handle_.get(), &ready) != RCL_RET_OK) {
-    throw std::runtime_error(std::string("Failed to check timer: ") + rcl_get_error_string().str);
+  rcl_ret_t ret = rcl_timer_is_ready(timer_handle_.get(), &ready);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to check timer");
   }
   return ready;
 }
@@ -118,14 +117,10 @@ std::chrono::nanoseconds
 TimerBase::time_until_trigger()
 {
   int64_t time_until_next_call = 0;
-  if (
-    rcl_timer_get_time_until_next_call(
-      timer_handle_.get(),
-      &time_until_next_call) != RCL_RET_OK)
-  {
-    throw std::runtime_error(
-            std::string(
-              "Timer could not get time until next call: ") + rcl_get_error_string().str);
+  rcl_ret_t ret = rcl_timer_get_time_until_next_call(
+    timer_handle_.get(), &time_until_next_call);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Timer could not get time until next call");
   }
   return std::chrono::nanoseconds(time_until_next_call);
 }

rclcpp/src/rclcpp/utilities.cpp

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

rclcpp/test/CMakeLists.txt

@@ -0,0 +1,18 @@
+find_package(rosidl_default_generators REQUIRED)
+
+find_package(test_msgs REQUIRED)
+
+include(cmake/rclcpp_add_build_failure_test.cmake)
+
+add_subdirectory(benchmark)
+add_subdirectory(rclcpp)
+
+ament_add_gtest(test_rclcpp_gtest_macros utils/test_rclcpp_gtest_macros.cpp)
+if(TARGET test_rclcpp_gtest_macros)
+  target_link_libraries(test_rclcpp_gtest_macros ${PROJECT_NAME})
+endif()
+
+# Install test resources
+install(
+  DIRECTORY resources
+  DESTINATION ${CMAKE_CURRENT_BINARY_DIR})

rclcpp/test/benchmark/CMakeLists.txt

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