2.0.2

link against thread library where necessary (#1210 ) (#1214 )
Signed-off-by: Dirk Thomas <dirk-thomas@users.noreply.github.com>
2020-07-07 21:07:12 -07:00 · 2020-07-07 20:34:33 -07:00 · 2020-06-25 09:45:13 -07:00 · 2020-06-23 17:32:15 -07:00 · 2020-06-23 14:44:39 -07:00 · 2020-06-23 10:44:19 -07:00
232 changed files with 3936 additions and 21208 deletions
--- a/rclcpp/CHANGELOG.rst
+++ b/rclcpp/CHANGELOG.rst
@@ -2,150 +2,22 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^

-5.1.0 (2020-11-02)
+2.0.2 (2020-07-07)
 ------------------
-* 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
+* Link against thread library where necessary (`#1210 <https://github.com/ros2/rclcpp/issues/1210>`_) (`#1214 <https://github.com/ros2/rclcpp/issues/1214>`_)
+* Fix exception message on rcl_clock_init (`#1182 <https://github.com/ros2/rclcpp/issues/1182>`_) (`#1193 <https://github.com/ros2/rclcpp/issues/1193>`_)
+* Contributors: Dirk Thomas, tomoya

-5.0.0 (2020-09-18)
+2.0.1 (2020-06-23)
 ------------------
-* 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>`_)
+* Add create_publisher include to create_subscription (`#1180 <https://github.com/ros2/rclcpp/issues/1180>`_) (`#1192 <https://github.com/ros2/rclcpp/issues/1192>`_)
+* Fix get_node_time_source_interface() docstring (`#988 <https://github.com/ros2/rclcpp/issues/988>`_) (`#1185 <https://github.com/ros2/rclcpp/issues/1185>`_)
+* Fixed doxygen warnings (`#1163 <https://github.com/ros2/rclcpp/issues/1163>`_) (`#1191 <https://github.com/ros2/rclcpp/issues/1191>`_)
 * Check 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
+  Make Executor::spin_once_impl private before backporting, to avoid API compatibility issues
+* Add check for invalid topic statistics publish period (`#1151 <https://github.com/ros2/rclcpp/issues/1151>`_) (`#1172 <https://github.com/ros2/rclcpp/issues/1172>`_)
+* Contributors: Alejandro Hernández Cordero, Devin Bonnie, DongheeYe, Ivan Santiago Paunovic, Jacob Perron, Stephen Brawner

 2.0.0 (2020-06-01)
 ------------------
--- a/rclcpp/CMakeLists.txt
+++ b/rclcpp/CMakeLists.txt
@@ -25,11 +25,7 @@ if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
 endif()
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  # 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)
+  add_compile_options(-Wall -Wextra -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual)
 endif()

 set(${PROJECT_NAME}_SRCS
@@ -219,10 +215,442 @@ 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()

-  add_subdirectory(test)
+  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)
 endif()

 ament_package()
--- a/rclcpp/QUALITY_DECLARATION.md
+++ b/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 2** category.
+The package `rclcpp` claims to be in the **Quality Level 4** category.

 Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-quality-categories) of the ROS2 developer guide.

@@ -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 its [ROS 2 API documentation](http://docs.ros2.org/latest/api/rclcpp/).
+The API is publicly available in the [ROS 2 API documentation](http://docs.ros2.org/latest/api/rclcpp/).

 ### License [3.iii]

 The license for `rclcpp` is Apache 2.0, and a summary is in each source file, the type is declared in the [`package.xml`](./package.xml) manifest file, and a full copy of the license is in the [`LICENSE`](../LICENSE) file.

-There is an automated test which runs a linter that ensures each file has a license statement. [Here](http://build.ros2.org/view/Fpr/job/Fpr__rclcpp__ubuntu_focal_amd64/lastSuccessfulBuild/testReport/rclcpp/) can be found a list with the latest results of the various linters being run on the package.
+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.

 ### Copyright Statements [3.iv]

 The copyright holders each provide a statement of copyright in each source code file in `rclcpp`.

-There is an automated test which runs a linter that ensures each file has at least one copyright statement. Latest linter result report can be seen [here](http://build.ros2.org/view/Fpr/job/Fpr__rclcpp__ubuntu_focal_amd64/lastSuccessfulBuild/testReport/rclcpp/copyright/).
+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/).

 ## Testing [4]

@@ -121,10 +121,6 @@ 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/lastSuccessfulBuild/cobertura/src_ros2_rclcpp_rclcpp_src_rclcpp/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://index.ros.org/doc/ros2/Contributing/ROS-2-On-boarding-Guide/#note-on-coverage-runs).
-
-`rclcpp` has a line coverage `>= 95%`, which is calculated over all directories within `rclcpp` with the exception of the `experimental` directory.
-
 ### Performance [4.iv]

 It is not yet defined if this package requires performance testing and how addresses this topic.
@@ -155,49 +151,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 2**, see its [Quality Declaration document](https://github.com/ros-tooling/libstatistics_collector/tree/master/QUALITY_DECLARATION.md).
+It is **Quality Level 4**, 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 2**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl/QUALITY_DECLARATION.md).
+It is **Quality Level 4**, 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 2**, see its [Quality Declaration document](https://github.com/ros2/rcl/tree/master/rcl_yaml_param_parser/QUALITY_DECLARATION.md).
+It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcl/tree/master/rcl_yaml_param_parser/QUALITY_DECLARATION.md).

 #### `rcpputils`

 The `rcpputils` package provides an API which contains common utilities and data structures useful when programming in C++.

-It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rcpputils/blob/master/QUALITY_DECLARATION.md).
+It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcpputils/blob/master/QUALITY_DECLARATION.md).

 #### `rcutils`

 The `rcutils` package provides an API which contains common utilities and data structures useful when programming in C.

-It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rcutils/blob/master/QUALITY_DECLARATION.md).
+It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcutils/blob/master/QUALITY_DECLARATION.md).

 #### `rmw`

 `rmw` is the ROS 2 middleware library.

-It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rmw/blob/master/rmw/QUALITY_DECLARATION.md).
+It is **Quality Level 4**, 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 2**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/statistics_msgs/QUALITY_DECLARATION.md).
+It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/statistics_msgs/QUALITY_DECLARATION.md).

 #### `tracetools`

-The `tracetools` package provides utilities for instrumenting the code in `rclcpp` so that it may be traced for debugging and performance analysis.
+The `tracetools` package provides utilities for instrumenting the code in `rcl` so that it may be traced for debugging and performance analysis.

-It is **Quality Level 1**, see its [Quality Declaration document](https://gitlab.com/ros-tracing/ros2_tracing/-/blob/master/tracetools/QUALITY_DECLARATION.md).
+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).

 ### Direct Runtime non-ROS Dependency [5.iii]

--- a/rclcpp/README.md
+++ b/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 2** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details.
+This package claims to be in the **Quality Level 4** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details.
--- a/rclcpp/test/cmake/rclcpp_add_build_failure_test.cmake
+++ b/rclcpp/test/cmake/rclcpp_add_build_failure_test.cmake
--- a/rclcpp/include/rclcpp/any_service_callback.hpp
+++ b/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, static_cast<const void *>(this), false);
+    TRACEPOINT(callback_start, (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, static_cast<const void *>(this));
+    TRACEPOINT(callback_end, (const void *)this);
  }

  void register_callback_for_tracing()
@@ -106,12 +106,12 @@ public:
    if (shared_ptr_callback_) {
      TRACEPOINT(
        rclcpp_callback_register,
-        static_cast<const void *>(this),
+        (const void *)this,
        get_symbol(shared_ptr_callback_));
    } else if (shared_ptr_with_request_header_callback_) {
      TRACEPOINT(
        rclcpp_callback_register,
-        static_cast<const void *>(this),
+        (const void *)this,
        get_symbol(shared_ptr_with_request_header_callback_));
    }
 #endif  // TRACETOOLS_DISABLED
--- a/rclcpp/include/rclcpp/any_subscription_callback.hpp
+++ b/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, static_cast<const void *>(this), false);
+    TRACEPOINT(callback_start, (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, static_cast<const void *>(this));
+    TRACEPOINT(callback_end, (const void *)this);
  }

  void dispatch_intra_process(
    ConstMessageSharedPtr message, const rclcpp::MessageInfo & message_info)
  {
-    TRACEPOINT(callback_start, static_cast<const void *>(this), true);
+    TRACEPOINT(callback_start, (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, static_cast<const void *>(this));
+    TRACEPOINT(callback_end, (const void *)this);
  }

  void dispatch_intra_process(
    MessageUniquePtr message, const rclcpp::MessageInfo & message_info)
  {
-    TRACEPOINT(callback_start, static_cast<const void *>(this), true);
+    TRACEPOINT(callback_start, (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, static_cast<const void *>(this));
+    TRACEPOINT(callback_end, (const void *)this);
  }

  bool use_take_shared_method() const
@@ -239,22 +239,22 @@ public:
    if (shared_ptr_callback_) {
      TRACEPOINT(
        rclcpp_callback_register,
-        static_cast<const void *>(this),
+        (const void *)this,
        get_symbol(shared_ptr_callback_));
    } else if (shared_ptr_with_info_callback_) {
      TRACEPOINT(
        rclcpp_callback_register,
-        static_cast<const void *>(this),
+        (const void *)this,
        get_symbol(shared_ptr_with_info_callback_));
    } else if (unique_ptr_callback_) {
      TRACEPOINT(
        rclcpp_callback_register,
-        static_cast<const void *>(this),
+        (const void *)this,
        get_symbol(unique_ptr_callback_));
    } else if (unique_ptr_with_info_callback_) {
      TRACEPOINT(
        rclcpp_callback_register,
-        static_cast<const void *>(this),
+        (const void *)this,
        get_symbol(unique_ptr_with_info_callback_));
    }
 #endif  // TRACETOOLS_DISABLED
--- a/rclcpp/include/rclcpp/callback_group.hpp
+++ b/rclcpp/include/rclcpp/callback_group.hpp
@@ -56,44 +56,8 @@ 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,
-    bool automatically_add_to_executor_with_node = true);
+  explicit CallbackGroup(CallbackGroupType group_type);

  template<typename Function>
  rclcpp::SubscriptionBase::SharedPtr
@@ -138,31 +102,6 @@ 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)

@@ -197,14 +136,12 @@ 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>
--- a/rclcpp/include/rclcpp/client.hpp
+++ b/rclcpp/include/rclcpp/client.hpp
@@ -361,8 +361,7 @@ public:
    SharedPromiseWithRequest promise = std::make_shared<PromiseWithRequest>();
    SharedFutureWithRequest future_with_request(promise->get_future());

-    auto wrapping_cb = [future_with_request, promise, request,
-        cb = std::forward<CallbackWithRequestType>(cb)](SharedFuture future) {
+    auto wrapping_cb = [future_with_request, promise, request, &cb](SharedFuture future) {
        auto response = future.get();
        promise->set_value(std::make_pair(request, response));
        cb(future_with_request);
--- a/rclcpp/include/rclcpp/context.hpp
+++ b/rclcpp/include/rclcpp/context.hpp
@@ -139,11 +139,6 @@ 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.
--- a/rclcpp/include/rclcpp/create_subscription.hpp
+++ b/rclcpp/include/rclcpp/create_subscription.hpp
@@ -117,14 +117,8 @@ create_subscription(
      rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>
      >(node_topics->get_node_base_interface()->get_name(), publisher);

-    std::weak_ptr<
-      rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>
-    > weak_subscription_topic_stats(subscription_topic_stats);
-    auto sub_call_back = [weak_subscription_topic_stats]() {
-        auto subscription_topic_stats = weak_subscription_topic_stats.lock();
-        if (subscription_topic_stats) {
-          subscription_topic_stats->publish_message();
-        }
+    auto sub_call_back = [subscription_topic_stats]() {
+        subscription_topic_stats->publish_message();
      };

    auto node_timer_interface = node_topics->get_node_timers_interface();
--- a/rclcpp/include/rclcpp/create_timer.hpp
+++ b/rclcpp/include/rclcpp/create_timer.hpp
@@ -76,14 +76,14 @@ create_timer(
 * \tparam DurationRepT
 * \tparam DurationT
 * \tparam CallbackT
- * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max()
+ * \param period period to exectute callback
 * \param callback callback to execute via the timer period
 * \param group
 * \param node_base
 * \param node_timers
 * \return
 * \throws std::invalid argument if either node_base or node_timers
- * are null, or period is negative or too large
+ * are null
 */
 template<typename DurationRepT, typename DurationT, typename CallbackT>
 typename rclcpp::WallTimer<CallbackT>::SharedPtr
@@ -102,38 +102,10 @@ 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(
-    period_ns, std::move(callback), node_base->get_context());
+    std::chrono::duration_cast<std::chrono::nanoseconds>(period),
+    std::move(callback),
+    node_base->get_context());
  node_timers->add_timer(timer, group);
  return timer;
 }
--- a/rclcpp/include/rclcpp/duration.hpp
+++ b/rclcpp/include/rclcpp/duration.hpp
@@ -21,13 +21,6 @@
 #include "rcl/time.h"
 #include "rclcpp/visibility_control.hpp"

-// Windows has preprocessor defines for "min" and "max", which can cause
-// problems when things like std::numeric_limits<int>::max are used.  This
-// macro is used to defeat the macro matching logic and hence workaround the
-// issue.  The idea comes from:
-// https://stackoverflow.com/questions/11544073/how-do-i-deal-with-the-max-macro-in-windows-h-colliding-with-max-in-std/11550864#11550864
-#define PREVENT_WINDOWS_MIN_MAX
-
 namespace rclcpp
 {
 class RCLCPP_PUBLIC Duration
@@ -45,14 +38,10 @@ public:
   */
  Duration(int32_t seconds, uint32_t nanoseconds);

-  /// 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);")]]
+  // This constructor matches any numeric value - ints or floats.
  explicit Duration(rcl_duration_value_t nanoseconds);

-  /// Construct duration from the specified std::chrono::nanoseconds.
+  // This constructor matches std::chrono::nanoseconds.
  explicit Duration(std::chrono::nanoseconds nanoseconds);

  // This constructor matches any std::chrono value other than nanoseconds
@@ -83,14 +72,11 @@ 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;

@@ -115,7 +101,7 @@ public:
   */
  static
  Duration
-  max PREVENT_WINDOWS_MIN_MAX();
+  max();

  Duration
  operator*(double scale) const;
@@ -140,10 +126,6 @@ public:
  static Duration
  from_seconds(double seconds);

-  /// Create a duration object from an integer number representing nanoseconds
-  static Duration
-  from_nanoseconds(rcl_duration_value_t nanoseconds);
-
  /// Convert Duration into a std::chrono::Duration.
  template<class DurationT>
  DurationT
@@ -158,8 +140,6 @@ public:

 private:
  rcl_duration_t rcl_duration_;
-
-  Duration() = default;
 };

 }  // namespace rclcpp
--- a/rclcpp/include/rclcpp/exceptions/exceptions.hpp
+++ b/rclcpp/include/rclcpp/exceptions/exceptions.hpp
@@ -100,15 +100,6 @@ 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
--- a/rclcpp/include/rclcpp/executor.hpp
+++ b/rclcpp/include/rclcpp/executor.hpp
@@ -21,7 +21,6 @@
 #include <cstdlib>
 #include <iostream>
 #include <list>
-#include <map>
 #include <memory>
 #include <mutex>
 #include <string>
@@ -43,10 +42,6 @@
 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;

@@ -81,118 +76,13 @@ 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.
  /**
-   * 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.
-   *
+   * 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 a node is already associated to an executor
   */
  RCLCPP_PUBLIC
  virtual void
@@ -208,19 +98,10 @@ 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
@@ -279,7 +160,7 @@ public:
  void
  spin_node_some(std::shared_ptr<rclcpp::Node> node);

-  /// Collect work once and execute all available work, optionally within a duration.
+  /// Complete all available queued work without blocking.
  /**
   * This function can be overridden. The default implementation is suitable for a
   * single-threaded model of execution.
@@ -294,23 +175,6 @@ 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));
@@ -325,10 +189,10 @@ public:
   *   code.
   * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
   */
-  template<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::milli>
+  template<typename ResponseT, typename TimeRepT = int64_t, typename TimeT = std::milli>
  FutureReturnCode
  spin_until_future_complete(
-    const FutureT & future,
+    const std::shared_future<ResponseT> & 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
@@ -406,10 +270,6 @@ 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,
@@ -446,79 +306,22 @@ protected:

  RCLCPP_PUBLIC
  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
-  get_node_by_group(
-    WeakCallbackGroupsToNodesMap weak_groups_to_nodes,
-    rclcpp::CallbackGroup::SharedPtr group);
-
-  /// Return true if the node has been added to this executor.
-  /**
-   * \param[in] node_ptr a shared pointer that points to a node base interface
-   * \return true if the node is associated with the executor, otherwise false
-   */
-  RCLCPP_PUBLIC
-  bool
-  has_node(
-    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    WeakCallbackGroupsToNodesMap weak_groups_to_nodes) const;
+  get_node_by_group(rclcpp::CallbackGroup::SharedPtr group);

  RCLCPP_PUBLIC
  rclcpp::CallbackGroup::SharedPtr
  get_group_by_timer(rclcpp::TimerBase::SharedPtr timer);

-  /// Add a callback group to an executor
-  /**
-   * \see rclcpp::Executor::add_callback_group
-   */
-  RCLCPP_PUBLIC
-  virtual void
-  add_callback_group_to_map(
-    rclcpp::CallbackGroup::SharedPtr group_ptr,
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
-    bool notify = true);
-
-  /// Remove a callback group from the executor.
-  /**
-   * \see rclcpp::Executor::remove_callback_group
-   */
-  RCLCPP_PUBLIC
-  virtual void
-  remove_callback_group_from_map(
-    rclcpp::CallbackGroup::SharedPtr group_ptr,
-    WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
-    bool notify = true);
-
  RCLCPP_PUBLIC
  bool
  get_next_ready_executable(AnyExecutable & any_executable);

-  RCLCPP_PUBLIC
-  bool
-  get_next_ready_executable_from_map(
-    AnyExecutable & any_executable,
-    WeakCallbackGroupsToNodesMap weak_groups_to_nodes);
-
  RCLCPP_PUBLIC
  bool
  get_next_executable(
    AnyExecutable & any_executable,
    std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));

-  /// Add all callback groups that can be automatically added from associated nodes.
-  /**
-   * The executor, before collecting entities, verifies if any callback group from
-   * nodes associated with the executor, which is not already associated to an executor,
-   * can be automatically added to this executor.
-   * This takes care of any callback group that has been added to a node but not explicitly added
-   * to the executor.
-   * It is important to note that in order for the callback groups to be automatically added to an
-   * executor through this function, the node of the callback groups needs to have been added
-   * through the `add_node` method.
-   */
-  RCLCPP_PUBLIC
-  virtual void
-  add_callback_groups_from_nodes_associated_to_executor();
-
  /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins.
  std::atomic_bool spinning;

@@ -539,29 +342,13 @@ protected:

  RCLCPP_DISABLE_COPY(Executor)

+  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
+  std::list<const rcl_guard_condition_t *> guard_conditions_;
+
+private:
  RCLCPP_PUBLIC
  void
  spin_once_impl(std::chrono::nanoseconds timeout);
-
-  typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
-      const rcl_guard_condition_t *,
-      std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
-    WeakNodesToGuardConditionsMap;
-
-  /// maps nodes to guard conditions
-  WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_;
-
-  /// maps callback groups associated to nodes
-  WeakCallbackGroupsToNodesMap weak_groups_associated_with_executor_to_nodes_;
-
-  /// maps callback groups to nodes associated with executor
-  WeakCallbackGroupsToNodesMap weak_groups_to_nodes_associated_with_executor_;
-
-  /// maps all callback groups to nodes
-  WeakCallbackGroupsToNodesMap weak_groups_to_nodes_;
-
-  /// nodes that are associated with the executor
-  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
 };

 namespace executor
--- a/rclcpp/include/rclcpp/executors.hpp
+++ b/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 FutureT, typename TimeRepT = int64_t, typename TimeT = std::milli>
+template<typename ResponseT, 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 FutureT & future,
+  const std::shared_future<ResponseT> & 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 FutureT, typename TimeRepT = int64_t,
+template<typename NodeT = rclcpp::Node, typename ResponseT, 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 FutureT & future,
+  const std::shared_future<ResponseT> & 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 FutureT & future,
+  const std::shared_future<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 FutureT & future,
+  const std::shared_future<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);
--- a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp
+++ b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp
@@ -17,9 +17,7 @@

 #include <chrono>
 #include <list>
-#include <map>
 #include <memory>
-#include <vector>

 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
@@ -34,9 +32,6 @@ 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,
@@ -50,7 +45,6 @@ public:
  StaticExecutorEntitiesCollector() = default;

  // Destructor
-  RCLCPP_PUBLIC
  ~StaticExecutorEntitiesCollector();

  /// Initialize StaticExecutorEntitiesCollector
@@ -64,23 +58,34 @@ public:
  void
  init(
    rcl_wait_set_t * p_wait_set,
-    rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy,
+    rclcpp::memory_strategy::MemoryStrategy::SharedPtr & memory_strategy,
    rcl_guard_condition_t * executor_guard_condition);

-  /// Finalize StaticExecutorEntitiesCollector to clear resources
-  RCLCPP_PUBLIC
-  void
-  fini();
-
  RCLCPP_PUBLIC
  void
  execute() override;

+  RCLCPP_PUBLIC
+  void
+  fill_memory_strategy();
+
+  RCLCPP_PUBLIC
+  void
+  fill_executable_list();
+
+  /// Function to reallocate space for entities in the wait set.
+  /**
+   * \throws std::runtime_error if wait set couldn't be cleared or resized.
+   */
+  RCLCPP_PUBLIC
+  void
+  prepare_wait_set();
+
  /// Function to add_handles_to_wait_set and wait for work and
  /**
   * block until the wait set is ready or until the timeout has been exceeded.
   * \throws std::runtime_error if wait set couldn't be cleared or filled.
-   * \throws any rcl errors from rcl_wait, \see rclcpp::exceptions::throw_from_rcl_error()
+   * \throws any rcl errors from rcl_wait, \sa rclcpp::exceptions::throw_from_rcl_error()
   */
  RCLCPP_PUBLIC
  void
@@ -97,58 +102,17 @@ public:
  size_t
  get_number_of_ready_guard_conditions() override;

-  /// Add a callback group to an executor.
  /**
-   * \see rclcpp::Executor::add_callback_group
-   */
-  RCLCPP_PUBLIC
-  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()
+   * \sa rclcpp::Executor::add_node()
   * \throw std::runtime_error if node was already added
   */
  RCLCPP_PUBLIC
-  bool
+  void
  add_node(
    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);

  /**
-   * \see rclcpp::Executor::remove_node()
+   * \sa rclcpp::Executor::remove_node()
   * \throw std::runtime_error if no guard condition is associated with node.
   */
  RCLCPP_PUBLIC
@@ -156,26 +120,6 @@ 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
@@ -272,55 +216,12 @@ public:
  get_waitable(size_t i) {return exec_list_.waitable[i];}

 private:
-  /// 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);
+  /// Nodes guard conditions which trigger this waitable
+  std::list<const rcl_guard_condition_t *> guard_conditions_;

  /// 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_;

--- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp
+++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp
@@ -78,30 +78,15 @@ 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.
  /**
-   * \sa rclcpp::Executor::add_node
+   * 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
   */
  RCLCPP_PUBLIC
  void
@@ -111,7 +96,8 @@ public:

  /// Convenience function which takes Node and forwards NodeBaseInterface.
  /**
-   * \sa rclcpp::StaticSingleThreadedExecutor::add_node
+   * \throw std::runtime_error if node was already added or if rcl_trigger_guard_condition
+   * returns an error
   */
  RCLCPP_PUBLIC
  void
@@ -119,7 +105,11 @@ public:

  /// Remove a node from the executor.
  /**
-   * \sa rclcpp::Executor::remove_node
+   * \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
   */
  RCLCPP_PUBLIC
  void
@@ -129,32 +119,12 @@ public:

  /// Convenience function which takes Node and forwards NodeBaseInterface.
  /**
-   * \sa rclcpp::Executor::remove_node
+   * \throw std::runtime_error if rcl_trigger_guard_condition returns an error
   */
  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
@@ -173,10 +143,10 @@ public:
   *  exec.add_node(node);
   *  exec.spin_until_future_complete(future);
   */
-  template<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::milli>
+  template<typename ResponseT, typename TimeRepT = int64_t, typename TimeT = std::milli>
  rclcpp::FutureReturnCode
  spin_until_future_complete(
-    FutureT & future,
+    std::shared_future<ResponseT> & future,
    std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
  {
    std::future_status status = future.wait_for(std::chrono::seconds(0));
@@ -192,8 +162,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.
@@ -221,38 +191,6 @@ 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.
  /**
--- a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp
+++ b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp
@@ -37,10 +37,6 @@ 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>
 {
@@ -59,125 +55,55 @@ 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)
  {
-    std::lock_guard<std::mutex> lock(mutex_);
-    return next_(val);
+    return (val + 1) % capacity_;
  }

-  /// 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
  {
-    std::lock_guard<std::mutex> lock(mutex_);
-    return has_data_();
+    return size_ != 0;
  }

-  /// 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
+  inline bool is_full()
  {
-    std::lock_guard<std::mutex> lock(mutex_);
-    return is_full_();
+    return size_ == capacity_;
  }

  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_;
@@ -186,7 +112,7 @@ private:
  size_t read_index_;
  size_t size_;

-  mutable std::mutex mutex_;
+  std::mutex mutex_;
 };

 }  // namespace buffers
--- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp
+++ b/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 this case is equivalent to all the buffers requiring ownership
+      // So we 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);
--- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp
+++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp
@@ -92,8 +92,8 @@ public:

    TRACEPOINT(
      rclcpp_subscription_callback_added,
-      static_cast<const void *>(this),
-      static_cast<const void *>(&any_callback_));
+      (const void *)this,
+      (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 +102,6 @@ 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)
  {
--- a/rclcpp/include/rclcpp/future_return_code.hpp
+++ b/rclcpp/include/rclcpp/future_return_code.hpp
@@ -42,6 +42,20 @@ 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_
--- a/rclcpp/include/rclcpp/graph_listener.hpp
+++ b/rclcpp/include/rclcpp/graph_listener.hpp
@@ -160,12 +160,6 @@ protected:
  void
  run_loop();

-  RCLCPP_PUBLIC
-  void init_wait_set();
-
-  RCLCPP_PUBLIC
-  void cleanup_wait_set();
-
 private:
  RCLCPP_DISABLE_COPY(GraphListener)

--- a/rclcpp/include/rclcpp/init_options.hpp
+++ b/rclcpp/include/rclcpp/init_options.hpp
@@ -16,7 +16,6 @@
 #define RCLCPP__INIT_OPTIONS_HPP_

 #include <memory>
-#include <mutex>

 #include "rcl/init_options.h"
 #include "rclcpp/visibility_control.hpp"
@@ -81,30 +80,11 @@ 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};
 };
--- a/rclcpp/include/rclcpp/logger.hpp
+++ b/rclcpp/include/rclcpp/logger.hpp
@@ -21,7 +21,6 @@
 #include "rclcpp/visibility_control.hpp"

 #include "rcl/node.h"
-#include "rcutils/logging.h"

 /**
 * \def RCLCPP_LOGGING_ENABLED
@@ -77,18 +76,6 @@ get_node_logger(const rcl_node_t * node);

 class Logger
 {
-public:
-  /// An enum for the type of logger level.
-  enum class Level
-  {
-    Unset = RCUTILS_LOG_SEVERITY_UNSET,  ///< The unset log level
-    Debug = RCUTILS_LOG_SEVERITY_DEBUG,  ///< The debug log level
-    Info = RCUTILS_LOG_SEVERITY_INFO,    ///< The info log level
-    Warn = RCUTILS_LOG_SEVERITY_WARN,    ///< The warn log level
-    Error = RCUTILS_LOG_SEVERITY_ERROR,  ///< The error log level
-    Fatal = RCUTILS_LOG_SEVERITY_FATAL,  ///< The fatal log level
-  };
-
 private:
  friend Logger rclcpp::get_logger(const std::string & name);
  friend ::rclcpp::node_interfaces::NodeLogging;
@@ -151,16 +138,6 @@ 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
--- a/rclcpp/include/rclcpp/memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/memory_strategy.hpp
@@ -16,7 +16,6 @@
 #define RCLCPP__MEMORY_STRATEGY_HPP_

 #include <list>
-#include <map>
 #include <memory>

 #include "rcl/allocator.h"
@@ -43,13 +42,11 @@ class RCLCPP_PUBLIC MemoryStrategy
 {
 public:
  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy)
-  using WeakCallbackGroupsToNodesMap = std::map<rclcpp::CallbackGroup::WeakPtr,
-      rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
-      std::owner_less<rclcpp::CallbackGroup::WeakPtr>>;
+  using WeakNodeList = std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>;

  virtual ~MemoryStrategy() = default;

-  virtual bool collect_entities(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0;
+  virtual bool collect_entities(const WeakNodeList & weak_nodes) = 0;

  virtual size_t number_of_ready_subscriptions() const = 0;
  virtual size_t number_of_ready_services() const = 0;
@@ -71,27 +68,27 @@ public:
  virtual void
  get_next_subscription(
    rclcpp::AnyExecutable & any_exec,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0;
+    const WeakNodeList & weak_nodes) = 0;

  virtual void
  get_next_service(
    rclcpp::AnyExecutable & any_exec,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0;
+    const WeakNodeList & weak_nodes) = 0;

  virtual void
  get_next_client(
    rclcpp::AnyExecutable & any_exec,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0;
+    const WeakNodeList & weak_nodes) = 0;

  virtual void
  get_next_timer(
    rclcpp::AnyExecutable & any_exec,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0;
+    const WeakNodeList & weak_nodes) = 0;

  virtual void
  get_next_waitable(
    rclcpp::AnyExecutable & any_exec,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0;
+    const WeakNodeList & weak_nodes) = 0;

  virtual rcl_allocator_t
  get_allocator() = 0;
@@ -99,52 +96,52 @@ public:
  static rclcpp::SubscriptionBase::SharedPtr
  get_subscription_by_handle(
    std::shared_ptr<const rcl_subscription_t> subscriber_handle,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
+    const WeakNodeList & weak_nodes);

  static rclcpp::ServiceBase::SharedPtr
  get_service_by_handle(
    std::shared_ptr<const rcl_service_t> service_handle,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
+    const WeakNodeList & weak_nodes);

  static rclcpp::ClientBase::SharedPtr
  get_client_by_handle(
    std::shared_ptr<const rcl_client_t> client_handle,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
+    const WeakNodeList & weak_nodes);

  static rclcpp::TimerBase::SharedPtr
  get_timer_by_handle(
    std::shared_ptr<const rcl_timer_t> timer_handle,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
+    const WeakNodeList & weak_nodes);

  static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
  get_node_by_group(
    rclcpp::CallbackGroup::SharedPtr group,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
+    const WeakNodeList & weak_nodes);

  static rclcpp::CallbackGroup::SharedPtr
  get_group_by_subscription(
    rclcpp::SubscriptionBase::SharedPtr subscription,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
+    const WeakNodeList & weak_nodes);

  static rclcpp::CallbackGroup::SharedPtr
  get_group_by_service(
    rclcpp::ServiceBase::SharedPtr service,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
+    const WeakNodeList & weak_nodes);

  static rclcpp::CallbackGroup::SharedPtr
  get_group_by_client(
    rclcpp::ClientBase::SharedPtr client,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
+    const WeakNodeList & weak_nodes);

  static rclcpp::CallbackGroup::SharedPtr
  get_group_by_timer(
    rclcpp::TimerBase::SharedPtr timer,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
+    const WeakNodeList & weak_nodes);

  static rclcpp::CallbackGroup::SharedPtr
  get_group_by_waitable(
    rclcpp::Waitable::SharedPtr waitable,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
+    const WeakNodeList & weak_nodes);
 };

 }  // namespace memory_strategy
--- a/rclcpp/include/rclcpp/node.hpp
+++ b/rclcpp/include/rclcpp/node.hpp
@@ -26,8 +26,6 @@
 #include <utility>
 #include <vector>

-#include "rcutils/macros.h"
-
 #include "rcl/error_handling.h"
 #include "rcl/node.h"

@@ -141,9 +139,7 @@ public:
  /// Create and return a callback group.
  RCLCPP_PUBLIC
  rclcpp::CallbackGroup::SharedPtr
-  create_callback_group(
-    rclcpp::CallbackGroupType group_type,
-    bool automatically_add_to_executor_with_node = true);
+  create_callback_group(rclcpp::CallbackGroupType group_type);

  /// Return the list of callback groups in the node.
  RCLCPP_PUBLIC
@@ -816,7 +812,6 @@ 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);

@@ -847,6 +842,26 @@ 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.
@@ -887,21 +902,15 @@ 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 created for a given topic.
+  /// 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.
-   * \return number of subscribers that have been created for the given topic.
-   * \throws std::runtime_error if subscribers could not be counted
+   * \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
   */
  RCLCPP_PUBLIC
  size_t
@@ -922,7 +931,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 actual topic name used; it will not be automatically remapped.
+   * \param[in] topic_name the topic_name on which to find the publishers.
   * \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.
@@ -948,7 +957,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 actual topic name used; it will not be automatically remapped.
+   * \param[in] topic_name the topic_name on which to find the subscriptions.
   * \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.
@@ -1182,6 +1191,10 @@ 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_;
--- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp
@@ -83,9 +83,7 @@ public:

  RCLCPP_PUBLIC
  rclcpp::CallbackGroup::SharedPtr
-  create_callback_group(
-    rclcpp::CallbackGroupType group_type,
-    bool automatically_add_to_executor_with_node = true) override;
+  create_callback_group(rclcpp::CallbackGroupType group_type) override;

  RCLCPP_PUBLIC
  rclcpp::CallbackGroup::SharedPtr
@@ -118,10 +116,6 @@ 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)

--- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp
@@ -106,9 +106,7 @@ public:
  RCLCPP_PUBLIC
  virtual
  rclcpp::CallbackGroup::SharedPtr
-  create_callback_group(
-    rclcpp::CallbackGroupType group_type,
-    bool automatically_add_to_executor_with_node = true) = 0;
+  create_callback_group(rclcpp::CallbackGroupType group_type) = 0;

  /// Return the default callback group.
  RCLCPP_PUBLIC
@@ -163,13 +161,6 @@ 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
--- a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp
@@ -110,7 +110,7 @@ public:

  RCLCPP_PUBLIC
  size_t
-  count_graph_users() const override;
+  count_graph_users() override;

  RCLCPP_PUBLIC
  std::vector<rclcpp::TopicEndpointInfo>
--- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp
@@ -146,7 +146,6 @@ 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
   */
@@ -160,7 +159,6 @@ 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
@@ -170,7 +168,6 @@ 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
@@ -183,36 +180,24 @@ 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
@@ -278,11 +263,10 @@ public:
  RCLCPP_PUBLIC
  virtual
  size_t
-  count_graph_users() const = 0;
+  count_graph_users() = 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
@@ -292,7 +276,6 @@ 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
--- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp
@@ -21,8 +21,6 @@
 #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"
@@ -162,7 +160,6 @@ 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;

@@ -170,6 +167,11 @@ 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;
--- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp
@@ -191,6 +191,17 @@ 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
--- a/rclcpp/include/rclcpp/node_interfaces/node_services.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_services.hpp
@@ -15,8 +15,6 @@
 #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"
@@ -55,10 +53,6 @@ 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)

--- a/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp
@@ -15,8 +15,6 @@
 #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"
@@ -51,12 +49,6 @@ 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
--- a/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp
@@ -24,7 +24,6 @@
 #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"

@@ -47,8 +46,7 @@ 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,
-    const rclcpp::QoS & qos = rclcpp::RosoutQoS()
+    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters
  );

  RCLCPP_PUBLIC
--- a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp
@@ -81,10 +81,6 @@ 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)

--- a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp
@@ -86,12 +86,6 @@ 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
--- a/rclcpp/include/rclcpp/node_options.hpp
+++ b/rclcpp/include/rclcpp/node_options.hpp
@@ -46,8 +46,6 @@ 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
@@ -245,19 +243,6 @@ 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 &
@@ -271,19 +256,6 @@ 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 &
@@ -355,6 +327,11 @@ 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.
@@ -382,14 +359,10 @@ 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};
--- a/rclcpp/include/rclcpp/parameter_client.hpp
+++ b/rclcpp/include/rclcpp/parameter_client.hpp
@@ -73,21 +73,12 @@ public:
   * \param[in] qos_profile (optional) The rmw qos profile to use to subscribe
   * \param[in] group (optional) The async parameter client will be added to this callback group.
   */
-  template<typename NodeT>
+  RCLCPP_PUBLIC
  AsyncParametersClient(
-    const std::shared_ptr<NodeT> node,
+    const rclcpp::Node::SharedPtr node,
    const std::string & remote_node_name = "",
    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters,
-    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::CallbackGroup::SharedPtr group = nullptr);

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

  RCLCPP_PUBLIC
  std::shared_future<std::vector<rclcpp::Parameter>>
@@ -195,7 +177,7 @@ public:
  {
    return rclcpp::create_subscription<rcl_interfaces::msg::ParameterEvent>(
      node,
-      "/parameter_events",
+      "parameter_events",
      qos,
      std::forward<CallbackT>(callback),
      options);
@@ -255,61 +237,31 @@ class SyncParametersClient
 public:
  RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient)

-  template<typename NodeT>
+  RCLCPP_PUBLIC
  explicit SyncParametersClient(
-    std::shared_ptr<NodeT> node,
+    rclcpp::Node::SharedPtr node,
    const std::string & remote_node_name = "",
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters)
-  : SyncParametersClient(
-      std::make_shared<rclcpp::executors::SingleThreadedExecutor>(),
-      node,
-      remote_node_name,
-      qos_profile)
-  {}
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);

-  template<typename NodeT>
+  RCLCPP_PUBLIC
  SyncParametersClient(
    rclcpp::Executor::SharedPtr executor,
-    std::shared_ptr<NodeT> node,
+    rclcpp::Node::SharedPtr node,
    const std::string & remote_node_name = "",
-    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)
-  {}
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);

-  template<typename NodeT>
-  SyncParametersClient(
-    NodeT * node,
+  RCLCPP_PUBLIC
+  explicit SyncParametersClient(
+    rclcpp::Node * node,
    const std::string & remote_node_name = "",
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters)
-  : SyncParametersClient(
-      std::make_shared<rclcpp::executors::SingleThreadedExecutor>(),
-      node,
-      remote_node_name,
-      qos_profile)
-  {}
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);

-  template<typename NodeT>
+  RCLCPP_PUBLIC
  SyncParametersClient(
    rclcpp::Executor::SharedPtr executor,
-    NodeT * node,
+    rclcpp::Node * node,
    const std::string & remote_node_name = "",
-    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)
-  {}
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);

  RCLCPP_PUBLIC
  SyncParametersClient(
@@ -319,18 +271,7 @@ 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)
-  : 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);
-  }
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);

  RCLCPP_PUBLIC
  std::vector<rclcpp::Parameter>
--- a/rclcpp/include/rclcpp/publisher.hpp
+++ b/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,
--- a/rclcpp/include/rclcpp/qos.hpp
+++ b/rclcpp/include/rclcpp/qos.hpp
@@ -19,7 +19,6 @@

 #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"
@@ -27,7 +26,6 @@
 namespace rclcpp
 {

-RCLCPP_PUBLIC
 std::string qos_policy_name_from_kind(rmw_qos_policy_kind_t policy_kind);

 /// QoS initialization values, cannot be created directly, use KeepAll or KeepLast instead.
@@ -163,26 +161,6 @@ 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,
@@ -271,28 +249,6 @@ 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,
--- a/rclcpp/include/rclcpp/qos_event.hpp
+++ b/rclcpp/include/rclcpp/qos_event.hpp
@@ -34,7 +34,6 @@ 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;

@@ -42,7 +41,6 @@ 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 &)>;
@@ -61,7 +59,6 @@ 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
--- a/rclcpp/include/rclcpp/service.hpp
+++ b/rclcpp/include/rclcpp/service.hpp
@@ -223,8 +223,8 @@ public:
    }
    TRACEPOINT(
      rclcpp_service_callback_added,
-      static_cast<const void *>(get_service_handle().get()),
-      static_cast<const void *>(&any_callback_));
+      (const void *)get_service_handle().get(),
+      (const void *)&any_callback_);
 #ifndef TRACETOOLS_DISABLED
    any_callback_.register_callback_for_tracing();
 #endif
@@ -258,8 +258,8 @@ public:
    service_handle_ = service_handle;
    TRACEPOINT(
      rclcpp_service_callback_added,
-      static_cast<const void *>(get_service_handle().get()),
-      static_cast<const void *>(&any_callback_));
+      (const void *)get_service_handle().get(),
+      (const void *)&any_callback_);
 #ifndef TRACETOOLS_DISABLED
    any_callback_.register_callback_for_tracing();
 #endif
@@ -295,8 +295,8 @@ public:
    service_handle_->impl = service_handle->impl;
    TRACEPOINT(
      rclcpp_service_callback_added,
-      static_cast<const void *>(get_service_handle().get()),
-      static_cast<const void *>(&any_callback_));
+      (const void *)get_service_handle().get(),
+      (const void *)&any_callback_);
 #ifndef TRACETOOLS_DISABLED
    any_callback_.register_callback_for_tracing();
 #endif
--- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
@@ -150,47 +150,48 @@ public:
    );
  }

-  bool collect_entities(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
+  bool collect_entities(const WeakNodeList & weak_nodes) override
  {
-    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;
+    bool has_invalid_weak_nodes = false;
+    for (auto & weak_node : weak_nodes) {
+      auto node = weak_node.lock();
+      if (!node) {
+        has_invalid_weak_nodes = true;
        continue;
      }
-      if (!group || !group->can_be_taken_from().load()) {
-        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;
+          });
      }
-      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_groups_or_nodes;
+    return has_invalid_weak_nodes;
  }

  void add_waitable_handle(const rclcpp::Waitable::SharedPtr & waitable) override
@@ -263,14 +264,14 @@ public:
  void
  get_next_subscription(
    rclcpp::AnyExecutable & any_exec,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
+    const WeakNodeList & weak_nodes) override
  {
    auto it = subscription_handles_.begin();
    while (it != subscription_handles_.end()) {
-      auto subscription = get_subscription_by_handle(*it, weak_groups_to_nodes);
+      auto subscription = get_subscription_by_handle(*it, weak_nodes);
      if (subscription) {
        // Find the group for this handle and see if it can be serviced
-        auto group = get_group_by_subscription(subscription, weak_groups_to_nodes);
+        auto group = get_group_by_subscription(subscription, weak_nodes);
        if (!group) {
          // Group was not found, meaning the subscription is not valid...
          // Remove it from the ready list and continue looking
@@ -286,7 +287,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_groups_to_nodes);
+        any_exec.node_base = get_node_by_group(group, weak_nodes);
        subscription_handles_.erase(it);
        return;
      }
@@ -298,14 +299,14 @@ public:
  void
  get_next_service(
    rclcpp::AnyExecutable & any_exec,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
+    const WeakNodeList & weak_nodes) override
  {
    auto it = service_handles_.begin();
    while (it != service_handles_.end()) {
-      auto service = get_service_by_handle(*it, weak_groups_to_nodes);
+      auto service = get_service_by_handle(*it, weak_nodes);
      if (service) {
        // Find the group for this handle and see if it can be serviced
-        auto group = get_group_by_service(service, weak_groups_to_nodes);
+        auto group = get_group_by_service(service, weak_nodes);
        if (!group) {
          // Group was not found, meaning the service is not valid...
          // Remove it from the ready list and continue looking
@@ -321,7 +322,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_groups_to_nodes);
+        any_exec.node_base = get_node_by_group(group, weak_nodes);
        service_handles_.erase(it);
        return;
      }
@@ -331,16 +332,14 @@ public:
  }

  void
-  get_next_client(
-    rclcpp::AnyExecutable & any_exec,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
+  get_next_client(rclcpp::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) override
  {
    auto it = client_handles_.begin();
    while (it != client_handles_.end()) {
-      auto client = get_client_by_handle(*it, weak_groups_to_nodes);
+      auto client = get_client_by_handle(*it, weak_nodes);
      if (client) {
        // Find the group for this handle and see if it can be serviced
-        auto group = get_group_by_client(client, weak_groups_to_nodes);
+        auto group = get_group_by_client(client, weak_nodes);
        if (!group) {
          // Group was not found, meaning the service is not valid...
          // Remove it from the ready list and continue looking
@@ -356,7 +355,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_groups_to_nodes);
+        any_exec.node_base = get_node_by_group(group, weak_nodes);
        client_handles_.erase(it);
        return;
      }
@@ -368,14 +367,14 @@ public:
  void
  get_next_timer(
    rclcpp::AnyExecutable & any_exec,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
+    const WeakNodeList & weak_nodes) override
  {
    auto it = timer_handles_.begin();
    while (it != timer_handles_.end()) {
-      auto timer = get_timer_by_handle(*it, weak_groups_to_nodes);
+      auto timer = get_timer_by_handle(*it, weak_nodes);
      if (timer) {
        // Find the group for this handle and see if it can be serviced
-        auto group = get_group_by_timer(timer, weak_groups_to_nodes);
+        auto group = get_group_by_timer(timer, weak_nodes);
        if (!group) {
          // Group was not found, meaning the timer is not valid...
          // Remove it from the ready list and continue looking
@@ -391,7 +390,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_groups_to_nodes);
+        any_exec.node_base = get_node_by_group(group, weak_nodes);
        timer_handles_.erase(it);
        return;
      }
@@ -401,16 +400,14 @@ public:
  }

  void
-  get_next_waitable(
-    rclcpp::AnyExecutable & any_exec,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
+  get_next_waitable(rclcpp::AnyExecutable & any_exec, const WeakNodeList & weak_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_groups_to_nodes);
+        auto group = get_group_by_waitable(waitable, weak_nodes);
        if (!group) {
          // Group was not found, meaning the waitable is not valid...
          // Remove it from the ready list and continue looking
@@ -426,7 +423,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_groups_to_nodes);
+        any_exec.node_base = get_node_by_group(group, weak_nodes);
        waitable_handles_.erase(it);
        return;
      }
--- a/rclcpp/include/rclcpp/subscription.hpp
+++ b/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,11 +144,6 @@ 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)) {
@@ -184,8 +179,8 @@ public:
        resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback));
      TRACEPOINT(
        rclcpp_subscription_init,
-        static_cast<const void *>(get_subscription_handle().get()),
-        static_cast<const void *>(subscription_intra_process.get()));
+        (const void *)get_subscription_handle().get(),
+        (const void *)subscription_intra_process.get());

      // Add it to the intra process manager.
      using rclcpp::experimental::IntraProcessManager;
@@ -200,12 +195,12 @@ public:

    TRACEPOINT(
      rclcpp_subscription_init,
-      static_cast<const void *>(get_subscription_handle().get()),
-      static_cast<const void *>(this));
+      (const void *)get_subscription_handle().get(),
+      (const void *)this);
    TRACEPOINT(
      rclcpp_subscription_callback_added,
-      static_cast<const void *>(this),
-      static_cast<const void *>(&any_callback_));
+      (const void *)this,
+      (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.
--- a/rclcpp/include/rclcpp/subscription_base.hpp
+++ b/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
--- a/rclcpp/include/rclcpp/time.hpp
+++ b/rclcpp/include/rclcpp/time.hpp
@@ -23,13 +23,6 @@

 #include "rclcpp/duration.hpp"

-// Windows has preprocessor defines for "min" and "max", which can cause
-// problems when things like std::numeric_limits<int>::max are used.  This
-// macro is used to defeat the macro matching logic and hence workaround the
-// issue.  The idea comes from:
-// https://stackoverflow.com/questions/11544073/how-do-i-deal-with-the-max-macro-in-windows-h-colliding-with-max-in-std/11550864#11550864
-#define PREVENT_WINDOWS_MIN_MAX
-
 namespace rclcpp
 {

@@ -55,10 +48,10 @@ public:
  /// Time constructor
  /**
   * \param nanoseconds since time epoch
-   * \param clock_type clock type
+   * \param clock clock type
   */
  RCLCPP_PUBLIC
-  explicit Time(int64_t nanoseconds = 0, rcl_clock_type_t clock_type = RCL_SYSTEM_TIME);
+  explicit Time(int64_t nanoseconds = 0, rcl_clock_type_t clock = RCL_SYSTEM_TIME);

  /// Copy constructor
  RCLCPP_PUBLIC
@@ -67,13 +60,13 @@ public:
  /// Time constructor
  /**
   * \param time_msg builtin_interfaces time message to copy
-   * \param clock_type clock type
+   * \param ros_time clock type
   * \throws std::runtime_error if seconds are negative
   */
  RCLCPP_PUBLIC
  Time(
    const builtin_interfaces::msg::Time & time_msg,
-    rcl_clock_type_t clock_type = RCL_ROS_TIME);
+    rcl_clock_type_t ros_time = RCL_ROS_TIME);

  /// Time constructor
  /**
@@ -97,12 +90,6 @@ 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);
@@ -196,7 +183,7 @@ public:
   */
  RCLCPP_PUBLIC
  static Time
-  max PREVENT_WINDOWS_MIN_MAX();
+  max();

  /// Get the seconds since epoch
  /**
@@ -225,7 +212,6 @@ private:
 /**
 * \throws std::overflow_error if addition leads to overflow
 */
-RCLCPP_PUBLIC
 Time
 operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs);

--- a/rclcpp/include/rclcpp/time_source.hpp
+++ b/rclcpp/include/rclcpp/time_source.hpp
@@ -40,19 +40,16 @@ 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, const rclcpp::QoS & qos = rclcpp::ClockQoS());
+  explicit TimeSource(rclcpp::Node::SharedPtr node);

  /// Empty constructor
  /**
   * An Empty TimeSource class
-   *
-   * \param qos QoS that will be used when creating a `/clock` subscription.
   */
  RCLCPP_PUBLIC
-  explicit TimeSource(const rclcpp::QoS & qos = rclcpp::ClockQoS());
+  TimeSource();

  /// Attack node to the time source.
  /**
@@ -117,14 +114,11 @@ 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_{nullptr};
+  std::shared_ptr<SubscriptionT> clock_subscription_;
  std::mutex clock_sub_lock_;

  // The clock callback itself
@@ -154,6 +148,8 @@ 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,
@@ -161,7 +157,7 @@ private:

  // Local storage of validity of ROS time
  // This is needed when new clocks are added.
-  bool ros_time_active_{false};
+  bool ros_time_active_;
  // Last set message to be passed to newly registered clocks
  rosgraph_msgs::msg::Clock::SharedPtr last_msg_set_;

@@ -170,7 +166,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
--- a/rclcpp/include/rclcpp/timer.hpp
+++ b/rclcpp/include/rclcpp/timer.hpp
@@ -176,11 +176,11 @@ public:
  {
    TRACEPOINT(
      rclcpp_timer_callback_added,
-      static_cast<const void *>(get_timer_handle().get()),
-      static_cast<const void *>(&callback_));
+      (const void *)get_timer_handle().get(),
+      (const void *)&callback_);
    TRACEPOINT(
      rclcpp_callback_register,
-      static_cast<const void *>(&callback_),
+      (const void *)&callback_,
      get_symbol(callback_));
  }

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

  // void specialization
--- a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp
+++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp
@@ -73,17 +73,7 @@ protected:
    size_t services_from_waitables = 0;
    size_t events_from_waitables = 0;
    for (const auto & waitable_entry : waitables) {
-      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;
+      rclcpp::Waitable & waitable = *waitable_entry.waitable.get();
      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();
--- a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp
+++ b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp
@@ -382,13 +382,11 @@ 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),
-      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());
+    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));
  }

  void
--- a/rclcpp/include/rclcpp/wait_set_template.hpp
+++ b/rclcpp/include/rclcpp/wait_set_template.hpp
@@ -233,9 +233,9 @@ public:
        }
        if (mask.include_intra_process_waitable) {
          auto local_waitable = inner_subscription->get_intra_process_waitable();
+          inner_subscription->exchange_in_use_by_wait_set_state(local_waitable.get(), false);
          if (nullptr != local_waitable) {
-            // This is the case when intra process is enabled for the subscription.
-            inner_subscription->exchange_in_use_by_wait_set_state(local_waitable.get(), false);
+            // This is the case when intra process is disabled for the subscription.
            this->storage_remove_waitable(std::move(local_waitable));
          }
        }
--- a/rclcpp/package.xml
+++ b/rclcpp/package.xml
@@ -2,13 +2,10 @@
 <?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>5.1.0</version>
+  <version>2.0.2</version>
  <description>The ROS client library in C++.</description>
-  <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
-  <maintainer email="mabel@openrobotics.org">Mabel Zhang</maintainer>
-  <maintainer email="william@openrobotics.org">William Woodall</maintainer>
+  <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
  <license>Apache License 2.0</license>
-  <author email="dthomas@openrobotics.org">Dirk Thomas</author>

  <buildtool_depend>ament_cmake_ros</buildtool_depend>

@@ -38,8 +35,6 @@
  <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>
--- a/rclcpp/src/rclcpp/callback_group.cpp
+++ b/rclcpp/src/rclcpp/callback_group.cpp
@@ -19,12 +19,8 @@
 using rclcpp::CallbackGroup;
 using rclcpp::CallbackGroupType;

-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)
+CallbackGroup::CallbackGroup(CallbackGroupType group_type)
+: type_(group_type), can_be_taken_from_(true)
 {}


@@ -40,18 +36,6 @@ 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)
--- a/rclcpp/src/rclcpp/context.cpp
+++ b/rclcpp/src/rclcpp/context.cpp
@@ -199,17 +199,13 @@ Context::init(
    throw rclcpp::ContextAlreadyInitialized();
  }
  this->clean_up();
-  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);
+  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());
  if (RCL_RET_OK != ret) {
-    delete context;
+    rcl_context_.reset();
    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();
@@ -279,17 +275,6 @@ 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()
 {
--- a/rclcpp/src/rclcpp/duration.cpp
+++ b/rclcpp/src/rclcpp/duration.cpp
@@ -37,7 +37,7 @@ Duration::Duration(int32_t seconds, uint32_t nanoseconds)
  rcl_duration_.nanoseconds += nanoseconds;
 }

-Duration::Duration(rcl_duration_value_t nanoseconds)
+Duration::Duration(int64_t nanoseconds)
 {
  rcl_duration_.nanoseconds = nanoseconds;
 }
@@ -47,14 +47,17 @@ Duration::Duration(std::chrono::nanoseconds nanoseconds)
  rcl_duration_.nanoseconds = nanoseconds.count();
 }

-Duration::Duration(const Duration & rhs) = default;
+Duration::Duration(const Duration & rhs)
+{
+  rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds;
+}

 Duration::Duration(
  const builtin_interfaces::msg::Duration & duration_msg)
 {
  rcl_duration_.nanoseconds =
-    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);
+    static_cast<rcl_duration_value_t>(RCL_S_TO_NS(duration_msg.sec));
+  rcl_duration_.nanoseconds += duration_msg.nanosec;
 }

 Duration::Duration(const rcl_duration_t & duration)
@@ -66,25 +69,24 @@ Duration::Duration(const rcl_duration_t & duration)
 Duration::operator builtin_interfaces::msg::Duration() const
 {
  builtin_interfaces::msg::Duration msg_duration;
-  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);
-  }
+  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));
  return msg_duration;
 }

 Duration &
-Duration::operator=(const Duration & rhs) = default;
+Duration::operator=(const Duration & rhs)
+{
+  rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds;
+  return *this;
+}

 Duration &
 Duration::operator=(const builtin_interfaces::msg::Duration & duration_msg)
 {
-  *this = Duration(duration_msg);
+  rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast<int64_t>(duration_msg.sec));
+  rcl_duration_.nanoseconds += duration_msg.nanosec;
  return *this;
 }

@@ -94,12 +96,6 @@ 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
 {
@@ -148,7 +144,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::from_nanoseconds(
+  return Duration(
    rcl_duration_.nanoseconds + rhs.rcl_duration_.nanoseconds);
 }

@@ -177,7 +173,7 @@ Duration::operator-(const rclcpp::Duration & rhs) const
    rhs.rcl_duration_.nanoseconds,
    std::numeric_limits<rcl_duration_value_t>::max());

-  return Duration::from_nanoseconds(
+  return Duration(
    rcl_duration_.nanoseconds - rhs.rcl_duration_.nanoseconds);
 }

@@ -208,7 +204,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::from_nanoseconds(
+  return Duration(
    static_cast<rcl_duration_value_t>(
      static_cast<long double>(rcl_duration_.nanoseconds) * scale_ld));
 }
@@ -234,10 +230,6 @@ 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;
@@ -249,17 +241,7 @@ Duration::to_rmw_time() const
 Duration
 Duration::from_seconds(double 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;
+  return Duration(static_cast<int64_t>(RCL_S_TO_NS(seconds)));
 }

 }  // namespace rclcpp
--- a/rclcpp/src/rclcpp/executor.cpp
+++ b/rclcpp/src/rclcpp/executor.cpp
@@ -14,26 +14,20 @@

 #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/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;
@@ -80,39 +74,25 @@ Executor::Executor(const rclcpp::ExecutorOptions & options)
        "failed to destroy guard condition: %s", rcl_get_error_string().str);
      rcl_reset_error();
    }
-    throw_from_rcl_error(ret, "Failed to create wait set in Executor constructor");
+    throw std::runtime_error("Failed to create wait set in Executor constructor");
  }
 }

 Executor::~Executor()
 {
-  // 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();
+  // 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();
      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();
-  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;
+  for (auto & guard_condition : guard_conditions_) {
    memory_strategy_->remove_guard_condition(guard_condition);
  }
-  weak_nodes_to_guard_conditions_.clear();
+  guard_conditions_.clear();

  // Finalize the wait set.
  if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) {
@@ -133,117 +113,6 @@ Executor::~Executor()
  context_->release_interrupt_guard_condition(&wait_set_, std::nothrow);
 }

-std::vector<rclcpp::CallbackGroup::WeakPtr>
-Executor::get_all_callback_groups()
-{
-  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
-  for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
-    groups.push_back(group_node_ptr.first);
-  }
-  for (auto const & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) {
-    groups.push_back(group_node_ptr.first);
-  }
-  return groups;
-}
-
-std::vector<rclcpp::CallbackGroup::WeakPtr>
-Executor::get_manually_added_callback_groups()
-{
-  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
-  for (auto const & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
-    groups.push_back(group_node_ptr.first);
-  }
-  return groups;
-}
-
-std::vector<rclcpp::CallbackGroup::WeakPtr>
-Executor::get_automatically_added_callback_groups_from_nodes()
-{
-  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
-  for (auto const & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) {
-    groups.push_back(group_node_ptr.first);
-  }
-  return groups;
-}
-
-void
-Executor::add_callback_groups_from_nodes_associated_to_executor()
-{
-  for (auto & weak_node : weak_nodes_) {
-    auto node = weak_node.lock();
-    if (node) {
-      auto group_ptrs = node->get_callback_groups();
-      std::for_each(
-        group_ptrs.begin(), group_ptrs.end(),
-        [this, node](rclcpp::CallbackGroup::WeakPtr group_ptr)
-        {
-          auto shared_group_ptr = group_ptr.lock();
-          if (shared_group_ptr && shared_group_ptr->automatically_add_to_executor_with_node() &&
-          !shared_group_ptr->get_associated_with_executor_atomic().load())
-          {
-            this->add_callback_group_to_map(
-              shared_group_ptr,
-              node,
-              weak_groups_to_nodes_associated_with_executor_,
-              true);
-          }
-        });
-    }
-  }
-}
-
-void
-Executor::add_callback_group_to_map(
-  rclcpp::CallbackGroup::SharedPtr group_ptr,
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  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
-    std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
-    memory_strategy_->add_guard_condition(node_ptr->get_notify_guard_condition());
-  }
-}
-
-void
-Executor::add_callback_group(
-  rclcpp::CallbackGroup::SharedPtr group_ptr,
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  bool notify)
-{
-  this->add_callback_group_to_map(
-    group_ptr,
-    node_ptr,
-    weak_groups_associated_with_executor_to_nodes_,
-    notify);
-}
-
 void
 Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
 {
@@ -252,68 +121,25 @@ 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.");
  }
-  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);
+  // 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.");
    }
  }
  weak_nodes_.push_back(node_ptr);
-}
-
-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).");
+  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);
    }
-    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.");
  }
-  // If the node was matched and removed, interrupt waiting.
-  if (!has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) &&
-    !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_))
-  {
-    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr);
-    weak_nodes_to_guard_conditions_.erase(node_weak_ptr);
-    if (notify) {
-      rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
-      if (ret != RCL_RET_OK) {
-        throw_from_rcl_error(ret, "Failed to trigger guard condition on callback group remove");
-      }
-    }
-    std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
-    memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition());
-  }
-}
-
-void
-Executor::remove_callback_group(
-  rclcpp::CallbackGroup::SharedPtr group_ptr,
-  bool notify)
-{
-  this->remove_callback_group_from_map(
-    group_ptr,
-    weak_groups_associated_with_executor_to_nodes_,
-    notify);
+  // Add the node's notify condition to the guard condition handles
+  std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
+  memory_strategy_->add_guard_condition(node_ptr->get_notify_guard_condition());
 }

 void
@@ -325,44 +151,34 @@ 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)
 {
-  if (!node_ptr->get_associated_with_executor_atomic().load()) {
-    throw std::runtime_error("Node needs to be associated with an executor.");
-  }
-
-  bool found_node = false;
-  auto node_it = weak_nodes_.begin();
-  while (node_it != weak_nodes_.end()) {
-    bool matched = (node_it->lock() == node_ptr);
-    if (matched) {
-      found_node = true;
-      node_it = weak_nodes_.erase(node_it);
-    } else {
-      ++node_it;
-    }
-  }
-  if (!found_node) {
-    throw std::runtime_error("Node needs to be associated with this executor.");
-  }
-
-  for (auto it = weak_groups_to_nodes_associated_with_executor_.begin();
-    it != weak_groups_to_nodes_associated_with_executor_.end(); )
+  bool node_removed = false;
  {
-    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);
+    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;
+      }
    }
  }
-
  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
@@ -396,21 +212,8 @@ Executor::spin_node_some(std::shared_ptr<rclcpp::Node> node)
  this->spin_node_some(node->get_node_base_interface());
 }

-void Executor::spin_some(std::chrono::nanoseconds max_duration)
-{
-  return this->spin_some_impl(max_duration, false);
-}
-
-void Executor::spin_all(std::chrono::nanoseconds max_duration)
-{
-  if (max_duration <= 0ns) {
-    throw std::invalid_argument("max_duration must be positive");
-  }
-  return this->spin_some_impl(max_duration, true);
-}
-
 void
-Executor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive)
+Executor::spin_some(std::chrono::nanoseconds max_duration)
 {
  auto start = std::chrono::steady_clock::now();
  auto max_duration_not_elapsed = [max_duration, start]() {
@@ -429,20 +232,14 @@ Executor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive)
    throw std::runtime_error("spin_some() called while already spinning");
  }
  RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
-  bool work_available = false;
+  // non-blocking call to pre-load all available work
+  wait_for_work(std::chrono::milliseconds::zero());
  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 {
-      if (!work_available || !exhaustive) {
-        break;
-      }
-      work_available = false;
+      break;
    }
  }
 }
@@ -470,9 +267,8 @@ void
 Executor::cancel()
 {
  spinning.store(false);
-  rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
-  if (ret != RCL_RET_OK) {
-    throw_from_rcl_error(ret, "Failed to trigger guard condition in cancel");
+  if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+    throw std::runtime_error(rcl_get_error_string().str);
  }
 }

@@ -510,9 +306,8 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
  any_exec.callback_group->can_be_taken_from().store(true);
  // Wake the wait, because it may need to be recalculated or work that
  // was previously blocked is now available.
-  rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
-  if (ret != RCL_RET_OK) {
-    throw_from_rcl_error(ret, "Failed to trigger guard condition from execute_any_executable");
+  if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+    throw std::runtime_error(rcl_get_error_string().str);
  }
 }

@@ -657,67 +452,45 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
  {
    std::unique_lock<std::mutex> lock(memory_strategy_mutex_);

-    // Check weak_nodes_ to find any callback group that is not owned
-    // by an executor and add it to the list of callbackgroups for
-    // collect entities. Also exchange to false so it is not
-    // allowed to add to another executor
-    add_callback_groups_from_nodes_associated_to_executor();
-
    // Collect the subscriptions and timers to be waited on
    memory_strategy_->clear_handles();
-    bool has_invalid_weak_groups_or_nodes =
-      memory_strategy_->collect_entities(weak_groups_to_nodes_);
+    bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_);

-    if (has_invalid_weak_groups_or_nodes) {
-      std::vector<rclcpp::CallbackGroup::WeakPtr> invalid_group_ptrs;
-      for (auto pair : weak_groups_to_nodes_) {
-        auto weak_group_ptr = pair.first;
-        auto weak_node_ptr = pair.second;
-        if (weak_group_ptr.expired() || weak_node_ptr.expired()) {
-          invalid_group_ptrs.push_back(weak_group_ptr);
-          auto node_guard_pair = weak_nodes_to_guard_conditions_.find(weak_node_ptr);
-          weak_nodes_to_guard_conditions_.erase(weak_node_ptr);
-          memory_strategy_->remove_guard_condition(node_guard_pair->second);
+    // 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;
        }
      }
-      std::for_each(
-        invalid_group_ptrs.begin(), invalid_group_ptrs.end(),
-        [this](rclcpp::CallbackGroup::WeakPtr group_ptr) {
-          if (weak_groups_to_nodes_associated_with_executor_.find(group_ptr) !=
-          weak_groups_to_nodes_associated_with_executor_.end())
-          {
-            weak_groups_to_nodes_associated_with_executor_.erase(group_ptr);
-          }
-          if (weak_groups_associated_with_executor_to_nodes_.find(group_ptr) !=
-          weak_groups_associated_with_executor_to_nodes_.end())
-          {
-            weak_groups_associated_with_executor_to_nodes_.erase(group_ptr);
-          }
-          weak_groups_to_nodes_.erase(group_ptr);
-        });
    }
-
    // clear wait set
-    rcl_ret_t ret = rcl_wait_set_clear(&wait_set_);
-    if (ret != RCL_RET_OK) {
-      throw_from_rcl_error(ret, "Couldn't clear wait set");
+    if (rcl_wait_set_clear(&wait_set_) != RCL_RET_OK) {
+      throw std::runtime_error("Couldn't clear wait set");
    }

    // The size of waitables are accounted for in size of the other entities
-    ret = rcl_wait_set_resize(
+    rcl_ret_t ret = rcl_wait_set_resize(
      &wait_set_, memory_strategy_->number_of_ready_subscriptions(),
      memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(),
      memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(),
      memory_strategy_->number_of_ready_events());
    if (RCL_RET_OK != ret) {
-      throw_from_rcl_error(ret, "Couldn't resize the wait set");
+      throw std::runtime_error(
+              std::string("Couldn't resize the wait set : ") + rcl_get_error_string().str);
    }

    if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) {
      throw std::runtime_error("Couldn't fill wait set");
    }
  }
-
  rcl_ret_t status =
    rcl_wait(&wait_set_, std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count());
  if (status == RCL_RET_WAIT_SET_EMPTY) {
@@ -735,18 +508,22 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
 }

 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
-Executor::get_node_by_group(
-  rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap weak_groups_to_nodes,
-  rclcpp::CallbackGroup::SharedPtr group)
+Executor::get_node_by_group(rclcpp::CallbackGroup::SharedPtr group)
 {
  if (!group) {
    return nullptr;
  }
-  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;
+  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;
+      }
+    }
  }
  return nullptr;
 }
@@ -754,103 +531,80 @@ Executor::get_node_by_group(
 rclcpp::CallbackGroup::SharedPtr
 Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer)
 {
-  for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) {
-    auto group = pair.first.lock();
-    if (!group) {
+  for (auto & weak_node : weak_nodes_) {
+    auto node = weak_node.lock();
+    if (!node) {
      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;
+    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;
+      }
    }
  }
-
-  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;
+  return rclcpp::CallbackGroup::SharedPtr();
 }

 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,
-  rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap weak_groups_to_nodes)
 {
  bool success = false;
  // Check the timers to see if there are any that are ready
-  memory_strategy_->get_next_timer(any_executable, weak_groups_to_nodes);
+  memory_strategy_->get_next_timer(any_executable, weak_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_groups_to_nodes);
+    memory_strategy_->get_next_subscription(any_executable, weak_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_groups_to_nodes);
+    memory_strategy_->get_next_service(any_executable, weak_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_groups_to_nodes);
+    memory_strategy_->get_next_client(any_executable, weak_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_groups_to_nodes);
+    memory_strategy_->get_next_waitable(any_executable, weak_nodes_);
    if (any_executable.waitable) {
      success = true;
    }
  }
-  // At this point any_executable should be valid with either a valid subscription
+  // At this point any_exec 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 ..Check if the callback_group belongs to this executor
-    if (any_executable.callback_group && any_executable.callback_group->type() == \
-      CallbackGroupType::MutuallyExclusive)
+    // not, then mark it accordingly
+    using callback_group::CallbackGroupType;
+    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_executable is executed or when the
-      // any_executable is destructued
+      // This is reset to true either when the any_exec is executed or when the
+      // any_exec is destructued
      any_executable.callback_group->can_be_taken_from().store(false);
    }
  }
@@ -877,18 +631,3 @@ 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,
-  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();
-}
--- a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp
+++ b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp
@@ -18,8 +18,6 @@
 #include <memory>
 #include <stdexcept>
 #include <string>
-#include <utility>
-#include <vector>

 #include "rclcpp/memory_strategy.hpp"
 #include "rclcpp/executors/static_single_threaded_executor.hpp"
@@ -28,40 +26,23 @@ 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 (const auto & weak_node : weak_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();
      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();
-  weak_nodes_to_guard_conditions_.clear();
+  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
@@ -75,18 +56,12 @@ StaticExecutorEntitiesCollector::init(
  memory_strategy_ = memory_strategy;

  // Add executor's guard condition
-  memory_strategy_->add_guard_condition(executor_guard_condition);
+  guard_conditions_.push_back(executor_guard_condition);
+
  // Get memory strategy and executable list. Prepare wait_set_
  execute();
 }

-void
-StaticExecutorEntitiesCollector::fini()
-{
-  memory_strategy_->clear_handles();
-  exec_list_.clear();
-}
-
 void
 StaticExecutorEntitiesCollector::execute()
 {
@@ -102,41 +77,18 @@ void
 StaticExecutorEntitiesCollector::fill_memory_strategy()
 {
  memory_strategy_->clear_handles();
-  bool has_invalid_weak_groups_or_nodes =
-    memory_strategy_->collect_entities(weak_groups_to_nodes_associated_with_executor_);
+  bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_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_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);
+  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;
      }
    }
-    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
@@ -147,59 +99,59 @@ void
 StaticExecutorEntitiesCollector::fill_executable_list()
 {
  exec_list_.clear();
-  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()) {
+
+  for (auto & weak_node : weak_nodes_) {
+    auto node = weak_node.lock();
+    if (!node) {
      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;
-      });
+    // 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 the executor's waitable to the executable list
+  exec_list_.add_waitable(shared_from_this());
 }

 void
@@ -219,14 +171,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 (memset to '0' all wait_set_ entities
+  // clear wait set (memeset 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");
@@ -253,8 +205,7 @@ 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 & pair : weak_nodes_to_guard_conditions_) {
-    auto & gc = pair.second;
+  for (const auto & gc : guard_conditions_) {
    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");
@@ -265,152 +216,55 @@ StaticExecutorEntitiesCollector::add_to_wait_set(rcl_wait_set_t * wait_set)

 size_t StaticExecutorEntitiesCollector::get_number_of_ready_guard_conditions()
 {
-  return weak_nodes_to_guard_conditions_.size();
+  return guard_conditions_.size();
 }

-bool
+void
 StaticExecutorEntitiesCollector::add_node(
  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
 {
-  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);
+  // 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.");
    }
  }
+
  weak_nodes_.push_back(node_ptr);
-  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) {
-    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr);
-    weak_nodes_to_guard_conditions_[node_weak_ptr] = node_ptr->get_notify_guard_condition();
-    return true;
-  }
-  return false;
-}
-
-bool
-StaticExecutorEntitiesCollector::add_callback_group(
-  rclcpp::CallbackGroup::SharedPtr group_ptr,
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
-{
-  return add_callback_group(group_ptr, node_ptr, weak_groups_associated_with_executor_to_nodes_);
-}
-
-bool
-StaticExecutorEntitiesCollector::remove_callback_group(
-  rclcpp::CallbackGroup::SharedPtr group_ptr)
-{
-  return this->remove_callback_group_from_map(
-    group_ptr,
-    weak_groups_associated_with_executor_to_nodes_);
-}
-
-bool
-StaticExecutorEntitiesCollector::remove_callback_group_from_map(
-  rclcpp::CallbackGroup::SharedPtr group_ptr,
-  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;
+  guard_conditions_.push_back(node_ptr->get_notify_guard_condition());
 }

 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) {
-      weak_nodes_.erase(node_it);
-      node_found = true;
-      break;
-    }
-    ++node_it;
-  }
-  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);
+      // 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;
      }
-    });
-  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;
+
+      throw std::runtime_error("Didn't find guard condition associated with node.");
+
+    } else {
+      ++node_it;
+    }
+  }
+
+  return false;
 }

 bool
@@ -419,13 +273,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) {
-      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()) {
+      // 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()) {
        return true;
      }
    }
@@ -433,77 +287,3 @@ 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;
-}
--- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp
+++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp
@@ -15,7 +15,6 @@
 #include "rclcpp/executors/static_single_threaded_executor.hpp"

 #include <memory>
-#include <vector>

 #include "rclcpp/scope_exit.hpp"

@@ -42,7 +41,6 @@ StaticSingleThreadedExecutor::spin()
  // Set memory_strategy_ and exec_list_ based on weak_nodes_
  // Prepare wait_set_ based on memory_strategy_
  entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_);
-  RCLCPP_SCOPE_EXIT(entities_collector_->fini());

  while (rclcpp::ok(this->context_) && spinning.load()) {
    // Refresh wait set and wait for work
@@ -51,32 +49,24 @@ StaticSingleThreadedExecutor::spin()
  }
 }

-void
-StaticSingleThreadedExecutor::add_callback_group(
-  rclcpp::CallbackGroup::SharedPtr group_ptr,
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  bool 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);
-    }
-  }
-}
-
 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) {
+  // 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) {
    // 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
@@ -85,51 +75,23 @@ 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 (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-      throw std::runtime_error(rcl_get_error_string().str);
+    // 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::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();
+  std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
+  has_executor.store(false);
 }

 void
--- a/rclcpp/src/rclcpp/graph_listener.cpp
+++ b/rclcpp/src/rclcpp/graph_listener.cpp
@@ -62,27 +62,6 @@ GraphListener::~GraphListener()
  this->shutdown(std::nothrow);
 }

-void GraphListener::init_wait_set()
-{
-  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");
-  }
-}
-
 void
 GraphListener::start_if_not_started()
 {
@@ -92,8 +71,24 @@ GraphListener::start_if_not_started()
  }
  if (!is_started_) {
    // Initialize the wait set before starting.
-    init_wait_set();
-    // Register an on_shutdown hook to shutdown the graph listener.
+    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");
+    }
+    // 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();
@@ -355,15 +350,6 @@ GraphListener::remove_node(rclcpp::node_interfaces::NodeGraphInterface * node_gr
  remove_node_(&node_graph_interfaces_, node_graph);
 }

-void
-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(bool should_throw)
 {
@@ -400,7 +386,10 @@ GraphListener::__shutdown(bool should_throw)
      shutdown_guard_condition_ = nullptr;
    }
    if (is_started_) {
-      cleanup_wait_set();
+      ret = rcl_wait_set_fini(&wait_set_);
+      if (RCL_RET_OK != ret) {
+        throw_from_rcl_error(ret, "failed to finalize wait set");
+      }
    }
  }
 }
--- a/rclcpp/src/rclcpp/init_options.cpp
+++ b/rclcpp/src/rclcpp/init_options.cpp
@@ -26,7 +26,7 @@ InitOptions::InitOptions(rcl_allocator_t allocator)
  *init_options_ = rcl_get_zero_initialized_init_options();
  rcl_ret_t ret = rcl_init_options_init(init_options_.get(), allocator);
  if (RCL_RET_OK != ret) {
-    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl init options");
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialized rcl init options");
  }
 }

@@ -44,7 +44,6 @@ InitOptions::InitOptions(const InitOptions & other)
 : InitOptions(*other.get_rcl_init_options())
 {
  shutdown_on_sigint = other.shutdown_on_sigint;
-  initialize_logging_ = other.initialize_logging_;
 }

 bool
@@ -64,14 +63,12 @@ InitOptions &
 InitOptions::operator=(const InitOptions & other)
 {
  if (this != &other) {
-    std::lock_guard<std::mutex> init_options_lock(init_options_mutex_);
-    this->finalize_init_options_impl();
+    this->finalize_init_options();
    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;
 }
@@ -83,13 +80,6 @@ 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());
@@ -109,38 +99,4 @@ 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
--- a/rclcpp/src/rclcpp/logger.cpp
+++ b/rclcpp/src/rclcpp/logger.cpp
@@ -14,8 +14,8 @@

 #include <string>

-#include "rclcpp/exceptions.hpp"
 #include "rclcpp/logger.hpp"
+
 #include "rclcpp/logging.hpp"

 namespace rclcpp
@@ -44,22 +44,4 @@ get_node_logger(const rcl_node_t * node)
  return rclcpp::get_logger(logger_name);
 }

-void
-Logger::set_level(Level level)
-{
-  rcutils_ret_t rcutils_ret = rcutils_logging_set_logger_level(
-    get_name(),
-    static_cast<RCUTILS_LOG_SEVERITY>(level));
-  if (rcutils_ret != RCUTILS_RET_OK) {
-    if (rcutils_ret == RCUTILS_RET_INVALID_ARGUMENT) {
-      exceptions::throw_from_rcl_error(
-        RCL_RET_INVALID_ARGUMENT, "Invalid parameter",
-        rcutils_get_error_state(), rcutils_reset_error);
-    }
-    exceptions::throw_from_rcl_error(
-      RCL_RET_ERROR, "Couldn't set logger level",
-      rcutils_get_error_state(), rcutils_reset_error);
-  }
-}
-
 }  // namespace rclcpp
--- a/rclcpp/src/rclcpp/memory_strategy.cpp
+++ b/rclcpp/src/rclcpp/memory_strategy.cpp
@@ -20,19 +20,25 @@ using rclcpp::memory_strategy::MemoryStrategy;
 rclcpp::SubscriptionBase::SharedPtr
 MemoryStrategy::get_subscription_by_handle(
  std::shared_ptr<const rcl_subscription_t> subscriber_handle,
-  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
+  const WeakNodeList & weak_nodes)
 {
-  for (const auto & pair : weak_groups_to_nodes) {
-    auto group = pair.first.lock();
-    if (!group) {
+  for (auto & weak_node : weak_nodes) {
+    auto node = weak_node.lock();
+    if (!node) {
      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;
+    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;
+      }
    }
  }
  return nullptr;
@@ -41,19 +47,25 @@ MemoryStrategy::get_subscription_by_handle(
 rclcpp::ServiceBase::SharedPtr
 MemoryStrategy::get_service_by_handle(
  std::shared_ptr<const rcl_service_t> service_handle,
-  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
+  const WeakNodeList & weak_nodes)
 {
-  for (const auto & pair : weak_groups_to_nodes) {
-    auto group = pair.first.lock();
-    if (!group) {
+  for (auto & weak_node : weak_nodes) {
+    auto node = weak_node.lock();
+    if (!node) {
      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;
+    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;
+      }
    }
  }
  return nullptr;
@@ -62,19 +74,25 @@ MemoryStrategy::get_service_by_handle(
 rclcpp::ClientBase::SharedPtr
 MemoryStrategy::get_client_by_handle(
  std::shared_ptr<const rcl_client_t> client_handle,
-  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
+  const WeakNodeList & weak_nodes)
 {
-  for (const auto & pair : weak_groups_to_nodes) {
-    auto group = pair.first.lock();
-    if (!group) {
+  for (auto & weak_node : weak_nodes) {
+    auto node = weak_node.lock();
+    if (!node) {
      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;
+    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;
+      }
    }
  }
  return nullptr;
@@ -83,19 +101,25 @@ MemoryStrategy::get_client_by_handle(
 rclcpp::TimerBase::SharedPtr
 MemoryStrategy::get_timer_by_handle(
  std::shared_ptr<const rcl_timer_t> timer_handle,
-  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
+  const WeakNodeList & weak_nodes)
 {
-  for (const auto & pair : weak_groups_to_nodes) {
-    auto group = pair.first.lock();
-    if (!group) {
+  for (auto & weak_node : weak_nodes) {
+    auto node = weak_node.lock();
+    if (!node) {
      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;
+    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;
+      }
    }
  }
  return nullptr;
@@ -104,17 +128,22 @@ MemoryStrategy::get_timer_by_handle(
 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
 MemoryStrategy::get_node_by_group(
  rclcpp::CallbackGroup::SharedPtr group,
-  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
+  const WeakNodeList & weak_nodes)
 {
  if (!group) {
    return nullptr;
  }
-
-  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;
+  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;
+      }
+    }
  }
  return nullptr;
 }
@@ -122,20 +151,25 @@ MemoryStrategy::get_node_by_group(
 rclcpp::CallbackGroup::SharedPtr
 MemoryStrategy::get_group_by_subscription(
  rclcpp::SubscriptionBase::SharedPtr subscription,
-  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
+  const WeakNodeList & weak_nodes)
 {
-  for (const auto & pair : weak_groups_to_nodes) {
-    auto group = pair.first.lock();
-    auto node = pair.second.lock();
-    if (!group || !node) {
+  for (auto & weak_node : weak_nodes) {
+    auto node = weak_node.lock();
+    if (!node) {
      continue;
    }
-    auto match_sub = group->find_subscription_ptrs_if(
-      [&subscription](const rclcpp::SubscriptionBase::SharedPtr & sub) -> bool {
-        return sub == subscription;
-      });
-    if (match_sub) {
-      return group;
+    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;
+      }
    }
  }
  return nullptr;
@@ -144,20 +178,25 @@ MemoryStrategy::get_group_by_subscription(
 rclcpp::CallbackGroup::SharedPtr
 MemoryStrategy::get_group_by_service(
  rclcpp::ServiceBase::SharedPtr service,
-  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
+  const WeakNodeList & weak_nodes)
 {
-  for (const auto & pair : weak_groups_to_nodes) {
-    auto group = pair.first.lock();
-    auto node = pair.second.lock();
-    if (!group || !node) {
+  for (auto & weak_node : weak_nodes) {
+    auto node = weak_node.lock();
+    if (!node) {
      continue;
    }
-    auto service_ref = group->find_service_ptrs_if(
-      [&service](const rclcpp::ServiceBase::SharedPtr & serv) -> bool {
-        return serv == service;
-      });
-    if (service_ref) {
-      return group;
+    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;
+      }
    }
  }
  return nullptr;
@@ -166,20 +205,25 @@ MemoryStrategy::get_group_by_service(
 rclcpp::CallbackGroup::SharedPtr
 MemoryStrategy::get_group_by_client(
  rclcpp::ClientBase::SharedPtr client,
-  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
+  const WeakNodeList & weak_nodes)
 {
-  for (const auto & pair : weak_groups_to_nodes) {
-    auto group = pair.first.lock();
-    auto node = pair.second.lock();
-    if (!group || !node) {
+  for (auto & weak_node : weak_nodes) {
+    auto node = weak_node.lock();
+    if (!node) {
      continue;
    }
-    auto client_ref = group->find_client_ptrs_if(
-      [&client](const rclcpp::ClientBase::SharedPtr & cli) -> bool {
-        return cli == client;
-      });
-    if (client_ref) {
-      return group;
+    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;
+      }
    }
  }
  return nullptr;
@@ -188,20 +232,25 @@ MemoryStrategy::get_group_by_client(
 rclcpp::CallbackGroup::SharedPtr
 MemoryStrategy::get_group_by_timer(
  rclcpp::TimerBase::SharedPtr timer,
-  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
+  const WeakNodeList & weak_nodes)
 {
-  for (const auto & pair : weak_groups_to_nodes) {
-    auto group = pair.first.lock();
-    auto node = pair.second.lock();
-    if (!group || !node) {
+  for (auto & weak_node : weak_nodes) {
+    auto node = weak_node.lock();
+    if (!node) {
      continue;
    }
-    auto timer_ref = group->find_timer_ptrs_if(
-      [&timer](const rclcpp::TimerBase::SharedPtr & time) -> bool {
-        return time == timer;
-      });
-    if (timer_ref) {
-      return group;
+    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;
+      }
    }
  }
  return nullptr;
@@ -210,20 +259,25 @@ MemoryStrategy::get_group_by_timer(
 rclcpp::CallbackGroup::SharedPtr
 MemoryStrategy::get_group_by_waitable(
  rclcpp::Waitable::SharedPtr waitable,
-  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
+  const WeakNodeList & weak_nodes)
 {
-  for (const auto & pair : weak_groups_to_nodes) {
-    auto group = pair.first.lock();
-    auto node = pair.second.lock();
-    if (!group || !node) {
+  for (auto & weak_node : weak_nodes) {
+    auto node = weak_node.lock();
+    if (!node) {
      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;
+    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;
+      }
    }
  }
  return nullptr;
--- a/rclcpp/src/rclcpp/node.cpp
+++ b/rclcpp/src/rclcpp/node.cpp
@@ -138,8 +138,7 @@ Node::Node(
      node_services_,
      node_logging_,
      node_clock_,
-      node_parameters_,
-      options.clock_qos()
+      node_parameters_
    )),
  node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())),
  node_options_(options),
@@ -212,11 +211,15 @@ Node::get_logger() const
 }

 rclcpp::CallbackGroup::SharedPtr
-Node::create_callback_group(
-  rclcpp::CallbackGroupType group_type,
-  bool automatically_add_to_executor_with_node)
+Node::create_callback_group(rclcpp::CallbackGroupType group_type)
 {
-  return node_base_->create_callback_group(group_type, 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);
 }

 const rclcpp::ParameterValue &
@@ -325,6 +328,28 @@ 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
 {
--- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp
+++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp
@@ -216,16 +216,11 @@ NodeBase::get_shared_rcl_node_handle() const
 }

 rclcpp::CallbackGroup::SharedPtr
-NodeBase::create_callback_group(
-  rclcpp::CallbackGroupType group_type,
-  bool automatically_add_to_executor_with_node)
+NodeBase::create_callback_group(rclcpp::CallbackGroupType group_type)
 {
  using rclcpp::CallbackGroup;
  using rclcpp::CallbackGroupType;
-  auto group = CallbackGroup::SharedPtr(
-    new CallbackGroup(
-      group_type,
-      automatically_add_to_executor_with_node));
+  auto group = CallbackGroup::SharedPtr(new CallbackGroup(group_type));
  callback_groups_.push_back(group);
  return group;
 }
@@ -288,24 +283,3 @@ 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;
-}
--- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp
+++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp
@@ -414,7 +414,7 @@ NodeGraph::wait_for_graph_change(
 }

 size_t
-NodeGraph::count_graph_users() const
+NodeGraph::count_graph_users()
 {
  return graph_users_count_.load();
 }
--- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp
+++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp
@@ -159,7 +159,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, double ulp = 100.0)
+__are_doubles_equal(double x, double y, size_t ulp = 100)
 {
  return std::abs(x - y) <= std::numeric_limits<double>::epsilon() * std::abs(x + y) * ulp;
 }
@@ -874,6 +874,18 @@ 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
 {
--- a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp
+++ b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp
@@ -78,9 +78,3 @@ 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);
-}
--- a/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp
+++ b/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp
@@ -26,16 +26,14 @@ 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,
-  const rclcpp::QoS & qos)
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters)
 : 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),
-  time_source_(qos)
+  node_parameters_(node_parameters)
 {
  time_source_.attachNode(
    node_base_,
--- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp
+++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp
@@ -129,9 +129,3 @@ 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);
-}
--- a/rclcpp/src/rclcpp/node_options.cpp
+++ b/rclcpp/src/rclcpp/node_options.cpp
@@ -67,7 +67,6 @@ NodeOptions &
 NodeOptions::operator=(const NodeOptions & other)
 {
  if (this != &other) {
-    this->node_options_.reset();
    this->context_ = other.context_;
    this->arguments_ = other.arguments_;
    this->parameter_overrides_ = other.parameter_overrides_;
@@ -76,15 +75,10 @@ NodeOptions::operator=(const NodeOptions & other)
    this->use_intra_process_comms_ = other.use_intra_process_comms_;
    this->enable_topic_statistics_ = other.enable_topic_statistics_;
    this->start_parameter_services_ = other.start_parameter_services_;
-    this->start_parameter_event_publisher_ = other.start_parameter_event_publisher_;
-    this->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->allocator_ = other.allocator_;
    this->allow_undeclared_parameters_ = other.allow_undeclared_parameters_;
    this->automatically_declare_parameters_from_overrides_ =
      other.automatically_declare_parameters_from_overrides_;
-    this->allocator_ = other.allocator_;
  }
  return *this;
 }
@@ -98,8 +92,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;
@@ -182,7 +176,7 @@ NodeOptions::parameter_overrides(const std::vector<rclcpp::Parameter> & paramete
 bool
 NodeOptions::use_global_arguments() const
 {
-  return this->use_global_arguments_;
+  return this->node_options_->use_global_arguments;
 }

 NodeOptions &
@@ -259,19 +253,6 @@ 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
 {
@@ -285,20 +266,6 @@ 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
 {
@@ -355,4 +322,36 @@ 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
--- a/rclcpp/src/rclcpp/parameter_client.cpp
+++ b/rclcpp/src/rclcpp/parameter_client.cpp
@@ -15,12 +15,7 @@
 #include "rclcpp/parameter_client.hpp"

 #include <algorithm>
-#include <chrono>
-#include <functional>
-#include <future>
-#include <iterator>
 #include <memory>
-#include <stdexcept>
 #include <string>
 #include <vector>

@@ -104,6 +99,36 @@ 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,
@@ -312,6 +337,78 @@ 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)
 {
--- a/rclcpp/src/rclcpp/parameter_service.cpp
+++ b/rclcpp/src/rclcpp/parameter_service.cpp
@@ -119,7 +119,7 @@ ParameterService::ParameterService(
        RCLCPP_WARN(
          rclcpp::get_logger("rclcpp"), "Failed to set parameters atomically: %s", ex.what());
        response->result.successful = false;
-        response->result.reason = "One or more parameters were not declared before setting";
+        response->result.reason = "One or more parameters wer not declared before setting";
      }
    },
    qos_profile, nullptr);
--- a/rclcpp/src/rclcpp/publisher_base.cpp
+++ b/rclcpp/src/rclcpp/publisher_base.cpp
@@ -262,9 +262,7 @@ 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 topic '%s', requesting incompatible QoS. "
+    "New subscription discovered on this topic, requesting incompatible QoS. "
    "No messages will be sent to it. "
-    "Last incompatible policy: %s",
-    get_topic_name(),
-    policy_name.c_str());
+    "Last incompatible policy: %s", policy_name.c_str());
 }
--- a/rclcpp/src/rclcpp/qos.cpp
+++ b/rclcpp/src/rclcpp/qos.cpp
@@ -235,12 +235,6 @@ 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)
 {}
@@ -257,10 +251,6 @@ 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)
 {}
--- a/rclcpp/src/rclcpp/subscription_base.cpp
+++ b/rclcpp/src/rclcpp/subscription_base.cpp
@@ -245,11 +245,9 @@ 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 topic '%s', offering incompatible QoS. "
+    "New publisher discovered on this topic, offering incompatible QoS. "
    "No messages will be sent to it. "
-    "Last incompatible policy: %s",
-    get_topic_name(),
-    policy_name.c_str());
+    "Last incompatible policy: %s", policy_name.c_str());
 }

 bool
--- a/rclcpp/src/rclcpp/time.cpp
+++ b/rclcpp/src/rclcpp/time.cpp
@@ -63,13 +63,17 @@ Time::Time(int64_t nanoseconds, rcl_clock_type_t clock_type)
  rcl_time_.nanoseconds = nanoseconds;
 }

-Time::Time(const Time & rhs) = default;
+Time::Time(const Time & rhs)
+: rcl_time_(rhs.rcl_time_)
+{
+  rcl_time_.nanoseconds = rhs.rcl_time_.nanoseconds;
+}

 Time::Time(
  const builtin_interfaces::msg::Time & time_msg,
-  rcl_clock_type_t clock_type)
-: rcl_time_(init_time_point(clock_type))
+  rcl_clock_type_t ros_time)
 {
+  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");
  }
@@ -91,25 +95,31 @@ Time::~Time()
 Time::operator builtin_interfaces::msg::Time() const
 {
  builtin_interfaces::msg::Time msg_time;
-  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);
-  }
+  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));
  return msg_time;
 }

 Time &
-Time::operator=(const Time & rhs) = default;
+Time::operator=(const Time & rhs)
+{
+  rcl_time_ = rhs.rcl_time_;
+  return *this;
+}

 Time &
 Time::operator=(const builtin_interfaces::msg::Time & time_msg)
 {
-  *this = 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;
  return *this;
 }

@@ -199,7 +209,7 @@ Time::operator-(const rclcpp::Time & rhs) const
    throw std::underflow_error("time subtraction leads to int64_t underflow");
  }

-  return Duration::from_nanoseconds(rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds);
+  return Duration(rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds);
 }

 Time
--- a/rclcpp/src/rclcpp/time_source.cpp
+++ b/rclcpp/src/rclcpp/time_source.cpp
@@ -33,16 +33,17 @@
 namespace rclcpp
 {

-TimeSource::TimeSource(std::shared_ptr<rclcpp::Node> node, const rclcpp::QoS & qos)
+TimeSource::TimeSource(std::shared_ptr<rclcpp::Node> node)
 : logger_(rclcpp::get_logger("rclcpp")),
-  qos_(qos)
+  clock_subscription_(nullptr),
+  ros_time_active_(false)
 {
  this->attachNode(node);
 }

-TimeSource::TimeSource(const rclcpp::QoS & qos)
+TimeSource::TimeSource()
 : logger_(rclcpp::get_logger("rclcpp")),
-  qos_(qos)
+  ros_time_active_(false)
 {
 }

@@ -184,21 +185,11 @@ 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()) {
-    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");
-    }
+    disable_ros_time(clock);
  } else if (set_ros_time_enabled && !clock->ros_time_is_active()) {
-    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");
-    }
+    enable_ros_time(clock);
  }

  auto ret = rcl_set_ros_time_override(clock->get_clock_handle(), rclcpp::Time(*msg).nanoseconds());
@@ -236,7 +227,7 @@ void TimeSource::create_clock_sub()
  clock_subscription_ = rclcpp::create_subscription<rosgraph_msgs::msg::Clock>(
    node_topics_,
    "/clock",
-    rclcpp::QoS(KeepLast(1)).best_effort(),
+    rclcpp::QoS(QoSInitialization::from_rmw(rmw_qos_profile_default)),
    std::bind(&TimeSource::clock_cb, this, std::placeholders::_1)
  );
 }
@@ -282,6 +273,24 @@ 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_) {
--- a/rclcpp/src/rclcpp/timer.cpp
+++ b/rclcpp/src/rclcpp/timer.cpp
@@ -61,11 +61,15 @@ TimerBase::TimerBase(
  rcl_clock_t * clock_handle = clock_->get_clock_handle();
  {
    std::lock_guard<std::mutex> clock_guard(clock_->get_clock_mutex());
-    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");
+    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();
    }
  }
 }
@@ -76,9 +80,8 @@ TimerBase::~TimerBase()
 void
 TimerBase::cancel()
 {
-  rcl_ret_t ret = rcl_timer_cancel(timer_handle_.get());
-  if (ret != RCL_RET_OK) {
-    rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't cancel timer");
+  if (rcl_timer_cancel(timer_handle_.get()) != RCL_RET_OK) {
+    throw std::runtime_error(std::string("Couldn't cancel timer: ") + rcl_get_error_string().str);
  }
 }

@@ -96,9 +99,8 @@ TimerBase::is_canceled()
 void
 TimerBase::reset()
 {
-  rcl_ret_t ret = rcl_timer_reset(timer_handle_.get());
-  if (ret != RCL_RET_OK) {
-    rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't reset timer");
+  if (rcl_timer_reset(timer_handle_.get()) != RCL_RET_OK) {
+    throw std::runtime_error(std::string("Couldn't reset timer: ") + rcl_get_error_string().str);
  }
 }

@@ -106,9 +108,8 @@ bool
 TimerBase::is_ready()
 {
  bool ready = false;
-  rcl_ret_t ret = rcl_timer_is_ready(timer_handle_.get(), &ready);
-  if (ret != RCL_RET_OK) {
-    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to check timer");
+  if (rcl_timer_is_ready(timer_handle_.get(), &ready) != RCL_RET_OK) {
+    throw std::runtime_error(std::string("Failed to check timer: ") + rcl_get_error_string().str);
  }
  return ready;
 }
@@ -117,10 +118,14 @@ std::chrono::nanoseconds
 TimerBase::time_until_trigger()
 {
  int64_t time_until_next_call = 0;
-  rcl_ret_t ret = rcl_timer_get_time_until_next_call(
-    timer_handle_.get(), &time_until_next_call);
-  if (ret != RCL_RET_OK) {
-    rclcpp::exceptions::throw_from_rcl_error(ret, "Timer could not get time until next call");
+  if (
+    rcl_timer_get_time_until_next_call(
+      timer_handle_.get(),
+      &time_until_next_call) != RCL_RET_OK)
+  {
+    throw std::runtime_error(
+            std::string(
+              "Timer could not get time until next call: ") + rcl_get_error_string().str);
  }
  return std::chrono::nanoseconds(time_until_next_call);
 }
--- a/rclcpp/src/rclcpp/utilities.cpp
+++ b/rclcpp/src/rclcpp/utilities.cpp
@@ -14,14 +14,11 @@

 #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"
@@ -57,47 +54,6 @@ 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,
@@ -105,10 +61,7 @@ init_and_remove_ros_arguments(
  const InitOptions & init_options)
 {
  init(argc, argv, init_options);
-
-  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());
+  return remove_ros_arguments(argc, argv);
 }

 std::vector<std::string>
@@ -124,17 +77,40 @@ remove_ros_arguments(int argc, char const * const argv[])
    exceptions::throw_from_rcl_error(ret, "failed to parse arguments");
  }

-  std::vector<std::string> return_arguments;
-  try {
-    return_arguments = _remove_ros_arguments(argv, &parsed_args, alloc);
-  } catch (exceptions::RCLError & exc) {
+  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);
+    }
    if (RCL_RET_OK != rcl_arguments_fini(&parsed_args)) {
-      exc.formatted_message += std::string(
+      base_exc.formatted_message += std::string(
        ", failed also to cleanup parsed arguments, leaking memory: ") +
        rcl_get_error_string().str;
      rcl_reset_error();
    }
-    throw exc;
+    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);
  }

  ret = rcl_arguments_fini(&parsed_args);
--- a/rclcpp/test/CMakeLists.txt
+++ b/rclcpp/test/CMakeLists.txt
@@ -1,18 +0,0 @@
-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})
--- a/rclcpp/test/benchmark/CMakeLists.txt
+++ b/rclcpp/test/benchmark/CMakeLists.txt
@@ -1,38 +0,0 @@
-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()
-
-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()
--- a/rclcpp/test/benchmark/benchmark_client.cpp
+++ b/rclcpp/test/benchmark/benchmark_client.cpp
@@ -1,153 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <memory>
-#include <string>
-
-#include "performance_test_fixture/performance_test_fixture.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "test_msgs/srv/empty.hpp"
-
-using performance_test_fixture::PerformanceTest;
-
-constexpr char empty_service_name[] = "empty_service";
-
-class ClientPerformanceTest : public PerformanceTest
-{
-public:
-  explicit ClientPerformanceTest(rclcpp::NodeOptions = rclcpp::NodeOptions()) {}
-  void SetUp(benchmark::State & state)
-  {
-    rclcpp::init(0, nullptr);
-    node = std::make_unique<rclcpp::Node>("node", "ns");
-
-    auto empty_service_callback =
-      [](
-      const test_msgs::srv::Empty::Request::SharedPtr,
-      test_msgs::srv::Empty::Response::SharedPtr) {};
-    empty_service =
-      node->create_service<test_msgs::srv::Empty>(empty_service_name, empty_service_callback);
-
-    performance_test_fixture::PerformanceTest::SetUp(state);
-  }
-
-  void TearDown(benchmark::State & state)
-  {
-    performance_test_fixture::PerformanceTest::TearDown(state);
-    empty_service.reset();
-    node.reset();
-    rclcpp::shutdown();
-  }
-
-protected:
-  std::unique_ptr<rclcpp::Node> node;
-  std::shared_ptr<rclcpp::Service<test_msgs::srv::Empty>> empty_service;
-};
-
-BENCHMARK_F(ClientPerformanceTest, construct_client_no_service)(benchmark::State & state) {
-  // Prime cache
-  auto outer_client = node->create_client<test_msgs::srv::Empty>("not_an_existing_service");
-  outer_client.reset();
-
-  reset_heap_counters();
-  for (auto _ : state) {
-    auto client = node->create_client<test_msgs::srv::Empty>("not_an_existing_service");
-    benchmark::DoNotOptimize(client);
-    benchmark::ClobberMemory();
-
-    state.PauseTiming();
-    client.reset();
-    state.ResumeTiming();
-  }
-}
-
-BENCHMARK_F(ClientPerformanceTest, construct_client_empty_srv)(benchmark::State & state) {
-  // Prime cache
-  auto outer_client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
-  outer_client.reset();
-
-  reset_heap_counters();
-  for (auto _ : state) {
-    auto client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
-    benchmark::DoNotOptimize(client);
-    benchmark::ClobberMemory();
-
-    state.PauseTiming();
-    client.reset();
-    state.ResumeTiming();
-  }
-}
-
-BENCHMARK_F(ClientPerformanceTest, destroy_client_empty_srv)(benchmark::State & state) {
-  // Prime cache
-  auto outer_client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
-  outer_client.reset();
-
-  reset_heap_counters();
-  for (auto _ : state) {
-    state.PauseTiming();
-    auto client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
-    state.ResumeTiming();
-    benchmark::DoNotOptimize(client);
-    benchmark::ClobberMemory();
-
-    client.reset();
-  }
-}
-
-BENCHMARK_F(ClientPerformanceTest, wait_for_service)(benchmark::State & state) {
-  int count = 0;
-  for (auto _ : state) {
-    state.PauseTiming();
-    const std::string service_name = std::string("service_") + std::to_string(count++);
-    // Create client before service so it has to 'discover' the service after construction
-    auto client = node->create_client<test_msgs::srv::Empty>(service_name);
-    auto callback =
-      [](
-      const test_msgs::srv::Empty::Request::SharedPtr,
-      test_msgs::srv::Empty::Response::SharedPtr) {};
-    auto service =
-      node->create_service<test_msgs::srv::Empty>(service_name, callback);
-    state.ResumeTiming();
-
-    client->wait_for_service(std::chrono::seconds(1));
-    benchmark::ClobberMemory();
-  }
-}
-
-BENCHMARK_F(ClientPerformanceTest, async_send_request_only)(benchmark::State & state) {
-  auto client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
-  auto shared_request = std::make_shared<test_msgs::srv::Empty::Request>();
-
-  reset_heap_counters();
-  for (auto _ : state) {
-    auto future = client->async_send_request(shared_request);
-    benchmark::DoNotOptimize(future);
-    benchmark::ClobberMemory();
-  }
-}
-
-BENCHMARK_F(ClientPerformanceTest, async_send_request_and_response)(benchmark::State & state) {
-  auto client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
-  auto shared_request = std::make_shared<test_msgs::srv::Empty::Request>();
-
-  reset_heap_counters();
-  for (auto _ : state) {
-    auto future = client->async_send_request(shared_request);
-    rclcpp::spin_until_future_complete(
-      node->get_node_base_interface(), future, std::chrono::seconds(1));
-    benchmark::DoNotOptimize(future);
-    benchmark::ClobberMemory();
-  }
-}
--- a/rclcpp/test/benchmark/benchmark_executor.cpp
+++ b/rclcpp/test/benchmark/benchmark_executor.cpp
@@ -1,389 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <memory>
-#include <string>
-#include <utility>
-#include <vector>
-
-#include "performance_test_fixture/performance_test_fixture.hpp"
-
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp/scope_exit.hpp"
-#include "test_msgs/msg/empty.hpp"
-
-using namespace std::chrono_literals;
-using performance_test_fixture::PerformanceTest;
-
-constexpr unsigned int kNumberOfNodes = 10;
-
-class PerformanceTestExecutor : public PerformanceTest
-{
-public:
-  void SetUp(benchmark::State & st)
-  {
-    rclcpp::init(0, nullptr);
-    callback_count = 0;
-    for (unsigned int i = 0u; i < kNumberOfNodes; i++) {
-      nodes.push_back(std::make_shared<rclcpp::Node>("my_node_" + std::to_string(i)));
-
-      publishers.push_back(
-        nodes[i]->create_publisher<test_msgs::msg::Empty>(
-          "/empty_msgs_" + std::to_string(i), rclcpp::QoS(10)));
-
-      auto callback = [this](test_msgs::msg::Empty::SharedPtr) {this->callback_count++;};
-      subscriptions.push_back(
-        nodes[i]->create_subscription<test_msgs::msg::Empty>(
-          "/empty_msgs_" + std::to_string(i), rclcpp::QoS(10), std::move(callback)));
-    }
-    PerformanceTest::SetUp(st);
-  }
-  void TearDown(benchmark::State & st)
-  {
-    PerformanceTest::TearDown(st);
-    subscriptions.clear();
-    publishers.clear();
-    nodes.clear();
-    rclcpp::shutdown();
-  }
-
-  test_msgs::msg::Empty empty_msgs;
-  std::vector<rclcpp::Node::SharedPtr> nodes;
-  std::vector<rclcpp::Publisher<test_msgs::msg::Empty>::SharedPtr> publishers;
-  std::vector<rclcpp::Subscription<test_msgs::msg::Empty>::SharedPtr> subscriptions;
-  int callback_count;
-};
-
-BENCHMARK_F(PerformanceTestExecutor, single_thread_executor_spin_some)(benchmark::State & st)
-{
-  rclcpp::executors::SingleThreadedExecutor executor;
-  for (unsigned int i = 0u; i < kNumberOfNodes; i++) {
-    executor.add_node(nodes[i]);
-    publishers[i]->publish(empty_msgs);
-    executor.spin_some(100ms);
-  }
-
-  callback_count = 0;
-  reset_heap_counters();
-
-  for (auto _ : st) {
-    st.PauseTiming();
-    for (unsigned int i = 0u; i < kNumberOfNodes; i++) {
-      publishers[i]->publish(empty_msgs);
-    }
-    st.ResumeTiming();
-
-    executor.spin_some(100ms);
-  }
-  if (callback_count == 0) {
-    st.SkipWithError("No message was received");
-  }
-}
-
-BENCHMARK_F(PerformanceTestExecutor, multi_thread_executor_spin_some)(benchmark::State & st)
-{
-  rclcpp::executors::MultiThreadedExecutor executor;
-  for (unsigned int i = 0u; i < kNumberOfNodes; i++) {
-    executor.add_node(nodes[i]);
-    publishers[i]->publish(empty_msgs);
-    executor.spin_some(100ms);
-  }
-
-  callback_count = 0;
-  reset_heap_counters();
-
-  for (auto _ : st) {
-    st.PauseTiming();
-    for (unsigned int i = 0u; i < kNumberOfNodes; i++) {
-      publishers[i]->publish(empty_msgs);
-    }
-    st.ResumeTiming();
-
-    executor.spin_some(100ms);
-  }
-  if (callback_count == 0) {
-    st.SkipWithError("No message was received");
-  }
-}
-
-class PerformanceTestExecutorSimple : public PerformanceTest
-{
-public:
-  void SetUp(benchmark::State & st)
-  {
-    rclcpp::init(0, nullptr);
-    node = std::make_shared<rclcpp::Node>("my_node");
-
-    PerformanceTest::SetUp(st);
-  }
-  void TearDown(benchmark::State & st)
-  {
-    PerformanceTest::TearDown(st);
-    node.reset();
-    rclcpp::shutdown();
-  }
-
-  rclcpp::Node::SharedPtr node;
-};
-
-
-BENCHMARK_F(PerformanceTestExecutorSimple, single_thread_executor_add_node)(benchmark::State & st)
-{
-  rclcpp::executors::SingleThreadedExecutor executor;
-  for (auto _ : st) {
-    executor.add_node(node);
-    st.PauseTiming();
-    executor.remove_node(node);
-    st.ResumeTiming();
-  }
-}
-
-BENCHMARK_F(
-  PerformanceTestExecutorSimple, single_thread_executor_remove_node)(benchmark::State & st)
-{
-  rclcpp::executors::SingleThreadedExecutor executor;
-  for (auto _ : st) {
-    st.PauseTiming();
-    executor.add_node(node);
-    st.ResumeTiming();
-    executor.remove_node(node);
-  }
-}
-
-BENCHMARK_F(PerformanceTestExecutorSimple, multi_thread_executor_add_node)(benchmark::State & st)
-{
-  rclcpp::executors::MultiThreadedExecutor executor;
-  for (auto _ : st) {
-    executor.add_node(node);
-    st.PauseTiming();
-    executor.remove_node(node);
-    st.ResumeTiming();
-  }
-}
-
-BENCHMARK_F(PerformanceTestExecutorSimple, multi_thread_executor_remove_node)(benchmark::State & st)
-{
-  rclcpp::executors::MultiThreadedExecutor executor;
-  for (auto _ : st) {
-    st.PauseTiming();
-    executor.add_node(node);
-    st.ResumeTiming();
-    executor.remove_node(node);
-  }
-}
-
-BENCHMARK_F(
-  PerformanceTestExecutorSimple,
-  static_single_thread_executor_add_node)(benchmark::State & st)
-{
-  rclcpp::executors::StaticSingleThreadedExecutor executor;
-  for (auto _ : st) {
-    executor.add_node(node);
-    st.PauseTiming();
-    executor.remove_node(node);
-    st.ResumeTiming();
-  }
-}
-
-BENCHMARK_F(
-  PerformanceTestExecutorSimple,
-  static_single_thread_executor_remove_node)(benchmark::State & st)
-{
-  rclcpp::executors::StaticSingleThreadedExecutor executor;
-  for (auto _ : st) {
-    st.PauseTiming();
-    executor.add_node(node);
-    st.ResumeTiming();
-    executor.remove_node(node);
-  }
-}
-
-BENCHMARK_F(
-  PerformanceTestExecutorSimple,
-  static_single_thread_executor_spin_until_future_complete)(benchmark::State & st)
-{
-  rclcpp::executors::StaticSingleThreadedExecutor executor;
-  // test success of an immediately finishing future
-  std::promise<bool> promise;
-  std::future<bool> future = promise.get_future();
-  promise.set_value(true);
-  auto shared_future = future.share();
-
-  auto ret = executor.spin_until_future_complete(shared_future, 100ms);
-  if (ret != rclcpp::FutureReturnCode::SUCCESS) {
-    st.SkipWithError(rcutils_get_error_string().str);
-  }
-
-  reset_heap_counters();
-
-  for (auto _ : st) {
-    // static_single_thread_executor has a special design. We need to add/remove the node each
-    // time you call spin
-    st.PauseTiming();
-    executor.add_node(node);
-    st.ResumeTiming();
-
-    ret = executor.spin_until_future_complete(shared_future, 100ms);
-    if (ret != rclcpp::FutureReturnCode::SUCCESS) {
-      st.SkipWithError(rcutils_get_error_string().str);
-      break;
-    }
-    st.PauseTiming();
-    executor.remove_node(node);
-    st.ResumeTiming();
-  }
-}
-
-BENCHMARK_F(
-  PerformanceTestExecutorSimple,
-  single_thread_executor_spin_node_until_future_complete)(benchmark::State & st)
-{
-  rclcpp::executors::SingleThreadedExecutor executor;
-  // test success of an immediately finishing future
-  std::promise<bool> promise;
-  std::future<bool> future = promise.get_future();
-  promise.set_value(true);
-  auto shared_future = future.share();
-
-  auto ret = rclcpp::executors::spin_node_until_future_complete(
-    executor, node, shared_future, 1s);
-  if (ret != rclcpp::FutureReturnCode::SUCCESS) {
-    st.SkipWithError(rcutils_get_error_string().str);
-  }
-
-  reset_heap_counters();
-
-  for (auto _ : st) {
-    ret = rclcpp::executors::spin_node_until_future_complete(
-      executor, node, shared_future, 1s);
-    if (ret != rclcpp::FutureReturnCode::SUCCESS) {
-      st.SkipWithError(rcutils_get_error_string().str);
-      break;
-    }
-  }
-}
-
-BENCHMARK_F(
-  PerformanceTestExecutorSimple,
-  multi_thread_executor_spin_node_until_future_complete)(benchmark::State & st)
-{
-  rclcpp::executors::MultiThreadedExecutor executor;
-  // test success of an immediately finishing future
-  std::promise<bool> promise;
-  std::future<bool> future = promise.get_future();
-  promise.set_value(true);
-  auto shared_future = future.share();
-
-  auto ret = rclcpp::executors::spin_node_until_future_complete(
-    executor, node, shared_future, 1s);
-  if (ret != rclcpp::FutureReturnCode::SUCCESS) {
-    st.SkipWithError(rcutils_get_error_string().str);
-  }
-
-  reset_heap_counters();
-
-  for (auto _ : st) {
-    ret = rclcpp::executors::spin_node_until_future_complete(
-      executor, node, shared_future, 1s);
-    if (ret != rclcpp::FutureReturnCode::SUCCESS) {
-      st.SkipWithError(rcutils_get_error_string().str);
-      break;
-    }
-  }
-}
-
-BENCHMARK_F(
-  PerformanceTestExecutorSimple,
-  static_single_thread_executor_spin_node_until_future_complete)(benchmark::State & st)
-{
-  rclcpp::executors::StaticSingleThreadedExecutor executor;
-  // test success of an immediately finishing future
-  std::promise<bool> promise;
-  std::future<bool> future = promise.get_future();
-  promise.set_value(true);
-  auto shared_future = future.share();
-
-  reset_heap_counters();
-
-  for (auto _ : st) {
-    auto ret = rclcpp::executors::spin_node_until_future_complete(
-      executor, node, shared_future, 1s);
-    if (ret != rclcpp::FutureReturnCode::SUCCESS) {
-      st.SkipWithError(rcutils_get_error_string().str);
-      break;
-    }
-  }
-}
-
-BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_future_complete)(benchmark::State & st)
-{
-  // test success of an immediately finishing future
-  std::promise<bool> promise;
-  std::future<bool> future = promise.get_future();
-  promise.set_value(true);
-  auto shared_future = future.share();
-
-  auto ret = rclcpp::spin_until_future_complete(node, shared_future, 1s);
-  if (ret != rclcpp::FutureReturnCode::SUCCESS) {
-    st.SkipWithError(rcutils_get_error_string().str);
-  }
-
-  reset_heap_counters();
-
-  for (auto _ : st) {
-    ret = rclcpp::spin_until_future_complete(node, shared_future, 1s);
-    if (ret != rclcpp::FutureReturnCode::SUCCESS) {
-      st.SkipWithError(rcutils_get_error_string().str);
-      break;
-    }
-  }
-}
-
-BENCHMARK_F(
-  PerformanceTestExecutorSimple,
-  static_executor_entities_collector_execute)(benchmark::State & st)
-{
-  rclcpp::executors::StaticExecutorEntitiesCollector::SharedPtr entities_collector_ =
-    std::make_shared<rclcpp::executors::StaticExecutorEntitiesCollector>();
-  entities_collector_->add_node(node->get_node_base_interface());
-
-  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  rcl_allocator_t allocator = rcl_get_default_allocator();
-  auto shared_context = node->get_node_base_interface()->get_context();
-  rcl_context_t * context = shared_context->get_rcl_context().get();
-  rcl_ret_t ret = rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator);
-  if (ret != RCL_RET_OK) {
-    st.SkipWithError(rcutils_get_error_string().str);
-  }
-  RCLCPP_SCOPE_EXIT(
-  {
-    rcl_ret_t ret = rcl_wait_set_fini(&wait_set);
-    if (ret != RCL_RET_OK) {
-      st.SkipWithError(rcutils_get_error_string().str);
-    }
-  });
-
-  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
-  rclcpp::GuardCondition guard_condition(shared_context);
-  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
-
-  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
-  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
-
-  reset_heap_counters();
-
-  for (auto _ : st) {
-    entities_collector_->execute();
-  }
-}
--- a/rclcpp/test/benchmark/benchmark_init_shutdown.cpp
+++ b/rclcpp/test/benchmark/benchmark_init_shutdown.cpp
@@ -1,53 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include "performance_test_fixture/performance_test_fixture.hpp"
-
-#include "rclcpp/rclcpp.hpp"
-
-using performance_test_fixture::PerformanceTest;
-
-BENCHMARK_F(PerformanceTest, rclcpp_init)(benchmark::State & state)
-{
-  // Warmup and prime caches
-  rclcpp::init(0, nullptr);
-  rclcpp::shutdown();
-
-  reset_heap_counters();
-  for (auto _ : state) {
-    rclcpp::init(0, nullptr);
-
-    state.PauseTiming();
-    rclcpp::shutdown();
-    state.ResumeTiming();
-    benchmark::ClobberMemory();
-  }
-}
-
-BENCHMARK_F(PerformanceTest, rclcpp_shutdown)(benchmark::State & state)
-{
-  // Warmup and prime caches
-  rclcpp::init(0, nullptr);
-  rclcpp::shutdown();
-
-  reset_heap_counters();
-  for (auto _ : state) {
-    state.PauseTiming();
-    rclcpp::init(0, nullptr);
-    state.ResumeTiming();
-
-    rclcpp::shutdown();
-    benchmark::ClobberMemory();
-  }
-}
--- a/rclcpp/test/benchmark/benchmark_node.cpp
+++ b/rclcpp/test/benchmark/benchmark_node.cpp
@@ -1,77 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <memory>
-#include <string>
-
-#include "performance_test_fixture/performance_test_fixture.hpp"
-#include "rclcpp/rclcpp.hpp"
-
-using performance_test_fixture::PerformanceTest;
-
-class NodePerformanceTest : public PerformanceTest
-{
-public:
-  void SetUp(benchmark::State & state)
-  {
-    rclcpp::init(0, nullptr);
-    performance_test_fixture::PerformanceTest::SetUp(state);
-  }
-
-  void TearDown(benchmark::State & state)
-  {
-    performance_test_fixture::PerformanceTest::TearDown(state);
-    rclcpp::shutdown();
-  }
-};
-
-BENCHMARK_F(NodePerformanceTest, create_node)(benchmark::State & state)
-{
-  // Warmup and prime caches
-  auto outer_node = std::make_shared<rclcpp::Node>("node");
-  outer_node.reset();
-
-  reset_heap_counters();
-  for (auto _ : state) {
-    // Using pointer to separate construction and destruction in timing
-    auto node = std::make_shared<rclcpp::Node>("node");
-    benchmark::DoNotOptimize(node);
-    benchmark::ClobberMemory();
-
-    // Ensure destruction of node is not counted toward timing
-    state.PauseTiming();
-    node.reset();
-    state.ResumeTiming();
-  }
-}
-
-BENCHMARK_F(NodePerformanceTest, destroy_node)(benchmark::State & state)
-{
-  // Warmup and prime caches
-  auto outer_node = std::make_shared<rclcpp::Node>("node");
-  outer_node.reset();
-
-  reset_heap_counters();
-  for (auto _ : state) {
-    // Using pointer to separate construction and destruction in timing
-    state.PauseTiming();
-    auto node = std::make_shared<rclcpp::Node>("node");
-    state.ResumeTiming();
-
-    benchmark::DoNotOptimize(node);
-    benchmark::ClobberMemory();
-
-    node.reset();
-  }
-}
--- a/rclcpp/test/benchmark/benchmark_node_parameters_interface.cpp
+++ b/rclcpp/test/benchmark/benchmark_node_parameters_interface.cpp
@@ -1,262 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <memory>
-#include <string>
-#include <vector>
-
-#include "performance_test_fixture/performance_test_fixture.hpp"
-
-#include "rclcpp/rclcpp.hpp"
-
-class NodeParametersInterfaceTest : public performance_test_fixture::PerformanceTest
-{
-public:
-  NodeParametersInterfaceTest()
-  : node_name("my_node"),
-    param_prefix("my_prefix"),
-    param1_name(param_prefix + ".my_param_1"),
-    param2_name(param_prefix + ".my_param_2"),
-    param3_name(param_prefix + ".my_param_3")
-  {
-  }
-
-  void SetUp(benchmark::State & state)
-  {
-    rclcpp::init(0, nullptr);
-    node = std::make_shared<rclcpp::Node>(node_name);
-
-    node->declare_parameter(param1_name);
-    node->declare_parameter(param2_name);
-    node->declare_parameter(param3_name);
-    node->undeclare_parameter(param3_name);
-
-    performance_test_fixture::PerformanceTest::SetUp(state);
-  }
-
-  void TearDown(benchmark::State & state)
-  {
-    performance_test_fixture::PerformanceTest::TearDown(state);
-
-    node.reset();
-    rclcpp::shutdown();
-  }
-
-  const std::string node_name;
-  const std::string param_prefix;
-  const std::string param1_name;
-  const std::string param2_name;
-  const std::string param3_name;
-
-protected:
-  rclcpp::Node::SharedPtr node;
-};
-
-BENCHMARK_F(NodeParametersInterfaceTest, declare_undeclare)(benchmark::State & state)
-{
-  for (auto _ : state) {
-    node->declare_parameter(param3_name);
-    node->undeclare_parameter(param3_name);
-  }
-}
-
-BENCHMARK_F(NodeParametersInterfaceTest, has_parameter_hit)(benchmark::State & state)
-{
-  for (auto _ : state) {
-    if (!node->has_parameter(param1_name)) {
-      state.SkipWithError("Parameter was expected");
-      break;
-    }
-  }
-}
-
-BENCHMARK_F(NodeParametersInterfaceTest, has_parameter_miss)(benchmark::State & state)
-{
-  for (auto _ : state) {
-    if (node->has_parameter(param3_name)) {
-      state.SkipWithError("Parameter was not expected");
-      break;
-    }
-  }
-}
-
-BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_bool)(benchmark::State & state)
-{
-  const std::vector<rclcpp::Parameter> param_values1
-  {
-    rclcpp::Parameter(param1_name, true),
-    rclcpp::Parameter(param2_name, false),
-  };
-  const std::vector<rclcpp::Parameter> param_values2
-  {
-    rclcpp::Parameter(param1_name, false),
-    rclcpp::Parameter(param2_name, true),
-  };
-
-  reset_heap_counters();
-
-  for (auto _ : state) {
-    node->set_parameters(param_values2);
-    node->set_parameters(param_values1);
-  }
-}
-
-BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_atomically_bool)(benchmark::State & state)
-{
-  const std::vector<rclcpp::Parameter> param_values1
-  {
-    rclcpp::Parameter(param1_name, true),
-    rclcpp::Parameter(param2_name, false),
-  };
-  const std::vector<rclcpp::Parameter> param_values2
-  {
-    rclcpp::Parameter(param1_name, false),
-    rclcpp::Parameter(param2_name, true),
-  };
-
-  reset_heap_counters();
-
-  for (auto _ : state) {
-    node->set_parameters_atomically(param_values2);
-    node->set_parameters_atomically(param_values1);
-  }
-}
-
-BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_callback_bool)(benchmark::State & state)
-{
-  const std::vector<rclcpp::Parameter> param_values1
-  {
-    rclcpp::Parameter(param1_name, true),
-    rclcpp::Parameter(param2_name, false),
-  };
-  const std::vector<rclcpp::Parameter> param_values2
-  {
-    rclcpp::Parameter(param1_name, false),
-    rclcpp::Parameter(param2_name, true),
-  };
-
-  rcl_interfaces::msg::SetParametersResult callback_result;
-  bool callback_received = false;
-  callback_result.successful = true;
-  auto callback =
-    [&callback_result, &callback_received](const std::vector<rclcpp::Parameter> &) {
-      callback_received = true;
-      return callback_result;
-    };
-  auto handle = node->add_on_set_parameters_callback(callback);
-
-  reset_heap_counters();
-
-  for (auto _ : state) {
-    node->set_parameters(param_values2);
-    node->set_parameters(param_values1);
-  }
-
-  if (!callback_received) {
-    state.SkipWithError("Callback is not functioning");
-  }
-
-  node->remove_on_set_parameters_callback(handle.get());
-}
-
-BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_string)(benchmark::State & state)
-{
-  const std::vector<rclcpp::Parameter> param_values1
-  {
-    rclcpp::Parameter(param1_name, "param 1 value A"),
-    rclcpp::Parameter(param2_name, "param 2 value B"),
-  };
-  const std::vector<rclcpp::Parameter> param_values2
-  {
-    rclcpp::Parameter(param1_name, "param 1 value B"),
-    rclcpp::Parameter(param2_name, "param 2 value A"),
-  };
-
-  reset_heap_counters();
-
-  for (auto _ : state) {
-    node->set_parameters(param_values2);
-    node->set_parameters(param_values1);
-  }
-}
-
-BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_array)(benchmark::State & state)
-{
-  const std::vector<rclcpp::Parameter> param_values1
-  {
-    rclcpp::Parameter(param1_name, std::vector<int> {0, 1, 2}),
-    rclcpp::Parameter(param2_name, std::vector<int> {3, 4, 5}),
-  };
-  const std::vector<rclcpp::Parameter> param_values2
-  {
-    rclcpp::Parameter(param1_name, std::vector<int> {4, 5, 6}),
-    rclcpp::Parameter(param2_name, std::vector<int> {7, 8, 9}),
-  };
-
-  reset_heap_counters();
-
-  for (auto _ : state) {
-    node->set_parameters(param_values2);
-    node->set_parameters(param_values1);
-  }
-}
-
-BENCHMARK_F(NodeParametersInterfaceTest, get_parameter)(benchmark::State & state)
-{
-  rclcpp::Parameter param1_value;
-
-  reset_heap_counters();
-
-  for (auto _ : state) {
-    node->get_parameter(param1_name, param1_value);
-  }
-}
-
-BENCHMARK_F(NodeParametersInterfaceTest, list_parameters_hit)(benchmark::State & state)
-{
-  rcl_interfaces::msg::ListParametersResult param_list;
-  const std::vector<std::string> prefixes
-  {
-    param_prefix,
-  };
-
-  reset_heap_counters();
-
-  for (auto _ : state) {
-    param_list = node->list_parameters(prefixes, 10);
-    if (param_list.names.size() != 2) {
-      state.SkipWithError("Expected node names");
-      break;
-    }
-  }
-}
-
-BENCHMARK_F(NodeParametersInterfaceTest, list_parameters_miss)(benchmark::State & state)
-{
-  rcl_interfaces::msg::ListParametersResult param_list;
-  const std::vector<std::string> prefixes
-  {
-    "your_param",
-  };
-
-  reset_heap_counters();
-
-  for (auto _ : state) {
-    param_list = node->list_parameters(prefixes, 10);
-    if (param_list.names.size() != 0) {
-      state.SkipWithError("Expected no node names");
-      break;
-    }
-  }
-}
--- a/rclcpp/test/benchmark/benchmark_service.cpp
+++ b/rclcpp/test/benchmark/benchmark_service.cpp
@@ -1,141 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <memory>
-
-#include "performance_test_fixture/performance_test_fixture.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "test_msgs/srv/empty.hpp"
-
-using performance_test_fixture::PerformanceTest;
-
-constexpr char empty_service_name[] = "empty_service";
-
-class ServicePerformanceTest : public PerformanceTest
-{
-public:
-  ServicePerformanceTest()
-  : callback_count(0) {}
-
-  void SetUp(benchmark::State & state)
-  {
-    rclcpp::init(0, nullptr);
-    node = std::make_unique<rclcpp::Node>("node", "ns");
-    empty_client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
-    callback_count = 0;
-
-    performance_test_fixture::PerformanceTest::SetUp(state);
-  }
-
-  void TearDown(benchmark::State & state)
-  {
-    performance_test_fixture::PerformanceTest::TearDown(state);
-
-    empty_client.reset();
-    node.reset();
-    rclcpp::shutdown();
-  }
-
-  void ServiceCallback(
-    const test_msgs::srv::Empty::Request::SharedPtr,
-    test_msgs::srv::Empty::Response::SharedPtr)
-  {
-    callback_count++;
-  }
-
-protected:
-  std::unique_ptr<rclcpp::Node> node;
-  std::shared_ptr<rclcpp::Client<test_msgs::srv::Empty>> empty_client;
-  int callback_count;
-};
-
-BENCHMARK_F(ServicePerformanceTest, construct_service_no_client)(benchmark::State & state) {
-  auto callback = std::bind(
-    &ServicePerformanceTest::ServiceCallback,
-    this, std::placeholders::_1, std::placeholders::_2);
-
-  auto outer_service = node->create_service<test_msgs::srv::Empty>("not_a_service", callback);
-
-  reset_heap_counters();
-  for (auto _ : state) {
-    auto service = node->create_service<test_msgs::srv::Empty>("not_a_service", callback);
-    benchmark::DoNotOptimize(service);
-    benchmark::ClobberMemory();
-
-    state.PauseTiming();
-    service.reset();
-    state.ResumeTiming();
-  }
-}
-
-BENCHMARK_F(ServicePerformanceTest, construct_service_empty_srv)(benchmark::State & state) {
-  auto callback = std::bind(
-    &ServicePerformanceTest::ServiceCallback,
-    this, std::placeholders::_1, std::placeholders::_2);
-  auto outer_service = node->create_service<test_msgs::srv::Empty>(empty_service_name, callback);
-
-  reset_heap_counters();
-  for (auto _ : state) {
-    auto service = node->create_service<test_msgs::srv::Empty>(empty_service_name, callback);
-    benchmark::DoNotOptimize(service);
-    benchmark::ClobberMemory();
-
-    state.PauseTiming();
-    service.reset();
-    state.ResumeTiming();
-  }
-}
-
-BENCHMARK_F(ServicePerformanceTest, destroy_service_empty_srv)(benchmark::State & state) {
-  auto callback = std::bind(
-    &ServicePerformanceTest::ServiceCallback,
-    this, std::placeholders::_1, std::placeholders::_2);
-  auto outer_service = node->create_service<test_msgs::srv::Empty>(empty_service_name, callback);
-
-  reset_heap_counters();
-  for (auto _ : state) {
-    state.PauseTiming();
-    auto service = node->create_service<test_msgs::srv::Empty>(empty_service_name, callback);
-    state.ResumeTiming();
-    benchmark::DoNotOptimize(service);
-    benchmark::ClobberMemory();
-
-    service.reset();
-  }
-}
-
-BENCHMARK_F(ServicePerformanceTest, async_send_response)(benchmark::State & state) {
-  auto callback = std::bind(
-    &ServicePerformanceTest::ServiceCallback,
-    this, std::placeholders::_1, std::placeholders::_2);
-  auto service = node->create_service<test_msgs::srv::Empty>(empty_service_name, callback);
-
-  reset_heap_counters();
-  for (auto _ : state) {
-    state.PauseTiming();
-    // Clear executor queue
-    rclcpp::spin_some(node->get_node_base_interface());
-
-    auto request = std::make_shared<test_msgs::srv::Empty::Request>();
-    auto future = empty_client->async_send_request(request);
-    state.ResumeTiming();
-    benchmark::DoNotOptimize(service);
-    benchmark::ClobberMemory();
-
-    rclcpp::spin_until_future_complete(node->get_node_base_interface(), future);
-  }
-  if (callback_count == 0) {
-    state.SkipWithError("Service callback was not called");
-  }
-}
--- a/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp
+++ b/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp
@@ -83,7 +83,7 @@ TEST_F(TestMultiThreadedExecutor, timer_over_take) {

      {
        std::lock_guard<std::mutex> lock(last_mutex);
-        double diff = static_cast<double>(std::abs((now - last).nanoseconds())) / 1.0e9;
+        double diff = std::abs((now - last).nanoseconds()) / 1.0e9;
        last = now;

        if (diff < PERIOD - TOLERANCE) {
--- a/rclcpp/test/mocking_utils/patch.hpp
+++ b/rclcpp/test/mocking_utils/patch.hpp
@@ -1,578 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-// Original file taken from:
-// https://github.com/ros2/rcutils/blob/master/test/mocking_utils/patch.hpp
-
-#ifndef MOCKING_UTILS__PATCH_HPP_
-#define MOCKING_UTILS__PATCH_HPP_
-
-#define MOCKING_UTILS_SUPPORT_VA_LIST
-#if (defined(__aarch64__) || defined(__arm__) || defined(_M_ARM) || defined(__thumb__))
-// In ARM machines, va_list does not define comparison operators
-// nor the compiler allows defining them via operator overloads.
-// Thus, Mimick argument matching code will not compile.
-#undef MOCKING_UTILS_SUPPORT_VA_LIST
-#endif
-
-#ifdef MOCKING_UTILS_SUPPORT_VA_LIST
-#include <cstdarg>
-#endif
-
-#include <functional>
-#include <string>
-#include <type_traits>
-#include <utility>
-
-#include "mimick/mimick.h"
-
-#include "rcutils/error_handling.h"
-#include "rcutils/macros.h"
-
-namespace mocking_utils
-{
-
-/// Mimick specific traits for each mocking_utils::Patch instance.
-/**
- * \tparam ID Numerical identifier of the patch. Ought to be unique.
- * \tparam SignatureT Type of the symbol to be patched.
-*/
-template<size_t ID, typename SignatureT>
-struct PatchTraits;
-
-/// Traits specialization for ReturnT(void) free functions.
-/**
- * \tparam ID Numerical identifier of the patch. Ought to be unique.
- * \tparam ReturnT Return value type.
- */
-template<size_t ID, typename ReturnT>
-struct PatchTraits<ID, ReturnT(void)>
-{
-  mmk_mock_define(mock_type, ReturnT);
-};
-
-/// Traits specialization for void(void) free functions.
-/**
- * Necessary for Mimick macros to adjust accordingly when the return
- * type is `void`.
- *
- * \tparam ID Numerical identifier of the patch. Ought to be unique.
- */
-template<size_t ID>
-struct PatchTraits<ID, void(void)>
-{
-  mmk_mock_define(mock_type, void);
-};
-
-/// Traits specialization for ReturnT(ArgT0) free functions.
-/**
- * \tparam ID Numerical identifier of the patch. Ought to be unique.
- * \tparam ReturnT Return value type.
- * \tparam ArgT0 Argument type.
- */
-template<size_t ID, typename ReturnT, typename ArgT0>
-struct PatchTraits<ID, ReturnT(ArgT0)>
-{
-  mmk_mock_define(mock_type, ReturnT, ArgT0);
-};
-
-/// Traits specialization for void(ArgT0) free functions.
-/**
- * Necessary for Mimick macros to adjust accordingly when the return
- * type is `void`.
- *
- * \tparam ID Numerical identifier of the patch. Ought to be unique.
- * \tparam ArgT0 Argument type.
- */
-template<size_t ID, typename ArgT0>
-struct PatchTraits<ID, void(ArgT0)>
-{
-  mmk_mock_define(mock_type, void, ArgT0);
-};
-
-/// Traits specialization for ReturnT(ArgT0, ArgT1) free functions.
-/**
- * \tparam ID Numerical identifier of the patch. Ought to be unique.
- * \tparam ReturnT Return value type.
- * \tparam ArgTx Argument types.
- */
-template<size_t ID, typename ReturnT,
-  typename ArgT0, typename ArgT1>
-struct PatchTraits<ID, ReturnT(ArgT0, ArgT1)>
-{
-  mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1);
-};
-
-/// Traits specialization for void(ArgT0, ArgT1) free functions.
-/**
- * Necessary for Mimick macros to adjust accordingly when the return
- * type is `void`.
- *
- * \tparam ID Numerical identifier of the patch. Ought to be unique.
- * \tparam ArgTx Argument types.
- */
-template<size_t ID, typename ArgT0, typename ArgT1>
-struct PatchTraits<ID, void(ArgT0, ArgT1)>
-{
-  mmk_mock_define(mock_type, void, ArgT0, ArgT1);
-};
-
-/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2) free functions.
-/**
- * \tparam ID Numerical identifier of the patch. Ought to be unique.
- * \tparam ReturnT Return value type.
- * \tparam ArgTx Argument types.
- */
-template<size_t ID, typename ReturnT,
-  typename ArgT0, typename ArgT1, typename ArgT2>
-struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2)>
-{
-  mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2);
-};
-
-/// Traits specialization for void(ArgT0, ArgT1, ArgT2) free functions.
-/**
- * Necessary for Mimick macros to adjust accordingly when the return
- * type is `void`.
- *
- * \tparam ID Numerical identifier of the patch. Ought to be unique.
- * \tparam ArgTx Argument types.
- */
-template<size_t ID, typename ArgT0, typename ArgT1, typename ArgT2>
-struct PatchTraits<ID, void(ArgT0, ArgT1, ArgT2)>
-{
-  mmk_mock_define(mock_type, void, ArgT0, ArgT1, ArgT2);
-};
-
-/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3) free functions.
-/**
- * \tparam ID Numerical identifier of the patch. Ought to be unique.
- * \tparam ReturnT Return value type.
- * \tparam ArgTx Argument types.
- */
-template<size_t ID, typename ReturnT,
-  typename ArgT0, typename ArgT1,
-  typename ArgT2, typename ArgT3>
-struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3)>
-{
-  mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3);
-};
-
-/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3) free functions.
-/**
- * Necessary for Mimick macros to adjust accordingly when the return
- * type is `void`.
- *
- * \tparam ID Numerical identifier of the patch. Ought to be unique.
- * \tparam ArgTx Argument types.
- */
-template<size_t ID,
-  typename ArgT0, typename ArgT1,
-  typename ArgT2, typename ArgT3>
-struct PatchTraits<ID, void(ArgT0, ArgT1, ArgT2, ArgT3)>
-{
-  mmk_mock_define(mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3);
-};
-
-/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4)
-/// free functions.
-/**
- * \tparam ID Numerical identifier of the patch. Ought to be unique.
- * \tparam ReturnT Return value type.
- * \tparam ArgTx Argument types.
- */
-template<size_t ID, typename ReturnT,
-  typename ArgT0, typename ArgT1,
-  typename ArgT2, typename ArgT3, typename ArgT4>
-struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4)>
-{
-  mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4);
-};
-
-/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4)
-/// free functions.
-/**
- * Necessary for Mimick macros to adjust accordingly when the return
- * type is `void`.
- *
- * \tparam ID Numerical identifier of the patch. Ought to be unique.
- * \tparam ArgTx Argument types.
- */
-template<size_t ID,
-  typename ArgT0, typename ArgT1,
-  typename ArgT2, typename ArgT3, typename ArgT4>
-struct PatchTraits<ID, void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4)>
-{
-  mmk_mock_define(mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4);
-};
-
-/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)
-/// free functions.
-/**
- * \tparam ID Numerical identifier of the patch. Ought to be unique.
- * \tparam ReturnT Return value type.
- * \tparam ArgTx Argument types.
- */
-template<size_t ID, typename ReturnT,
-  typename ArgT0, typename ArgT1,
-  typename ArgT2, typename ArgT3,
-  typename ArgT4, typename ArgT5>
-struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)>
-{
-  mmk_mock_define(
-    mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5);
-};
-
-/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)
-/// free functions.
-/**
- * Necessary for Mimick macros to adjust accordingly when the return
- * type is `void`.
- *
- * \tparam ID Numerical identifier of the patch. Ought to be unique.
- * \tparam ArgTx Argument types.
- */
-template<size_t ID,
-  typename ArgT0, typename ArgT1,
-  typename ArgT2, typename ArgT3,
-  typename ArgT4, typename ArgT5>
-struct PatchTraits<ID, void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)>
-{
-  mmk_mock_define(
-    mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5);
-};
-
-/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6)
-/// free functions.
-/**
- * \tparam ID Numerical identifier of the patch. Ought to be unique.
- * \tparam ReturnT Return value type.
- * \tparam ArgTx Argument types.
- */
-template<size_t ID, typename ReturnT,
-  typename ArgT0, typename ArgT1,
-  typename ArgT2, typename ArgT3,
-  typename ArgT4, typename ArgT5, typename ArgT6>
-struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6)>
-{
-  mmk_mock_define(
-    mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6);
-};
-
-/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7)
-/// free functions.
-/**
- * \tparam ID Numerical identifier of the patch. Ought to be unique.
- * \tparam ReturnT Return value type.
- * \tparam ArgTx Argument types.
- */
-template<size_t ID, typename ReturnT,
-  typename ArgT0, typename ArgT1,
-  typename ArgT2, typename ArgT3,
-  typename ArgT4, typename ArgT5,
-  typename ArgT6, typename ArgT7>
-struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7)>
-{
-  mmk_mock_define(
-    mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7);
-};
-
-/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8)
-/// free functions.
-/**
- * \tparam ID Numerical identifier of the patch. Ought to be unique.
- * \tparam ReturnT Return value type.
- * \tparam ArgTx Argument types.
- */
-template<size_t ID, typename ReturnT,
-  typename ArgT0, typename ArgT1,
-  typename ArgT2, typename ArgT3,
-  typename ArgT4, typename ArgT5,
-  typename ArgT6, typename ArgT7, typename ArgT8>
-struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8)>
-{
-  mmk_mock_define(
-    mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8);
-};
-
-/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7,
-/// ArgT8, ArgT9) free functions.
-/**
- * \tparam ID Numerical identifier of the patch. Ought to be unique.
- * \tparam ReturnT Return value type.
- * \tparam ArgTx Argument types.
- */
-template<size_t ID, typename ReturnT,
-  typename ArgT0, typename ArgT1,
-  typename ArgT2, typename ArgT3,
-  typename ArgT4, typename ArgT5,
-  typename ArgT6, typename ArgT7,
-  typename ArgT8, typename ArgT9>
-struct PatchTraits<ID, ReturnT(
-    ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8, ArgT9)>
-{
-  mmk_mock_define(
-    mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8, ArgT9);
-};
-
-/// Generic trampoline to wrap generalized callables in plain functions.
-/**
- * \tparam ID Numerical identifier of this trampoline. Ought to be unique.
- * \tparam SignatureT Type of the symbol this trampoline replaces.
- */
-template<size_t ID, typename SignatureT>
-struct Trampoline;
-
-/// Trampoline specialization for free functions.
-template<size_t ID, typename ReturnT, typename ... ArgTs>
-struct Trampoline<ID, ReturnT(ArgTs...)>
-{
-  static ReturnT base(ArgTs... args)
-  {
-    return target(std::forward<ArgTs>(args)...);
-  }
-
-  static std::function<ReturnT(ArgTs...)> target;
-};
-
-template<size_t ID, typename ReturnT, typename ... ArgTs>
-std::function<ReturnT(ArgTs...)>
-Trampoline<ID, ReturnT(ArgTs...)>::target;
-
-/// Setup trampoline with the given @p target.
-/**
- * \param[in] target Callable that this trampoline will target.
- * \return the plain base function of this trampoline.
- *
- * \tparam ID Numerical identifier of this trampoline. Ought to be unique.
- * \tparam SignatureT Type of the symbol this trampoline replaces.
- */
-template<size_t ID, typename SignatureT>
-auto prepare_trampoline(std::function<SignatureT> target)
-{
-  Trampoline<ID, SignatureT>::target = target;
-  return Trampoline<ID, SignatureT>::base;
-}
-
-/// Patch class for binary API mocking
-/**
- * Built on top of Mimick, to enable symbol mocking on a per dynamically
- * linked binary object basis.
- *
- * \tparam ID Numerical identifier for this patch. Ought to be unique.
- * \tparam SignatureT Type of the symbol to be patched.
- */
-template<size_t ID, typename SignatureT>
-class Patch;
-
-/// Patch specialization for ReturnT(ArgTs...) free functions.
-/**
- * \tparam ID Numerical identifier for this patch. Ought to be unique.
- * \tparam ReturnT Return value type.
- * \tparam ArgTs Argument types.
- */
-template<size_t ID, typename ReturnT, typename ... ArgTs>
-class Patch<ID, ReturnT(ArgTs...)>
-{
-public:
-  using mock_type = typename PatchTraits<ID, ReturnT(ArgTs...)>::mock_type;
-
-  /// Construct a patch.
-  /**
-   * \param[in] target Symbol target string, using Mimick syntax
-   *   i.e. "symbol(@scope)?", where scope may be "self" to target the current
-   *   binary, "lib:library_name" to target a given library, "file:path/to/library"
-   *   to target a given file, or "sym:other_symbol" to target the first library
-   *   that defines said symbol.
-   * \param[in] proxy An indirection to call the target function.
-   *   This indirection must ensure this call goes through the function's
-   *   trampoline, as setup by the dynamic linker.
-   * \return a mocking_utils::Patch instance.
-   */
-  explicit Patch(const std::string & target, std::function<ReturnT(ArgTs...)> proxy)
-  : target_(target), proxy_(proxy)
-  {
-  }
-
-  // Copy construction and assignment are disabled.
-  Patch(const Patch &) = delete;
-  Patch & operator=(const Patch &) = delete;
-
-  Patch(Patch && other)
-  {
-    mock_ = other.mock_;
-    other.mock_ = nullptr;
-  }
-
-  Patch & operator=(Patch && other)
-  {
-    if (mock_) {
-      mmk_reset(mock_);
-    }
-    mock_ = other.mock_;
-    other.mock_ = nullptr;
-  }
-
-  ~Patch()
-  {
-    if (mock_) {
-      mmk_reset(mock_);
-    }
-  }
-
-  /// Inject a @p replacement for the patched function.
-  Patch & then_call(std::function<ReturnT(ArgTs...)> replacement) &
-  {
-    replace_with(replacement);
-    return *this;
-  }
-
-  /// Inject a @p replacement for the patched function.
-  Patch && then_call(std::function<ReturnT(ArgTs...)> replacement) &&
-  {
-    replace_with(replacement);
-    return std::move(*this);
-  }
-
-private:
-  // Helper for template parameter pack expansion using `mmk_any`
-  // macro as pattern.
-  template<typename T>
-  T any() {return mmk_any(T);}
-
-  void replace_with(std::function<ReturnT(ArgTs...)> replacement)
-  {
-    if (mock_) {
-      throw std::logic_error("Cannot configure patch more than once");
-    }
-    auto type_erased_trampoline =
-      reinterpret_cast<mmk_fn>(prepare_trampoline<ID>(replacement));
-    auto MMK_MANGLE(mock_type, create) =
-      PatchTraits<ID, ReturnT(ArgTs...)>::MMK_MANGLE(mock_type, create);
-    mock_ = mmk_mock(target_.c_str(), mock_type);
-    mmk_when(proxy_(any<ArgTs>()...), .then_call = type_erased_trampoline);
-  }
-
-  mock_type mock_{nullptr};
-  std::string target_;
-  std::function<ReturnT(ArgTs...)> proxy_;
-};
-
-/// Make a patch for a `target` function.
-/**
- * Useful for type deduction during \ref mocking_utils::Patch construction.
- *
- * \param[in] target Symbol target string, using Mimick syntax.
- * \param[in] proxy An indirection to call the target function.
- * \return a mocking_utils::Patch instance.
- *
- * \tparam ID Numerical identifier for this patch. Ought to be unique.
- * \tparam SignatureT Type of the function to be patched.
- *
- * \sa mocking_utils::Patch for further reference.
- */
-template<size_t ID, typename SignatureT>
-auto make_patch(const std::string & target, std::function<SignatureT> proxy)
-{
-  return Patch<ID, SignatureT>(target, proxy);
-}
-
-/// Define a dummy operator `op` for a given `type`.
-/**
- * Useful to enable patching functions that take arguments whose types
- * do not define basic comparison operators, as required by Mimick.
-*/
-#define MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(type_, op) \
-  template<typename T> \
-  typename std::enable_if<std::is_same<T, type_>::value, bool>::type \
-  operator op(const T &, const T &) { \
-    return false; \
-  }
-
-/// Get the exact \ref mocking_utils::Patch type for a given `id` and `function`.
-/**
- * Useful to avoid ignored attribute warnings when using the \b decltype operator.
- */
-#define MOCKING_UTILS_PATCH_TYPE(id, function) \
-  decltype(mocking_utils::make_patch<id, decltype(function)>("", nullptr))
-
-/// A transparent forwarding proxy to a given `function`.
-/**
- * Useful to ensure a call to `function` goes through its trampoline.
- */
-#define MOCKING_UTILS_PATCH_PROXY(function) \
-  [] (auto && ... args)->decltype(auto) { \
-    return function(std::forward<decltype(args)>(args)...); \
-  }
-
-/// Compute a Mimick symbol target string based on which `function` is to be patched
-/// in which `scope`.
-#define MOCKING_UTILS_PATCH_TARGET(scope, function) \
-  (std::string(RCUTILS_STRINGIFY(function)) + "@" + (scope))
-
-/// Prepare a mocking_utils::Patch for patching a `function` in a given `scope`
-/// but defer applying any changes.
-#define prepare_patch(scope, function) \
-  make_patch<__COUNTER__, decltype(function)>( \
-    MOCKING_UTILS_PATCH_TARGET(scope, function), MOCKING_UTILS_PATCH_PROXY(function) \
-  )
-
-/// Patch a `function` with a used-provided `replacement` in a given `scope`.
-#define patch(scope, function, replacement) \
-  prepare_patch(scope, function).then_call(replacement)
-
-/// Patch a `function` to always yield a given `return_code` in a given `scope`.
-#define patch_and_return(scope, function, return_code) \
-  patch(scope, function, [&](auto && ...) {return return_code;})
-
-/// Patch a `function` to always yield a given `return_code` in a given `scope`.
-#define patch_to_fail(scope, function, error_message, return_code) \
-  patch( \
-    scope, function, [&](auto && ...) { \
-      RCUTILS_SET_ERROR_MSG(error_message); \
-      return return_code; \
-    })
-
-/// Patch a `function` to execute normally but always yield a given `return_code`
-/// in a given `scope`.
-/**
- * \warning On some Linux distributions (e.g. CentOS), pointers to function
- *   reference their PLT trampolines. In such cases, it is not possible to
- *   call `function` from within the mock.
- */
-#define inject_on_return(scope, function, return_code) \
-  patch( \
-    scope, function, ([&, base = function](auto && ... __args) { \
-      if (base != function) { \
-        static_cast<void>(base(std::forward<decltype(__args)>(__args)...)); \
-      } else { \
-        RCUTILS_SAFE_FWRITE_TO_STDERR( \
-          "[WARNING] mocking_utils::inject_on_return() cannot forward call to " \
-          "original '" RCUTILS_STRINGIFY(function) "' function before injection\n" \
-          "    at " __FILE__ ":" RCUTILS_STRINGIFY(__LINE__) "\n"); \
-      } \
-      return return_code; \
-    }))
-
-}  // namespace mocking_utils
-
-#ifdef MOCKING_UTILS_SUPPORT_VA_LIST
-// Define dummy comparison operators for C standard va_list type
-MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, ==)
-MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, !=)
-MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, <)
-MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, >)
-#endif
-
-#endif  // MOCKING_UTILS__PATCH_HPP_
--- a/rclcpp/test/rclcpp/node_interfaces/node_wrapper.hpp
+++ b/rclcpp/test/rclcpp/node_interfaces/node_wrapper.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.

-#ifndef RCLCPP__NODE_INTERFACES__NODE_WRAPPER_HPP_
-#define RCLCPP__NODE_INTERFACES__NODE_WRAPPER_HPP_
+#ifndef NODE_INTERFACES__NODE_WRAPPER_HPP_
+#define NODE_INTERFACES__NODE_WRAPPER_HPP_

 #include <memory>
 #include <string>
@@ -61,4 +61,4 @@ private:
  rclcpp::Node::SharedPtr node;
 };

-#endif  // RCLCPP__NODE_INTERFACES__NODE_WRAPPER_HPP_
+#endif  // NODE_INTERFACES__NODE_WRAPPER_HPP_
--- a/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp
+++ b/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp
--- a/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp
+++ b/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp
--- a/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp
+++ b/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp
--- a/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp
+++ b/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp
--- a/rclcpp/test/rclcpp/node_interfaces/test_get_node_interfaces.cpp
+++ b/rclcpp/test/rclcpp/node_interfaces/test_get_node_interfaces.cpp
--- a/rclcpp/test/rclcpp/CMakeLists.txt
+++ b/rclcpp/test/rclcpp/CMakeLists.txt
@@ -1,653 +0,0 @@
-find_package(ament_cmake_gtest REQUIRED)
-
-find_package(rmw_implementation_cmake REQUIRED)
-
-add_definitions(-DTEST_RESOURCES_DIRECTORY="${CMAKE_CURRENT_BINARY_DIR}/../resources")
-
-rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs
-  ../msg/Header.msg
-  ../msg/MessageWithHeader.msg
-  DEPENDENCIES builtin_interfaces
-  LIBRARY_NAME ${PROJECT_NAME}
-  SKIP_INSTALL
-)
-
-ament_add_gtest(
-  test_allocator_common
-  allocator/test_allocator_common.cpp)
-if(TARGET test_allocator_common)
-  target_link_libraries(test_allocator_common ${PROJECT_NAME})
-endif()
-ament_add_gtest(
-  test_allocator_deleter
-  allocator/test_allocator_deleter.cpp)
-if(TARGET test_allocator_deleter)
-  target_link_libraries(test_allocator_deleter ${PROJECT_NAME})
-endif()
-ament_add_gtest(
-  test_exceptions
-  exceptions/test_exceptions.cpp)
-if(TARGET test_exceptions)
-  target_link_libraries(test_exceptions ${PROJECT_NAME} mimick)
-endif()
-
-# Increasing timeout because connext can take a long time to destroy nodes
-# TODO(brawner) remove when destroying Node for Connext is resolved. See:
-# https://github.com/ros2/rclcpp/issues/1250
-ament_add_gtest(
-  test_allocator_memory_strategy
-  strategies/test_allocator_memory_strategy.cpp
-  TIMEOUT 360)
-if(TARGET test_allocator_memory_strategy)
-  ament_target_dependencies(test_allocator_memory_strategy
-    "rcl"
-    "test_msgs"
-  )
-  target_link_libraries(test_allocator_memory_strategy ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_message_pool_memory_strategy strategies/test_message_pool_memory_strategy.cpp)
-if(TARGET test_message_pool_memory_strategy)
-  ament_target_dependencies(test_message_pool_memory_strategy
-    "rcl"
-    "test_msgs"
-  )
-  target_link_libraries(test_message_pool_memory_strategy ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_any_service_callback test_any_service_callback.cpp)
-if(TARGET test_any_service_callback)
-  ament_target_dependencies(test_any_service_callback
-    "test_msgs"
-  )
-  target_link_libraries(test_any_service_callback ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_any_subscription_callback test_any_subscription_callback.cpp)
-if(TARGET test_any_subscription_callback)
-  ament_target_dependencies(test_any_subscription_callback
-    "test_msgs"
-  )
-  target_link_libraries(test_any_subscription_callback ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_client test_client.cpp)
-if(TARGET test_client)
-  ament_target_dependencies(test_client
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "test_msgs"
-  )
-  target_link_libraries(test_client ${PROJECT_NAME} mimick)
-endif()
-ament_add_gtest(test_create_timer test_create_timer.cpp)
-if(TARGET test_create_timer)
-  ament_target_dependencies(test_create_timer
-    "rcl_interfaces"
-    "rmw"
-    "rcl"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_create_timer ${PROJECT_NAME})
-  target_include_directories(test_create_timer PRIVATE ./)
-endif()
-ament_add_gtest(test_create_subscription test_create_subscription.cpp)
-if(TARGET test_create_subscription)
-  target_link_libraries(test_create_subscription ${PROJECT_NAME})
-  ament_target_dependencies(test_create_subscription
-    "test_msgs"
-  )
-endif()
-ament_add_gtest(test_add_callback_groups_to_executor
-  test_add_callback_groups_to_executor.cpp
-  TIMEOUT 120)
-if(TARGET test_add_callback_groups_to_executor)
-  target_link_libraries(test_add_callback_groups_to_executor ${PROJECT_NAME})
-  ament_target_dependencies(test_add_callback_groups_to_executor
-    "test_msgs"
-  )
-endif()
-ament_add_gtest(test_expand_topic_or_service_name test_expand_topic_or_service_name.cpp)
-if(TARGET test_expand_topic_or_service_name)
-  ament_target_dependencies(test_expand_topic_or_service_name
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME} mimick)
-endif()
-ament_add_gtest(test_function_traits test_function_traits.cpp)
-if(TARGET test_function_traits)
-  target_include_directories(test_function_traits PUBLIC ../../include)
-  ament_target_dependencies(test_function_traits
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-endif()
-ament_add_gtest(
-  test_future_return_code
-  test_future_return_code.cpp)
-if(TARGET test_future_return_code)
-  target_link_libraries(test_future_return_code ${PROJECT_NAME})
-endif()
-ament_add_gmock(test_intra_process_manager test_intra_process_manager.cpp)
-if(TARGET test_intra_process_manager)
-  ament_target_dependencies(test_intra_process_manager
-    "rcl"
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_intra_process_manager ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_ring_buffer_implementation test_ring_buffer_implementation.cpp)
-if(TARGET test_ring_buffer_implementation)
-  ament_target_dependencies(test_ring_buffer_implementation
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_ring_buffer_implementation ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_intra_process_buffer test_intra_process_buffer.cpp)
-if(TARGET test_intra_process_buffer)
-  ament_target_dependencies(test_intra_process_buffer
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_intra_process_buffer ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_loaned_message test_loaned_message.cpp)
-ament_target_dependencies(test_loaned_message
-  "test_msgs"
-)
-target_link_libraries(test_loaned_message ${PROJECT_NAME} mimick)
-
-ament_add_gtest(test_memory_strategy test_memory_strategy.cpp)
-ament_target_dependencies(test_memory_strategy
-  "test_msgs"
-)
-target_link_libraries(test_memory_strategy ${PROJECT_NAME})
-
-ament_add_gtest(test_message_memory_strategy test_message_memory_strategy.cpp)
-ament_target_dependencies(test_message_memory_strategy
-  "test_msgs"
-)
-target_link_libraries(test_message_memory_strategy ${PROJECT_NAME})
-
-ament_add_gtest(test_node test_node.cpp TIMEOUT 240)
-if(TARGET test_node)
-  ament_target_dependencies(test_node
-    "rcl_interfaces"
-    "rcpputils"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "test_msgs"
-  )
-  target_link_libraries(test_node ${PROJECT_NAME} mimick)
-endif()
-
-ament_add_gtest(test_node_interfaces__get_node_interfaces
-  node_interfaces/test_get_node_interfaces.cpp)
-if(TARGET test_node_interfaces__get_node_interfaces)
-  target_link_libraries(test_node_interfaces__get_node_interfaces ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_node_interfaces__node_base
-  node_interfaces/test_node_base.cpp)
-if(TARGET test_node_interfaces__node_base)
-  target_link_libraries(test_node_interfaces__node_base ${PROJECT_NAME} mimick)
-endif()
-ament_add_gtest(test_node_interfaces__node_clock
-  node_interfaces/test_node_clock.cpp)
-if(TARGET test_node_interfaces__node_clock)
-  target_link_libraries(test_node_interfaces__node_clock ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_node_interfaces__node_graph
-  node_interfaces/test_node_graph.cpp
-  TIMEOUT 120)
-if(TARGET test_node_interfaces__node_graph)
-  ament_target_dependencies(
-    test_node_interfaces__node_graph
-    "test_msgs")
-  target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME} mimick)
-endif()
-ament_add_gtest(test_node_interfaces__node_parameters
-  node_interfaces/test_node_parameters.cpp)
-if(TARGET test_node_interfaces__node_parameters)
-  target_link_libraries(test_node_interfaces__node_parameters ${PROJECT_NAME} mimick)
-endif()
-ament_add_gtest(test_node_interfaces__node_services
-  node_interfaces/test_node_services.cpp)
-if(TARGET test_node_interfaces__node_services)
-  target_link_libraries(test_node_interfaces__node_services ${PROJECT_NAME} mimick)
-endif()
-ament_add_gtest(test_node_interfaces__node_timers
-  node_interfaces/test_node_timers.cpp)
-if(TARGET test_node_interfaces__node_timers)
-  target_link_libraries(test_node_interfaces__node_timers ${PROJECT_NAME} mimick)
-endif()
-ament_add_gtest(test_node_interfaces__node_topics
-  node_interfaces/test_node_topics.cpp)
-if(TARGET test_node_interfaces__node_topics)
-  ament_target_dependencies(
-    test_node_interfaces__node_topics
-    "test_msgs")
-  target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME} mimick)
-endif()
-ament_add_gtest(test_node_interfaces__node_waitables
-  node_interfaces/test_node_waitables.cpp)
-if(TARGET test_node_interfaces__node_waitables)
-  target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME} mimick)
-endif()
-
-# TODO(wjwwood): reenable these build failure tests when I can get Jenkins to ignore their output
-# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_rclcpp_node
-#   node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp)
-# target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node
-#   ${PROJECT_NAME})
-
-# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
-#   node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp)
-# target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
-#   ${PROJECT_NAME})
-
-# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_wrapped_node
-#   node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp)
-# target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node
-#   ${PROJECT_NAME})
-
-# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_wrapped_node
-#   node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp)
-# target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
-#   ${PROJECT_NAME})
-
-ament_add_gtest(test_node_global_args test_node_global_args.cpp)
-if(TARGET test_node_global_args)
-  ament_target_dependencies(test_node_global_args
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_node_global_args ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_node_options test_node_options.cpp)
-if(TARGET test_node_options)
-  ament_target_dependencies(test_node_options "rcl")
-  target_link_libraries(test_node_options ${PROJECT_NAME} mimick)
-endif()
-ament_add_gtest(test_init_options test_init_options.cpp)
-if(TARGET test_init_options)
-  ament_target_dependencies(test_init_options "rcl")
-  target_link_libraries(test_init_options ${PROJECT_NAME} mimick)
-endif()
-ament_add_gtest(test_parameter_client test_parameter_client.cpp)
-if(TARGET test_parameter_client)
-  ament_target_dependencies(test_parameter_client
-    "rcl_interfaces"
-  )
-  target_link_libraries(test_parameter_client ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_parameter_service test_parameter_service.cpp)
-if(TARGET test_parameter_service)
-  ament_target_dependencies(test_parameter_service
-    "rcl_interfaces"
-  )
-  target_link_libraries(test_parameter_service ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_parameter_events_filter test_parameter_events_filter.cpp)
-if(TARGET test_parameter_events_filter)
-  ament_target_dependencies(test_parameter_events_filter
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_parameter_events_filter ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_parameter test_parameter.cpp)
-if(TARGET test_parameter)
-  ament_target_dependencies(test_parameter
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_parameter ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_parameter_map test_parameter_map.cpp)
-if(TARGET test_parameter_map)
-  target_link_libraries(test_parameter_map ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_publisher test_publisher.cpp TIMEOUT 120)
-if(TARGET test_publisher)
-  ament_target_dependencies(test_publisher
-    "rcl"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "test_msgs"
-  )
-  target_link_libraries(test_publisher ${PROJECT_NAME} mimick)
-endif()
-ament_add_gtest(test_publisher_subscription_count_api test_publisher_subscription_count_api.cpp)
-if(TARGET test_publisher_subscription_count_api)
-  ament_target_dependencies(test_publisher_subscription_count_api
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "test_msgs"
-  )
-  target_link_libraries(test_publisher_subscription_count_api ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_qos test_qos.cpp)
-if(TARGET test_qos)
-  ament_target_dependencies(test_qos
-    "rmw"
-  )
-  target_link_libraries(test_qos
-    ${PROJECT_NAME}
-  )
-endif()
-ament_add_gtest(test_qos_event test_qos_event.cpp)
-if(TARGET test_qos_event)
-  ament_target_dependencies(test_qos_event
-    "rmw"
-    "test_msgs"
-  )
-  target_link_libraries(test_qos_event
-    ${PROJECT_NAME}
-    mimick
-  )
-endif()
-ament_add_gtest(test_rate test_rate.cpp)
-if(TARGET test_rate)
-  ament_target_dependencies(test_rate
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_rate
-    ${PROJECT_NAME}
-  )
-endif()
-ament_add_gtest(test_serialized_message_allocator test_serialized_message_allocator.cpp)
-if(TARGET test_serialized_message_allocator)
-  ament_target_dependencies(test_serialized_message_allocator
-    test_msgs
-  )
-  target_link_libraries(test_serialized_message_allocator
-    ${PROJECT_NAME}
-  )
-endif()
-ament_add_gtest(test_serialized_message test_serialized_message.cpp)
-if(TARGET test_serialized_message)
-  ament_target_dependencies(test_serialized_message
-    test_msgs
-  )
-  target_link_libraries(test_serialized_message
-    ${PROJECT_NAME}
-  )
-endif()
-ament_add_gtest(test_service test_service.cpp)
-if(TARGET test_service)
-  ament_target_dependencies(test_service
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "test_msgs"
-  )
-  target_link_libraries(test_service ${PROJECT_NAME} mimick)
-endif()
-# Creating and destroying nodes is slow with Connext, so this needs larger timeout.
-ament_add_gtest(test_subscription test_subscription.cpp TIMEOUT 120)
-if(TARGET test_subscription)
-  ament_target_dependencies(test_subscription
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "test_msgs"
-  )
-  target_link_libraries(test_subscription ${PROJECT_NAME} mimick)
-endif()
-ament_add_gtest(test_subscription_publisher_count_api test_subscription_publisher_count_api.cpp)
-if(TARGET test_subscription_publisher_count_api)
-  ament_target_dependencies(test_subscription_publisher_count_api
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "test_msgs"
-  )
-  target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_subscription_traits test_subscription_traits.cpp)
-if(TARGET test_subscription_traits)
-  ament_target_dependencies(test_subscription_traits
-    "rcl"
-    "test_msgs"
-  )
-  target_link_libraries(test_subscription_traits ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_type_support test_type_support.cpp)
-if(TARGET test_type_support)
-  ament_target_dependencies(test_type_support
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "test_msgs"
-  )
-  target_link_libraries(test_type_support ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_find_weak_nodes test_find_weak_nodes.cpp)
-if(TARGET test_find_weak_nodes)
-  ament_target_dependencies(test_find_weak_nodes
-    "rcl"
-  )
-  target_link_libraries(test_find_weak_nodes ${PROJECT_NAME})
-endif()
-
-set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}")
-if(WIN32)
-  set(append_library_dirs "${append_library_dirs}/$<CONFIG>")
-endif()
-
-ament_add_gtest(test_externally_defined_services test_externally_defined_services.cpp)
-ament_target_dependencies(test_externally_defined_services
-  "rcl"
-  "test_msgs"
-)
-target_link_libraries(test_externally_defined_services ${PROJECT_NAME})
-
-ament_add_gtest(test_duration test_duration.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_duration)
-  ament_target_dependencies(test_duration
-    "rcl")
-  target_link_libraries(test_duration ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_logger test_logger.cpp)
-target_link_libraries(test_logger ${PROJECT_NAME})
-
-ament_add_gmock(test_logging test_logging.cpp)
-target_link_libraries(test_logging ${PROJECT_NAME})
-
-ament_add_gtest(test_time test_time.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_time)
-  ament_target_dependencies(test_time
-    "rcl")
-  target_link_libraries(test_time ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_timer test_timer.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_timer)
-  ament_target_dependencies(test_timer
-    "rcl")
-  target_link_libraries(test_timer ${PROJECT_NAME} mimick)
-endif()
-
-ament_add_gtest(test_time_source test_time_source.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_time_source)
-  ament_target_dependencies(test_time_source
-    "rcl")
-  target_link_libraries(test_time_source ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_utilities test_utilities.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_utilities)
-  ament_target_dependencies(test_utilities
-    "rcl")
-  target_link_libraries(test_utilities ${PROJECT_NAME} mimick)
-endif()
-
-ament_add_gtest(test_init test_init.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_init)
-  ament_target_dependencies(test_init
-    "rcl")
-  target_link_libraries(test_init ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_interface_traits test_interface_traits.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_interface_traits)
-  ament_target_dependencies(test_interface_traits
-    "rcl")
-  target_link_libraries(test_interface_traits ${PROJECT_NAME})
-endif()
-
-# TODO(brawner) remove when destroying Node for Connext is resolved. See:
-# https://github.com/ros2/rclcpp/issues/1250
-ament_add_gtest(
-  test_executors
-  executors/test_executors.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}"
-  TIMEOUT 180)
-if(TARGET test_executors)
-  ament_target_dependencies(test_executors
-    "rcl"
-    "test_msgs")
-  target_link_libraries(test_executors ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_static_single_threaded_executor executors/test_static_single_threaded_executor.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_static_single_threaded_executor)
-  ament_target_dependencies(test_static_single_threaded_executor
-    "test_msgs")
-  target_link_libraries(test_static_single_threaded_executor ${PROJECT_NAME} mimick)
-endif()
-
-ament_add_gtest(test_multi_threaded_executor executors/test_multi_threaded_executor.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_multi_threaded_executor)
-  ament_target_dependencies(test_multi_threaded_executor
-    "rcl")
-  target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_static_executor_entities_collector executors/test_static_executor_entities_collector.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120)
-if(TARGET test_static_executor_entities_collector)
-  ament_target_dependencies(test_static_executor_entities_collector
-    "rcl"
-    "test_msgs")
-  target_link_libraries(test_static_executor_entities_collector ${PROJECT_NAME} mimick)
-endif()
-
-ament_add_gtest(test_guard_condition test_guard_condition.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_guard_condition)
-  target_link_libraries(test_guard_condition ${PROJECT_NAME} mimick)
-endif()
-
-ament_add_gtest(test_wait_set test_wait_set.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_wait_set)
-  ament_target_dependencies(test_wait_set "test_msgs")
-  target_link_libraries(test_wait_set ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_subscription_topic_statistics topic_statistics/test_subscription_topic_statistics.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}"
-)
-if(TARGET test_subscription_topic_statistics)
-  ament_target_dependencies(test_subscription_topic_statistics
-    "builtin_interfaces"
-    "libstatistics_collector"
-    "rcl_interfaces"
-    "rcutils"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "statistics_msgs"
-    "test_msgs")
-  rosidl_target_interfaces(test_subscription_topic_statistics ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp")
-  target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_subscription_options test_subscription_options.cpp)
-if(TARGET test_subscription_options)
-  ament_target_dependencies(test_subscription_options "rcl")
-  target_link_libraries(test_subscription_options ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_dynamic_storage wait_set_policies/test_dynamic_storage.cpp)
-if(TARGET test_dynamic_storage)
-  ament_target_dependencies(test_dynamic_storage "rcl" "test_msgs")
-  target_link_libraries(test_dynamic_storage ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_storage_policy_common wait_set_policies/test_storage_policy_common.cpp)
-if(TARGET test_storage_policy_common)
-  ament_target_dependencies(test_storage_policy_common "rcl" "test_msgs")
-  target_link_libraries(test_storage_policy_common ${PROJECT_NAME} mimick)
-endif()
-
-ament_add_gtest(test_static_storage wait_set_policies/test_static_storage.cpp)
-if(TARGET test_static_storage)
-  ament_target_dependencies(test_static_storage "rcl" "test_msgs")
-  target_link_libraries(test_static_storage ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_thread_safe_synchronization wait_set_policies/test_thread_safe_synchronization.cpp)
-if(TARGET test_thread_safe_synchronization)
-  ament_target_dependencies(test_thread_safe_synchronization "rcl" "test_msgs")
-  target_link_libraries(test_thread_safe_synchronization ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_rosout_qos test_rosout_qos.cpp)
-if(TARGET test_rosout_qos)
-  ament_target_dependencies(test_rosout_qos "rcl")
-  target_link_libraries(test_rosout_qos ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_executor test_executor.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}"
-  TIMEOUT 120)
-if(TARGET test_executor)
-  ament_target_dependencies(test_executor "rcl")
-  target_link_libraries(test_executor ${PROJECT_NAME} mimick)
-endif()
-
-ament_add_gtest(test_graph_listener test_graph_listener.cpp)
-if(TARGET test_graph_listener)
-  target_link_libraries(test_graph_listener ${PROJECT_NAME} mimick)
-endif()
--- a/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp
+++ b/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp
@@ -1,67 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <gtest/gtest.h>
-
-#include <memory>
-
-#include "rclcpp/allocator/allocator_common.hpp"
-
-TEST(TestAllocatorCommon, retyped_allocate) {
-  std::allocator<int> allocator;
-  void * untyped_allocator = &allocator;
-  void * allocated_mem =
-    rclcpp::allocator::retyped_allocate<std::allocator<char>>(1u, untyped_allocator);
-  ASSERT_NE(nullptr, allocated_mem);
-
-  auto code = [&untyped_allocator, allocated_mem]() {
-      rclcpp::allocator::retyped_deallocate<int, std::allocator<int>>(
-        allocated_mem, untyped_allocator);
-    };
-  EXPECT_NO_THROW(code());
-
-  allocated_mem = allocator.allocate(1);
-  ASSERT_NE(nullptr, allocated_mem);
-  void * reallocated_mem =
-    rclcpp::allocator::retyped_reallocate<int, std::allocator<int>>(
-    allocated_mem, 2u, untyped_allocator);
-  ASSERT_NE(nullptr, reallocated_mem);
-
-  auto code2 = [&untyped_allocator, reallocated_mem]() {
-      rclcpp::allocator::retyped_deallocate<int, std::allocator<int>>(
-        reallocated_mem, untyped_allocator);
-    };
-  EXPECT_NO_THROW(code2());
-}
-
-TEST(TestAllocatorCommon, get_rcl_allocator) {
-  std::allocator<int> allocator;
-  auto rcl_allocator = rclcpp::allocator::get_rcl_allocator<int>(allocator);
-  EXPECT_NE(nullptr, rcl_allocator.allocate);
-  EXPECT_NE(nullptr, rcl_allocator.deallocate);
-  EXPECT_NE(nullptr, rcl_allocator.reallocate);
-  EXPECT_NE(nullptr, rcl_allocator.zero_allocate);
-  // Not testing state as that may or may not be null depending on platform
-}
-
-TEST(TestAllocatorCommon, get_void_rcl_allocator) {
-  std::allocator<void> allocator;
-  auto rcl_allocator =
-    rclcpp::allocator::get_rcl_allocator<void, std::allocator<void>>(allocator);
-  EXPECT_NE(nullptr, rcl_allocator.allocate);
-  EXPECT_NE(nullptr, rcl_allocator.deallocate);
-  EXPECT_NE(nullptr, rcl_allocator.reallocate);
-  EXPECT_NE(nullptr, rcl_allocator.zero_allocate);
-  // Not testing state as that may or may not be null depending on platform
-}
--- a/rclcpp/test/rclcpp/allocator/test_allocator_deleter.cpp
+++ b/rclcpp/test/rclcpp/allocator/test_allocator_deleter.cpp
@@ -1,101 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <gtest/gtest.h>
-
-#include <memory>
-
-#include "rclcpp/allocator/allocator_deleter.hpp"
-
-#include "../../utils/rclcpp_gtest_macros.hpp"
-
-TEST(TestAllocatorDeleter, construct_destruct) {
-  std::allocator<int> allocator;
-
-  rclcpp::allocator::AllocatorDeleter<std::allocator<int>> deleter;
-  EXPECT_EQ(nullptr, deleter.get_allocator());
-  deleter.set_allocator(&allocator);
-  EXPECT_EQ(&allocator, deleter.get_allocator());
-
-  rclcpp::allocator::AllocatorDeleter<std::allocator<int>> deleter2(&allocator);
-  EXPECT_EQ(&allocator, deleter2.get_allocator());
-
-  rclcpp::allocator::AllocatorDeleter<std::allocator<int>> deleter3(deleter2);
-  EXPECT_EQ(&allocator, deleter3.get_allocator());
-}
-
-TEST(TestAllocatorDeleter, delete) {
-  std::allocator<int> allocator;
-  int * some_mem = allocator.allocate(1u);
-  ASSERT_NE(nullptr, some_mem);
-
-  rclcpp::allocator::AllocatorDeleter<std::allocator<int>> deleter(&allocator);
-  EXPECT_NO_THROW(deleter(some_mem));
-}
-
-TEST(TestAllocatorDeleter, set_allocator_for_deleter_AllocatorDeleter) {
-  using AllocatorT = std::allocator<int>;
-  using DeleterT = rclcpp::allocator::AllocatorDeleter<AllocatorT>;
-  AllocatorT allocator;
-  DeleterT deleter(&allocator);
-
-  std::allocator<int> allocator2;
-  rclcpp::allocator::set_allocator_for_deleter<AllocatorT, AllocatorT>(&deleter, &allocator2);
-  EXPECT_EQ(&allocator2, deleter.get_allocator());
-
-  auto throwing_statement = [&allocator]() {
-      DeleterT * null_del_ptr = nullptr;
-      rclcpp::allocator::set_allocator_for_deleter<AllocatorT, AllocatorT>(
-        null_del_ptr, &allocator);
-    };
-  RCLCPP_EXPECT_THROW_EQ(
-    throwing_statement(),
-    std::invalid_argument("Argument was NULL to set_allocator_for_deleter"));
-
-  auto throwing_statement2 = [&deleter]() {
-      AllocatorT * null_alloc_ptr = nullptr;
-      rclcpp::allocator::set_allocator_for_deleter<AllocatorT, AllocatorT>(
-        &deleter, null_alloc_ptr);
-    };
-
-  RCLCPP_EXPECT_THROW_EQ(
-    throwing_statement2(),
-    std::invalid_argument("Argument was NULL to set_allocator_for_deleter"));
-}
-
-TEST(TestAllocatorDeleter, set_allocator_for_deleter_std_default_delete) {
-  using AllocatorT = std::allocator<int>;
-  using DeleterT = std::default_delete<int>;
-  auto not_throwing_statement = []() {
-      AllocatorT allocator;
-      DeleterT deleter;
-      rclcpp::allocator::set_allocator_for_deleter<int, int>(&deleter, &allocator);
-    };
-  EXPECT_NO_THROW(not_throwing_statement());
-}
-
-TEST(TestAllocatorDeleter, set_allocator_for_deleter_unexpected_template) {
-  class SomeAllocatorClass {};
-  class SomeDeleterClass {};
-  using AllocatorT = SomeAllocatorClass;
-  using DeleterT = SomeDeleterClass;
-  auto throwing_statement = []() {
-      DeleterT deleter;
-      AllocatorT allocator;
-      rclcpp::allocator::set_allocator_for_deleter<AllocatorT, int, DeleterT>(&deleter, &allocator);
-    };
-  RCLCPP_EXPECT_THROW_EQ(
-    throwing_statement(),
-    std::runtime_error("Reached unexpected template specialization"));
-}
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Jacob Perron	dc528ad710	2.0.2	2020-07-07 21:07:12 -07:00
Dirk Thomas	26e824c7c0	link against thread library where necessary (#1210 ) (#1214 ) Signed-off-by: Dirk Thomas <dirk-thomas@users.noreply.github.com>	2020-07-07 20:34:33 -07:00
tomoya	3a3ba55fa2	fix exception message on rcl_clock_init (#1182 ) (#1193 ) * fix exception message on rcl_clock_init. Signed-off-by: Tomoya.Fujita <Tomoya.Fujita@sony.com> * error messages start with lower case. Signed-off-by: Tomoya.Fujita <Tomoya.Fujita@sony.com>	2020-06-25 09:45:13 -07:00
Jacob Perron	1745db6dcd	2.0.1	2020-06-23 17:32:15 -07:00
brawner	7ed387f862	Add create_publisher include to create_subscription (#1180 ) (#1192 ) Signed-off-by: Stephen Brawner <brawner@gmail.com>	2020-06-23 14:44:39 -07:00
Jacob Perron	a10ae56629	Fix get_node_time_source_interface() docstring (#988 ) (#1185 ) Signed-off-by: Jacob Perron <jacob@openrobotics.org>	2020-06-23 10:44:19 -07:00
Alejandro Hernández Cordero	1f000b8d97	Fixed doxygen warnings (#1163 ) (#1191 ) Signed-off-by: ahcorde <ahcorde@gmail.com>	2020-06-23 19:43:41 +02:00
Ivan Santiago Paunovic	c14f46e6f3	Check if context is valid when looping in spin_some (#1167 ) Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>	2020-06-17 09:52:04 -03:00
DongheeYe	70e1830ecd	Fix spin_until_future_complete: check spinning value (#1023 ) Signed-off-by: Donghee Ye <donghee.ye@samsung.com> Make Executor::spin_once_impl private before backporting, to avoid API compatibility issues Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>	2020-06-17 09:52:04 -03:00
Devin Bonnie	77564eb2ff	Add check for invalid topic statistics publish period (#1151 ) (#1172 ) * Add check for invalid topic statistics publish period Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Update documentation Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Address review comments Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Address doc formatting comments Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Update doc spacing Signed-off-by: Devin Bonnie <dbbonnie@amazon.com>	2020-06-15 10:25:17 -07:00