2.4.3

Signed-off-by: Dharini Dutia <dharini@openrobotics.org>

README.md

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

rclcpp/CHANGELOG.rst

@@ -2,6 +2,163 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.4.3 (2023-05-27)
+------------------
+* Do not attempt to use void allocators for memory allocation. (backport `#1657 <https://github.com/ros2/rclcpp/issues/1657>`_) (`#2004 <https://github.com/ros2/rclcpp/issues/2004>`_)
+* Contributors: Michel Hidalgo
+
+2.4.2 (2022-07-25)
+------------------
+* Add statistics for handle_loaned_message (`#1927 <https://github.com/ros2/rclcpp/issues/1927>`_) (`#1934 <https://github.com/ros2/rclcpp/issues/1934>`_)
+* Add test-dep ament_cmake_google_benchmark (`#1904 <https://github.com/ros2/rclcpp/issues/1904>`_) (`#1910 <https://github.com/ros2/rclcpp/issues/1910>`_)
+* Use parantheses around logging macro parameter (`#1820 <https://github.com/ros2/rclcpp/issues/1820>`_) (`#1823 <https://github.com/ros2/rclcpp/issues/1823>`_)
+* Contributors: Abrar Rahman Protyasha, Barry Xu, Gaël Écorchard
+
+2.4.1 (2022-01-31)
+------------------
+* Fix subscription instrumentation for ConstSharedPtr[WithInfo]Callback (`#1872 <https://github.com/ros2/rclcpp/issues/1872>`_)
+* Add node_waitables\_ to copy constructor (backport `#1799 <https://github.com/ros2/rclcpp/issues/1799>`_) (`#1834 <https://github.com/ros2/rclcpp/issues/1834>`_)
+* Fix returning invalid namespace if sub_namespace is empty (`#1658 <https://github.com/ros2/rclcpp/issues/1658>`_) (`#1811 <https://github.com/ros2/rclcpp/issues/1811>`_)
+* [service] Don't use a weak_ptr to avoid leaking (`#1668 <https://github.com/ros2/rclcpp/issues/1668>`_) (`#1669 <https://github.com/ros2/rclcpp/issues/1669>`_)
+* Use dynamic_pointer_cast to detect allocator mismatch in intra process manager (backport `#1643 <https://github.com/ros2/rclcpp/issues/1643>`_) (`#1645 <https://github.com/ros2/rclcpp/issues/1645>`_)
+* Contributors: Abrar Rahman Protyasha, Christophe Bedard, Ivan Santiago Paunovic, M. Hofstätter, Michel Hidalgo, Tomoya Fujita, William Woodall
+
+2.4.0 (2021-09-01)
+------------------
+* Guard against integer overflow in duration conversion (`#1584 <https://github.com/ros2/rclcpp/issues/1584>`_) (`#1761 <https://github.com/ros2/rclcpp/issues/1761>`_)
+* Update for checking correct variable (`#1534 <https://github.com/ros2/rclcpp/issues/1534>`_) (`#1760 <https://github.com/ros2/rclcpp/issues/1760>`_)
+* Fix SEGV caused by order of destruction of Node sub-interfaces (`#1469 <https://github.com/ros2/rclcpp/issues/1469>`_) (`#1736 <https://github.com/ros2/rclcpp/issues/1736>`_)
+* Add wait for message API (`#1705 <https://github.com/ros2/rclcpp/issues/1705>`_) (`#1737 <https://github.com/ros2/rclcpp/issues/1737>`_)
+* Fix documentation bug (`#1719 <https://github.com/ros2/rclcpp/issues/1719>`_) (`#1721 <https://github.com/ros2/rclcpp/issues/1721>`_)
+* Fix clock thread issue (`#1266 <https://github.com/ros2/rclcpp/issues/1266>`_) (`#1267 <https://github.com/ros2/rclcpp/issues/1267>`_) (`#1685 <https://github.com/ros2/rclcpp/issues/1685>`_)
+* Allow timers to keep up the intended rate in MultiThreadedExecutor `#1516 <https://github.com/ros2/rclcpp/issues/1516>`_ (`#1636 <https://github.com/ros2/rclcpp/issues/1636>`_)
+  Backports `#1516 <https://github.com/ros2/rclcpp/issues/1516>`_ and follow-up fix `#1628 <https://github.com/ros2/rclcpp/issues/1628>`_
+* Contributors: Chen Lihui, Colin MacKenzie, Daisuke Sato, Jacob Perron, Karsten Knese, Tomoya Fujita, hsgwa, William Woodall
+
+2.3.1 (2021-04-14)
+------------------
+* Update quality declaration links (re: `ros2/docs.ros2.org#52 <https://github.com/ros2/docs.ros2.org/issues/52>`_) (`#1616 <https://github.com/ros2/rclcpp/issues/1616>`_)
+* Fix documented example in create_publisher (`#1558 <https://github.com/ros2/rclcpp/issues/1558>`_) (`#1559 <https://github.com/ros2/rclcpp/issues/1559>`_)
+* Fix runtime error: reference binding to null pointer of type (`#1547 <https://github.com/ros2/rclcpp/issues/1547>`_) (`#1548 <https://github.com/ros2/rclcpp/issues/1548>`_)
+* Contributors: Jacob Perron, Simon Honigmann, Tomoya Fujita
+
+2.3.0 (2020-12-09)
+------------------
+* Update QD to QL 1 (`#1480 <https://github.com/ros2/rclcpp/issues/1480>`_)
+* Add performance tests for parameter transport (`#1470 <https://github.com/ros2/rclcpp/issues/1470>`_)
+* Add benchmarks for node parameters interface (`#1470 <https://github.com/ros2/rclcpp/issues/1470>`_)
+* Fix NodeOptions copy constructor (`#1376 <https://github.com/ros2/rclcpp/issues/1376>`_) (`#1451 <https://github.com/ros2/rclcpp/issues/1451>`_)
+* Avoid reference cycle to fix memory leak (`#1301 <https://github.com/ros2/rclcpp/issues/1301>`_) (`#1450 <https://github.com/ros2/rclcpp/issues/1450>`_)
+* Bump rclcpp packages to Quality Level 2 (`#1445 <https://github.com/ros2/rclcpp/issues/1445>`_) (`#1446 <https://github.com/ros2/rclcpp/issues/1446>`_)
+* Added executor benchmark tests (`#1413 <https://github.com/ros2/rclcpp/issues/1413>`_)
+* Add service and client benchmarks (`#1425 <https://github.com/ros2/rclcpp/issues/1425>`_)
+* Set CMakeLists to only use default rmw for benchmarks (`#1427 <https://github.com/ros2/rclcpp/issues/1427>`_)
+* Initial benchmark tests for rclcpp::init/shutdown create/destroy node (`#1411 <https://github.com/ros2/rclcpp/issues/1411>`_)
+* Use global namespace for parameter events subscription topic (`#1257 <https://github.com/ros2/rclcpp/issues/1257>`_) (`#1261 <https://github.com/ros2/rclcpp/issues/1261>`_)
+* Refactor test CMakeLists.txt (`#1422 <https://github.com/ros2/rclcpp/issues/1422>`_) and moving rosidl_generate_interfaces_call (`#1424 <https://github.com/ros2/rclcpp/issues/1424>`_)  (`#1437 <https://github.com/ros2/rclcpp/issues/1437>`_)
+* Update tracetools' QL in rclcpp's QD (`#1428 <https://github.com/ros2/rclcpp/issues/1428>`_) (`#1430 <https://github.com/ros2/rclcpp/issues/1430>`_)
+* Add coverage statement (`#1367 <https://github.com/ros2/rclcpp/issues/1367>`_)
+* Update tracetools' QL to 2 in rclcpp's QD (`#1187 <https://github.com/ros2/rclcpp/issues/1187>`_)
+* Fix reference to rclcpp in its QD (`#1161 <https://github.com/ros2/rclcpp/issues/1161>`_)
+* Clear members for StaticExecutorEntitiesCollector to avoid shared_ptr dependency (`#1303 <https://github.com/ros2/rclcpp/issues/1303>`_)
+* Increase test timeouts of slow running tests with rmw_connext_cpp (`#1400 <https://github.com/ros2/rclcpp/issues/1400>`_)
+* Make sure to clean the external client/service handle. (`#1296 <https://github.com/ros2/rclcpp/issues/1296>`_)
+* Increase coverage of WaitSetTemplate (`#1368 <https://github.com/ros2/rclcpp/issues/1368>`_)
+* Increase coverage of guard_condition.cpp to 100% (`#1369 <https://github.com/ros2/rclcpp/issues/1369>`_)
+* Tests for LoanedMessage with mocked loaned message publisher (`#1366 <https://github.com/ros2/rclcpp/issues/1366>`_)
+* Add unit tests for qos and qos_event files (`#1352 <https://github.com/ros2/rclcpp/issues/1352>`_)
+* Finish coverage of publisher API (`#1365 <https://github.com/ros2/rclcpp/issues/1365>`_)
+* Finish API coverage on executors (`#1364 <https://github.com/ros2/rclcpp/issues/1364>`_)
+* Only exchange intra_process waitable if nonnull (`#1317 <https://github.com/ros2/rclcpp/issues/1317>`_)
+* Add test for ParameterService (`#1355 <https://github.com/ros2/rclcpp/issues/1355>`_)
+* Add time API coverage tests (`#1347 <https://github.com/ros2/rclcpp/issues/1347>`_)
+* Add timer coverage tests (`#1363 <https://github.com/ros2/rclcpp/issues/1363>`_)
+* Add in additional tests for parameter_client.cpp coverage.
+* Minor fixes to the parameter_service.cpp file.
+* Improve test publisher - zero qos history depth value exception (`#1360 <https://github.com/ros2/rclcpp/issues/1360>`_)
+* Covered resolve_use_intra_process (`#1359 <https://github.com/ros2/rclcpp/issues/1359>`_)
+* Improve test_subscription_options (`#1358 <https://github.com/ros2/rclcpp/issues/1358>`_)
+* Add in more tests for init_options coverage (`#1353 <https://github.com/ros2/rclcpp/issues/1353>`_)
+* Test the remaining node public API (`#1342 <https://github.com/ros2/rclcpp/issues/1342>`_)
+* Complete coverage of Parameter and ParameterValue API (`#1344 <https://github.com/ros2/rclcpp/issues/1344>`_)
+* Add in more tests for the utilities (`#1349 <https://github.com/ros2/rclcpp/issues/1349>`_)
+* Add in two more tests for expand_topic_or_service_name (`#1350 <https://github.com/ros2/rclcpp/issues/1350>`_)
+* Add tests for node_options API (`#1343 <https://github.com/ros2/rclcpp/issues/1343>`_)
+* Add in more coverage for expand_topic_or_service_name. (`#1346 <https://github.com/ros2/rclcpp/issues/1346>`_)
+* Add coverage tests graph_listener (`#1330 <https://github.com/ros2/rclcpp/issues/1330>`_)
+* Add executor unit tests `#1336 <https://github.com/ros2/rclcpp/issues/1336>`_ (`#1395 <https://github.com/ros2/rclcpp/issues/1395>`_)
+* Add coverage for client API (`#1329 <https://github.com/ros2/rclcpp/issues/1329>`_)
+* Increase service coverage (`#1332 <https://github.com/ros2/rclcpp/issues/1332>`_)
+* Add ostream test for FutureReturnCode (`#1327 <https://github.com/ros2/rclcpp/issues/1327>`_) (`#1393 <https://github.com/ros2/rclcpp/issues/1393>`_)
+* Increase coverage of publisher/subscription API (`#1325 <https://github.com/ros2/rclcpp/issues/1325>`_)
+* Add coverage for missing API (except executors) (`#1326 <https://github.com/ros2/rclcpp/issues/1326>`_)
+* Add coverage tests context functions (`#1321 <https://github.com/ros2/rclcpp/issues/1321>`_)
+* Increase coverage of node_interfaces, including with mocking rcl errors (`#1322 <https://github.com/ros2/rclcpp/issues/1322>`_)
+* Add coverage for wait_set_policies (`#1316 <https://github.com/ros2/rclcpp/issues/1316>`_)
+* 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>`_)
+* Adding tests basic getters (`#1291 <https://github.com/ros2/rclcpp/issues/1291>`_)
+* Refactor Subscription Topic Statistics Tests (`#1281 <https://github.com/ros2/rclcpp/issues/1281>`_)
+* Fix topic stats test, wait for more messages, only check the ones with samples (`#1274 <https://github.com/ros2/rclcpp/issues/1274>`_)
+* Fixes for unit tests that fail under cyclonedds (`#1270 <https://github.com/ros2/rclcpp/issues/1270>`_)
+* initialize_logging\_ should be copied (`#1272 <https://github.com/ros2/rclcpp/issues/1272>`_)
+* Ability to configure domain_id via InitOptions (`#1165 <https://github.com/ros2/rclcpp/issues/1165>`_)
+* Simplify and fix allocator memory strategy unit test for connext (`#1252 <https://github.com/ros2/rclcpp/issues/1252>`_)
+* 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>`_)
+* Unittests for memory strategy files, except allocator_memory_strategy (`#1189 <https://github.com/ros2/rclcpp/issues/1189>`_)
+* EXPECT_THROW_EQ and ASSERT_THROW_EQ macros for unittests (`#1232 <https://github.com/ros2/rclcpp/issues/1232>`_)
+* Parameterize test executors for all executor types (`#1222 <https://github.com/ros2/rclcpp/issues/1222>`_) (`#1386 <https://github.com/ros2/rclcpp/issues/1386>`_)
+* Derive and throw exception in spin_some spin_all for StaticSingleThreadedExecutor (`#1385 <https://github.com/ros2/rclcpp/issues/1385>`_)
+* Add unit test for static_executor_entities_collector (`#1221 <https://github.com/ros2/rclcpp/issues/1221>`_)
+* Unit tests for allocator_memory_strategy.cpp part 2 (`#1198 <https://github.com/ros2/rclcpp/issues/1198>`_)
+* Unit tests for allocator_memory_strategy.hpp (`#1197 <https://github.com/ros2/rclcpp/issues/1197>`_)
+* Unit tests for node interfaces (`#1202 <https://github.com/ros2/rclcpp/issues/1202>`_)
+* Unit tests for some header-only functions/classes (`#1181 <https://github.com/ros2/rclcpp/issues/1181>`_)
+* Add unit tests for logging functionality (`#1184 <https://github.com/ros2/rclcpp/issues/1184>`_)
+* Fix rclcpp::NodeOptions::operator= (`#1211 <https://github.com/ros2/rclcpp/issues/1211>`_)
+* Check period duration in create_wall_timer (`#1178 <https://github.com/ros2/rclcpp/issues/1178>`_)
+* Throw exception if rcl_timer_init fails (`#1179 <https://github.com/ros2/rclcpp/issues/1179>`_)
+* Remove finalization of guard_condition from GraphListener::__shutdown() (`#1401 <https://github.com/ros2/rclcpp/issues/1401>`_)
+* Contributors: Alejandro Hernández Cordero, Chen Lihui, Chris Lalancette, Christophe Bedard, Devin Bonnie, Dirk Thomas, Jacob Perron, Jorge Perez, Louise Poubel, Michel Hidalgo, Scott K Logan, Stephen Brawner, tomoya
+
+2.2.0 (2020-10-07)
+------------------
+* Fix implementation of NodeOptions::use_global_arguments() (`#1176 <https://github.com/ros2/rclcpp/issues/1176>`_) (`#1372 <https://github.com/ros2/rclcpp/issues/1372>`_)
+* Fix conversion of negative durations to messages (`#1188 <https://github.com/ros2/rclcpp/issues/1188>`_) (`#1371 <https://github.com/ros2/rclcpp/issues/1371>`_)
+* Call vector.erase with end iterator overload (`#1314 <https://github.com/ros2/rclcpp/issues/1314>`_) (`#1380 <https://github.com/ros2/rclcpp/issues/1380>`_)
+* Check waitable for nullptr during constructor (`#1315 <https://github.com/ros2/rclcpp/issues/1315>`_) (`#1379 <https://github.com/ros2/rclcpp/issues/1379>`_)
+* Fix pub/sub count API tests. (`#1203 <https://github.com/ros2/rclcpp/issues/1203>`_) (`#1319 <https://github.com/ros2/rclcpp/issues/1319>`_)
+* Reorganize test directory and split CMakeLists.txt (`#1173 <https://github.com/ros2/rclcpp/issues/1173>`_) (`#1262 <https://github.com/ros2/rclcpp/issues/1262>`_)
+* Add operator!= for duration (`#1236 <https://github.com/ros2/rclcpp/issues/1236>`_) (`#1278 <https://github.com/ros2/rclcpp/issues/1278>`_)
+* Contributors: Ivan Santiago Paunovic, Jacob Perron, Jannik Abbenseth, Johannes Meyer, Michel Hidalgo, Stephen Brawner
+
+2.1.0 (2020-08-03)
+------------------
+* Warn about unused result of add_on_set_parameters_callback (`#1238 <https://github.com/ros2/rclcpp/issues/1238>`_) (`#1244 <https://github.com/ros2/rclcpp/issues/1244>`_)
+* Add missing RCLCPP_PUBLIC to ~StaticExecutorEntitiesCollector (`#1227 <https://github.com/ros2/rclcpp/issues/1227>`_) (`#1228 <https://github.com/ros2/rclcpp/issues/1228>`_)
+* Remove recreation of entities_collector (`#1217 <https://github.com/ros2/rclcpp/issues/1217>`_) (`#1224 <https://github.com/ros2/rclcpp/issues/1224>`_)
+* Contributors: Jacob Perron, brawner
+
+2.0.2 (2020-07-07)
+------------------
+* Link against thread library where necessary (`#1210 <https://github.com/ros2/rclcpp/issues/1210>`_) (`#1214 <https://github.com/ros2/rclcpp/issues/1214>`_)
+* Fix exception message on rcl_clock_init (`#1182 <https://github.com/ros2/rclcpp/issues/1182>`_) (`#1193 <https://github.com/ros2/rclcpp/issues/1193>`_)
+* Contributors: Dirk Thomas, tomoya
+
+2.0.1 (2020-06-23)
+------------------
+* Add create_publisher include to create_subscription (`#1180 <https://github.com/ros2/rclcpp/issues/1180>`_) (`#1192 <https://github.com/ros2/rclcpp/issues/1192>`_)
+* Fix get_node_time_source_interface() docstring (`#988 <https://github.com/ros2/rclcpp/issues/988>`_) (`#1185 <https://github.com/ros2/rclcpp/issues/1185>`_)
+* Fixed doxygen warnings (`#1163 <https://github.com/ros2/rclcpp/issues/1163>`_) (`#1191 <https://github.com/ros2/rclcpp/issues/1191>`_)
+* Check if context is valid when looping in spin_some (`#1167 <https://github.com/ros2/rclcpp/issues/1167>`_)
+* Fix spin_until_future_complete: check spinning value (`#1023 <https://github.com/ros2/rclcpp/issues/1023>`_)
+  Make Executor::spin_once_impl private before backporting, to avoid API compatibility issues
+* Add check for invalid topic statistics publish period (`#1151 <https://github.com/ros2/rclcpp/issues/1151>`_) (`#1172 <https://github.com/ros2/rclcpp/issues/1172>`_)
+* Contributors: Alejandro Hernández Cordero, Devin Bonnie, DongheeYe, Ivan Santiago Paunovic, Jacob Perron, Stephen Brawner
+
 2.0.0 (2020-06-01)
 ------------------
 * Added missing virtual destructors. (`#1149 <https://github.com/ros2/rclcpp/issues/1149>`_)

rclcpp/CMakeLists.txt

@@ -2,6 +2,8 @@ cmake_minimum_required(VERSION 3.5)
 
 project(rclcpp)
 
+find_package(Threads REQUIRED)
+
 find_package(ament_cmake_ros REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(libstatistics_collector REQUIRED)
@@ -23,7 +25,11 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
 endif()
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual)
+  # About -Wno-sign-conversion: With Clang, -Wconversion implies -Wsign-conversion. There are a number of
+  # implicit sign conversions in rclcpp and gtest.cc, see https://ci.ros2.org/job/ci_osx/9265/.
+  # Hence disabling -Wsign-conversion for now until all those have eventually been fixed.
+  # (from https://github.com/ros2/rclcpp/pull/1188#issuecomment-650229140)
+  add_compile_options(-Wall -Wextra -Wconversion -Wno-sign-conversion -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual)
 endif()
 
 set(${PROJECT_NAME}_SRCS
@@ -33,6 +39,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/clock.cpp
   src/rclcpp/context.cpp
   src/rclcpp/contexts/default_context.cpp
+  src/rclcpp/detail/mutex_two_priorities.cpp
   src/rclcpp/detail/rmw_implementation_specific_payload.cpp
   src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp
   src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp
@@ -166,6 +173,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>"
   "$<INSTALL_INTERFACE:include>")
+target_link_libraries(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
 # specific order: dependents before dependencies
 ament_target_dependencies(${PROJECT_NAME}
   "libstatistics_collector"
@@ -212,442 +220,10 @@ ament_export_dependencies(statistics_msgs)
 ament_export_dependencies(tracetools)
 
 if(BUILD_TESTING)
-  find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 
-  find_package(rmw_implementation_cmake REQUIRED)
-  find_package(rosidl_default_generators REQUIRED)
-
-  find_package(test_msgs REQUIRED)
-
-  include(cmake/rclcpp_add_build_failure_test.cmake)
-
-  add_definitions(-DTEST_RESOURCES_DIRECTORY="${CMAKE_CURRENT_BINARY_DIR}/test/resources")
-
-  rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs
-    test/msg/Header.msg
-    test/msg/MessageWithHeader.msg
-    DEPENDENCIES builtin_interfaces
-    LIBRARY_NAME ${PROJECT_NAME}
-    SKIP_INSTALL
-  )
-
-  ament_add_gtest(test_client test/test_client.cpp)
-  if(TARGET test_client)
-    ament_target_dependencies(test_client
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_client ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_create_timer test/test_create_timer.cpp)
-  if(TARGET test_create_timer)
-    ament_target_dependencies(test_create_timer
-      "rcl_interfaces"
-      "rmw"
-      "rcl"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_create_timer ${PROJECT_NAME})
-    target_include_directories(test_create_timer PRIVATE test/)
-  endif()
-  ament_add_gtest(test_expand_topic_or_service_name test/test_expand_topic_or_service_name.cpp)
-  if(TARGET test_expand_topic_or_service_name)
-    ament_target_dependencies(test_expand_topic_or_service_name
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_function_traits test/test_function_traits.cpp)
-  if(TARGET test_function_traits)
-    target_include_directories(test_function_traits PUBLIC include)
-    ament_target_dependencies(test_function_traits
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-  endif()
-  ament_add_gmock(test_intra_process_manager test/test_intra_process_manager.cpp)
-  if(TARGET test_intra_process_manager)
-    ament_target_dependencies(test_intra_process_manager
-      "rcl"
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_intra_process_manager ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_ring_buffer_implementation test/test_ring_buffer_implementation.cpp)
-  if(TARGET test_ring_buffer_implementation)
-    ament_target_dependencies(test_ring_buffer_implementation
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_ring_buffer_implementation ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_intra_process_buffer test/test_intra_process_buffer.cpp)
-  if(TARGET test_intra_process_buffer)
-    ament_target_dependencies(test_intra_process_buffer
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_intra_process_buffer ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_loaned_message test/test_loaned_message.cpp)
-  ament_target_dependencies(test_loaned_message
-    "test_msgs"
-  )
-  target_link_libraries(test_loaned_message ${PROJECT_NAME})
-
-  ament_add_gtest(test_node test/test_node.cpp TIMEOUT 240)
-  if(TARGET test_node)
-    ament_target_dependencies(test_node
-      "rcl_interfaces"
-      "rcpputils"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-      "test_msgs"
-    )
-    target_link_libraries(test_node ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_node_interfaces__get_node_interfaces
-    test/node_interfaces/test_get_node_interfaces.cpp)
-  if(TARGET test_node_interfaces__get_node_interfaces)
-    target_link_libraries(test_node_interfaces__get_node_interfaces ${PROJECT_NAME})
-  endif()
-
-  # TODO(wjwwood): reenable these build failure tests when I can get Jenkins to ignore their output
-  # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_rclcpp_node
-  #   test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp)
-  # target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node
-  #   ${PROJECT_NAME})
-
-  # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
-  #   test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp)
-  # target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
-  #   ${PROJECT_NAME})
-
-  # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_wrapped_node
-  #   test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp)
-  # target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node
-  #   ${PROJECT_NAME})
-
-  # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_wrapped_node
-  #   test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp)
-  # target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
-  #   ${PROJECT_NAME})
-
-  ament_add_gtest(test_node_global_args test/test_node_global_args.cpp)
-  if(TARGET test_node_global_args)
-    ament_target_dependencies(test_node_global_args
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_node_global_args ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_node_options test/test_node_options.cpp)
-  if(TARGET test_node_options)
-    ament_target_dependencies(test_node_options "rcl")
-    target_link_libraries(test_node_options ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_parameter_client test/test_parameter_client.cpp)
-  if(TARGET test_parameter_client)
-    ament_target_dependencies(test_parameter_client
-      "rcl_interfaces"
-    )
-    target_link_libraries(test_parameter_client ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_parameter_events_filter test/test_parameter_events_filter.cpp)
-  if(TARGET test_parameter_events_filter)
-    ament_target_dependencies(test_parameter_events_filter
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_parameter_events_filter ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_parameter test/test_parameter.cpp)
-  if(TARGET test_parameter)
-    ament_target_dependencies(test_parameter
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_parameter ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_parameter_map test/test_parameter_map.cpp)
-  if(TARGET test_parameter_map)
-    target_link_libraries(test_parameter_map ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_publisher test/test_publisher.cpp)
-  if(TARGET test_publisher)
-    ament_target_dependencies(test_publisher
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-      "test_msgs"
-    )
-    target_link_libraries(test_publisher ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_publisher_subscription_count_api test/test_publisher_subscription_count_api.cpp)
-  if(TARGET test_publisher_subscription_count_api)
-    ament_target_dependencies(test_publisher_subscription_count_api
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-      "test_msgs"
-    )
-    target_link_libraries(test_publisher_subscription_count_api ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_qos test/test_qos.cpp)
-  if(TARGET test_qos)
-    ament_target_dependencies(test_qos
-      "rmw"
-    )
-    target_link_libraries(test_qos
-      ${PROJECT_NAME}
-    )
-  endif()
-  ament_add_gtest(test_qos_event test/test_qos_event.cpp)
-  if(TARGET test_qos_event)
-    ament_target_dependencies(test_qos_event
-      "rmw"
-      "test_msgs"
-    )
-    target_link_libraries(test_qos_event
-      ${PROJECT_NAME}
-    )
-  endif()
-  ament_add_gtest(test_rate test/test_rate.cpp)
-  if(TARGET test_rate)
-    ament_target_dependencies(test_rate
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_rate
-      ${PROJECT_NAME}
-    )
-  endif()
-  ament_add_gtest(test_serialized_message_allocator test/test_serialized_message_allocator.cpp)
-  if(TARGET test_serialized_message_allocator)
-    ament_target_dependencies(test_serialized_message_allocator
-      test_msgs
-    )
-    target_link_libraries(test_serialized_message_allocator
-      ${PROJECT_NAME}
-    )
-  endif()
-  ament_add_gtest(test_serialized_message test/test_serialized_message.cpp)
-  if(TARGET test_serialized_message)
-    ament_target_dependencies(test_serialized_message
-      test_msgs
-    )
-    target_link_libraries(test_serialized_message
-      ${PROJECT_NAME}
-    )
-  endif()
-  ament_add_gtest(test_service test/test_service.cpp)
-  if(TARGET test_service)
-    ament_target_dependencies(test_service
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_service ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_subscription test/test_subscription.cpp)
-  if(TARGET test_subscription)
-    ament_target_dependencies(test_subscription
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-      "test_msgs"
-    )
-    target_link_libraries(test_subscription ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_subscription_publisher_count_api test/test_subscription_publisher_count_api.cpp)
-  if(TARGET test_subscription_publisher_count_api)
-    ament_target_dependencies(test_subscription_publisher_count_api
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-      "test_msgs"
-    )
-    target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_subscription_traits test/test_subscription_traits.cpp)
-  if(TARGET test_subscription_traits)
-    ament_target_dependencies(test_subscription_traits
-      "rcl"
-      "test_msgs"
-    )
-    target_link_libraries(test_subscription_traits ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_find_weak_nodes test/test_find_weak_nodes.cpp)
-  if(TARGET test_find_weak_nodes)
-    ament_target_dependencies(test_find_weak_nodes
-      "rcl"
-    )
-    target_link_libraries(test_find_weak_nodes ${PROJECT_NAME})
-  endif()
-
-  set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}")
-  if(WIN32)
-    set(append_library_dirs "${append_library_dirs}/$<CONFIG>")
-  endif()
-
-  ament_add_gtest(test_externally_defined_services test/test_externally_defined_services.cpp)
-  ament_target_dependencies(test_externally_defined_services
-    "rcl"
-    "test_msgs"
-  )
-  target_link_libraries(test_externally_defined_services ${PROJECT_NAME})
-
-  ament_add_gtest(test_duration test/test_duration.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_duration)
-    ament_target_dependencies(test_duration
-      "rcl")
-    target_link_libraries(test_duration ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_executor test/test_executor.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_executor)
-    ament_target_dependencies(test_executor
-      "rcl")
-    target_link_libraries(test_executor ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_logger test/test_logger.cpp)
-  target_link_libraries(test_logger ${PROJECT_NAME})
-
-  ament_add_gmock(test_logging test/test_logging.cpp)
-  target_link_libraries(test_logging ${PROJECT_NAME})
-
-  ament_add_gtest(test_time test/test_time.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_time)
-    ament_target_dependencies(test_time
-      "rcl")
-    target_link_libraries(test_time ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_timer test/test_timer.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_timer)
-    ament_target_dependencies(test_timer
-      "rcl")
-    target_link_libraries(test_timer ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_time_source test/test_time_source.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_time_source)
-    ament_target_dependencies(test_time_source
-      "rcl")
-    target_link_libraries(test_time_source ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_utilities test/test_utilities.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_utilities)
-    ament_target_dependencies(test_utilities
-      "rcl")
-    target_link_libraries(test_utilities ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_init test/test_init.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_init)
-    ament_target_dependencies(test_init
-      "rcl")
-    target_link_libraries(test_init ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_interface_traits test/test_interface_traits.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_interface_traits)
-    ament_target_dependencies(test_interface_traits
-      "rcl")
-    target_link_libraries(test_interface_traits ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_multi_threaded_executor test/executors/test_multi_threaded_executor.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_multi_threaded_executor)
-    ament_target_dependencies(test_multi_threaded_executor
-      "rcl")
-    target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_guard_condition test/test_guard_condition.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_guard_condition)
-    target_link_libraries(test_guard_condition ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_wait_set test/test_wait_set.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_wait_set)
-    ament_target_dependencies(test_wait_set "test_msgs")
-    target_link_libraries(test_wait_set ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_subscription_topic_statistics test/topic_statistics/test_subscription_topic_statistics.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}"
-  )
-  if(TARGET test_subscription_topic_statistics)
-    ament_target_dependencies(test_subscription_topic_statistics
-      "builtin_interfaces"
-      "libstatistics_collector"
-      "rcl_interfaces"
-      "rcutils"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-      "statistics_msgs"
-      "test_msgs")
-    rosidl_target_interfaces(test_subscription_topic_statistics ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp")
-    target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_subscription_options test/test_subscription_options.cpp)
-  if(TARGET test_subscription_options)
-    ament_target_dependencies(test_subscription_options "rcl")
-    target_link_libraries(test_subscription_options ${PROJECT_NAME})
-  endif()
-
-  # Install test resources
-  install(
-    DIRECTORY test/resources
-    DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test)
+  add_subdirectory(test)
 endif()
 
 ament_package()

rclcpp/QUALITY_DECLARATION.md

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

rclcpp/README.md

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

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -246,6 +246,16 @@ public:
         rclcpp_callback_register,
         (const void *)this,
         get_symbol(shared_ptr_with_info_callback_));
+    } else if (const_shared_ptr_callback_) {
+      TRACEPOINT(
+        rclcpp_callback_register,
+        (const void *)this,
+        get_symbol(const_shared_ptr_callback_));
+    } else if (const_shared_ptr_with_info_callback_) {
+      TRACEPOINT(
+        rclcpp_callback_register,
+        (const void *)this,
+        get_symbol(const_shared_ptr_with_info_callback_));
     } else if (unique_ptr_callback_) {
       TRACEPOINT(
         rclcpp_callback_register,

rclcpp/include/rclcpp/clock.hpp

@@ -115,9 +115,9 @@ public:
    *
    * Function is only applicable if the clock_type is `RCL_ROS_TIME`
    *
-   * \param pre_callback. Must be non-throwing
-   * \param post_callback. Must be non-throwing.
-   * \param threshold. Callbacks will be triggered if the time jump is greater
+   * \param pre_callback Must be non-throwing
+   * \param post_callback Must be non-throwing.
+   * \param threshold Callbacks will be triggered if the time jump is greater
    * then the threshold.
    * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
    * \throws std::bad_alloc if the allocation of the JumpHandler fails.

rclcpp/include/rclcpp/create_subscription.hpp

@@ -18,6 +18,7 @@
 #include <chrono>
 #include <functional>
 #include <memory>
+#include <stdexcept>
 #include <string>
 #include <utility>
 
@@ -28,6 +29,7 @@
 #include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
 
+#include "rclcpp/create_publisher.hpp"
 #include "rclcpp/create_timer.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/subscription_factory.hpp"
@@ -44,6 +46,23 @@ namespace rclcpp
  * The NodeT type only needs to have a method called get_node_topics_interface()
  * which returns a shared_ptr to a NodeTopicsInterface, or be a
  * NodeTopicsInterface pointer itself.
+ *
+ * \tparam MessageT
+ * \tparam CallbackT
+ * \tparam AllocatorT
+ * \tparam CallbackMessageT
+ * \tparam SubscriptionT
+ * \tparam MessageMemoryStrategyT
+ * \tparam NodeT
+ * \param node
+ * \param topic_name
+ * \param qos
+ * \param callback
+ * \param options
+ * \param msg_mem_strat
+ * \return the created subscription
+ * \throws std::invalid_argument if topic statistics is enabled and the publish period is
+ * less than or equal to zero.
  */
 template<
   typename MessageT,
@@ -81,6 +100,13 @@ create_subscription(
       options,
       *node_topics->get_node_base_interface()))
   {
+    if (options.topic_stats_options.publish_period <= std::chrono::milliseconds(0)) {
+      throw std::invalid_argument(
+              "topic_stats_options.publish_period must be greater than 0, specified value of " +
+              std::to_string(options.topic_stats_options.publish_period.count()) +
+              " ms");
+    }
+
     std::shared_ptr<Publisher<statistics_msgs::msg::MetricsMessage>> publisher =
       create_publisher<statistics_msgs::msg::MetricsMessage>(
       node,
@@ -91,8 +117,14 @@ create_subscription(
       rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>
       >(node_topics->get_node_base_interface()->get_name(), publisher);
 
-    auto sub_call_back = [subscription_topic_stats]() {
-        subscription_topic_stats->publish_message();
+    std::weak_ptr<
+      rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>
+    > weak_subscription_topic_stats(subscription_topic_stats);
+    auto sub_call_back = [weak_subscription_topic_stats]() {
+        auto subscription_topic_stats = weak_subscription_topic_stats.lock();
+        if (subscription_topic_stats) {
+          subscription_topic_stats->publish_message();
+        }
       };
 
     auto node_timer_interface = node_topics->get_node_timers_interface();

rclcpp/include/rclcpp/create_timer.hpp

@@ -76,14 +76,14 @@ create_timer(
  * \tparam DurationRepT
  * \tparam DurationT
  * \tparam CallbackT
- * \param period period to exectute callback
+ * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max()
  * \param callback callback to execute via the timer period
  * \param group
  * \param node_base
  * \param node_timers
  * \return
  * \throws std::invalid argument if either node_base or node_timers
- * are null
+ * are null, or period is negative or too large
  */
 template<typename DurationRepT, typename DurationT, typename CallbackT>
 typename rclcpp::WallTimer<CallbackT>::SharedPtr
@@ -102,10 +102,38 @@ create_wall_timer(
     throw std::invalid_argument{"input node_timers cannot be null"};
   }
 
+  if (period < std::chrono::duration<DurationRepT, DurationT>::zero()) {
+    throw std::invalid_argument{"timer period cannot be negative"};
+  }
+
+  // Casting to a double representation might lose precision and allow the check below to succeed
+  // but the actual cast to nanoseconds fail. Using 1 DurationT worth of nanoseconds less than max.
+  constexpr auto maximum_safe_cast_ns =
+    std::chrono::nanoseconds::max() - std::chrono::duration<DurationRepT, DurationT>(1);
+
+  // If period is greater than nanoseconds::max(), the duration_cast to nanoseconds will overflow
+  // a signed integer, which is undefined behavior. Checking whether any std::chrono::duration is
+  // greater than nanoseconds::max() is a difficult general problem. This is a more conservative
+  // version of Howard Hinnant's (the <chrono> guy>) response here:
+  // https://stackoverflow.com/a/44637334/2089061
+  // However, this doesn't solve the issue for all possible duration types of period.
+  // Follow-up issue: https://github.com/ros2/rclcpp/issues/1177
+  constexpr auto ns_max_as_double =
+    std::chrono::duration_cast<std::chrono::duration<double, std::chrono::nanoseconds::period>>(
+    maximum_safe_cast_ns);
+  if (period > ns_max_as_double) {
+    throw std::invalid_argument{
+            "timer period must be less than std::chrono::nanoseconds::max()"};
+  }
+
+  const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(period);
+  if (period_ns < std::chrono::nanoseconds::zero()) {
+    throw std::runtime_error{
+            "Casting timer period to nanoseconds resulted in integer overflow."};
+  }
+
   auto timer = rclcpp::WallTimer<CallbackT>::make_shared(
-    std::chrono::duration_cast<std::chrono::nanoseconds>(period),
-    std::move(callback),
-    node_base->get_context());
+    period_ns, std::move(callback), node_base->get_context());
   node_timers->add_timer(timer, group);
   return timer;
 }

rclcpp/include/rclcpp/detail/mutex_two_priorities.hpp

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

rclcpp/include/rclcpp/duration.hpp

@@ -72,11 +72,14 @@ public:
   operator=(const Duration & rhs);
 
   Duration &
-  operator=(const builtin_interfaces::msg::Duration & Duration_msg);
+  operator=(const builtin_interfaces::msg::Duration & duration_msg);
 
   bool
   operator==(const rclcpp::Duration & rhs) const;
 
+  bool
+  operator!=(const rclcpp::Duration & rhs) const;
+
   bool
   operator<(const rclcpp::Duration & rhs) const;
 

rclcpp/include/rclcpp/exceptions/exceptions.hpp

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

rclcpp/include/rclcpp/executor.hpp

@@ -37,6 +37,7 @@
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/scope_exit.hpp"
 
 namespace rclcpp
 {
@@ -212,9 +213,14 @@ public:
     }
     std::chrono::nanoseconds timeout_left = timeout_ns;
 
-    while (rclcpp::ok(this->context_)) {
+    if (spinning.exchange(true)) {
+      throw std::runtime_error("spin_until_future_complete() called while already spinning");
+    }
+    RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
+    while (rclcpp::ok(this->context_) && spinning.load()) {
       // Do one item of work.
-      spin_once(timeout_left);
+      spin_once_impl(timeout_left);
+
       // Check if the future is set, return SUCCESS if it is.
       status = future.wait_for(std::chrono::seconds(0));
       if (status == std::future_status::ready) {
@@ -338,6 +344,11 @@ protected:
 
   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);
 };
 
 namespace executor

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -22,6 +22,7 @@
 #include <thread>
 #include <unordered_map>
 
+#include "rclcpp/detail/mutex_two_priorities.hpp"
 #include "rclcpp/executor.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/memory_strategies.hpp"
@@ -49,6 +50,7 @@ public:
    * \param number_of_threads number of threads to have in the thread pool,
    *   the default 0 will use the number of cpu cores found instead
    * \param yield_before_execute if true std::this_thread::yield() is called
+   * \param timeout maximum time to wait
    */
   RCLCPP_PUBLIC
   MultiThreadedExecutor(
@@ -80,12 +82,17 @@ protected:
 private:
   RCLCPP_DISABLE_COPY(MultiThreadedExecutor)
 
-  std::mutex wait_mutex_;
+  std::mutex wait_mutex_;  // Unused. Leave it for ABI compatibility.
   size_t number_of_threads_;
   bool yield_before_execute_;
   std::chrono::nanoseconds next_exec_timeout_;
 
   std::set<TimerBase::SharedPtr> scheduled_timers_;
+  static std::unordered_map<MultiThreadedExecutor *,
+    std::shared_ptr<detail::MutexTwoPriorities>> wait_mutex_set_;
+  static std::mutex shared_wait_mutex_;
+  // These variables are declared as static variables for ABI-compatibiliity.
+  // And they mimic member variables needed to backport from master.
 };
 
 }  // namespace executors

rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp

@@ -45,6 +45,7 @@ public:
   StaticExecutorEntitiesCollector() = default;
 
   // Destructor
+  RCLCPP_PUBLIC
   ~StaticExecutorEntitiesCollector();
 
   /// Initialize StaticExecutorEntitiesCollector
@@ -61,6 +62,11 @@ public:
     rclcpp::memory_strategy::MemoryStrategy::SharedPtr & memory_strategy,
     rcl_guard_condition_t * executor_guard_condition);
 
+  /// Finalize StaticExecutorEntitiesCollector to clear resources
+  RCLCPP_PUBLIC
+  void
+  fini();
+
   RCLCPP_PUBLIC
   void
   execute() override;

rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp

@@ -162,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.

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

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

rclcpp/include/rclcpp/node.hpp

@@ -26,6 +26,8 @@
 #include <utility>
 #include <vector>
 
+#include "rcutils/macros.h"
+
 #include "rcl/error_handling.h"
 #include "rcl/node.h"
 
@@ -159,7 +161,7 @@ public:
    * pub = node->create_publisher<MsgT>("chatter", QoS(10));  // implicitly KeepLast
    * pub = node->create_publisher<MsgT>("chatter", QoS(KeepLast(10)));
    * pub = node->create_publisher<MsgT>("chatter", QoS(KeepAll()));
-   * pub = node->create_publisher<MsgT>("chatter", QoS(1).best_effort().volatile());
+   * pub = node->create_publisher<MsgT>("chatter", QoS(1).best_effort().durability_volatile());
    * {
    *   rclcpp::QoS custom_qos(KeepLast(10), rmw_qos_profile_sensor_data);
    *   pub = node->create_publisher<MsgT>("chatter", custom_qos);
@@ -235,7 +237,7 @@ public:
   /// Create and return a Client.
   /**
    * \param[in] service_name The topic to service on.
-   * \param[in] rmw_qos_profile_t Quality of service profile for client.
+   * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client.
    * \param[in] group Callback group to call the service.
    * \return Shared pointer to the created client.
    */
@@ -250,7 +252,7 @@ public:
   /**
    * \param[in] service_name The topic to service on.
    * \param[in] callback User-defined callback function.
-   * \param[in] rmw_qos_profile_t Quality of service profile for client.
+   * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client.
    * \param[in] group Callback group to call the service.
    * \return Shared pointer to the created service.
    */
@@ -812,6 +814,7 @@ public:
    * \throws std::bad_alloc if the allocation of the OnSetParametersCallbackHandle fails.
    */
   RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
   OnSetParametersCallbackHandle::SharedPtr
   add_on_set_parameters_callback(OnParametersSetCallbackType callback);
 
@@ -889,11 +892,15 @@ public:
   std::map<std::string, std::vector<std::string>>
   get_service_names_and_types() const;
 
-  /// Return the number of publishers that are advertised on a given topic.
+  /// Return a map of existing service names to list of service types for a specific node.
   /**
-   * \param[in] topic_name the topic_name on which to count the publishers.
-   * \return number of publishers that are advertised on a given topic.
-   * \throws std::runtime_error if publishers could not be counted
+   * This function only considers services - not clients.
+   * The returned names are the actual names used and do not have remap rules applied.
+   *
+   * \param[in] node_name name of the node.
+   * \param[in] namespace_ namespace of the node.
+   * \return a map of existing service names to list of service types.
+   * \throws std::runtime_error anything that rcl_error can throw.
    */
   RCLCPP_PUBLIC
   std::map<std::string, std::vector<std::string>>
@@ -1062,7 +1069,7 @@ public:
   rclcpp::node_interfaces::NodeParametersInterface::SharedPtr
   get_node_parameters_interface();
 
-  /// Return the Node's internal NodeParametersInterface implementation.
+  /// Return the Node's internal NodeTimeSourceInterface implementation.
   RCLCPP_PUBLIC
   rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr
   get_node_time_source_interface();

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -21,6 +21,8 @@
 #include <string>
 #include <vector>
 
+#include "rcutils/macros.h"
+
 #include "rcl_interfaces/msg/list_parameters_result.hpp"
 #include "rcl_interfaces/msg/parameter_descriptor.hpp"
 #include "rcl_interfaces/msg/parameter_event.hpp"
@@ -160,6 +162,7 @@ public:
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const override;
 
   RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
   OnSetParametersCallbackHandle::SharedPtr
   add_on_set_parameters_callback(OnParametersSetCallbackType callback) override;
 

rclcpp/include/rclcpp/parameter_client.hpp

@@ -48,13 +48,13 @@ public:
 
   /// Create an async parameters client.
   /**
-   * \param node_base_interface[in] The node base interface of the corresponding node.
-   * \param node_topics_interface[in] Node topic base interface.
-   * \param node_graph_interface[in] The node graph interface of the corresponding node.
-   * \param node_services_interface[in] Node service interface.
-   * \param remote_node_name[in] (optional) name of the remote node
-   * \param qos_profile[in] (optional) The rmw qos profile to use to subscribe
-   * \param group[in] (optional)  The async parameter client will be added to this callback group.
+   * \param[in] node_base_interface The node base interface of the corresponding node.
+   * \param[in] node_topics_interface Node topic base interface.
+   * \param[in] node_graph_interface The node graph interface of the corresponding node.
+   * \param[in] node_services_interface Node service interface.
+   * \param[in] remote_node_name (optional) name of the remote node
+   * \param[in] qos_profile (optional) The rmw qos profile to use to subscribe
+   * \param[in] group (optional) The async parameter client will be added to this callback group.
    */
   RCLCPP_PUBLIC
   AsyncParametersClient(
@@ -68,10 +68,10 @@ public:
 
   /// Constructor
   /**
-   * \param node[in] The  async paramters client will be added to this node.
-   * \param remote_node_name[in] (optional) name of the remote node
-   * \param qos_profile[in] (optional) The rmw qos profile to use to subscribe
-   * \param group[in] (optional)  The async parameter client will be added to this callback group.
+   * \param[in] node The async paramters client will be added to this node.
+   * \param[in] remote_node_name (optional) name of the remote node
+   * \param[in] qos_profile (optional) The rmw qos profile to use to subscribe
+   * \param[in] group (optional) The async parameter client will be added to this callback group.
    */
   RCLCPP_PUBLIC
   AsyncParametersClient(
@@ -82,10 +82,10 @@ public:
 
   /// Constructor
   /**
-   * \param node[in] The  async paramters client will be added to this node.
-   * \param remote_node_name[in] (optional) name of the remote node
-   * \param qos_profile[in] (optional) The rmw qos profile to use to subscribe
-   * \param group[in] (optional)  The async parameter client will be added to this callback group.
+   * \param[in] node The  async paramters client will be added to this node.
+   * \param[in] remote_node_name (optional) name of the remote node
+   * \param[in] qos_profile (optional) The rmw qos profile to use to subscribe
+   * \param[in] group (optional) The async parameter client will be added to this callback group.
    */
   RCLCPP_PUBLIC
   AsyncParametersClient(
@@ -177,7 +177,7 @@ public:
   {
     return rclcpp::create_subscription<rcl_interfaces::msg::ParameterEvent>(
       node,
-      "parameter_events",
+      "/parameter_events",
       qos,
       std::forward<CallbackT>(callback),
       options);

rclcpp/include/rclcpp/publisher_options.hpp

@@ -72,10 +72,7 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
   to_rcl_publisher_options(const rclcpp::QoS & qos) const
   {
     rcl_publisher_options_t result = rcl_publisher_get_default_options();
-    using AllocatorTraits = std::allocator_traits<Allocator>;
-    using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
-    auto message_alloc = std::make_shared<MessageAllocatorT>(*this->get_allocator().get());
-    result.allocator = rclcpp::allocator::get_rcl_allocator<MessageT>(*message_alloc);
+    result.allocator = this->get_rcl_allocator();
     result.qos = qos.get_rmw_qos_profile();
 
     // Apply payload to rcl_publisher_options if necessary.
@@ -92,15 +89,32 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
   get_allocator() const
   {
     if (!this->allocator) {
-      // TODO(wjwwood): I would like to use the commented line instead, but
-      //   cppcheck 1.89 fails with:
-      //     Syntax Error: AST broken, binary operator '>' doesn't have two operands.
-      // return std::make_shared<Allocator>();
-      std::shared_ptr<Allocator> tmp(new Allocator());
-      return tmp;
+      return std::make_shared<Allocator>();
     }
     return this->allocator;
   }
+
+private:
+  using PlainAllocator =
+    typename std::allocator_traits<Allocator>::template rebind_alloc<char>;
+
+  rcl_allocator_t
+  get_rcl_allocator() const
+  {
+    if (!plain_allocator_storage_) {
+      plain_allocator_storage_ =
+        std::make_shared<PlainAllocator>(*this->get_allocator());
+    }
+    return rclcpp::allocator::get_rcl_allocator<char>(*plain_allocator_storage_);
+  }
+
+  // This is a temporal workaround, to make sure that get_allocator()
+  // always returns a copy of the same allocator.
+  mutable std::shared_ptr<Allocator> allocator_storage_;
+
+  // This is a temporal workaround, to keep the plain allocator that backs
+  // up the rcl allocator returned in rcl_publisher_options_t alive.
+  mutable std::shared_ptr<PlainAllocator> plain_allocator_storage_;
 };
 
 using PublisherOptions = PublisherOptionsWithAllocator<std::allocator<void>>;

rclcpp/include/rclcpp/qos.hpp

@@ -26,6 +26,7 @@
 namespace rclcpp
 {
 
+RCLCPP_PUBLIC
 std::string qos_policy_name_from_kind(rmw_qos_policy_kind_t policy_kind);
 
 /// QoS initialization values, cannot be created directly, use KeepAll or KeepLast instead.

rclcpp/include/rclcpp/serialization.hpp

@@ -60,7 +60,7 @@ public:
 
   /// Serialize a ROS2 message to a serialized stream
   /**
-   * \param[in] message The ROS2 message which is read and serialized by rmw.
+   * \param[in] ros_message The ROS2 message which is read and serialized by rmw.
    * \param[out] serialized_message The serialized message.
    */
   void serialize_message(
@@ -69,7 +69,7 @@ public:
   /// Deserialize a serialized stream to a ROS message
   /**
    * \param[in] serialized_message The serialized message to be converted to ROS2 by rmw.
-   * \param[out] message The deserialized ROS2 message.
+   * \param[out] ros_message The deserialized ROS2 message.
    */
   void deserialize_message(
     const SerializedMessage * serialized_message, void * ros_message) const;

rclcpp/include/rclcpp/service.hpp

@@ -177,25 +177,16 @@ public:
     using rosidl_typesupport_cpp::get_service_type_support_handle;
     auto service_type_support_handle = get_service_type_support_handle<ServiceT>();
 
-    std::weak_ptr<rcl_node_t> weak_node_handle(node_handle_);
     // rcl does the static memory allocation here
     service_handle_ = std::shared_ptr<rcl_service_t>(
-      new rcl_service_t, [weak_node_handle](rcl_service_t * service)
+      new rcl_service_t, [handle = node_handle_](rcl_service_t * service)
       {
-        auto handle = weak_node_handle.lock();
-        if (handle) {
-          if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) {
-            RCLCPP_ERROR(
-              rclcpp::get_node_logger(handle.get()).get_child("rclcpp"),
-              "Error in destruction of rcl service handle: %s",
-              rcl_get_error_string().str);
-            rcl_reset_error();
-          }
-        } else {
+        if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) {
           RCLCPP_ERROR(
-            rclcpp::get_logger("rclcpp"),
-            "Error in destruction of rcl service handle: "
-            "the Node Handle was destructed too early. You will leak memory");
+            rclcpp::get_node_logger(handle.get()).get_child("rclcpp"),
+            "Error in destruction of rcl service handle: %s",
+            rcl_get_error_string().str);
+          rcl_reset_error();
         }
         delete service;
       });
@@ -237,7 +228,7 @@ public:
    * rclcpp::create_service().
    *
    * \param[in] node_handle NodeBaseInterface pointer that is used in part of the setup.
-   * \param[in] service_name Name of the topic to publish to.
+   * \param[in] service_handle service handle.
    * \param[in] any_callback User defined callback to call when a client request is received.
    */
   Service(
@@ -272,7 +263,7 @@ public:
    * rclcpp::create_service().
    *
    * \param[in] node_handle NodeBaseInterface pointer that is used in part of the setup.
-   * \param[in] service_name Name of the topic to publish to.
+   * \param[in] service_handle service handle.
    * \param[in] any_callback User defined callback to call when a client request is received.
    */
   Service(

rclcpp/include/rclcpp/subscription.hpp

@@ -287,11 +287,31 @@ public:
     void * loaned_message,
     const rclcpp::MessageInfo & message_info) override
   {
+    if (matches_any_intra_process_publishers(&message_info.get_rmw_message_info().publisher_gid)) {
+      // In this case, the message will be delivered via intra process and
+      // we should ignore this copy of the message.
+      return;
+    }
+
     auto typed_message = static_cast<CallbackMessageT *>(loaned_message);
     // message is loaned, so we have to make sure that the deleter does not deallocate the message
     auto sptr = std::shared_ptr<CallbackMessageT>(
       typed_message, [](CallbackMessageT * msg) {(void) msg;});
+
+    std::chrono::time_point<std::chrono::system_clock> now;
+    if (subscription_topic_statistics_) {
+      // get current time before executing callback to
+      // exclude callback duration from topic statistics result.
+      now = std::chrono::system_clock::now();
+    }
+
     any_callback_.dispatch(sptr, message_info);
+
+    if (subscription_topic_statistics_) {
+      const auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(now);
+      const auto time = rclcpp::Time(nanos.time_since_epoch().count());
+      subscription_topic_statistics_->handle_message(*typed_message, time);
+    }
   }
 
   /// Return the borrowed message.

rclcpp/include/rclcpp/subscription_factory.hpp

@@ -68,7 +68,7 @@ struct SubscriptionFactory
  * \param[in] callback The user-defined callback function to receive a message
  * \param[in] options Additional options for the creation of the Subscription.
  * \param[in] msg_mem_strat The message memory strategy to use for allocating messages.
- * \param[in] subscription_topic_Optinal stats callback for topic_statistics
+ * \param[in] subscription_topic_stats Optional stats callback for topic_statistics
  */
 template<
   typename MessageT,

rclcpp/include/rclcpp/subscription_options.hpp

@@ -66,7 +66,8 @@ struct SubscriptionOptionsBase
     // Topic to which topic statistics get published when enabled. Defaults to /statistics.
     std::string publish_topic = "/statistics";
 
-    // Topic statistics publication period in ms. Defaults to one minute.
+    // Topic statistics publication period in ms. Defaults to one second.
+    // Only values greater than zero are allowed.
     std::chrono::milliseconds publish_period{std::chrono::seconds(1)};
   };
 
@@ -98,10 +99,7 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
   to_rcl_subscription_options(const rclcpp::QoS & qos) const
   {
     rcl_subscription_options_t result = rcl_subscription_get_default_options();
-    using AllocatorTraits = std::allocator_traits<Allocator>;
-    using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
-    auto message_alloc = std::make_shared<MessageAllocatorT>(*allocator.get());
-    result.allocator = allocator::get_rcl_allocator<MessageT>(*message_alloc);
+    result.allocator = this->get_rcl_allocator();
     result.qos = qos.get_rmw_qos_profile();
     result.rmw_subscription_options.ignore_local_publications = this->ignore_local_publications;
 
@@ -122,6 +120,28 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
     }
     return this->allocator;
   }
+
+private:
+  using PlainAllocator =
+    typename std::allocator_traits<Allocator>::template rebind_alloc<char>;
+
+  rcl_allocator_t
+  get_rcl_allocator() const
+  {
+    if (!plain_allocator_storage_) {
+      plain_allocator_storage_ =
+        std::make_shared<PlainAllocator>(*this->get_allocator());
+    }
+    return rclcpp::allocator::get_rcl_allocator<char>(*plain_allocator_storage_);
+  }
+
+  // This is a temporal workaround, to make sure that get_allocator()
+  // always returns a copy of the same allocator.
+  mutable std::shared_ptr<Allocator> allocator_storage_;
+
+  // This is a temporal workaround, to keep the plain allocator that backs
+  // up the rcl allocator returned in rcl_subscription_options_t alive.
+  mutable std::shared_ptr<PlainAllocator> plain_allocator_storage_;
 };
 
 using SubscriptionOptions = SubscriptionOptionsWithAllocator<std::allocator<void>>;

rclcpp/include/rclcpp/time.hpp

@@ -51,7 +51,7 @@ public:
    * \param clock_type clock type
    */
   RCLCPP_PUBLIC
-  explicit Time(int64_t nanoseconds = 0, rcl_clock_type_t clock = RCL_SYSTEM_TIME);
+  explicit Time(int64_t nanoseconds = 0, rcl_clock_type_t clock_type = RCL_SYSTEM_TIME);
 
   /// Copy constructor
   RCLCPP_PUBLIC
@@ -66,12 +66,11 @@ public:
   RCLCPP_PUBLIC
   Time(
     const builtin_interfaces::msg::Time & time_msg,
-    rcl_clock_type_t ros_time = RCL_ROS_TIME);
+    rcl_clock_type_t clock_type = RCL_ROS_TIME);
 
   /// Time constructor
   /**
    * \param time_point rcl_time_point_t structure to copy
-   * \param clock_type clock type
    */
   RCLCPP_PUBLIC
   explicit Time(const rcl_time_point_t & time_point);
@@ -91,6 +90,12 @@ public:
   Time &
   operator=(const Time & rhs);
 
+  /**
+   * Assign Time from a builtin_interfaces::msg::Time instance.
+   * The clock_type will be reset to RCL_ROS_TIME.
+   * Equivalent to *this = Time(time_msg, RCL_ROS_TIME).
+   * \throws std::runtime_error if seconds are negative
+   */
   RCLCPP_PUBLIC
   Time &
   operator=(const builtin_interfaces::msg::Time & time_msg);
@@ -213,6 +218,7 @@ private:
 /**
  * \throws std::overflow_error if addition leads to overflow
  */
+RCLCPP_PUBLIC
 Time
 operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs);
 

rclcpp/include/rclcpp/time_source.hpp

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

rclcpp/include/rclcpp/timer.hpp

@@ -166,6 +166,7 @@ public:
    * \param[in] clock The clock providing the current time.
    * \param[in] period The interval at which the timer fires.
    * \param[in] callback User-specified callback function.
+   * \param[in] context custom context to be used.
    */
   explicit GenericTimer(
     Clock::SharedPtr clock, std::chrono::nanoseconds period, FunctorT && callback,

rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp

@@ -116,7 +116,7 @@ public:
 
   /// Set the timer used to publish statistics messages.
   /**
-   * \param measurement_timer the timer to fire the publisher, created by the node
+   * \param publisher_timer the timer to fire the publisher, created by the node
    */
   void set_publisher_timer(rclcpp::TimerBase::SharedPtr publisher_timer)
   {

rclcpp/include/rclcpp/wait_for_message.hpp

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

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

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

rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp

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

rclcpp/include/rclcpp/wait_set_template.hpp

@@ -61,6 +61,8 @@ public:
    * \param[in] subscriptions Vector of subscriptions to be added.
    * \param[in] guard_conditions Vector of guard conditions to be added.
    * \param[in] timers Vector of timers to be added.
+   * \param[in] clients Vector of clients and their associated entity to be added.
+   * \param[in] services Vector of services and their associated entity to be added.
    * \param[in] waitables Vector of waitables and their associated entity to be added.
    * \param[in] context Custom context to be used, defaults to global default.
    * \throws std::invalid_argument If context is nullptr.
@@ -231,9 +233,9 @@ public:
         }
         if (mask.include_intra_process_waitable) {
           auto local_waitable = inner_subscription->get_intra_process_waitable();
-          inner_subscription->exchange_in_use_by_wait_set_state(local_waitable.get(), false);
           if (nullptr != local_waitable) {
-            // This is the case when intra process is disabled for the subscription.
+            // This is the case when intra process is enabled for the subscription.
+            inner_subscription->exchange_in_use_by_wait_set_state(local_waitable.get(), false);
             this->storage_remove_waitable(std::move(local_waitable));
           }
         }

rclcpp/include/rclcpp/waitable.hpp

@@ -123,7 +123,7 @@ public:
   RCLCPP_PUBLIC
   virtual
   bool
-  is_ready(rcl_wait_set_t *) = 0;
+  is_ready(rcl_wait_set_t * wait_set) = 0;
 
   /// Execute any entities of the Waitable that are ready.
   /**

rclcpp/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp</name>
-  <version>2.0.0</version>
+  <version>2.4.3</version>
   <description>The ROS client library in C++.</description>
   <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
   <license>Apache License 2.0</license>
@@ -32,9 +32,12 @@
   <depend>tracetools</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>ament_cmake_google_benchmark</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>mimick_vendor</test_depend>
+  <test_depend>performance_test_fixture</test_depend>
   <test_depend>rmw</test_depend>
   <test_depend>rmw_implementation_cmake</test_depend>
   <test_depend>rosidl_default_generators</test_depend>

rclcpp/resource/logging.hpp.em

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

rclcpp/src/rclcpp/clock.cpp

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

rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp

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

rclcpp/src/rclcpp/duration.cpp

@@ -47,17 +47,14 @@ Duration::Duration(std::chrono::nanoseconds nanoseconds)
   rcl_duration_.nanoseconds = nanoseconds.count();
 }
 
-Duration::Duration(const Duration & rhs)
-{
-  rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds;
-}
+Duration::Duration(const Duration & rhs) = default;
 
 Duration::Duration(
   const builtin_interfaces::msg::Duration & duration_msg)
 {
   rcl_duration_.nanoseconds =
-    static_cast<rcl_duration_value_t>(RCL_S_TO_NS(duration_msg.sec));
-  rcl_duration_.nanoseconds += duration_msg.nanosec;
+    RCL_S_TO_NS(static_cast<rcl_duration_value_t>(duration_msg.sec));
+  rcl_duration_.nanoseconds += static_cast<rcl_duration_value_t>(duration_msg.nanosec);
 }
 
 Duration::Duration(const rcl_duration_t & duration)
@@ -69,24 +66,39 @@ Duration::Duration(const rcl_duration_t & duration)
 Duration::operator builtin_interfaces::msg::Duration() const
 {
   builtin_interfaces::msg::Duration msg_duration;
-  msg_duration.sec = static_cast<std::int32_t>(RCL_NS_TO_S(rcl_duration_.nanoseconds));
-  msg_duration.nanosec =
-    static_cast<std::uint32_t>(rcl_duration_.nanoseconds % (1000 * 1000 * 1000));
+  constexpr rcl_duration_value_t kDivisor = RCL_S_TO_NS(1);
+  constexpr int32_t max_s = std::numeric_limits<int32_t>::max();
+  constexpr int32_t min_s = std::numeric_limits<int32_t>::min();
+  constexpr uint32_t max_ns = std::numeric_limits<uint32_t>::max();
+  const auto result = std::div(rcl_duration_.nanoseconds, kDivisor);
+  if (result.rem >= 0) {
+    // saturate if we will overflow
+    if (result.quot > max_s) {
+      msg_duration.sec = max_s;
+      msg_duration.nanosec = max_ns;
+    } else {
+      msg_duration.sec = static_cast<int32_t>(result.quot);
+      msg_duration.nanosec = static_cast<uint32_t>(result.rem);
+    }
+  } else {
+    if (result.quot <= min_s) {
+      msg_duration.sec = min_s;
+      msg_duration.nanosec = 0u;
+    } else {
+      msg_duration.sec = static_cast<int32_t>(result.quot - 1);
+      msg_duration.nanosec = static_cast<uint32_t>(kDivisor + result.rem);
+    }
+  }
   return msg_duration;
 }
 
 Duration &
-Duration::operator=(const Duration & rhs)
-{
-  rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds;
-  return *this;
-}
+Duration::operator=(const Duration & rhs) = default;
 
 Duration &
 Duration::operator=(const builtin_interfaces::msg::Duration & duration_msg)
 {
-  rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast<int64_t>(duration_msg.sec));
-  rcl_duration_.nanoseconds += duration_msg.nanosec;
+  *this = Duration(duration_msg);
   return *this;
 }
 
@@ -96,6 +108,12 @@ Duration::operator==(const rclcpp::Duration & rhs) const
   return rcl_duration_.nanoseconds == rhs.rcl_duration_.nanoseconds;
 }
 
+bool
+Duration::operator!=(const rclcpp::Duration & rhs) const
+{
+  return rcl_duration_.nanoseconds != rhs.rcl_duration_.nanoseconds;
+}
+
 bool
 Duration::operator<(const rclcpp::Duration & rhs) const
 {
@@ -230,11 +248,18 @@ Duration::seconds() const
 rmw_time_t
 Duration::to_rmw_time() const
 {
-  // reuse conversion logic from msg creation
-  builtin_interfaces::msg::Duration msg = *this;
+  if (rcl_duration_.nanoseconds < 0) {
+    throw std::runtime_error("rmw_time_t cannot be negative");
+  }
+
+  // Purposefully avoid creating from builtin_interfaces::msg::Duration
+  // to avoid possible overflow converting from int64_t to int32_t, then back to uint64_t
   rmw_time_t result;
-  result.sec = static_cast<uint64_t>(msg.sec);
-  result.nsec = static_cast<uint64_t>(msg.nanosec);
+  constexpr rcl_duration_value_t kDivisor = RCL_S_TO_NS(1);
+  const auto div_result = std::div(rcl_duration_.nanoseconds, kDivisor);
+  result.sec = static_cast<uint64_t>(div_result.quot);
+  result.nsec = static_cast<uint64_t>(div_result.rem);
+
   return result;
 }
 

rclcpp/src/rclcpp/executor.cpp

@@ -22,6 +22,7 @@
 
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/executor.hpp"
+#include "rclcpp/executors/static_single_threaded_executor.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/scope_exit.hpp"
 #include "rclcpp/utilities.hpp"
@@ -74,7 +75,7 @@ Executor::Executor(const rclcpp::ExecutorOptions & options)
         "failed to destroy guard condition: %s", rcl_get_error_string().str);
       rcl_reset_error();
     }
-    throw std::runtime_error("Failed to create wait set in Executor constructor");
+    throw_from_rcl_error(ret, "Failed to create wait set in Executor constructor");
   }
 }
 
@@ -215,6 +216,12 @@ Executor::spin_node_some(std::shared_ptr<rclcpp::Node> node)
 void
 Executor::spin_some(std::chrono::nanoseconds max_duration)
 {
+  if (nullptr != dynamic_cast<executors::StaticSingleThreadedExecutor *>(this)) {
+    throw rclcpp::exceptions::UnimplementedError(
+            "spin_some is not implemented for StaticSingleThreadedExecutor, use spin or "
+            "spin_until_future_complete");
+  }
+
   auto start = std::chrono::steady_clock::now();
   auto max_duration_not_elapsed = [max_duration, start]() {
       if (std::chrono::nanoseconds(0) == max_duration) {
@@ -234,7 +241,7 @@ Executor::spin_some(std::chrono::nanoseconds max_duration)
   RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
   // non-blocking call to pre-load all available work
   wait_for_work(std::chrono::milliseconds::zero());
-  while (spinning.load() && max_duration_not_elapsed()) {
+  while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) {
     AnyExecutable any_exec;
     if (get_next_ready_executable(any_exec)) {
       execute_any_executable(any_exec);
@@ -245,24 +252,36 @@ Executor::spin_some(std::chrono::nanoseconds max_duration)
 }
 
 void
-Executor::spin_once(std::chrono::nanoseconds timeout)
+Executor::spin_once_impl(std::chrono::nanoseconds timeout)
 {
-  if (spinning.exchange(true)) {
-    throw std::runtime_error("spin_once() called while already spinning");
-  }
-  RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
   AnyExecutable any_exec;
   if (get_next_executable(any_exec, timeout)) {
     execute_any_executable(any_exec);
   }
 }
 
+void
+Executor::spin_once(std::chrono::nanoseconds timeout)
+{
+  if (nullptr != dynamic_cast<executors::StaticSingleThreadedExecutor *>(this)) {
+    throw rclcpp::exceptions::UnimplementedError(
+            "spin_once is not implemented for StaticSingleThreadedExecutor, use spin or "
+            "spin_until_future_complete");
+  }
+  if (spinning.exchange(true)) {
+    throw std::runtime_error("spin_once() called while already spinning");
+  }
+  RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
+  spin_once_impl(timeout);
+}
+
 void
 Executor::cancel()
 {
   spinning.store(false);
-  if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-    throw std::runtime_error(rcl_get_error_string().str);
+  rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
+  if (ret != RCL_RET_OK) {
+    throw_from_rcl_error(ret, "Failed to trigger guard condition in cancel");
   }
 }
 
@@ -300,8 +319,9 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
   any_exec.callback_group->can_be_taken_from().store(true);
   // Wake the wait, because it may need to be recalculated or work that
   // was previously blocked is now available.
-  if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-    throw std::runtime_error(rcl_get_error_string().str);
+  rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
+  if (ret != RCL_RET_OK) {
+    throw_from_rcl_error(ret, "Failed to trigger guard condition from execute_any_executable");
   }
 }
 
@@ -466,19 +486,19 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
       }
     }
     // clear wait set
-    if (rcl_wait_set_clear(&wait_set_) != RCL_RET_OK) {
-      throw std::runtime_error("Couldn't clear wait set");
+    rcl_ret_t ret = rcl_wait_set_clear(&wait_set_);
+    if (ret != RCL_RET_OK) {
+      throw_from_rcl_error(ret, "Couldn't clear wait set");
     }
 
     // The size of waitables are accounted for in size of the other entities
-    rcl_ret_t ret = rcl_wait_set_resize(
+    ret = rcl_wait_set_resize(
       &wait_set_, memory_strategy_->number_of_ready_subscriptions(),
       memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(),
       memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(),
       memory_strategy_->number_of_ready_events());
     if (RCL_RET_OK != ret) {
-      throw std::runtime_error(
-              std::string("Couldn't resize the wait set : ") + rcl_get_error_string().str);
+      throw_from_rcl_error(ret, "Couldn't resize the wait set");
     }
 
     if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) {

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

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

rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp

@@ -62,6 +62,13 @@ StaticExecutorEntitiesCollector::init(
   execute();
 }
 
+void
+StaticExecutorEntitiesCollector::fini()
+{
+  memory_strategy_->clear_handles();
+  exec_list_.clear();
+}
+
 void
 StaticExecutorEntitiesCollector::execute()
 {

rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp

@@ -41,6 +41,7 @@ StaticSingleThreadedExecutor::spin()
   // Set memory_strategy_ and exec_list_ based on weak_nodes_
   // Prepare wait_set_ based on memory_strategy_
   entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
 
   while (rclcpp::ok(this->context_) && spinning.load()) {
     // Refresh wait set and wait for work

rclcpp/src/rclcpp/graph_listener.cpp

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

rclcpp/src/rclcpp/init_options.cpp

@@ -26,7 +26,7 @@ InitOptions::InitOptions(rcl_allocator_t allocator)
   *init_options_ = rcl_get_zero_initialized_init_options();
   rcl_ret_t ret = rcl_init_options_init(init_options_.get(), allocator);
   if (RCL_RET_OK != ret) {
-    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialized rcl init options");
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl init options");
   }
 }
 
@@ -44,6 +44,7 @@ InitOptions::InitOptions(const InitOptions & other)
 : InitOptions(*other.get_rcl_init_options())
 {
   shutdown_on_sigint = other.shutdown_on_sigint;
+  initialize_logging_ = other.initialize_logging_;
 }
 
 bool
@@ -69,6 +70,7 @@ InitOptions::operator=(const InitOptions & other)
       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;
 }

rclcpp/src/rclcpp/node.cpp

@@ -54,6 +54,12 @@ extend_sub_namespace(const std::string & existing_sub_namespace, const std::stri
             extension.c_str(),
             "a sub-namespace should not have a leading /",
             0);
+  } else if (existing_sub_namespace.empty() && extension.empty()) {
+    throw rclcpp::exceptions::NameValidationError(
+            "sub_namespace",
+            extension.c_str(),
+            "sub-nodes should not extend nodes by an empty sub-namespace",
+            0);
   }
 
   std::string new_sub_namespace;
@@ -79,7 +85,11 @@ create_effective_namespace(const std::string & node_namespace, const std::string
   // and do not need trimming of `/` and other things, as they were validated
   // in other functions already.
 
-  if (node_namespace.back() == '/') {
+  // A node may not have a sub_namespace if it is no sub_node. In this case,
+  // just return the original namespace
+  if (sub_namespace.empty()) {
+    return node_namespace;
+  } else if (node_namespace.back() == '/') {
     // this is the special case where node_namespace is just `/`
     return node_namespace + sub_namespace;
   } else {
@@ -158,6 +168,8 @@ Node::Node(
   node_services_(other.node_services_),
   node_clock_(other.node_clock_),
   node_parameters_(other.node_parameters_),
+  node_time_source_(other.node_time_source_),
+  node_waitables_(other.node_waitables_),
   node_options_(other.node_options_),
   sub_namespace_(extend_sub_namespace(other.get_sub_namespace(), sub_namespace)),
   effective_namespace_(create_effective_namespace(other.get_namespace(), sub_namespace_))
@@ -184,7 +196,18 @@ Node::Node(
 }
 
 Node::~Node()
-{}
+{
+  // release sub-interfaces in an order that allows them to consult with node_base during tear-down
+  node_waitables_.reset();
+  node_time_source_.reset();
+  node_parameters_.reset();
+  node_clock_.reset();
+  node_services_.reset();
+  node_topics_.reset();
+  node_timers_.reset();
+  node_logging_.reset();
+  node_graph_.reset();
+}
 
 const char *
 Node::get_name() const

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, size_t ulp = 100)
+__are_doubles_equal(double x, double y, double ulp = 100.0)
 {
   return std::abs(x - y) <= std::numeric_limits<double>::epsilon() * std::abs(x + y) * ulp;
 }

rclcpp/src/rclcpp/node_options.cpp

@@ -67,6 +67,7 @@ NodeOptions &
 NodeOptions::operator=(const NodeOptions & other)
 {
   if (this != &other) {
+    this->node_options_.reset();
     this->context_ = other.context_;
     this->arguments_ = other.arguments_;
     this->parameter_overrides_ = other.parameter_overrides_;
@@ -75,10 +76,13 @@ NodeOptions::operator=(const NodeOptions & other)
     this->use_intra_process_comms_ = other.use_intra_process_comms_;
     this->enable_topic_statistics_ = other.enable_topic_statistics_;
     this->start_parameter_services_ = other.start_parameter_services_;
-    this->allocator_ = other.allocator_;
+    this->start_parameter_event_publisher_ = other.start_parameter_event_publisher_;
+    this->parameter_event_qos_ = other.parameter_event_qos_;
+    this->parameter_event_publisher_options_ = other.parameter_event_publisher_options_;
     this->allow_undeclared_parameters_ = other.allow_undeclared_parameters_;
     this->automatically_declare_parameters_from_overrides_ =
       other.automatically_declare_parameters_from_overrides_;
+    this->allocator_ = other.allocator_;
   }
   return *this;
 }
@@ -176,7 +180,7 @@ NodeOptions::parameter_overrides(const std::vector<rclcpp::Parameter> & paramete
 bool
 NodeOptions::use_global_arguments() const
 {
-  return this->node_options_->use_global_arguments;
+  return this->use_global_arguments_;
 }
 
 NodeOptions &
@@ -338,7 +342,7 @@ NodeOptions::get_domain_id_from_env() const
   _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);
+    uint32_t number = static_cast<uint32_t>(strtoul(ros_domain_id, NULL, 0));
     if (number == (std::numeric_limits<uint32_t>::max)()) {
 #ifdef _WIN32
       // free the ros_domain_id before throwing, if getenv was used on Windows

rclcpp/src/rclcpp/parameter_client.cpp

@@ -15,7 +15,12 @@
 #include "rclcpp/parameter_client.hpp"
 
 #include <algorithm>
+#include <chrono>
+#include <functional>
+#include <future>
+#include <iterator>
 #include <memory>
+#include <stdexcept>
 #include <string>
 #include <vector>
 

rclcpp/src/rclcpp/parameter_service.cpp

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

rclcpp/src/rclcpp/time.cpp

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

rclcpp/src/rclcpp/time_source.cpp

@@ -185,11 +185,21 @@ void TimeSource::set_clock(
   const builtin_interfaces::msg::Time::SharedPtr msg, bool set_ros_time_enabled,
   std::shared_ptr<rclcpp::Clock> clock)
 {
+  std::lock_guard<std::mutex> clock_guard(clock->get_clock_mutex());
+
   // Do change
   if (!set_ros_time_enabled && clock->ros_time_is_active()) {
-    disable_ros_time(clock);
+    auto ret = rcl_disable_ros_time_override(clock->get_clock_handle());
+    if (ret != RCL_RET_OK) {
+      rclcpp::exceptions::throw_from_rcl_error(
+        ret, "Failed to disable ros_time_override_status");
+    }
   } else if (set_ros_time_enabled && !clock->ros_time_is_active()) {
-    enable_ros_time(clock);
+    auto ret = rcl_enable_ros_time_override(clock->get_clock_handle());
+    if (ret != RCL_RET_OK) {
+      rclcpp::exceptions::throw_from_rcl_error(
+        ret, "Failed to enable ros_time_override_status");
+    }
   }
 
   auto ret = rcl_set_ros_time_override(clock->get_clock_handle(), rclcpp::Time(*msg).nanoseconds());
@@ -273,24 +283,6 @@ void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::S
   }
 }
 
-void TimeSource::enable_ros_time(std::shared_ptr<rclcpp::Clock> clock)
-{
-  auto ret = rcl_enable_ros_time_override(clock->get_clock_handle());
-  if (ret != RCL_RET_OK) {
-    rclcpp::exceptions::throw_from_rcl_error(
-      ret, "Failed to enable ros_time_override_status");
-  }
-}
-
-void TimeSource::disable_ros_time(std::shared_ptr<rclcpp::Clock> clock)
-{
-  auto ret = rcl_disable_ros_time_override(clock->get_clock_handle());
-  if (ret != RCL_RET_OK) {
-    rclcpp::exceptions::throw_from_rcl_error(
-      ret, "Failed to enable ros_time_override_status");
-  }
-}
-
 void TimeSource::enable_ros_time()
 {
   if (ros_time_active_) {

rclcpp/src/rclcpp/timer.cpp

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

rclcpp/src/rclcpp/utilities.cpp

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

rclcpp/test/CMakeLists.txt

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

rclcpp/test/benchmark/CMakeLists.txt

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

rclcpp/test/benchmark/benchmark_client.cpp

@@ -0,0 +1,153 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <string>
+
+#include "performance_test_fixture/performance_test_fixture.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+using performance_test_fixture::PerformanceTest;
+
+constexpr char empty_service_name[] = "empty_service";
+
+class ClientPerformanceTest : public PerformanceTest
+{
+public:
+  explicit ClientPerformanceTest(rclcpp::NodeOptions = rclcpp::NodeOptions()) {}
+  void SetUp(benchmark::State & state)
+  {
+    rclcpp::init(0, nullptr);
+    node = std::make_unique<rclcpp::Node>("node", "ns");
+
+    auto empty_service_callback =
+      [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {};
+    empty_service =
+      node->create_service<test_msgs::srv::Empty>(empty_service_name, empty_service_callback);
+
+    performance_test_fixture::PerformanceTest::SetUp(state);
+  }
+
+  void TearDown(benchmark::State & state)
+  {
+    performance_test_fixture::PerformanceTest::TearDown(state);
+    empty_service.reset();
+    node.reset();
+    rclcpp::shutdown();
+  }
+
+protected:
+  std::unique_ptr<rclcpp::Node> node;
+  std::shared_ptr<rclcpp::Service<test_msgs::srv::Empty>> empty_service;
+};
+
+BENCHMARK_F(ClientPerformanceTest, construct_client_no_service)(benchmark::State & state) {
+  // Prime cache
+  auto outer_client = node->create_client<test_msgs::srv::Empty>("not_an_existing_service");
+  outer_client.reset();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    auto client = node->create_client<test_msgs::srv::Empty>("not_an_existing_service");
+    benchmark::DoNotOptimize(client);
+    benchmark::ClobberMemory();
+
+    state.PauseTiming();
+    client.reset();
+    state.ResumeTiming();
+  }
+}
+
+BENCHMARK_F(ClientPerformanceTest, construct_client_empty_srv)(benchmark::State & state) {
+  // Prime cache
+  auto outer_client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
+  outer_client.reset();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    auto client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
+    benchmark::DoNotOptimize(client);
+    benchmark::ClobberMemory();
+
+    state.PauseTiming();
+    client.reset();
+    state.ResumeTiming();
+  }
+}
+
+BENCHMARK_F(ClientPerformanceTest, destroy_client_empty_srv)(benchmark::State & state) {
+  // Prime cache
+  auto outer_client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
+  outer_client.reset();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    state.PauseTiming();
+    auto client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
+    state.ResumeTiming();
+    benchmark::DoNotOptimize(client);
+    benchmark::ClobberMemory();
+
+    client.reset();
+  }
+}
+
+BENCHMARK_F(ClientPerformanceTest, wait_for_service)(benchmark::State & state) {
+  int count = 0;
+  for (auto _ : state) {
+    state.PauseTiming();
+    const std::string service_name = std::string("service_") + std::to_string(count++);
+    // Create client before service so it has to 'discover' the service after construction
+    auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+    auto callback =
+      [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {};
+    auto service =
+      node->create_service<test_msgs::srv::Empty>(service_name, callback);
+    state.ResumeTiming();
+
+    client->wait_for_service(std::chrono::seconds(1));
+    benchmark::ClobberMemory();
+  }
+}
+
+BENCHMARK_F(ClientPerformanceTest, async_send_request_only)(benchmark::State & state) {
+  auto client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
+  auto shared_request = std::make_shared<test_msgs::srv::Empty::Request>();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    auto future = client->async_send_request(shared_request);
+    benchmark::DoNotOptimize(future);
+    benchmark::ClobberMemory();
+  }
+}
+
+BENCHMARK_F(ClientPerformanceTest, async_send_request_and_response)(benchmark::State & state) {
+  auto client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
+  auto shared_request = std::make_shared<test_msgs::srv::Empty::Request>();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    auto future = client->async_send_request(shared_request);
+    rclcpp::spin_until_future_complete(
+      node->get_node_base_interface(), future, std::chrono::seconds(1));
+    benchmark::DoNotOptimize(future);
+    benchmark::ClobberMemory();
+  }
+}

rclcpp/test/benchmark/benchmark_executor.cpp

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

rclcpp/test/benchmark/benchmark_init_shutdown.cpp

@@ -0,0 +1,53 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "performance_test_fixture/performance_test_fixture.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+
+using performance_test_fixture::PerformanceTest;
+
+BENCHMARK_F(PerformanceTest, rclcpp_init)(benchmark::State & state)
+{
+  // Warmup and prime caches
+  rclcpp::init(0, nullptr);
+  rclcpp::shutdown();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    rclcpp::init(0, nullptr);
+
+    state.PauseTiming();
+    rclcpp::shutdown();
+    state.ResumeTiming();
+    benchmark::ClobberMemory();
+  }
+}
+
+BENCHMARK_F(PerformanceTest, rclcpp_shutdown)(benchmark::State & state)
+{
+  // Warmup and prime caches
+  rclcpp::init(0, nullptr);
+  rclcpp::shutdown();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    state.PauseTiming();
+    rclcpp::init(0, nullptr);
+    state.ResumeTiming();
+
+    rclcpp::shutdown();
+    benchmark::ClobberMemory();
+  }
+}

rclcpp/test/benchmark/benchmark_node.cpp

@@ -0,0 +1,77 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <string>
+
+#include "performance_test_fixture/performance_test_fixture.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+using performance_test_fixture::PerformanceTest;
+
+class NodePerformanceTest : public PerformanceTest
+{
+public:
+  void SetUp(benchmark::State & state)
+  {
+    rclcpp::init(0, nullptr);
+    performance_test_fixture::PerformanceTest::SetUp(state);
+  }
+
+  void TearDown(benchmark::State & state)
+  {
+    performance_test_fixture::PerformanceTest::TearDown(state);
+    rclcpp::shutdown();
+  }
+};
+
+BENCHMARK_F(NodePerformanceTest, create_node)(benchmark::State & state)
+{
+  // Warmup and prime caches
+  auto outer_node = std::make_shared<rclcpp::Node>("node");
+  outer_node.reset();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    // Using pointer to separate construction and destruction in timing
+    auto node = std::make_shared<rclcpp::Node>("node");
+    benchmark::DoNotOptimize(node);
+    benchmark::ClobberMemory();
+
+    // Ensure destruction of node is not counted toward timing
+    state.PauseTiming();
+    node.reset();
+    state.ResumeTiming();
+  }
+}
+
+BENCHMARK_F(NodePerformanceTest, destroy_node)(benchmark::State & state)
+{
+  // Warmup and prime caches
+  auto outer_node = std::make_shared<rclcpp::Node>("node");
+  outer_node.reset();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    // Using pointer to separate construction and destruction in timing
+    state.PauseTiming();
+    auto node = std::make_shared<rclcpp::Node>("node");
+    state.ResumeTiming();
+
+    benchmark::DoNotOptimize(node);
+    benchmark::ClobberMemory();
+
+    node.reset();
+  }
+}

rclcpp/test/benchmark/benchmark_node_parameters_interface.cpp

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

rclcpp/test/benchmark/benchmark_parameter_client.cpp

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

rclcpp/test/benchmark/benchmark_service.cpp

@@ -0,0 +1,141 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include "performance_test_fixture/performance_test_fixture.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+using performance_test_fixture::PerformanceTest;
+
+constexpr char empty_service_name[] = "empty_service";
+
+class ServicePerformanceTest : public PerformanceTest
+{
+public:
+  ServicePerformanceTest()
+  : callback_count(0) {}
+
+  void SetUp(benchmark::State & state)
+  {
+    rclcpp::init(0, nullptr);
+    node = std::make_unique<rclcpp::Node>("node", "ns");
+    empty_client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
+    callback_count = 0;
+
+    performance_test_fixture::PerformanceTest::SetUp(state);
+  }
+
+  void TearDown(benchmark::State & state)
+  {
+    performance_test_fixture::PerformanceTest::TearDown(state);
+
+    empty_client.reset();
+    node.reset();
+    rclcpp::shutdown();
+  }
+
+  void ServiceCallback(
+    const test_msgs::srv::Empty::Request::SharedPtr,
+    test_msgs::srv::Empty::Response::SharedPtr)
+  {
+    callback_count++;
+  }
+
+protected:
+  std::unique_ptr<rclcpp::Node> node;
+  std::shared_ptr<rclcpp::Client<test_msgs::srv::Empty>> empty_client;
+  int callback_count;
+};
+
+BENCHMARK_F(ServicePerformanceTest, construct_service_no_client)(benchmark::State & state) {
+  auto callback = std::bind(
+    &ServicePerformanceTest::ServiceCallback,
+    this, std::placeholders::_1, std::placeholders::_2);
+
+  auto outer_service = node->create_service<test_msgs::srv::Empty>("not_a_service", callback);
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    auto service = node->create_service<test_msgs::srv::Empty>("not_a_service", callback);
+    benchmark::DoNotOptimize(service);
+    benchmark::ClobberMemory();
+
+    state.PauseTiming();
+    service.reset();
+    state.ResumeTiming();
+  }
+}
+
+BENCHMARK_F(ServicePerformanceTest, construct_service_empty_srv)(benchmark::State & state) {
+  auto callback = std::bind(
+    &ServicePerformanceTest::ServiceCallback,
+    this, std::placeholders::_1, std::placeholders::_2);
+  auto outer_service = node->create_service<test_msgs::srv::Empty>(empty_service_name, callback);
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    auto service = node->create_service<test_msgs::srv::Empty>(empty_service_name, callback);
+    benchmark::DoNotOptimize(service);
+    benchmark::ClobberMemory();
+
+    state.PauseTiming();
+    service.reset();
+    state.ResumeTiming();
+  }
+}
+
+BENCHMARK_F(ServicePerformanceTest, destroy_service_empty_srv)(benchmark::State & state) {
+  auto callback = std::bind(
+    &ServicePerformanceTest::ServiceCallback,
+    this, std::placeholders::_1, std::placeholders::_2);
+  auto outer_service = node->create_service<test_msgs::srv::Empty>(empty_service_name, callback);
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    state.PauseTiming();
+    auto service = node->create_service<test_msgs::srv::Empty>(empty_service_name, callback);
+    state.ResumeTiming();
+    benchmark::DoNotOptimize(service);
+    benchmark::ClobberMemory();
+
+    service.reset();
+  }
+}
+
+BENCHMARK_F(ServicePerformanceTest, async_send_response)(benchmark::State & state) {
+  auto callback = std::bind(
+    &ServicePerformanceTest::ServiceCallback,
+    this, std::placeholders::_1, std::placeholders::_2);
+  auto service = node->create_service<test_msgs::srv::Empty>(empty_service_name, callback);
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    state.PauseTiming();
+    // Clear executor queue
+    rclcpp::spin_some(node->get_node_base_interface());
+
+    auto request = std::make_shared<test_msgs::srv::Empty::Request>();
+    auto future = empty_client->async_send_request(request);
+    state.ResumeTiming();
+    benchmark::DoNotOptimize(service);
+    benchmark::ClobberMemory();
+
+    rclcpp::spin_until_future_complete(node->get_node_base_interface(), future);
+  }
+  if (callback_count == 0) {
+    state.SkipWithError("Service callback was not called");
+  }
+}

rclcpp/test/mocking_utils/patch.hpp

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

rclcpp/test/rclcpp/CMakeLists.txt

@@ -0,0 +1,652 @@
+find_package(ament_cmake_gtest REQUIRED)
+
+find_package(rmw_implementation_cmake REQUIRED)
+
+add_definitions(-DTEST_RESOURCES_DIRECTORY="${CMAKE_CURRENT_BINARY_DIR}/../resources")
+
+rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs
+  ../msg/Header.msg
+  ../msg/MessageWithHeader.msg
+  DEPENDENCIES builtin_interfaces
+  LIBRARY_NAME ${PROJECT_NAME}
+  SKIP_INSTALL
+)
+
+ament_add_gtest(
+  test_allocator_common
+  allocator/test_allocator_common.cpp)
+if(TARGET test_allocator_common)
+  target_link_libraries(test_allocator_common ${PROJECT_NAME})
+endif()
+ament_add_gtest(
+  test_allocator_deleter
+  allocator/test_allocator_deleter.cpp)
+if(TARGET test_allocator_deleter)
+  target_link_libraries(test_allocator_deleter ${PROJECT_NAME})
+endif()
+ament_add_gtest(
+  test_exceptions
+  exceptions/test_exceptions.cpp)
+if(TARGET test_exceptions)
+  target_link_libraries(test_exceptions ${PROJECT_NAME} mimick)
+endif()
+
+# Increasing timeout because connext can take a long time to destroy nodes
+# TODO(brawner) remove when destroying Node for Connext is resolved. See:
+# https://github.com/ros2/rclcpp/issues/1250
+ament_add_gtest(
+  test_allocator_memory_strategy
+  strategies/test_allocator_memory_strategy.cpp
+  TIMEOUT 360)
+if(TARGET test_allocator_memory_strategy)
+  ament_target_dependencies(test_allocator_memory_strategy
+    "rcl"
+    "test_msgs"
+  )
+  target_link_libraries(test_allocator_memory_strategy ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_message_pool_memory_strategy strategies/test_message_pool_memory_strategy.cpp)
+if(TARGET test_message_pool_memory_strategy)
+  ament_target_dependencies(test_message_pool_memory_strategy
+    "rcl"
+    "test_msgs"
+  )
+  target_link_libraries(test_message_pool_memory_strategy ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_any_service_callback test_any_service_callback.cpp)
+if(TARGET test_any_service_callback)
+  ament_target_dependencies(test_any_service_callback
+    "test_msgs"
+  )
+  target_link_libraries(test_any_service_callback ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_any_subscription_callback test_any_subscription_callback.cpp)
+if(TARGET test_any_subscription_callback)
+  ament_target_dependencies(test_any_subscription_callback
+    "test_msgs"
+  )
+  target_link_libraries(test_any_subscription_callback ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_client test_client.cpp)
+if(TARGET test_client)
+  ament_target_dependencies(test_client
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "test_msgs"
+  )
+  target_link_libraries(test_client ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_create_timer test_create_timer.cpp)
+if(TARGET test_create_timer)
+  ament_target_dependencies(test_create_timer
+    "rcl_interfaces"
+    "rmw"
+    "rcl"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+  target_link_libraries(test_create_timer ${PROJECT_NAME})
+  target_include_directories(test_create_timer PRIVATE ./)
+endif()
+ament_add_gtest(test_create_subscription test_create_subscription.cpp)
+if(TARGET test_create_subscription)
+  target_link_libraries(test_create_subscription ${PROJECT_NAME})
+  ament_target_dependencies(test_create_subscription
+    "test_msgs"
+  )
+endif()
+ament_add_gtest(test_expand_topic_or_service_name test_expand_topic_or_service_name.cpp)
+if(TARGET test_expand_topic_or_service_name)
+  ament_target_dependencies(test_expand_topic_or_service_name
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+  target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_function_traits test_function_traits.cpp)
+if(TARGET test_function_traits)
+  target_include_directories(test_function_traits PUBLIC ../../include)
+  ament_target_dependencies(test_function_traits
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+endif()
+ament_add_gtest(
+  test_future_return_code
+  test_future_return_code.cpp)
+if(TARGET test_future_return_code)
+  target_link_libraries(test_future_return_code ${PROJECT_NAME})
+endif()
+ament_add_gmock(test_intra_process_manager test_intra_process_manager.cpp)
+if(TARGET test_intra_process_manager)
+  ament_target_dependencies(test_intra_process_manager
+    "rcl"
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+  target_link_libraries(test_intra_process_manager ${PROJECT_NAME})
+endif()
+ament_add_gmock(test_intra_process_manager_with_allocators test_intra_process_manager_with_allocators.cpp)
+if(TARGET test_intra_process_manager_with_allocators)
+  ament_target_dependencies(test_intra_process_manager_with_allocators
+    "test_msgs"
+  )
+  target_link_libraries(test_intra_process_manager_with_allocators ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_ring_buffer_implementation test_ring_buffer_implementation.cpp)
+if(TARGET test_ring_buffer_implementation)
+  ament_target_dependencies(test_ring_buffer_implementation
+    "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_wait_for_message test_wait_for_message.cpp)
+if(TARGET test_wait_for_message)
+  ament_target_dependencies(test_wait_for_message
+    "test_msgs")
+  target_link_libraries(test_wait_for_message ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_init test_init.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_init)
+  ament_target_dependencies(test_init
+    "rcl")
+  target_link_libraries(test_init ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_interface_traits test_interface_traits.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_interface_traits)
+  ament_target_dependencies(test_interface_traits
+    "rcl")
+  target_link_libraries(test_interface_traits ${PROJECT_NAME})
+endif()
+
+# TODO(brawner) remove when destroying Node for Connext is resolved. See:
+# https://github.com/ros2/rclcpp/issues/1250
+ament_add_gtest(
+  test_executors
+  executors/test_executors.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}"
+  TIMEOUT 180)
+if(TARGET test_executors)
+  ament_target_dependencies(test_executors
+    "rcl"
+    "test_msgs")
+  target_link_libraries(test_executors ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_static_single_threaded_executor executors/test_static_single_threaded_executor.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_static_single_threaded_executor)
+  ament_target_dependencies(test_static_single_threaded_executor
+    "test_msgs")
+  target_link_libraries(test_static_single_threaded_executor ${PROJECT_NAME} mimick)
+endif()
+
+ament_add_gtest(test_multi_threaded_executor executors/test_multi_threaded_executor.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_multi_threaded_executor)
+  ament_target_dependencies(test_multi_threaded_executor
+    "rcl")
+  target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_static_executor_entities_collector executors/test_static_executor_entities_collector.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120)
+if(TARGET test_static_executor_entities_collector)
+  ament_target_dependencies(test_static_executor_entities_collector
+    "rcl"
+    "test_msgs")
+  target_link_libraries(test_static_executor_entities_collector ${PROJECT_NAME} mimick)
+endif()
+
+ament_add_gtest(test_guard_condition test_guard_condition.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_guard_condition)
+  target_link_libraries(test_guard_condition ${PROJECT_NAME} mimick)
+endif()
+
+ament_add_gtest(test_wait_set test_wait_set.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_wait_set)
+  ament_target_dependencies(test_wait_set "test_msgs")
+  target_link_libraries(test_wait_set ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_subscription_topic_statistics topic_statistics/test_subscription_topic_statistics.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}"
+)
+if(TARGET test_subscription_topic_statistics)
+  ament_target_dependencies(test_subscription_topic_statistics
+    "builtin_interfaces"
+    "libstatistics_collector"
+    "rcl_interfaces"
+    "rcutils"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "statistics_msgs"
+    "test_msgs")
+  rosidl_target_interfaces(test_subscription_topic_statistics ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp")
+  target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_subscription_options test_subscription_options.cpp)
+if(TARGET test_subscription_options)
+  ament_target_dependencies(test_subscription_options "rcl")
+  target_link_libraries(test_subscription_options ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_dynamic_storage wait_set_policies/test_dynamic_storage.cpp)
+if(TARGET test_dynamic_storage)
+  ament_target_dependencies(test_dynamic_storage "rcl" "test_msgs")
+  target_link_libraries(test_dynamic_storage ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_storage_policy_common wait_set_policies/test_storage_policy_common.cpp)
+if(TARGET test_storage_policy_common)
+  ament_target_dependencies(test_storage_policy_common "rcl" "test_msgs")
+  target_link_libraries(test_storage_policy_common ${PROJECT_NAME} mimick)
+endif()
+
+ament_add_gtest(test_static_storage wait_set_policies/test_static_storage.cpp)
+if(TARGET test_static_storage)
+  ament_target_dependencies(test_static_storage "rcl" "test_msgs")
+  target_link_libraries(test_static_storage ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_thread_safe_synchronization wait_set_policies/test_thread_safe_synchronization.cpp)
+if(TARGET test_thread_safe_synchronization)
+  ament_target_dependencies(test_thread_safe_synchronization "rcl" "test_msgs")
+  target_link_libraries(test_thread_safe_synchronization ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_executor test_executor.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}"
+  TIMEOUT 120)
+if(TARGET test_executor)
+  ament_target_dependencies(test_executor "rcl")
+  target_link_libraries(test_executor ${PROJECT_NAME} mimick)
+endif()
+
+ament_add_gtest(test_graph_listener test_graph_listener.cpp)
+if(TARGET test_graph_listener)
+  target_link_libraries(test_graph_listener ${PROJECT_NAME} mimick)
+endif()

rclcpp/test/rclcpp/allocator/test_allocator_common.cpp

@@ -0,0 +1,67 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+
+#include "rclcpp/allocator/allocator_common.hpp"
+
+TEST(TestAllocatorCommon, retyped_allocate) {
+  std::allocator<int> allocator;
+  void * untyped_allocator = &allocator;
+  void * allocated_mem =
+    rclcpp::allocator::retyped_allocate<std::allocator<char>>(1u, untyped_allocator);
+  ASSERT_NE(nullptr, allocated_mem);
+
+  auto code = [&untyped_allocator, allocated_mem]() {
+      rclcpp::allocator::retyped_deallocate<int, std::allocator<int>>(
+        allocated_mem, untyped_allocator);
+    };
+  EXPECT_NO_THROW(code());
+
+  allocated_mem = allocator.allocate(1);
+  ASSERT_NE(nullptr, allocated_mem);
+  void * reallocated_mem =
+    rclcpp::allocator::retyped_reallocate<int, std::allocator<int>>(
+    allocated_mem, 2u, untyped_allocator);
+  ASSERT_NE(nullptr, reallocated_mem);
+
+  auto code2 = [&untyped_allocator, reallocated_mem]() {
+      rclcpp::allocator::retyped_deallocate<int, std::allocator<int>>(
+        reallocated_mem, untyped_allocator);
+    };
+  EXPECT_NO_THROW(code2());
+}
+
+TEST(TestAllocatorCommon, get_rcl_allocator) {
+  std::allocator<int> allocator;
+  auto rcl_allocator = rclcpp::allocator::get_rcl_allocator<int>(allocator);
+  EXPECT_NE(nullptr, rcl_allocator.allocate);
+  EXPECT_NE(nullptr, rcl_allocator.deallocate);
+  EXPECT_NE(nullptr, rcl_allocator.reallocate);
+  EXPECT_NE(nullptr, rcl_allocator.zero_allocate);
+  // Not testing state as that may or may not be null depending on platform
+}
+
+TEST(TestAllocatorCommon, get_void_rcl_allocator) {
+  std::allocator<void> allocator;
+  auto rcl_allocator =
+    rclcpp::allocator::get_rcl_allocator<void, std::allocator<void>>(allocator);
+  EXPECT_NE(nullptr, rcl_allocator.allocate);
+  EXPECT_NE(nullptr, rcl_allocator.deallocate);
+  EXPECT_NE(nullptr, rcl_allocator.reallocate);
+  EXPECT_NE(nullptr, rcl_allocator.zero_allocate);
+  // Not testing state as that may or may not be null depending on platform
+}

rclcpp/test/rclcpp/allocator/test_allocator_deleter.cpp

@@ -0,0 +1,101 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+
+#include "rclcpp/allocator/allocator_deleter.hpp"
+
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+TEST(TestAllocatorDeleter, construct_destruct) {
+  std::allocator<int> allocator;
+
+  rclcpp::allocator::AllocatorDeleter<std::allocator<int>> deleter;
+  EXPECT_EQ(nullptr, deleter.get_allocator());
+  deleter.set_allocator(&allocator);
+  EXPECT_EQ(&allocator, deleter.get_allocator());
+
+  rclcpp::allocator::AllocatorDeleter<std::allocator<int>> deleter2(&allocator);
+  EXPECT_EQ(&allocator, deleter2.get_allocator());
+
+  rclcpp::allocator::AllocatorDeleter<std::allocator<int>> deleter3(deleter2);
+  EXPECT_EQ(&allocator, deleter3.get_allocator());
+}
+
+TEST(TestAllocatorDeleter, delete) {
+  std::allocator<int> allocator;
+  int * some_mem = allocator.allocate(1u);
+  ASSERT_NE(nullptr, some_mem);
+
+  rclcpp::allocator::AllocatorDeleter<std::allocator<int>> deleter(&allocator);
+  EXPECT_NO_THROW(deleter(some_mem));
+}
+
+TEST(TestAllocatorDeleter, set_allocator_for_deleter_AllocatorDeleter) {
+  using AllocatorT = std::allocator<int>;
+  using DeleterT = rclcpp::allocator::AllocatorDeleter<AllocatorT>;
+  AllocatorT allocator;
+  DeleterT deleter(&allocator);
+
+  std::allocator<int> allocator2;
+  rclcpp::allocator::set_allocator_for_deleter<AllocatorT, AllocatorT>(&deleter, &allocator2);
+  EXPECT_EQ(&allocator2, deleter.get_allocator());
+
+  auto throwing_statement = [&allocator]() {
+      DeleterT * null_del_ptr = nullptr;
+      rclcpp::allocator::set_allocator_for_deleter<AllocatorT, AllocatorT>(
+        null_del_ptr, &allocator);
+    };
+  RCLCPP_EXPECT_THROW_EQ(
+    throwing_statement(),
+    std::invalid_argument("Argument was NULL to set_allocator_for_deleter"));
+
+  auto throwing_statement2 = [&deleter]() {
+      AllocatorT * null_alloc_ptr = nullptr;
+      rclcpp::allocator::set_allocator_for_deleter<AllocatorT, AllocatorT>(
+        &deleter, null_alloc_ptr);
+    };
+
+  RCLCPP_EXPECT_THROW_EQ(
+    throwing_statement2(),
+    std::invalid_argument("Argument was NULL to set_allocator_for_deleter"));
+}
+
+TEST(TestAllocatorDeleter, set_allocator_for_deleter_std_default_delete) {
+  using AllocatorT = std::allocator<int>;
+  using DeleterT = std::default_delete<int>;
+  auto not_throwing_statement = []() {
+      AllocatorT allocator;
+      DeleterT deleter;
+      rclcpp::allocator::set_allocator_for_deleter<int, int>(&deleter, &allocator);
+    };
+  EXPECT_NO_THROW(not_throwing_statement());
+}
+
+TEST(TestAllocatorDeleter, set_allocator_for_deleter_unexpected_template) {
+  class SomeAllocatorClass {};
+  class SomeDeleterClass {};
+  using AllocatorT = SomeAllocatorClass;
+  using DeleterT = SomeDeleterClass;
+  auto throwing_statement = []() {
+      DeleterT deleter;
+      AllocatorT allocator;
+      rclcpp::allocator::set_allocator_for_deleter<AllocatorT, int, DeleterT>(&deleter, &allocator);
+    };
+  RCLCPP_EXPECT_THROW_EQ(
+    throwing_statement(),
+    std::runtime_error("Reached unexpected template specialization"));
+}

rclcpp/test/rclcpp/exceptions/test_exceptions.cpp

@@ -0,0 +1,58 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "rclcpp/exceptions/exceptions.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+TEST(TestExceptions, throw_from_rcl_error) {
+  EXPECT_THROW(
+    rclcpp::exceptions::throw_from_rcl_error(RCL_RET_BAD_ALLOC, ""),
+    rclcpp::exceptions::RCLBadAlloc);
+
+  EXPECT_THROW(
+    rclcpp::exceptions::throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, ""),
+    rclcpp::exceptions::RCLInvalidArgument);
+
+  EXPECT_THROW(
+    rclcpp::exceptions::throw_from_rcl_error(RCL_RET_INVALID_ROS_ARGS, ""),
+    rclcpp::exceptions::RCLInvalidROSArgsError);
+
+  EXPECT_THROW(
+    rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, ""),
+    rclcpp::exceptions::RCLError);
+
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::exceptions::throw_from_rcl_error(RCL_RET_OK, ""),
+    std::invalid_argument("ret is RCL_RET_OK"));
+
+  {
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_get_error_state, nullptr);
+    RCLCPP_EXPECT_THROW_EQ(
+      rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, ""),
+      std::runtime_error("rcl error state is not set"));
+  }
+}
+
+TEST(TestExceptions, basic_constructors) {
+  EXPECT_STREQ("node is invalid", rclcpp::exceptions::InvalidNodeError().what());
+  rcl_error_state_t error_state{"error", __FILE__, __LINE__};
+  EXPECT_STREQ(
+    "prefix: error not set",
+    rclcpp::exceptions::RCLInvalidROSArgsError(RCL_RET_ERROR, &error_state, "prefix: ").what());
+}

rclcpp/test/rclcpp/executors/test_executors.cpp

@@ -0,0 +1,414 @@
+// Copyright 2017 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.
+
+/**
+ * This test checks all implementations of rclcpp::executor to check they pass they basic API
+ * tests. Anything specific to any executor in particular should go in a separate test file.
+ *
+ */
+#include <gtest/gtest.h>
+
+#include <algorithm>
+#include <chrono>
+#include <limits>
+#include <memory>
+#include <string>
+#include <thread>
+#include <utility>
+
+#include "rcl/error_handling.h"
+#include "rcl/time.h"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/duration.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "test_msgs/msg/empty.hpp"
+
+using namespace std::chrono_literals;
+
+template<typename T>
+class TestExecutors : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  void SetUp()
+  {
+    const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info();
+    std::stringstream test_name;
+    test_name << test_info->test_case_name() << "_" << test_info->name();
+    node = std::make_shared<rclcpp::Node>("node", test_name.str());
+
+    callback_count = 0;
+
+    const std::string topic_name = std::string("topic_") + test_name.str();
+    publisher = node->create_publisher<test_msgs::msg::Empty>(topic_name, rclcpp::QoS(10));
+    auto callback = [this](test_msgs::msg::Empty::SharedPtr) {this->callback_count++;};
+    subscription =
+      node->create_subscription<test_msgs::msg::Empty>(
+      topic_name, rclcpp::QoS(10), std::move(callback));
+  }
+
+  void TearDown()
+  {
+    publisher.reset();
+    subscription.reset();
+    node.reset();
+  }
+
+  rclcpp::Node::SharedPtr node;
+  rclcpp::Publisher<test_msgs::msg::Empty>::SharedPtr publisher;
+  rclcpp::Subscription<test_msgs::msg::Empty>::SharedPtr subscription;
+  int callback_count;
+};
+
+// spin_all and spin_some are not implemented correctly in StaticSingleThreadedExecutor, see:
+// https://github.com/ros2/rclcpp/issues/1219 for tracking
+template<typename T>
+class TestExecutorsStable : public TestExecutors<T> {};
+
+using ExecutorTypes =
+  ::testing::Types<
+  rclcpp::executors::SingleThreadedExecutor,
+  rclcpp::executors::MultiThreadedExecutor,
+  rclcpp::executors::StaticSingleThreadedExecutor>;
+
+class ExecutorTypeNames
+{
+public:
+  template<typename T>
+  static std::string GetName(int idx)
+  {
+    (void)idx;
+    if (std::is_same<T, rclcpp::executors::SingleThreadedExecutor>()) {
+      return "SingleThreadedExecutor";
+    }
+
+    if (std::is_same<T, rclcpp::executors::MultiThreadedExecutor>()) {
+      return "MultiThreadedExecutor";
+    }
+
+    if (std::is_same<T, rclcpp::executors::StaticSingleThreadedExecutor>()) {
+      return "StaticSingleThreadedExecutor";
+    }
+
+    return "";
+  }
+};
+
+// TYPED_TEST_CASE is deprecated as of gtest 1.9, use TYPED_TEST_SUITE when gtest dependency
+// is updated.
+TYPED_TEST_CASE(TestExecutors, ExecutorTypes, ExecutorTypeNames);
+
+// StaticSingleThreadedExecutor is not included in these tests for now, due to:
+// https://github.com/ros2/rclcpp/issues/1219
+using StandardExecutors =
+  ::testing::Types<
+  rclcpp::executors::SingleThreadedExecutor,
+  rclcpp::executors::MultiThreadedExecutor>;
+TYPED_TEST_CASE(TestExecutorsStable, StandardExecutors, ExecutorTypeNames);
+
+// Make sure that executors detach from nodes when destructing
+TYPED_TEST(TestExecutors, detachOnDestruction) {
+  using ExecutorType = TypeParam;
+  {
+    ExecutorType executor;
+    executor.add_node(this->node);
+  }
+  {
+    ExecutorType executor;
+    EXPECT_NO_THROW(executor.add_node(this->node));
+  }
+}
+
+// Make sure that the executor can automatically remove expired nodes correctly
+// Currently fails for StaticSingleThreadedExecutor so it is being skipped, see:
+// https://github.com/ros2/rclcpp/issues/1231
+TYPED_TEST(TestExecutorsStable, addTemporaryNode) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+
+  {
+    // Let node go out of scope before executor.spin()
+    auto node = std::make_shared<rclcpp::Node>("temporary_node");
+    executor.add_node(node);
+  }
+
+  // Sleep for a short time to verify executor.spin() is going, and didn't throw.
+  std::thread spinner([&]() {EXPECT_NO_THROW(executor.spin());});
+
+  std::this_thread::sleep_for(50ms);
+  executor.cancel();
+  spinner.join();
+}
+
+// Check executor throws properly if the same node is added a second time
+TYPED_TEST(TestExecutors, addNodeTwoExecutors) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor1;
+  ExecutorType executor2;
+  EXPECT_NO_THROW(executor1.add_node(this->node));
+  EXPECT_THROW(executor2.add_node(this->node), std::runtime_error);
+  executor1.remove_node(this->node, true);
+}
+
+// Check simple spin example
+TYPED_TEST(TestExecutors, spinWithTimer) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+
+  bool timer_completed = false;
+  auto timer = this->node->create_wall_timer(1ms, [&]() {timer_completed = true;});
+  executor.add_node(this->node);
+
+  std::thread spinner([&]() {executor.spin();});
+
+  auto start = std::chrono::steady_clock::now();
+  while (!timer_completed && (std::chrono::steady_clock::now() - start) < 10s) {
+    std::this_thread::sleep_for(1ms);
+  }
+
+  EXPECT_TRUE(timer_completed);
+  // Cancel needs to be called before join, so that executor.spin() returns.
+  executor.cancel();
+  spinner.join();
+  executor.remove_node(this->node, true);
+}
+
+TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+  executor.add_node(this->node);
+
+  bool timer_completed = false;
+  auto timer = this->node->create_wall_timer(1ms, [&]() {timer_completed = true;});
+
+  std::thread spinner([&]() {executor.spin();});
+  // Sleep for a short time to verify executor.spin() is going, and didn't throw.
+
+  auto start = std::chrono::steady_clock::now();
+  while (!timer_completed && (std::chrono::steady_clock::now() - start) < 10s) {
+    std::this_thread::sleep_for(1ms);
+  }
+
+  EXPECT_TRUE(timer_completed);
+  EXPECT_THROW(executor.spin(), std::runtime_error);
+
+  // Shutdown needs to be called before join, so that executor.spin() returns.
+  executor.cancel();
+  spinner.join();
+  executor.remove_node(this->node, true);
+}
+
+// Check executor exits immediately if future is complete.
+TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+  executor.add_node(this->node);
+
+  // test success of an immediately finishing future
+  std::promise<bool> promise;
+  std::future<bool> future = promise.get_future();
+  promise.set_value(true);
+
+  // spin_until_future_complete is expected to exit immediately, but would block up until its
+  // timeout if the future is not checked before spin_once_impl.
+  auto start = std::chrono::steady_clock::now();
+  auto shared_future = future.share();
+  auto ret = executor.spin_until_future_complete(shared_future, 1s);
+  executor.remove_node(this->node, true);
+  // Check it didn't reach timeout
+  EXPECT_GT(500ms, (std::chrono::steady_clock::now() - start));
+  EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret);
+}
+
+class TestWaitable : public rclcpp::Waitable
+{
+public:
+  TestWaitable()
+  {
+    rcl_guard_condition_options_t guard_condition_options =
+      rcl_guard_condition_get_default_options();
+
+    gc_ = rcl_get_zero_initialized_guard_condition();
+    rcl_ret_t ret = rcl_guard_condition_init(
+      &gc_,
+      rclcpp::contexts::get_global_default_context()->get_rcl_context().get(),
+      guard_condition_options);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret);
+    }
+  }
+
+  ~TestWaitable()
+  {
+    rcl_ret_t ret = rcl_guard_condition_fini(&gc_);
+    if (RCL_RET_OK != ret) {
+      fprintf(stderr, "failed to call rcl_guard_condition_fini\n");
+    }
+  }
+
+  bool
+  add_to_wait_set(rcl_wait_set_t * wait_set) override
+  {
+    rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &gc_, NULL);
+    if (RCL_RET_OK != ret) {
+      return false;
+    }
+    ret = rcl_trigger_guard_condition(&gc_);
+    return RCL_RET_OK == ret;
+  }
+
+  bool
+  is_ready(rcl_wait_set_t * wait_set) override
+  {
+    (void)wait_set;
+    return true;
+  }
+
+  void
+  execute() override
+  {
+    count_++;
+    std::this_thread::sleep_for(1ms);
+  }
+
+  size_t
+  get_number_of_ready_guard_conditions() override {return 1;}
+
+  size_t
+  get_count()
+  {
+    return count_;
+  }
+
+private:
+  size_t count_ = 0;
+  rcl_guard_condition_t gc_;
+};
+
+TYPED_TEST(TestExecutorsStable, spinSome) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+  auto waitable_interfaces = this->node->get_node_waitables_interface();
+  auto my_waitable = std::make_shared<TestWaitable>();
+  waitable_interfaces->add_waitable(my_waitable, nullptr);
+  executor.add_node(this->node);
+
+  // Long timeout, doesn't block test from finishing because spin_some should exit after the
+  // first one completes.
+  bool spin_exited = false;
+  std::thread spinner([&spin_exited, &executor, this]() {
+      executor.spin_some(1s);
+      executor.remove_node(this->node, true);
+      spin_exited = true;
+    });
+
+  // Do some work until sufficient calls to the waitable occur, but keep going until either
+  // count becomes too large, spin exits, or the 1 second timeout completes.
+  auto start = std::chrono::steady_clock::now();
+  while (
+    my_waitable->get_count() <= 1 &&
+    !spin_exited &&
+    (std::chrono::steady_clock::now() - start < 1s))
+  {
+    this->publisher->publish(test_msgs::msg::Empty());
+    std::this_thread::sleep_for(1ms);
+  }
+
+  EXPECT_EQ(1u, my_waitable->get_count());
+  waitable_interfaces->remove_waitable(my_waitable, nullptr);
+  EXPECT_TRUE(spin_exited);
+  // Cancel if it hasn't exited already.
+  executor.cancel();
+
+  spinner.join();
+}
+
+// Check spin_node_until_future_complete with node base pointer
+TYPED_TEST(TestExecutorsStable, testSpinNodeUntilFutureCompleteNodeBasePtr) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+
+  std::promise<bool> promise;
+  std::future<bool> future = promise.get_future();
+  promise.set_value(true);
+
+  auto shared_future = future.share();
+  auto ret = rclcpp::executors::spin_node_until_future_complete(
+    executor, this->node->get_node_base_interface(), shared_future, 1s);
+  EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret);
+}
+
+// Check spin_node_until_future_complete with node pointer
+TYPED_TEST(TestExecutorsStable, testSpinNodeUntilFutureCompleteNodePtr) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+
+  std::promise<bool> promise;
+  std::future<bool> future = promise.get_future();
+  promise.set_value(true);
+
+  auto shared_future = future.share();
+  auto ret = rclcpp::executors::spin_node_until_future_complete(
+    executor, this->node, shared_future, 1s);
+  EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret);
+}
+
+// Check spin_until_future_complete with node base pointer (instantiates its own executor)
+TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) {
+  rclcpp::init(0, nullptr);
+
+  {
+    auto node = std::make_shared<rclcpp::Node>("node");
+
+    std::promise<bool> promise;
+    std::future<bool> future = promise.get_future();
+    promise.set_value(true);
+
+    auto shared_future = future.share();
+    auto ret = rclcpp::spin_until_future_complete(
+      node->get_node_base_interface(), shared_future, 1s);
+    EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret);
+  }
+
+  rclcpp::shutdown();
+}
+
+// Check spin_until_future_complete with node pointer (instantiates its own executor)
+TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr) {
+  rclcpp::init(0, nullptr);
+
+  {
+    auto node = std::make_shared<rclcpp::Node>("node");
+
+    std::promise<bool> promise;
+    std::future<bool> future = promise.get_future();
+    promise.set_value(true);
+
+    auto shared_future = future.share();
+    auto ret = rclcpp::spin_until_future_complete(node, shared_future, 1s);
+    EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret);
+  }
+
+  rclcpp::shutdown();
+}

rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp

@@ -83,7 +83,7 @@ TEST_F(TestMultiThreadedExecutor, timer_over_take) {
 
       {
         std::lock_guard<std::mutex> lock(last_mutex);
-        double diff = std::abs((now - last).nanoseconds()) / 1.0e9;
+        double diff = static_cast<double>(std::abs((now - last).nanoseconds())) / 1.0e9;
         last = now;
 
         if (diff < PERIOD - TOLERANCE) {

rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp

@@ -0,0 +1,298 @@
+// 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/rclcpp.hpp"
+
+#include "test_msgs/msg/empty.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+namespace
+{
+
+struct NumberOfEntities
+{
+  size_t subscriptions = 0;
+  size_t timers = 0;
+  size_t services = 0;
+  size_t clients = 0;
+  size_t waitables = 0;
+};
+
+std::unique_ptr<NumberOfEntities> get_number_of_default_entities(rclcpp::Node::SharedPtr node)
+{
+  auto number_of_entities = std::make_unique<NumberOfEntities>();
+  for (auto & weak_group : node->get_callback_groups()) {
+    auto group = weak_group.lock();
+    EXPECT_NE(nullptr, group);
+    if (!group || !group->can_be_taken_from().load()) {
+      return nullptr;
+    }
+    group->find_subscription_ptrs_if(
+      [&number_of_entities](rclcpp::SubscriptionBase::SharedPtr &)
+      {
+        number_of_entities->subscriptions++; return false;
+      });
+    group->find_timer_ptrs_if(
+      [&number_of_entities](rclcpp::TimerBase::SharedPtr &)
+      {
+        number_of_entities->timers++; return false;
+      });
+    group->find_service_ptrs_if(
+      [&number_of_entities](rclcpp::ServiceBase::SharedPtr &)
+      {
+        number_of_entities->services++; return false;
+      });
+    group->find_client_ptrs_if(
+      [&number_of_entities](rclcpp::ClientBase::SharedPtr &)
+      {
+        number_of_entities->clients++; return false;
+      });
+    group->find_waitable_ptrs_if(
+      [&number_of_entities](rclcpp::Waitable::SharedPtr &)
+      {
+        number_of_entities->waitables++; return false;
+      });
+  }
+
+  return number_of_entities;
+}
+
+}  // namespace
+
+class TestStaticExecutorEntitiesCollector : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+    entities_collector_ =
+      std::make_shared<rclcpp::executors::StaticExecutorEntitiesCollector>();
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+
+  rclcpp::executors::StaticExecutorEntitiesCollector::SharedPtr entities_collector_;
+};
+
+TEST_F(TestStaticExecutorEntitiesCollector, construct_destruct) {
+  EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_timers());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_services());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_clients());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_waitables());
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node) {
+  auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
+  EXPECT_NO_THROW(entities_collector_->add_node(node1->get_node_base_interface()));
+
+  // Check adding second time
+  EXPECT_THROW(entities_collector_->add_node(node1->get_node_base_interface()), std::runtime_error);
+
+  auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
+  EXPECT_FALSE(entities_collector_->remove_node(node2->get_node_base_interface()));
+  EXPECT_NO_THROW(entities_collector_->add_node(node2->get_node_base_interface()));
+
+  EXPECT_TRUE(entities_collector_->remove_node(node1->get_node_base_interface()));
+  EXPECT_FALSE(entities_collector_->remove_node(node1->get_node_base_interface()));
+  EXPECT_TRUE(entities_collector_->remove_node(node2->get_node_base_interface()));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, init_bad_arguments) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+
+  rclcpp::GuardCondition guard_condition(shared_context);
+  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
+
+  // Check memory strategy is nullptr
+  rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy = nullptr;
+  EXPECT_THROW(
+    entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition),
+    std::runtime_error);
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, add_remove_basic_node) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  const auto expected_number_of_entities = get_number_of_default_entities(node);
+  EXPECT_NE(nullptr, expected_number_of_entities);
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+
+  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
+  rclcpp::GuardCondition guard_condition(shared_context);
+  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
+
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+  EXPECT_EQ(
+    expected_number_of_entities->subscriptions,
+    entities_collector_->get_number_of_subscriptions());
+  EXPECT_EQ(expected_number_of_entities->timers, entities_collector_->get_number_of_timers());
+  EXPECT_EQ(expected_number_of_entities->services, entities_collector_->get_number_of_services());
+  EXPECT_EQ(expected_number_of_entities->clients, entities_collector_->get_number_of_clients());
+  // One extra for the executor
+  EXPECT_EQ(
+    1u + expected_number_of_entities->waitables,
+    entities_collector_->get_number_of_waitables());
+
+  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_timers());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_services());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_clients());
+
+  // Still one for the executor
+  EXPECT_EQ(1u, entities_collector_->get_number_of_waitables());
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) {
+  rclcpp::Context::SharedPtr shared_context = nullptr;
+  {
+    auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
+    auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
+    auto node3 = std::make_shared<rclcpp::Node>("node3", "ns");
+    entities_collector_->add_node(node1->get_node_base_interface());
+    entities_collector_->add_node(node2->get_node_base_interface());
+    entities_collector_->add_node(node3->get_node_base_interface());
+    shared_context = node1->get_node_base_interface()->get_context();
+  }
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+
+  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
+  rclcpp::GuardCondition guard_condition(shared_context);
+  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
+
+  // Expect weak_node pointers to be cleaned up and used
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_timers());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_services());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_clients());
+
+  // Still one for the executor
+  EXPECT_EQ(1u, entities_collector_->get_number_of_waitables());
+}
+
+class TestWaitable : public rclcpp::Waitable
+{
+public:
+  bool add_to_wait_set(rcl_wait_set_t *) override {return true;}
+
+  bool is_ready(rcl_wait_set_t *) override {return true;}
+
+  void execute() override {}
+};
+
+TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  auto expected_number_of_entities = get_number_of_default_entities(node);
+  EXPECT_NE(nullptr, expected_number_of_entities);
+
+  // Create 1 of each entity type
+  auto subscription =
+    node->create_subscription<test_msgs::msg::Empty>(
+    "topic", rclcpp::QoS(10), [](test_msgs::msg::Empty::SharedPtr) {});
+  auto timer =
+    node->create_wall_timer(std::chrono::seconds(60), []() {});
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  auto waitable = std::make_shared<TestWaitable>();
+
+  // Adding a subscription with rmw_connext_cpp adds another waitable, so we need to get the
+  // current number of waitables just before adding the new waitable.
+  expected_number_of_entities->waitables = get_number_of_default_entities(node)->waitables;
+  node->get_node_waitables_interface()->add_waitable(waitable, nullptr);
+
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+
+  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
+
+  rclcpp::GuardCondition guard_condition(shared_context);
+  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
+
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+
+  EXPECT_EQ(
+    1u + expected_number_of_entities->subscriptions,
+    entities_collector_->get_number_of_subscriptions());
+  EXPECT_EQ(1u + expected_number_of_entities->timers, entities_collector_->get_number_of_timers());
+  EXPECT_EQ(
+    1u + expected_number_of_entities->services,
+    entities_collector_->get_number_of_services());
+  EXPECT_EQ(
+    1u + expected_number_of_entities->clients,
+    entities_collector_->get_number_of_clients());
+
+  // One extra for the executor
+  EXPECT_EQ(
+    2u + expected_number_of_entities->waitables,
+    entities_collector_->get_number_of_waitables());
+
+  entities_collector_->remove_node(node->get_node_base_interface());
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_timers());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_services());
+  EXPECT_EQ(0u, entities_collector_->get_number_of_clients());
+  // Still one for the executor
+  EXPECT_EQ(1u, entities_collector_->get_number_of_waitables());
+}

rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp

@@ -0,0 +1,48 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <memory>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/executors.hpp"
+
+using namespace std::chrono_literals;
+
+class TestStaticSingleThreadedExecutor : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+};
+
+TEST_F(TestStaticSingleThreadedExecutor, check_unimplemented) {
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  executor.add_node(node);
+
+  EXPECT_THROW(executor.spin_some(), rclcpp::exceptions::UnimplementedError);
+  EXPECT_THROW(executor.spin_once(0ns), rclcpp::exceptions::UnimplementedError);
+}

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 NODE_INTERFACES__NODE_WRAPPER_HPP_
-#define NODE_INTERFACES__NODE_WRAPPER_HPP_
+#ifndef RCLCPP__NODE_INTERFACES__NODE_WRAPPER_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_WRAPPER_HPP_
 
 #include <memory>
 #include <string>
@@ -61,4 +61,4 @@ private:
   rclcpp::Node::SharedPtr node;
 };
 
-#endif  // NODE_INTERFACES__NODE_WRAPPER_HPP_
+#endif  // RCLCPP__NODE_INTERFACES__NODE_WRAPPER_HPP_

rclcpp/test/rclcpp/node_interfaces/test_node_base.cpp

@@ -0,0 +1,193 @@
+// 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 <string>
+
+#include "rcl/node_options.h"
+#include "rclcpp/node.hpp"
+#include "rclcpp/node_interfaces/node_base.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rmw/validate_namespace.h"
+#include "rmw/validate_node_name.h"
+
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+class TestNodeBase : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+};
+
+// Required for mocking_utils below
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, >)
+
+TEST_F(TestNodeBase, construct_from_node)
+{
+  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
+
+  // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
+  // the proper type is being tested and covered.
+  auto * node_base =
+    dynamic_cast<rclcpp::node_interfaces::NodeBase *>(node->get_node_base_interface().get());
+  ASSERT_NE(nullptr, node_base);
+
+  EXPECT_STREQ("node", node_base->get_name());
+  EXPECT_STREQ("/ns", node_base->get_namespace());
+
+  EXPECT_STREQ("/ns/node", node_base->get_fully_qualified_name());
+  EXPECT_NE(nullptr, node_base->get_context());
+  EXPECT_NE(nullptr, node_base->get_rcl_node_handle());
+  EXPECT_NE(nullptr, node_base->get_shared_rcl_node_handle());
+
+  const auto * const_node_base = node_base;
+  EXPECT_NE(nullptr, const_node_base->get_rcl_node_handle());
+  EXPECT_NE(nullptr, const_node_base->get_shared_rcl_node_handle());
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_guard_condition_init_error) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_guard_condition_init, RCL_RET_ERROR);
+  EXPECT_THROW(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_error) {
+  auto mock_node_init = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_node_init, RCL_RET_ERROR);
+
+  // This function is called only if rcl_node_init fails, so both mocked functions are required
+  // This just logs an error, so behavior shouldn't change
+  auto mock_guard_condition_fini = mocking_utils::inject_on_return(
+    "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR);
+
+  EXPECT_THROW(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_node_name) {
+  auto mock_node_init = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAME);
+
+  // `rmw_validate_node_name` is only called if `rcl_node_init` returns INVALID_NAME
+  auto mock_validate_node_name = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_node_name, RMW_RET_ERROR);
+
+  EXPECT_THROW(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_node_name_invalid_argument) {
+  auto mock_node_init = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAME);
+
+  // `rmw_validate_node_name` is only called if `rcl_node_init` returns INVALID_NAME
+  auto mock_validate_node_name = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_node_name, RMW_RET_INVALID_ARGUMENT);
+
+  EXPECT_THROW(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    rclcpp::exceptions::RCLInvalidArgument);
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_node_name_valid_rmw_node_name) {
+  auto mock_node_init = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAME);
+
+  // `rmw_validate_node_name` is only called if `rcl_node_init` returns INVALID_NAME
+  auto mock = mocking_utils::patch(
+    "lib:rclcpp", rmw_validate_node_name, [](const char *, int * validation_result, size_t *)
+    {
+      *validation_result = RMW_NODE_NAME_VALID;
+      return RMW_RET_OK;
+    });
+
+  RCLCPP_EXPECT_THROW_EQ(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    std::runtime_error("valid rmw node name but invalid rcl node name"));
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_namespace) {
+  auto mock_node_init = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAMESPACE);
+
+  // `rmw_validate_namespace` is only called if `rcl_node_init` returns INVALID_NAMESPACE
+  auto mock_validate_namespace = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_namespace, RMW_RET_ERROR);
+
+  EXPECT_THROW(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_namespace_rmw_invalid_argument) {
+  auto mock_node_init = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAMESPACE);
+
+  // `rmw_validate_namespace` is only called if `rcl_node_init` returns INVALID_NAMESPACE
+  auto mock_validate_namespace = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_namespace, RMW_RET_INVALID_ARGUMENT);
+
+  EXPECT_THROW(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    rclcpp::exceptions::RCLInvalidArgument);
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_namespace_valid_rmw_namespace) {
+  auto mock_node_init = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAMESPACE);
+
+  // `rmw_validate_namespace` is only called if `rcl_node_init` returns INVALID_NAMESPACE
+  auto mock = mocking_utils::patch(
+    "lib:rclcpp", rmw_validate_namespace, [](const char *, int * validation_result, size_t *)
+    {
+      *validation_result = RMW_NAMESPACE_VALID;
+      return RMW_RET_OK;
+    });
+
+  RCLCPP_EXPECT_THROW_EQ(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    std::runtime_error("valid rmw node namespace but invalid rcl node namespace"));
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_fini_error) {
+  auto mock_node_fini = mocking_utils::inject_on_return(
+    "lib:rclcpp", rcl_node_fini, RCL_RET_ERROR);
+
+  EXPECT_NO_THROW(std::make_shared<rclcpp::Node>("node", "ns").reset());
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_guard_condition_fini_error) {
+  auto mock_node_fini = mocking_utils::inject_on_return(
+    "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR);
+
+  EXPECT_NO_THROW(std::make_shared<rclcpp::Node>("node", "ns").reset());
+}

rclcpp/test/rclcpp/node_interfaces/test_node_clock.cpp

@@ -0,0 +1,49 @@
+// 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/node_interfaces/node_clock.hpp"
+#include "rclcpp/node.hpp"
+
+class TestNodeClock : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+};
+
+TEST_F(TestNodeClock, construct_from_node)
+{
+  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
+
+  // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
+  // the proper type is being tested and covered.
+  auto * node_clock =
+    dynamic_cast<rclcpp::node_interfaces::NodeClock *>(node->get_node_clock_interface().get());
+  ASSERT_NE(nullptr, node_clock);
+  EXPECT_NE(nullptr, node_clock->get_clock());
+
+  const auto * const_node_clock = node_clock;
+  EXPECT_NE(nullptr, const_node_clock->get_clock());
+}

rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp

@@ -0,0 +1,445 @@
+// 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 <string>
+#include <utility>
+
+#include "rcl/graph.h"
+#include "rcl/node_options.h"
+#include "rcl/remap.h"
+#include "rclcpp/node.hpp"
+#include "rclcpp/node_interfaces/node_base.hpp"
+#include "rclcpp/node_interfaces/node_graph.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rcutils/strdup.h"
+#include "test_msgs/msg/empty.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+namespace
+{
+
+constexpr char node_name[] = "node";
+constexpr char node_namespace[] = "ns";
+constexpr char absolute_namespace[] = "/ns";
+
+}  // namespace
+
+class TestNodeGraph : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+    node_ = std::make_shared<rclcpp::Node>(node_name, node_namespace);
+
+    // This dynamic cast is not necessary for the unittests, but instead is used to ensure
+    // the proper type is being tested and covered.
+    node_graph_ =
+      dynamic_cast<rclcpp::node_interfaces::NodeGraph *>(node_->get_node_graph_interface().get());
+    ASSERT_NE(nullptr, node_graph_);
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+
+protected:
+  std::shared_ptr<rclcpp::Node> node() {return node_;}
+
+  const rclcpp::node_interfaces::NodeGraph * node_graph() const {return node_graph_;}
+
+private:
+  std::shared_ptr<rclcpp::Node> node_;
+  rclcpp::node_interfaces::NodeGraph * node_graph_;
+};
+
+TEST_F(TestNodeGraph, construct_from_node)
+{
+  auto topic_names_and_types = node_graph()->get_topic_names_and_types(false);
+  EXPECT_LT(0u, topic_names_and_types.size());
+
+  auto service_names_and_types = node_graph()->get_service_names_and_types();
+  EXPECT_LT(0u, service_names_and_types.size());
+
+  auto names = node_graph()->get_node_names();
+  EXPECT_EQ(1u, names.size());
+
+  auto names_and_namespaces = node_graph()->get_node_names_and_namespaces();
+  EXPECT_EQ(1u, names_and_namespaces.size());
+
+  EXPECT_EQ(0u, node_graph()->count_publishers("not_a_topic"));
+  EXPECT_EQ(0u, node_graph()->count_subscribers("not_a_topic"));
+  EXPECT_NE(nullptr, node_graph()->get_graph_guard_condition());
+
+  // get_graph_event is non-const
+  EXPECT_NE(nullptr, node()->get_node_graph_interface()->get_graph_event());
+  EXPECT_EQ(1u, node()->get_node_graph_interface()->count_graph_users());
+}
+
+TEST_F(TestNodeGraph, get_topic_names_and_types)
+{
+  auto topic_names_and_types = node_graph()->get_topic_names_and_types();
+  EXPECT_LT(0u, topic_names_and_types.size());
+}
+
+TEST_F(TestNodeGraph, get_topic_names_and_types_rcl_error)
+{
+  auto mock_get_topic_names = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_topic_names_and_types, RCL_RET_ERROR);
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
+
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_topic_names_and_types(),
+    std::runtime_error(
+      "failed to get topic names and types: error not set, failed also to cleanup topic names and"
+      " types, leaking memory: error not set"));
+}
+
+TEST_F(TestNodeGraph, get_topic_names_and_types_rcl_names_and_types_fini_error)
+{
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
+
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_topic_names_and_types(),
+    std::runtime_error("could not destroy topic names and types: error not set"));
+}
+
+TEST_F(TestNodeGraph, get_service_names_and_types)
+{
+  auto service_names_and_types = node_graph()->get_service_names_and_types();
+  EXPECT_LT(0u, service_names_and_types.size());
+}
+
+TEST_F(TestNodeGraph, get_service_names_and_types_rcl_error)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_service_names_and_types, RCL_RET_ERROR);
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_service_names_and_types(),
+    std::runtime_error(
+      "failed to get service names and types: error not set, failed also to cleanup service names"
+      " and types, leaking memory: error not set"));
+}
+
+TEST_F(TestNodeGraph, get_service_names_and_types_rcl_names_and_types_fini)
+{
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_service_names_and_types(),
+    std::runtime_error("could not destroy service names and types: error not set"));
+}
+
+TEST_F(TestNodeGraph, get_service_names_and_types_by_node)
+{
+  auto callback = [](
+    const test_msgs::srv::Empty::Request::SharedPtr,
+    test_msgs::srv::Empty::Response::SharedPtr) {};
+  auto service =
+    node()->create_service<test_msgs::srv::Empty>("node1_service", std::move(callback));
+
+  const std::string node2_name = "node2";
+  auto node2 = std::make_shared<rclcpp::Node>(node2_name, node_namespace);
+
+  // rcl_get_service_names_and_types_by_node() expects the node to exist, otherwise it fails
+  EXPECT_THROW(
+    node_graph()->get_service_names_and_types_by_node("not_a_node", "not_absolute_namespace"),
+    std::runtime_error);
+
+  // Check that node1_service exists for node1 but not node2. This shouldn't exercise graph
+  // discovery as node_graph belongs to node1 anyway. This is just to test the API itself.
+  auto services_of_node1 =
+    node_graph()->get_service_names_and_types_by_node(node_name, absolute_namespace);
+  auto services_of_node2 =
+    node_graph()->get_service_names_and_types_by_node(node2_name, absolute_namespace);
+
+  auto start = std::chrono::steady_clock::now();
+  while (std::chrono::steady_clock::now() - start < std::chrono::seconds(1)) {
+    services_of_node1 =
+      node_graph()->get_service_names_and_types_by_node(node_name, absolute_namespace);
+    services_of_node2 =
+      node_graph()->get_service_names_and_types_by_node(node2_name, absolute_namespace);
+    if (services_of_node1.find("/ns/node1_service") != services_of_node1.end()) {
+      break;
+    }
+  }
+
+  EXPECT_TRUE(services_of_node1.find("/ns/node1_service") != services_of_node1.end());
+  EXPECT_FALSE(services_of_node2.find("/ns/node1_service") != services_of_node2.end());
+}
+
+TEST_F(TestNodeGraph, get_service_names_and_types_by_node_rcl_errors)
+{
+  auto callback = [](
+    const test_msgs::srv::Empty::Request::SharedPtr,
+    test_msgs::srv::Empty::Response::SharedPtr) {};
+  auto service =
+    node()->create_service<test_msgs::srv::Empty>("node1_service", std::move(callback));
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_service_names_and_types_by_node, RCL_RET_ERROR);
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_service_names_and_types_by_node(node_name, node_namespace),
+    std::runtime_error(
+      "failed to get service names and types by node: error not set, failed also to cleanup"
+      " service names and types, leaking memory: error not set"));
+}
+
+TEST_F(TestNodeGraph, get_service_names_and_types_by_node_names_and_types_fini_error)
+{
+  auto callback = [](
+    const test_msgs::srv::Empty::Request::SharedPtr,
+    test_msgs::srv::Empty::Response::SharedPtr) {};
+  auto service =
+    node()->create_service<test_msgs::srv::Empty>("node1_service", std::move(callback));
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
+
+  EXPECT_THROW(
+    node_graph()->get_service_names_and_types_by_node(node_name, absolute_namespace),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestNodeGraph, get_node_names_and_namespaces)
+{
+  auto names_and_namespaces = node_graph()->get_node_names_and_namespaces();
+  EXPECT_EQ(1u, names_and_namespaces.size());
+}
+
+TEST_F(TestNodeGraph, get_node_names_and_namespaces_rcl_errors)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_node_names, RCL_RET_ERROR);
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcutils_string_array_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_node_names_and_namespaces(),
+    std::runtime_error(
+      "failed to get node names: error not set, failed also to cleanup node names, leaking memory:"
+      " error not set, failed also to cleanup node namespaces, leaking memory: error not set"));
+}
+
+TEST_F(TestNodeGraph, get_node_names_and_namespaces_fini_errors)
+{
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcutils_string_array_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_node_names_and_namespaces(),
+    std::runtime_error("could not destroy node names, could not destroy node namespaces"));
+}
+
+TEST_F(TestNodeGraph, count_publishers_rcl_error)
+{
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_count_publishers, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->count_publishers("topic"),
+    std::runtime_error("could not count publishers: error not set"));
+}
+
+TEST_F(TestNodeGraph, count_subscribers_rcl_error)
+{
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_count_subscribers, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->count_subscribers("topic"),
+    std::runtime_error("could not count subscribers: error not set"));
+}
+
+TEST_F(TestNodeGraph, notify_shutdown)
+{
+  EXPECT_NO_THROW(node()->get_node_graph_interface()->notify_shutdown());
+}
+
+TEST_F(TestNodeGraph, wait_for_graph_change)
+{
+  auto node_graph_interface = node()->get_node_graph_interface();
+  EXPECT_NO_THROW(node_graph_interface->notify_graph_change());
+  EXPECT_THROW(
+    node_graph_interface->wait_for_graph_change(nullptr, std::chrono::milliseconds(1)),
+    rclcpp::exceptions::InvalidEventError);
+
+  auto event = std::make_shared<rclcpp::Event>();
+  EXPECT_THROW(
+    node_graph_interface->wait_for_graph_change(event, std::chrono::milliseconds(0)),
+    rclcpp::exceptions::EventNotRegisteredError);
+}
+
+TEST_F(TestNodeGraph, notify_graph_change_rcl_error)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  EXPECT_THROW(
+    node()->get_node_graph_interface()->notify_graph_change(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestNodeGraph, get_info_by_topic)
+{
+  const rclcpp::QoS publisher_qos(1);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+  auto callback = [](const test_msgs::msg::Empty::SharedPtr) {};
+
+  const rclcpp::QoS subscriber_qos(10);
+  auto subscription =
+    node()->create_subscription<test_msgs::msg::Empty>(
+    "topic", subscriber_qos, std::move(callback));
+
+  EXPECT_EQ(0u, node_graph()->get_publishers_info_by_topic("topic", true).size());
+
+  auto publishers = node_graph()->get_publishers_info_by_topic("topic", false);
+  ASSERT_EQ(1u, publishers.size());
+
+  auto publisher_endpoint_info = publishers[0];
+  const auto const_publisher_endpoint_info = publisher_endpoint_info;
+  EXPECT_STREQ(node_name, publisher_endpoint_info.node_name().c_str());
+  EXPECT_STREQ(node_name, const_publisher_endpoint_info.node_name().c_str());
+  EXPECT_STREQ(absolute_namespace, publisher_endpoint_info.node_namespace().c_str());
+  EXPECT_STREQ(absolute_namespace, const_publisher_endpoint_info.node_namespace().c_str());
+  EXPECT_STREQ("test_msgs/msg/Empty", publisher_endpoint_info.topic_type().c_str());
+  EXPECT_STREQ("test_msgs/msg/Empty", const_publisher_endpoint_info.topic_type().c_str());
+  EXPECT_EQ(rclcpp::EndpointType::Publisher, publisher_endpoint_info.endpoint_type());
+  EXPECT_EQ(rclcpp::EndpointType::Publisher, const_publisher_endpoint_info.endpoint_type());
+
+  rclcpp::QoS actual_qos = publisher_endpoint_info.qos_profile();
+  switch (actual_qos.get_rmw_qos_profile().history) {
+    case RMW_QOS_POLICY_HISTORY_KEEP_LAST:
+      EXPECT_EQ(1u, actual_qos.get_rmw_qos_profile().depth);
+      break;
+    case RMW_QOS_POLICY_HISTORY_UNKNOWN:
+      EXPECT_EQ(0u, actual_qos.get_rmw_qos_profile().depth);
+      break;
+    default:
+      ADD_FAILURE() << "unexpected history";
+  }
+
+  rclcpp::QoS const_actual_qos = const_publisher_endpoint_info.qos_profile();
+  switch (const_actual_qos.get_rmw_qos_profile().history) {
+    case RMW_QOS_POLICY_HISTORY_KEEP_LAST:
+      EXPECT_EQ(1u, const_actual_qos.get_rmw_qos_profile().depth);
+      break;
+    case RMW_QOS_POLICY_HISTORY_UNKNOWN:
+      EXPECT_EQ(0u, const_actual_qos.get_rmw_qos_profile().depth);
+      break;
+    default:
+      ADD_FAILURE() << "unexpected history";
+  }
+
+  auto endpoint_gid = publisher_endpoint_info.endpoint_gid();
+  auto const_endpoint_gid = const_publisher_endpoint_info.endpoint_gid();
+  bool endpoint_gid_is_all_zeros = true;
+  for (size_t i = 0; i < RMW_GID_STORAGE_SIZE; ++i) {
+    endpoint_gid_is_all_zeros &= (endpoint_gid[i] == 0);
+    EXPECT_EQ(endpoint_gid[i], const_endpoint_gid[i]);
+  }
+  EXPECT_FALSE(endpoint_gid_is_all_zeros);
+}
+
+TEST_F(TestNodeGraph, get_info_by_topic_rcl_node_get_options_error)
+{
+  const rclcpp::QoS publisher_qos(1);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_node_get_options, nullptr);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_publishers_info_by_topic("topic", false),
+    std::runtime_error("Need valid node options in get_info_by_topic()"));
+}
+
+// Required for mocking_utils below
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >)
+
+TEST_F(TestNodeGraph, get_info_by_topic_rcl_remap_topic_name_error)
+{
+  const rclcpp::QoS publisher_qos(1);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_remap_topic_name, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_publishers_info_by_topic("topic", false),
+    std::runtime_error("Failed to remap topic name /ns/topic: error not set"));
+}
+
+TEST_F(TestNodeGraph, get_info_by_topic_rcl_remap_topic_name_nullptr)
+{
+  const rclcpp::QoS publisher_qos(1);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+
+  // Should be cleaned up by get_info_by_topic
+  char * some_string = rcutils_strdup("", rcl_get_default_allocator());
+  ASSERT_NE(nullptr, some_string);
+  auto mock =
+    mocking_utils::patch(
+    "lib:rclcpp", rcl_remap_topic_name, [&some_string](
+      const rcl_arguments_t *, const rcl_arguments_t *, const char *, const char *, const char *,
+      rcl_allocator_t, char ** output_name)
+    {
+      *output_name = some_string;
+      return RCL_RET_OK;
+    });
+  EXPECT_NO_THROW(node_graph()->get_publishers_info_by_topic("topic", false));
+}
+
+TEST_F(TestNodeGraph, get_info_by_topic_rcl_errors)
+{
+  const rclcpp::QoS publisher_qos(1);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_publishers_info_by_topic, RCL_RET_ERROR);
+  auto mock_info_array_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_topic_endpoint_info_array_fini, RCL_RET_ERROR);
+  EXPECT_THROW(
+    node_graph()->get_publishers_info_by_topic("topic", false),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestNodeGraph, get_info_by_topic_unsupported)
+{
+  const rclcpp::QoS publisher_qos(1);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_publishers_info_by_topic, RCL_RET_UNSUPPORTED);
+  EXPECT_THROW(
+    node_graph()->get_publishers_info_by_topic("topic", false),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestNodeGraph, get_info_by_topic_endpoint_info_array_fini_error)
+{
+  const rclcpp::QoS publisher_qos(1);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+
+  auto mock_info_array_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_topic_endpoint_info_array_fini, RCL_RET_ERROR);
+  EXPECT_THROW(
+    node_graph()->get_publishers_info_by_topic("topic", false),
+    rclcpp::exceptions::RCLError);
+}

rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp

@@ -0,0 +1,201 @@
+// 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.
+
+/**
+ * NodeParameters is a complicated interface with lots of code, but it is tested elsewhere
+ * very thoroughly. This currently just includes unittests for the currently uncovered
+ * functionality.
+ */
+
+#include <gtest/gtest.h>
+
+#include <algorithm>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/node.hpp"
+#include "rclcpp/node_interfaces/node_parameters.hpp"
+
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+class TestNodeParameters : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+    rclcpp::NodeOptions options;
+    options.allow_undeclared_parameters(true);
+    node = std::make_shared<rclcpp::Node>("node", "ns", options);
+
+    // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
+    // the proper type is being tested and covered.
+    node_parameters =
+      dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
+      node->get_node_parameters_interface().get());
+    ASSERT_NE(nullptr, node_parameters);
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+
+protected:
+  std::shared_ptr<rclcpp::Node> node;
+  rclcpp::node_interfaces::NodeParameters * node_parameters;
+};
+
+TEST_F(TestNodeParameters, construct_destruct_rcl_errors) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_arguments_get_param_overrides, RCL_RET_ERROR);
+  EXPECT_THROW(
+    std::make_shared<rclcpp::Node>("node2", "ns").reset(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestNodeParameters, list_parameters)
+{
+  std::vector<std::string> prefixes;
+  const auto list_result = node_parameters->list_parameters(prefixes, 1u);
+
+  // Currently the only default parameter is 'use_sim_time', but that may change.
+  size_t number_of_parameters = list_result.names.size();
+  EXPECT_GE(1u, number_of_parameters);
+
+  const std::string parameter_name = "new_parameter";
+  const rclcpp::ParameterValue value(true);
+  const rcl_interfaces::msg::ParameterDescriptor descriptor;
+  const auto added_parameter_value =
+    node_parameters->declare_parameter(parameter_name, value, descriptor, false);
+  EXPECT_EQ(value.get<bool>(), added_parameter_value.get<bool>());
+
+  auto list_result2 = node_parameters->list_parameters(prefixes, 1u);
+  EXPECT_EQ(number_of_parameters + 1u, list_result2.names.size());
+
+  EXPECT_NE(
+    std::find(list_result2.names.begin(), list_result2.names.end(), parameter_name),
+    list_result2.names.end());
+
+  // Check prefixes
+  const std::string parameter_name2 = "prefix.new_parameter";
+  const rclcpp::ParameterValue value2(true);
+  const rcl_interfaces::msg::ParameterDescriptor descriptor2;
+  const auto added_parameter_value2 =
+    node_parameters->declare_parameter(parameter_name2, value2, descriptor2, false);
+  EXPECT_EQ(value.get<bool>(), added_parameter_value.get<bool>());
+  prefixes = {"prefix"};
+  auto list_result3 = node_parameters->list_parameters(prefixes, 2u);
+  EXPECT_EQ(1u, list_result3.names.size());
+  EXPECT_NE(
+    std::find(list_result3.names.begin(), list_result3.names.end(), parameter_name2),
+    list_result3.names.end());
+
+  // Check if prefix equals parameter name
+  prefixes = {"new_parameter"};
+  auto list_result4 = node_parameters->list_parameters(prefixes, 2u);
+  EXPECT_EQ(1u, list_result4.names.size());
+  EXPECT_NE(
+    std::find(list_result4.names.begin(), list_result4.names.end(), parameter_name),
+    list_result4.names.end());
+}
+
+TEST_F(TestNodeParameters, parameter_overrides)
+{
+  rclcpp::NodeOptions node_options;
+  node_options.automatically_declare_parameters_from_overrides(true);
+  node_options.append_parameter_override("param1", true);
+  node_options.append_parameter_override("param2", 42);
+
+  std::shared_ptr<rclcpp::Node> node2 = std::make_shared<rclcpp::Node>("node2", "ns", node_options);
+
+  auto * node_parameters_interface =
+    dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
+    node2->get_node_parameters_interface().get());
+  ASSERT_NE(nullptr, node_parameters_interface);
+
+  const auto & parameter_overrides = node_parameters_interface->get_parameter_overrides();
+  EXPECT_EQ(2u, parameter_overrides.size());
+}
+
+TEST_F(TestNodeParameters, set_parameters) {
+  rclcpp::NodeOptions node_options;
+  node_options.allow_undeclared_parameters(true);
+
+  rcl_interfaces::msg::ParameterDescriptor bool_descriptor;
+  bool_descriptor.name = "bool_parameter";
+  bool_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
+  bool_descriptor.read_only = false;
+  node_parameters->declare_parameter(
+    "bool_parameter", rclcpp::ParameterValue(false), bool_descriptor, false);
+
+  rcl_interfaces::msg::ParameterDescriptor read_only_descriptor;
+  read_only_descriptor.name = "read_only_parameter";
+  read_only_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
+  read_only_descriptor.read_only = true;
+  node_parameters->declare_parameter(
+    "read_only_parameter", rclcpp::ParameterValue(42), read_only_descriptor, false);
+
+  const std::vector<rclcpp::Parameter> parameters = {
+    rclcpp::Parameter("bool_parameter", true),
+    rclcpp::Parameter("read_only_parameter", 42),
+  };
+  auto result = node_parameters->set_parameters(parameters);
+  ASSERT_EQ(parameters.size(), result.size());
+  EXPECT_TRUE(result[0].successful);
+  EXPECT_FALSE(result[1].successful);
+  EXPECT_STREQ(
+    "parameter 'read_only_parameter' cannot be set because it is read-only",
+    result[1].reason.c_str());
+
+  RCLCPP_EXPECT_THROW_EQ(
+    node_parameters->set_parameters({rclcpp::Parameter("", true)}),
+    rclcpp::exceptions::InvalidParametersException("parameter name must not be empty"));
+
+  result = node_parameters->set_parameters({rclcpp::Parameter("undeclared_parameter", 3.14159)});
+  ASSERT_EQ(1u, result.size());
+  EXPECT_TRUE(result[0].successful);
+}
+
+TEST_F(TestNodeParameters, add_remove_parameters_callback) {
+  rcl_interfaces::msg::ParameterDescriptor bool_descriptor;
+  bool_descriptor.name = "bool_parameter";
+  bool_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
+  bool_descriptor.read_only = false;
+  node_parameters->declare_parameter(
+    "bool_parameter", rclcpp::ParameterValue(false), bool_descriptor, false);
+  const std::vector<rclcpp::Parameter> parameters = {rclcpp::Parameter("bool_parameter", true)};
+
+  const std::string reason = "some totally not made up reason";
+  auto callback = [reason](const std::vector<rclcpp::Parameter> &) {
+      rcl_interfaces::msg::SetParametersResult result;
+      result.successful = false;
+      result.reason = reason;
+      return result;
+    };
+
+  auto handle = node_parameters->add_on_set_parameters_callback(callback);
+  auto result = node_parameters->set_parameters(parameters);
+  ASSERT_EQ(1u, result.size());
+  EXPECT_FALSE(result[0].successful);
+  EXPECT_EQ(reason, result[0].reason);
+
+  EXPECT_NO_THROW(node_parameters->remove_on_set_parameters_callback(handle.get()));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    node_parameters->remove_on_set_parameters_callback(handle.get()),
+    std::runtime_error("Callback doesn't exist"));
+}

rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp

@@ -0,0 +1,131 @@
+// 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 <string>
+
+#include "rcl/node_options.h"
+#include "rclcpp/node.hpp"
+#include "rclcpp/node_interfaces/node_services.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+class TestService : public rclcpp::ServiceBase
+{
+public:
+  explicit TestService(rclcpp::Node * node)
+  : rclcpp::ServiceBase(node->get_node_base_interface()->get_shared_rcl_node_handle()) {}
+
+  std::shared_ptr<void> create_request() override {return nullptr;}
+  std::shared_ptr<rmw_request_id_t> create_request_header() override {return nullptr;}
+  void handle_request(std::shared_ptr<rmw_request_id_t>, std::shared_ptr<void>) override {}
+};
+
+class TestClient : public rclcpp::ClientBase
+{
+public:
+  explicit TestClient(rclcpp::Node * node)
+  : rclcpp::ClientBase(node->get_node_base_interface().get(), node->get_node_graph_interface()) {}
+
+  std::shared_ptr<void> create_response() override {return nullptr;}
+  std::shared_ptr<rmw_request_id_t> create_request_header() override {return nullptr;}
+  void handle_response(
+    std::shared_ptr<rmw_request_id_t>, std::shared_ptr<void>) override {}
+};
+
+class TestNodeService : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+
+    node = std::make_shared<rclcpp::Node>("node", "ns");
+
+    // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
+    // the proper type is being tested and covered.
+    node_services =
+      dynamic_cast<rclcpp::node_interfaces::NodeServices *>(
+      node->get_node_services_interface().get());
+    ASSERT_NE(nullptr, node_services);
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+
+protected:
+  std::shared_ptr<rclcpp::Node> node;
+  rclcpp::node_interfaces::NodeServices * node_services;
+};
+
+TEST_F(TestNodeService, add_service)
+{
+  auto service = std::make_shared<TestService>(node.get());
+  auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  EXPECT_NO_THROW(
+    node_services->add_service(service, callback_group));
+
+  // Check that adding a service from node to a callback group of different_node throws exception.
+  std::shared_ptr<rclcpp::Node> different_node = std::make_shared<rclcpp::Node>("node2", "ns");
+
+  auto callback_group_in_different_node =
+    different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_services->add_service(service, callback_group_in_different_node),
+    std::runtime_error("Cannot create service, group not in node."));
+}
+
+TEST_F(TestNodeService, add_service_rcl_trigger_guard_condition_error)
+{
+  auto service = std::make_shared<TestService>(node.get());
+  auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_services->add_service(service, callback_group),
+    std::runtime_error("Failed to notify wait set on service creation: error not set"));
+}
+
+TEST_F(TestNodeService, add_client)
+{
+  auto client = std::make_shared<TestClient>(node.get());
+  auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  EXPECT_NO_THROW(node_services->add_client(client, callback_group));
+
+  // Check that adding a client from node to a callback group of different_node throws exception.
+  std::shared_ptr<rclcpp::Node> different_node = std::make_shared<rclcpp::Node>("node2", "ns");
+
+  auto callback_group_in_different_node =
+    different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_services->add_client(client, callback_group_in_different_node),
+    std::runtime_error("Cannot create client, group not in node."));
+}
+
+TEST_F(TestNodeService, add_client_rcl_trigger_guard_condition_error)
+{
+  auto client = std::make_shared<TestClient>(node.get());
+  auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_services->add_client(client, callback_group),
+    std::runtime_error("Failed to notify wait set on client creation: error not set"));
+}

rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp

@@ -0,0 +1,90 @@
+// 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 <string>
+
+#include "rcl/node_options.h"
+#include "rclcpp/node.hpp"
+#include "rclcpp/node_interfaces/node_timers.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+class TestTimer : public rclcpp::TimerBase
+{
+public:
+  explicit TestTimer(rclcpp::Node * node)
+  : TimerBase(node->get_clock(), std::chrono::nanoseconds(1),
+      node->get_node_base_interface()->get_context()) {}
+
+  void execute_callback() override {}
+  bool is_steady() override {return false;}
+};
+
+class TestNodeTimers : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+    node = std::make_shared<rclcpp::Node>("node", "ns");
+
+    // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
+    // the proper type is being tested and covered.
+    node_timers =
+      dynamic_cast<rclcpp::node_interfaces::NodeTimers *>(node->get_node_timers_interface().get());
+    ASSERT_NE(nullptr, node_timers);
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+
+protected:
+  std::shared_ptr<rclcpp::Node> node;
+  rclcpp::node_interfaces::NodeTimers * node_timers;
+};
+
+TEST_F(TestNodeTimers, add_timer)
+{
+  auto timer = std::make_shared<TestTimer>(node.get());
+  auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  EXPECT_NO_THROW(node_timers->add_timer(timer, callback_group));
+
+  // Check that adding timer from node to callback group in different_node throws exception.
+  std::shared_ptr<rclcpp::Node> different_node = std::make_shared<rclcpp::Node>("node2", "ns");
+
+  auto callback_group_in_different_node =
+    different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_timers->add_timer(timer, callback_group_in_different_node),
+    std::runtime_error("Cannot create timer, group not in node."));
+}
+
+TEST_F(TestNodeTimers, add_timer_rcl_trigger_guard_condition_error)
+{
+  auto timer = std::make_shared<TestTimer>(node.get());
+  auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_timers->add_timer(timer, callback_group),
+    std::runtime_error("Failed to notify wait set on timer creation: error not set"));
+}

rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp

@@ -0,0 +1,156 @@
+// 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 <string>
+#include <type_traits>
+
+#include "rcl/node_options.h"
+#include "rclcpp/node.hpp"
+#include "rclcpp/node_interfaces/node_topics.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "test_msgs/msg/empty.hpp"
+
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+namespace
+{
+
+const rosidl_message_type_support_t EmptyTypeSupport()
+{
+  return *rosidl_typesupport_cpp::get_message_type_support_handle<test_msgs::msg::Empty>();
+}
+
+const rcl_publisher_options_t PublisherOptions()
+{
+  return rclcpp::PublisherOptionsWithAllocator<std::allocator<void>>().template
+         to_rcl_publisher_options<test_msgs::msg::Empty>(rclcpp::QoS(10));
+}
+
+const rcl_subscription_options_t SubscriptionOptions()
+{
+  return rclcpp::SubscriptionOptionsWithAllocator<std::allocator<void>>().template
+         to_rcl_subscription_options<test_msgs::msg::Empty>(rclcpp::QoS(10));
+}
+
+}  // namespace
+
+class TestPublisher : public rclcpp::PublisherBase
+{
+public:
+  explicit TestPublisher(rclcpp::Node * node)
+  : rclcpp::PublisherBase(
+      node->get_node_base_interface().get(), "topic", EmptyTypeSupport(), PublisherOptions()) {}
+};
+
+class TestSubscription : public rclcpp::SubscriptionBase
+{
+public:
+  explicit TestSubscription(rclcpp::Node * node)
+  : rclcpp::SubscriptionBase(
+      node->get_node_base_interface().get(), EmptyTypeSupport(), "topic", SubscriptionOptions()) {}
+  std::shared_ptr<void> create_message() override {return nullptr;}
+
+  std::shared_ptr<rclcpp::SerializedMessage>
+  create_serialized_message() override {return nullptr;}
+
+  void handle_message(std::shared_ptr<void> &, const rclcpp::MessageInfo &) override {}
+  void handle_loaned_message(void *, const rclcpp::MessageInfo &) override {}
+  void return_message(std::shared_ptr<void> &) override {}
+  void return_serialized_message(std::shared_ptr<rclcpp::SerializedMessage> &) override {}
+};
+
+class TestNodeTopics : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+    node = std::make_shared<rclcpp::Node>("node", "ns");
+
+    // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
+    // the proper type is being tested and covered.
+    node_topics =
+      dynamic_cast<rclcpp::node_interfaces::NodeTopics *>(node->get_node_topics_interface().get());
+    ASSERT_NE(nullptr, node_topics);
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+
+protected:
+  std::shared_ptr<rclcpp::Node> node;
+  rclcpp::node_interfaces::NodeTopics * node_topics;
+};
+
+TEST_F(TestNodeTopics, add_publisher)
+{
+  auto publisher = std::make_shared<TestPublisher>(node.get());
+  auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  EXPECT_NO_THROW(node_topics->add_publisher(publisher, callback_group));
+
+  // Check that adding publisher from node to a callback group in different_node throws exception.
+  std::shared_ptr<rclcpp::Node> different_node = std::make_shared<rclcpp::Node>("node2", "ns");
+
+  auto callback_group_in_different_node =
+    different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  EXPECT_THROW(
+    node_topics->add_publisher(publisher, callback_group_in_different_node),
+    std::runtime_error);
+}
+
+TEST_F(TestNodeTopics, add_publisher_rcl_trigger_guard_condition_error)
+{
+  auto publisher = std::make_shared<TestPublisher>(node.get());
+  auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_topics->add_publisher(publisher, callback_group),
+    std::runtime_error("Failed to notify wait set on publisher creation: error not set"));
+}
+
+TEST_F(TestNodeTopics, add_subscription)
+{
+  auto subscription = std::make_shared<TestSubscription>(node.get());
+  auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  EXPECT_NO_THROW(node_topics->add_subscription(subscription, callback_group));
+
+  // Check that adding subscription from node to callback group in different_node throws exception.
+  std::shared_ptr<rclcpp::Node> different_node = std::make_shared<rclcpp::Node>("node2", "ns");
+
+  auto callback_group_in_different_node =
+    different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  EXPECT_THROW(
+    node_topics->add_subscription(subscription, callback_group_in_different_node),
+    std::runtime_error);
+}
+
+TEST_F(TestNodeTopics, add_subscription_rcl_trigger_guard_condition_error)
+{
+  auto subscription = std::make_shared<TestSubscription>(node.get());
+  auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_topics->add_subscription(subscription, callback_group),
+    std::runtime_error("failed to notify wait set on subscription creation: error not set"));
+}

rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp

@@ -0,0 +1,90 @@
+// 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 <string>
+
+#include "rcl/node_options.h"
+#include "rclcpp/node.hpp"
+#include "rclcpp/node_interfaces/node_waitables.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+class TestWaitable : public rclcpp::Waitable
+{
+public:
+  bool add_to_wait_set(rcl_wait_set_t *) override {return false;}
+  bool is_ready(rcl_wait_set_t *) override {return false;}
+  void execute() override {}
+};
+
+class TestNodeWaitables : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+    node = std::make_shared<rclcpp::Node>("node", "ns");
+
+    node_waitables =
+      dynamic_cast<rclcpp::node_interfaces::NodeWaitables *>(
+      node->get_node_waitables_interface().get());
+    ASSERT_NE(nullptr, node_waitables);
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+
+protected:
+  std::shared_ptr<rclcpp::Node> node;
+  rclcpp::node_interfaces::NodeWaitables * node_waitables;
+};
+
+TEST_F(TestNodeWaitables, add_remove_waitable)
+{
+  std::shared_ptr<rclcpp::Node> node2 = std::make_shared<rclcpp::Node>("node2", "ns");
+
+  auto callback_group1 = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto callback_group2 = node2->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto waitable = std::make_shared<TestWaitable>();
+  EXPECT_NO_THROW(
+    node_waitables->add_waitable(waitable, callback_group1));
+  RCLCPP_EXPECT_THROW_EQ(
+    node_waitables->add_waitable(waitable, callback_group2),
+    std::runtime_error("Cannot create waitable, group not in node."));
+  EXPECT_NO_THROW(node_waitables->remove_waitable(waitable, callback_group1));
+  EXPECT_NO_THROW(node_waitables->remove_waitable(waitable, callback_group2));
+
+  auto waitable2 = std::make_shared<TestWaitable>();
+  EXPECT_NO_THROW(node_waitables->add_waitable(waitable2, nullptr));
+  EXPECT_NO_THROW(node_waitables->remove_waitable(waitable2, nullptr));
+}
+
+TEST_F(TestNodeWaitables, add_waitable_rcl_trigger_guard_condition_error)
+{
+  auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto waitable = std::make_shared<TestWaitable>();
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_waitables->add_waitable(waitable, callback_group),
+    std::runtime_error("Failed to notify wait set on waitable creation: error not set"));
+}

rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp

@@ -0,0 +1,784 @@
+// 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 <list>
+#include <memory>
+#include <string>
+#include <vector>
+#include <utility>
+
+#include "gtest/gtest.h"
+
+#include "rclcpp/scope_exit.hpp"
+#include "rclcpp/strategies/allocator_memory_strategy.hpp"
+#include "test_msgs/msg/empty.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy;
+using WeakNodeList = std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>;
+
+static bool test_waitable_result = false;
+
+/**
+ * Mock Waitable class, with a globally setable boolean result.
+ */
+class TestWaitable : public rclcpp::Waitable
+{
+public:
+  bool add_to_wait_set(rcl_wait_set_t *) override
+  {
+    return test_waitable_result;
+  }
+
+  bool is_ready(rcl_wait_set_t *) override
+  {
+    return test_waitable_result;
+  }
+
+  void execute() override {}
+};
+
+struct RclWaitSetSizes
+{
+  size_t size_of_subscriptions = 0;
+  size_t size_of_guard_conditions = 0;
+  size_t size_of_timers = 0;
+  size_t size_of_clients = 0;
+  size_t size_of_services = 0;
+  size_t size_of_events = 0;
+  size_t size_of_waitables = 0;
+};
+
+// For a standard rclcpp node, this should be more than enough capacity for each type.
+RclWaitSetSizes SufficientWaitSetCapacities()
+{
+  return {100, 100, 100, 100, 100, 100, 100};
+}
+
+class TestAllocatorMemoryStrategy : public ::testing::Test
+{
+public:
+  TestAllocatorMemoryStrategy()
+  : allocator_memory_strategy_(nullptr) {}
+
+  void SetUp() override
+  {
+    rclcpp::init(0, nullptr);
+    allocator_ = std::make_shared<std::allocator<void>>();
+    // Even though this is just using a basic allocator, the custom allocator constructor was
+    // not covered in general testing.
+    allocator_memory_strategy_ = std::make_shared<AllocatorMemoryStrategy<>>(allocator_);
+  }
+
+  void TearDown() override
+  {
+    allocator_memory_strategy_.reset();
+    rclcpp::shutdown();
+  }
+
+protected:
+  std::shared_ptr<AllocatorMemoryStrategy<>> allocator_memory_strategy()
+  {
+    return allocator_memory_strategy_;
+  }
+
+  //
+  // Convience methods, but it adds entities to vectors so the weak pointers kept by the node
+  // interfaces remain alive and valid
+  //
+
+  std::shared_ptr<rclcpp::Node> create_node_with_disabled_callback_groups(const std::string & name)
+  {
+    auto node = std::make_shared<rclcpp::Node>(name, "ns");
+
+    for (auto & group_weak_ptr : node->get_callback_groups()) {
+      auto group = group_weak_ptr.lock();
+      if (group) {
+        group->can_be_taken_from() = false;
+      }
+    }
+    return node;
+  }
+
+  std::shared_ptr<rclcpp::Node> create_node_with_subscription(const std::string & name)
+  {
+    auto subscription_callback = [](const test_msgs::msg::Empty::SharedPtr) {};
+    const rclcpp::QoS qos(10);
+    auto node_with_subscription = create_node_with_disabled_callback_groups(name);
+
+    rclcpp::SubscriptionOptions subscription_options;
+
+    auto callback_group =
+      node_with_subscription->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive);
+
+    subscription_options.callback_group = callback_group;
+    callback_groups_.push_back(callback_group);
+
+    auto subscription = node_with_subscription->create_subscription<
+      test_msgs::msg::Empty, decltype(subscription_callback)>(
+      "topic", qos, std::move(subscription_callback), subscription_options);
+    subscriptions_.push_back(subscription);
+
+    return node_with_subscription;
+  }
+
+  std::shared_ptr<rclcpp::Node> create_node_with_service(const std::string & name)
+  {
+    auto service_callback =
+      [](const test_msgs::srv::Empty::Request::SharedPtr,
+        test_msgs::srv::Empty::Response::SharedPtr) {};
+    auto node_with_service = create_node_with_disabled_callback_groups(name);
+
+    auto callback_group =
+      node_with_service->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive);
+
+    callback_groups_.push_back(callback_group);
+
+    services_.push_back(
+      node_with_service->create_service<test_msgs::srv::Empty>(
+        "service", std::move(service_callback), rmw_qos_profile_services_default, callback_group));
+    return node_with_service;
+  }
+
+  std::shared_ptr<rclcpp::Node> create_node_with_client(const std::string & name)
+  {
+    auto node_with_client = create_node_with_disabled_callback_groups(name);
+    auto callback_group =
+      node_with_client->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive);
+
+    callback_groups_.push_back(callback_group);
+
+    clients_.push_back(
+      node_with_client->create_client<test_msgs::srv::Empty>(
+        "service", rmw_qos_profile_services_default, callback_group));
+    return node_with_client;
+  }
+
+  std::shared_ptr<rclcpp::Node> create_node_with_timer(const std::string & name)
+  {
+    auto timer_callback = []() {};
+    auto node_with_timer = create_node_with_disabled_callback_groups(name);
+
+    auto callback_group =
+      node_with_timer->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive);
+    callback_groups_.push_back(callback_group);
+
+    timers_.push_back(
+      node_with_timer->create_wall_timer(
+        std::chrono::milliseconds(1), timer_callback, callback_group));
+    return node_with_timer;
+  }
+
+  ::testing::AssertionResult TestNumberOfEntitiesAfterCollection(
+    std::shared_ptr<rclcpp::Node> node,
+    const RclWaitSetSizes & expected)
+  {
+    WeakNodeList nodes;
+    nodes.push_back(node->get_node_base_interface());
+
+    allocator_memory_strategy()->collect_entities(nodes);
+    EXPECT_EQ(
+      expected.size_of_subscriptions, allocator_memory_strategy()->number_of_ready_subscriptions());
+    EXPECT_EQ(
+      expected.size_of_guard_conditions, allocator_memory_strategy()->number_of_guard_conditions());
+    EXPECT_EQ(expected.size_of_timers, allocator_memory_strategy()->number_of_ready_timers());
+    EXPECT_EQ(expected.size_of_clients, allocator_memory_strategy()->number_of_ready_clients());
+    EXPECT_EQ(expected.size_of_services, allocator_memory_strategy()->number_of_ready_services());
+    EXPECT_EQ(expected.size_of_events, allocator_memory_strategy()->number_of_ready_events());
+    EXPECT_EQ(expected.size_of_waitables, allocator_memory_strategy()->number_of_waitables());
+    if (::testing::Test::HasFailure()) {
+      return ::testing::AssertionFailure() <<
+             "Expected number of entities did not match actual counts";
+    }
+    return ::testing::AssertionSuccess();
+  }
+
+  ::testing::AssertionResult TestAddHandlesToWaitSet(
+    std::shared_ptr<rclcpp::Node> node,
+    const RclWaitSetSizes & insufficient_capacities)
+  {
+    WeakNodeList nodes;
+    nodes.push_back(node->get_node_base_interface());
+    allocator_memory_strategy()->collect_entities(nodes);
+
+    auto context = node->get_node_base_interface()->get_context();
+    rcl_context_t * rcl_context = context->get_rcl_context().get();
+    rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+    rcl_allocator_t allocator = rcl_get_default_allocator();
+    RclWaitSetSizes sufficient_capacities = SufficientWaitSetCapacities();
+
+    rcl_ret_t ret = rcl_wait_set_init(
+      &wait_set,
+      sufficient_capacities.size_of_subscriptions,
+      sufficient_capacities.size_of_guard_conditions,
+      sufficient_capacities.size_of_timers,
+      sufficient_capacities.size_of_clients,
+      sufficient_capacities.size_of_services,
+      sufficient_capacities.size_of_events,
+      rcl_context,
+      allocator);
+    if (ret != RCL_RET_OK) {
+      return ::testing::AssertionFailure() <<
+             "Calling rcl_wait_set_init() with expected sufficient capacities failed";
+    }
+
+    RCLCPP_SCOPE_EXIT(
+    {
+      EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));
+    });
+
+    if (!allocator_memory_strategy()->add_handles_to_wait_set(&wait_set)) {
+      return ::testing::AssertionFailure() <<
+             "Calling add_handles_to_wait_set() with a wait_set with expected sufficient capacities"
+             " failed";
+    }
+
+    rcl_wait_set_t wait_set_no_capacity = rcl_get_zero_initialized_wait_set();
+    ret = rcl_wait_set_init(
+      &wait_set_no_capacity,
+      insufficient_capacities.size_of_subscriptions,
+      insufficient_capacities.size_of_guard_conditions,
+      insufficient_capacities.size_of_timers,
+      insufficient_capacities.size_of_clients,
+      insufficient_capacities.size_of_services,
+      insufficient_capacities.size_of_events,
+      rcl_context,
+      allocator);
+
+    if (ret != RCL_RET_OK) {
+      return ::testing::AssertionFailure() <<
+             "Calling rcl_wait_set_init() with expected insufficient capacities failed";
+    }
+
+    RCLCPP_SCOPE_EXIT(
+    {
+      EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set_no_capacity));
+    });
+
+    if (allocator_memory_strategy()->add_handles_to_wait_set(&wait_set_no_capacity)) {
+      return ::testing::AssertionFailure() <<
+             "Calling add_handles_to_wait_set() with a wait_set with insufficient capacities"
+             " unexpectedly succeeded";
+    }
+    return ::testing::AssertionSuccess();
+  }
+
+  ::testing::AssertionResult TestGetNextEntity(
+    std::shared_ptr<rclcpp::Node> node_with_entity1,
+    std::shared_ptr<rclcpp::Node> node_with_entity2,
+    std::function<rclcpp::AnyExecutable(WeakNodeList)> get_next_entity_func)
+  {
+    WeakNodeList nodes;
+    auto basic_node = std::make_shared<rclcpp::Node>("basic_node", "ns");
+    nodes.push_back(node_with_entity1->get_node_base_interface());
+    nodes.push_back(basic_node->get_node_base_interface());
+    allocator_memory_strategy()->collect_entities(nodes);
+
+    rclcpp::AnyExecutable result = get_next_entity_func(nodes);
+    if (result.node_base != node_with_entity1->get_node_base_interface()) {
+      return ::testing::AssertionFailure() <<
+             "Failed to get expected entity with specified get_next_*() function";
+    }
+
+    WeakNodeList uncollected_nodes;
+    auto basic_node2 = std::make_shared<rclcpp::Node>("basic_node2", "ns");
+    uncollected_nodes.push_back(node_with_entity2->get_node_base_interface());
+    uncollected_nodes.push_back(basic_node2->get_node_base_interface());
+
+    rclcpp::AnyExecutable failed_result = get_next_entity_func(uncollected_nodes);
+    if (nullptr != failed_result.node_base) {
+      return ::testing::AssertionFailure() <<
+             "A node was retrieved with the specified get_next_*() function despite"
+             " none of the nodes that were passed to it were added to the"
+             " allocator_memory_strategy. Retrieved node: " << failed_result.node_base->get_name();
+    }
+    return ::testing::AssertionSuccess();
+  }
+
+  ::testing::AssertionResult TestGetNextEntityMutuallyExclusive(
+    std::shared_ptr<rclcpp::Node> node_with_entity,
+    std::function<rclcpp::AnyExecutable(WeakNodeList)> get_next_entity_func)
+  {
+    WeakNodeList nodes;
+    auto basic_node = std::make_shared<rclcpp::Node>("basic_node", "ns");
+
+    auto node_base = node_with_entity->get_node_base_interface();
+    auto basic_node_base = basic_node->get_node_base_interface();
+    nodes.push_back(node_base);
+    nodes.push_back(basic_node_base);
+    allocator_memory_strategy()->collect_entities(nodes);
+
+    // It's important that these be set after collect_entities() otherwise collect_entities() will
+    // not do anything
+    node_base->get_default_callback_group()->can_be_taken_from() = false;
+    basic_node_base->get_default_callback_group()->can_be_taken_from() = false;
+    for (auto & callback_group : callback_groups_) {
+      callback_group->can_be_taken_from() = false;
+    }
+
+    rclcpp::AnyExecutable result = get_next_entity_func(nodes);
+
+    if (nullptr != result.node_base) {
+      return ::testing::AssertionFailure() <<
+             "A node was retrieved with the specified get_next_*() function despite"
+             " setting can_be_taken_from() to false for all nodes and callback_groups";
+    }
+
+    return ::testing::AssertionSuccess();
+  }
+
+  std::vector<std::shared_ptr<rclcpp::CallbackGroup>> callback_groups_;
+
+private:
+  std::shared_ptr<std::allocator<void>> allocator_;
+  std::shared_ptr<AllocatorMemoryStrategy<>> allocator_memory_strategy_;
+
+  // These are generally kept as weak pointers in the rclcpp::Node interfaces, so they need to be
+  // owned by this class.
+  std::vector<rclcpp::SubscriptionBase::SharedPtr> subscriptions_;
+  std::vector<rclcpp::ServiceBase::SharedPtr> services_;
+  std::vector<rclcpp::ClientBase::SharedPtr> clients_;
+  std::vector<rclcpp::TimerBase::SharedPtr> timers_;
+  std::vector<rclcpp::Waitable::SharedPtr> waitables_;
+};
+
+TEST_F(TestAllocatorMemoryStrategy, construct_destruct) {
+  WeakNodeList nodes;
+  auto basic_node = create_node_with_disabled_callback_groups("basic_node");
+  nodes.push_back(basic_node->get_node_base_interface());
+  allocator_memory_strategy()->collect_entities(nodes);
+  EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_subscriptions());
+  EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_services());
+  EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_events());
+  EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_clients());
+  EXPECT_EQ(0u, allocator_memory_strategy()->number_of_guard_conditions());
+  EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_timers());
+  EXPECT_EQ(0u, allocator_memory_strategy()->number_of_waitables());
+}
+
+TEST_F(TestAllocatorMemoryStrategy, add_remove_guard_conditions) {
+  rcl_guard_condition_t guard_condition1 = rcl_get_zero_initialized_guard_condition();
+  rcl_guard_condition_t guard_condition2 = rcl_get_zero_initialized_guard_condition();
+  rcl_guard_condition_t guard_condition3 = rcl_get_zero_initialized_guard_condition();
+
+  EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition1));
+  EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition2));
+  EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition3));
+  EXPECT_EQ(3u, allocator_memory_strategy()->number_of_guard_conditions());
+
+  // Adding a second time should not add to vector
+  EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition1));
+  EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition2));
+  EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition3));
+  EXPECT_EQ(3u, allocator_memory_strategy()->number_of_guard_conditions());
+
+  EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition1));
+  EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition2));
+  EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition3));
+  EXPECT_EQ(0u, allocator_memory_strategy()->number_of_guard_conditions());
+
+  // Removing second time should have no effect
+  EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition1));
+  EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition2));
+  EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition3));
+  EXPECT_EQ(0u, allocator_memory_strategy()->number_of_guard_conditions());
+}
+
+TEST_F(TestAllocatorMemoryStrategy, add_remove_waitables) {
+  EXPECT_THROW(allocator_memory_strategy()->add_waitable_handle(nullptr), std::runtime_error);
+  EXPECT_EQ(0u, allocator_memory_strategy()->number_of_waitables());
+
+  rclcpp::Waitable::SharedPtr waitable = std::make_shared<TestWaitable>();
+  EXPECT_NO_THROW(allocator_memory_strategy()->add_waitable_handle(waitable));
+  EXPECT_EQ(1u, allocator_memory_strategy()->number_of_waitables());
+
+  EXPECT_NO_THROW(allocator_memory_strategy()->clear_handles());
+  EXPECT_EQ(0u, allocator_memory_strategy()->number_of_waitables());
+}
+
+TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_subscription) {
+  RclWaitSetSizes expected_sizes = {};
+  expected_sizes.size_of_subscriptions = 1;
+  const std::string implementation_identifier = rmw_get_implementation_identifier();
+  if (implementation_identifier == "rmw_connext_cpp" ||
+    implementation_identifier == "rmw_cyclonedds_cpp")
+  {
+    // For connext, a subscription will also add an event and waitable
+    expected_sizes.size_of_events += 1;
+    expected_sizes.size_of_waitables += 1;
+  }
+  auto node_with_subscription = create_node_with_subscription("subscription_node");
+  EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_subscription, expected_sizes));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_service) {
+  RclWaitSetSizes expected_sizes = {};
+  expected_sizes.size_of_services = 1;
+  auto node_with_service = create_node_with_service("service_node");
+  EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_service, expected_sizes));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_client) {
+  RclWaitSetSizes expected_sizes = {};
+  expected_sizes.size_of_clients = 1;
+  auto node_with_client = create_node_with_client("client_node");
+  EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_client, expected_sizes));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_timer) {
+  RclWaitSetSizes expected_sizes = {};
+  expected_sizes.size_of_timers = 1;
+  auto node_with_timer = create_node_with_timer("timer_node");
+  EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_timer, expected_sizes));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_bad_arguments) {
+  auto node = create_node_with_subscription("subscription_node");
+  WeakNodeList nodes;
+  nodes.push_back(node->get_node_base_interface());
+  allocator_memory_strategy()->collect_entities(nodes);
+  EXPECT_FALSE(allocator_memory_strategy()->add_handles_to_wait_set(nullptr));
+  EXPECT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+}
+
+TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_subscription) {
+  auto node_with_subscription = create_node_with_subscription("subscription_node");
+  RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities();
+  insufficient_capacities.size_of_subscriptions = 0;
+  EXPECT_TRUE(TestAddHandlesToWaitSet(node_with_subscription, insufficient_capacities));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_service) {
+  auto node_with_service = create_node_with_service("service_node");
+  RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities();
+  insufficient_capacities.size_of_services = 0;
+  EXPECT_TRUE(TestAddHandlesToWaitSet(node_with_service, insufficient_capacities));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_client) {
+  auto node_with_client = create_node_with_client("client_node");
+  RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities();
+  insufficient_capacities.size_of_clients = 0;
+  EXPECT_TRUE(TestAddHandlesToWaitSet(node_with_client, insufficient_capacities));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_guard_condition) {
+  auto node = create_node_with_disabled_callback_groups("node");
+  rcl_guard_condition_t guard_condition = rcl_get_zero_initialized_guard_condition();
+  auto context = node->get_node_base_interface()->get_context();
+  rcl_context_t * rcl_context = context->get_rcl_context().get();
+  rcl_guard_condition_options_t guard_condition_options = {
+    rcl_get_default_allocator()};
+
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_guard_condition_init(&guard_condition, rcl_context, guard_condition_options));
+  RCLCPP_SCOPE_EXIT(
+  {
+    EXPECT_EQ(RCL_RET_OK, rcl_guard_condition_fini(&guard_condition));
+  });
+
+  allocator_memory_strategy()->add_guard_condition(&guard_condition);
+  RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities();
+  insufficient_capacities.size_of_guard_conditions = 0;
+  EXPECT_TRUE(TestAddHandlesToWaitSet(node, insufficient_capacities));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_timer) {
+  auto node_with_timer = create_node_with_timer("timer_node");
+  RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities();
+  insufficient_capacities.size_of_timers = 0;
+  EXPECT_TRUE(TestAddHandlesToWaitSet(node_with_timer, insufficient_capacities));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_waitable) {
+  rcl_reset_error();
+
+  rclcpp::Waitable::SharedPtr waitable = std::make_shared<TestWaitable>();
+  EXPECT_NO_THROW(allocator_memory_strategy()->add_waitable_handle(waitable));
+  EXPECT_EQ(1u, allocator_memory_strategy()->number_of_waitables());
+
+  test_waitable_result = true;
+  EXPECT_TRUE(allocator_memory_strategy()->add_handles_to_wait_set(nullptr));
+
+  test_waitable_result = false;
+  EXPECT_FALSE(allocator_memory_strategy()->add_handles_to_wait_set(nullptr));
+
+  // This calls TestWaitable's functions, so rcl errors are not set
+  EXPECT_FALSE(rcl_error_is_set());
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_subscription) {
+  auto node1 = create_node_with_subscription("node1");
+  auto node2 = create_node_with_subscription("node2");
+
+  auto get_next_entity = [this](const WeakNodeList & nodes) {
+      rclcpp::AnyExecutable result;
+      allocator_memory_strategy()->get_next_subscription(result, nodes);
+      return result;
+    };
+
+  EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_service) {
+  auto node1 = create_node_with_service("node1");
+  auto node2 = create_node_with_service("node2");
+
+  auto get_next_entity = [this](const WeakNodeList & nodes) {
+      rclcpp::AnyExecutable result;
+      allocator_memory_strategy()->get_next_service(result, nodes);
+      return result;
+    };
+
+  EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_client) {
+  auto node1 = create_node_with_client("node1");
+  auto node2 = create_node_with_client("node2");
+
+  auto get_next_entity = [this](const WeakNodeList & nodes) {
+      rclcpp::AnyExecutable result;
+      allocator_memory_strategy()->get_next_client(result, nodes);
+      return result;
+    };
+
+  EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_timer) {
+  auto node1 = create_node_with_timer("node1");
+  auto node2 = create_node_with_timer("node2");
+
+  auto get_next_entity = [this](const WeakNodeList & nodes) {
+      rclcpp::AnyExecutable result;
+      allocator_memory_strategy()->get_next_timer(result, nodes);
+      return result;
+    };
+
+  EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_waitable) {
+  auto node1 = std::make_shared<rclcpp::Node>("waitable_node", "ns");
+  auto node2 = std::make_shared<rclcpp::Node>("waitable_node2", "ns");
+  rclcpp::Waitable::SharedPtr waitable1 = std::make_shared<TestWaitable>();
+  rclcpp::Waitable::SharedPtr waitable2 = std::make_shared<TestWaitable>();
+  node1->get_node_waitables_interface()->add_waitable(waitable1, nullptr);
+  node2->get_node_waitables_interface()->add_waitable(waitable2, nullptr);
+
+  auto get_next_entity = [this](const WeakNodeList & nodes) {
+      rclcpp::AnyExecutable result;
+      allocator_memory_strategy()->get_next_waitable(result, nodes);
+      return result;
+    };
+
+  EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_mutually_exclusive) {
+  auto node = create_node_with_subscription("node");
+
+  auto get_next_entity = [this](const WeakNodeList & nodes) {
+      rclcpp::AnyExecutable result;
+      allocator_memory_strategy()->get_next_subscription(result, nodes);
+      return result;
+    };
+
+  EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_service_mutually_exclusive) {
+  auto node = create_node_with_service("node");
+
+  auto get_next_entity = [this](const WeakNodeList & nodes) {
+      rclcpp::AnyExecutable result;
+      allocator_memory_strategy()->get_next_service(result, nodes);
+      return result;
+    };
+
+  EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_client_mutually_exclusive) {
+  auto node = create_node_with_client("node");
+
+  auto get_next_entity = [this](const WeakNodeList & nodes) {
+      rclcpp::AnyExecutable result;
+      allocator_memory_strategy()->get_next_client(result, nodes);
+      return result;
+    };
+
+  EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_timer_mutually_exclusive) {
+  auto node = create_node_with_timer("node");
+
+  auto get_next_entity = [this](const WeakNodeList & nodes) {
+      rclcpp::AnyExecutable result;
+      allocator_memory_strategy()->get_next_timer(result, nodes);
+      return result;
+    };
+
+  EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_waitable_mutually_exclusive) {
+  auto node = std::make_shared<rclcpp::Node>("waitable_node", "ns");
+  rclcpp::Waitable::SharedPtr waitable = std::make_shared<TestWaitable>();
+  auto callback_group =
+    node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  node->get_node_waitables_interface()->add_waitable(waitable, callback_group);
+
+  auto get_next_entity = [this, callback_group](const WeakNodeList & nodes) {
+      // This callback group isn't in the base class' callback_group list, so this needs to be done
+      // before get_next_waitable() is called.
+      callback_group->can_be_taken_from() = false;
+
+      rclcpp::AnyExecutable result;
+      allocator_memory_strategy()->get_next_waitable(result, nodes);
+      return result;
+    };
+
+  EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity));
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_out_of_scope) {
+  WeakNodeList nodes;
+  auto node = create_node_with_disabled_callback_groups("node");
+  nodes.push_back(node->get_node_base_interface());
+  // Force subscription to go out of scope and cleanup after collecting entities.
+  {
+    rclcpp::SubscriptionOptions subscription_options;
+
+    auto callback_group =
+      node->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive);
+
+    subscription_options.callback_group = callback_group;
+
+    auto subscription_callback = [](const test_msgs::msg::Empty::SharedPtr) {};
+    const rclcpp::QoS qos(10);
+
+    auto subscription = node->create_subscription<
+      test_msgs::msg::Empty, decltype(subscription_callback)>(
+      "topic", qos, std::move(subscription_callback), subscription_options);
+
+    allocator_memory_strategy()->collect_entities(nodes);
+  }
+  EXPECT_EQ(1u, allocator_memory_strategy()->number_of_ready_subscriptions());
+
+  rclcpp::AnyExecutable result;
+  allocator_memory_strategy()->get_next_subscription(result, nodes);
+  EXPECT_EQ(nullptr, result.node_base);
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_service_out_of_scope) {
+  WeakNodeList nodes;
+  auto node = create_node_with_disabled_callback_groups("node");
+  nodes.push_back(node->get_node_base_interface());
+  // Force service to go out of scope and cleanup after collecting entities.
+  {
+    auto callback_group =
+      node->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive);
+
+    auto service_callback =
+      [](const test_msgs::srv::Empty::Request::SharedPtr,
+        test_msgs::srv::Empty::Response::SharedPtr) {};
+    auto service = node->create_service<test_msgs::srv::Empty>(
+      "service", std::move(service_callback), rmw_qos_profile_services_default, callback_group);
+
+    allocator_memory_strategy()->collect_entities(nodes);
+  }
+  EXPECT_EQ(1u, allocator_memory_strategy()->number_of_ready_services());
+
+  rclcpp::AnyExecutable result;
+  allocator_memory_strategy()->get_next_service(result, nodes);
+  EXPECT_EQ(nullptr, result.node_base);
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_client_out_of_scope) {
+  WeakNodeList nodes;
+  auto node = create_node_with_disabled_callback_groups("node");
+  nodes.push_back(node->get_node_base_interface());
+  // Force client to go out of scope and cleanup after collecting entities.
+  {
+    auto callback_group =
+      node->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive);
+    auto client = node->create_client<test_msgs::srv::Empty>(
+      "service", rmw_qos_profile_services_default, callback_group);
+
+    allocator_memory_strategy()->collect_entities(nodes);
+  }
+  EXPECT_EQ(1u, allocator_memory_strategy()->number_of_ready_clients());
+
+  rclcpp::AnyExecutable result;
+  allocator_memory_strategy()->get_next_client(result, nodes);
+  EXPECT_EQ(nullptr, result.node_base);
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_timer_out_of_scope) {
+  WeakNodeList nodes;
+  auto node = create_node_with_disabled_callback_groups("node");
+  nodes.push_back(node->get_node_base_interface());
+
+  // Force timer to go out of scope and cleanup after collecting entities.
+  {
+    auto callback_group =
+      node->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive);
+
+    auto timer = node->create_wall_timer(
+      std::chrono::seconds(10), []() {}, callback_group);
+
+    allocator_memory_strategy()->collect_entities(nodes);
+  }
+  EXPECT_EQ(1u, allocator_memory_strategy()->number_of_ready_timers());
+
+  rclcpp::AnyExecutable result;
+  allocator_memory_strategy()->get_next_timer(result, nodes);
+  EXPECT_EQ(nullptr, result.node_base);
+}
+
+TEST_F(TestAllocatorMemoryStrategy, get_next_waitable_out_of_scope) {
+  WeakNodeList nodes;
+  auto node = create_node_with_disabled_callback_groups("node");
+  nodes.push_back(node->get_node_base_interface());
+
+  // Force waitable to go out of scope and cleanup after collecting entities.
+  {
+    auto callback_group =
+      node->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive);
+    allocator_memory_strategy()->collect_entities(nodes);
+    auto waitable = std::make_shared<TestWaitable>();
+    node->get_node_waitables_interface()->add_waitable(waitable, callback_group);
+    allocator_memory_strategy()->add_waitable_handle(waitable);
+  }
+  // Since all callback groups have been locked, except the one we added, this should only be 1
+  EXPECT_EQ(1u, allocator_memory_strategy()->number_of_waitables());
+
+  rclcpp::AnyExecutable result;
+  allocator_memory_strategy()->get_next_waitable(result, nodes);
+  EXPECT_EQ(nullptr, result.node_base);
+}

rclcpp/test/rclcpp/strategies/test_message_pool_memory_strategy.cpp

@@ -0,0 +1,73 @@
+// 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 "gtest/gtest.h"
+
+#include "rclcpp/strategies/message_pool_memory_strategy.hpp"
+#include "test_msgs/msg/empty.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+using rclcpp::strategies::message_pool_memory_strategy::MessagePoolMemoryStrategy;
+
+class TestMessagePoolMemoryStrategy : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    message_memory_strategy_ =
+      std::make_shared<MessagePoolMemoryStrategy<test_msgs::msg::Empty, 1>>();
+  }
+
+protected:
+  std::shared_ptr<MessagePoolMemoryStrategy<test_msgs::msg::Empty, 1>> message_memory_strategy_;
+};
+
+TEST_F(TestMessagePoolMemoryStrategy, construct_destruct) {
+  ASSERT_NE(nullptr, message_memory_strategy_);
+  EXPECT_NE(nullptr, message_memory_strategy_->message_allocator_);
+  EXPECT_NE(nullptr, message_memory_strategy_->serialized_message_allocator_);
+  EXPECT_NE(nullptr, message_memory_strategy_->buffer_allocator_);
+}
+
+TEST_F(TestMessagePoolMemoryStrategy, borrow_return) {
+  auto message = message_memory_strategy_->borrow_message();
+  ASSERT_NE(nullptr, message);
+
+  EXPECT_NO_THROW(message_memory_strategy_->return_message(message));
+}
+
+TEST_F(TestMessagePoolMemoryStrategy, borrow_too_many) {
+  auto message = message_memory_strategy_->borrow_message();
+  ASSERT_NE(nullptr, message);
+
+  // Size is 1, borrowing second time should fail
+  RCLCPP_EXPECT_THROW_EQ(
+    message_memory_strategy_->borrow_message(),
+    std::runtime_error("Tried to access message that was still in use! Abort."));
+  EXPECT_NO_THROW(message_memory_strategy_->return_message(message));
+}
+
+TEST_F(TestMessagePoolMemoryStrategy, return_unrecognized) {
+  auto message = message_memory_strategy_->borrow_message();
+  ASSERT_NE(nullptr, message);
+
+  auto unrecognized = std::make_shared<test_msgs::msg::Empty>();
+  // Unrecognized does not belong to pool
+  RCLCPP_EXPECT_THROW_EQ(
+    message_memory_strategy_->return_message(unrecognized),
+    std::runtime_error("Unrecognized message ptr in return_message."));
+  EXPECT_NO_THROW(message_memory_strategy_->return_message(message));
+}

rclcpp/test/rclcpp/test_any_service_callback.cpp

@@ -0,0 +1,78 @@
+// 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.
+
+// This file includes basic API tests for the AnyServiceCallback class.
+// It is also tested in test_externally_defined_services.cpp
+
+#include <gtest/gtest.h>
+
+#include <functional>
+#include <memory>
+#include <utility>
+
+#include "rclcpp/any_service_callback.hpp"
+#include "test_msgs/srv/empty.hpp"
+#include "test_msgs/srv/empty.h"
+
+class TestAnyServiceCallback : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    request_header_ = std::make_shared<rmw_request_id_t>();
+    request_ = std::make_shared<test_msgs::srv::Empty::Request>();
+    response_ = std::make_shared<test_msgs::srv::Empty::Response>();
+  }
+
+protected:
+  rclcpp::AnyServiceCallback<test_msgs::srv::Empty> any_service_callback_;
+  std::shared_ptr<rmw_request_id_t> request_header_;
+  std::shared_ptr<test_msgs::srv::Empty::Request> request_;
+  std::shared_ptr<test_msgs::srv::Empty::Response> response_;
+};
+
+TEST_F(TestAnyServiceCallback, no_set_and_dispatch_throw) {
+  EXPECT_THROW(
+    any_service_callback_.dispatch(request_header_, request_, response_),
+    std::runtime_error);
+}
+
+TEST_F(TestAnyServiceCallback, set_and_dispatch_no_header) {
+  int callback_calls = 0;
+  auto callback = [&callback_calls](const std::shared_ptr<test_msgs::srv::Empty::Request>,
+      std::shared_ptr<test_msgs::srv::Empty::Response>) {
+      callback_calls++;
+    };
+
+  any_service_callback_.set(callback);
+  EXPECT_NO_THROW(
+    any_service_callback_.dispatch(request_header_, request_, response_));
+  EXPECT_EQ(callback_calls, 1);
+}
+
+
+TEST_F(TestAnyServiceCallback, set_and_dispatch_header) {
+  int callback_with_header_calls = 0;
+  auto callback_with_header =
+    [&callback_with_header_calls](const std::shared_ptr<rmw_request_id_t>,
+      const std::shared_ptr<test_msgs::srv::Empty::Request>,
+      std::shared_ptr<test_msgs::srv::Empty::Response>) {
+      callback_with_header_calls++;
+    };
+
+  any_service_callback_.set(callback_with_header);
+  EXPECT_NO_THROW(
+    any_service_callback_.dispatch(request_header_, request_, response_));
+  EXPECT_EQ(callback_with_header_calls, 1);
+}

rclcpp/test/rclcpp/test_any_subscription_callback.cpp

@@ -0,0 +1,205 @@
+// 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 <functional>
+#include <memory>
+#include <utility>
+
+#include "rclcpp/any_subscription_callback.hpp"
+#include "test_msgs/msg/empty.hpp"
+#include "test_msgs/msg/empty.h"
+
+class TestAnySubscriptionCallback : public ::testing::Test
+{
+public:
+  TestAnySubscriptionCallback()
+  : any_subscription_callback_(allocator_) {}
+  void SetUp()
+  {
+    msg_shared_ptr_ = std::make_shared<test_msgs::msg::Empty>();
+    msg_const_shared_ptr_ = std::make_shared<const test_msgs::msg::Empty>();
+    msg_unique_ptr_ = std::make_unique<test_msgs::msg::Empty>();
+  }
+
+protected:
+  std::shared_ptr<std::allocator<void>> allocator_;
+  rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty, std::allocator<void>>
+  any_subscription_callback_;
+
+  std::shared_ptr<test_msgs::msg::Empty> msg_shared_ptr_;
+  std::shared_ptr<const test_msgs::msg::Empty> msg_const_shared_ptr_;
+  std::unique_ptr<test_msgs::msg::Empty> msg_unique_ptr_;
+  rclcpp::MessageInfo message_info_;
+};
+
+TEST_F(TestAnySubscriptionCallback, construct_destruct) {
+}
+
+TEST_F(TestAnySubscriptionCallback, unset_dispatch_throw) {
+  EXPECT_THROW(
+    any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_),
+    std::runtime_error);
+  EXPECT_THROW(
+    any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_),
+    std::runtime_error);
+  EXPECT_THROW(
+    any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_),
+    std::runtime_error);
+}
+
+TEST_F(TestAnySubscriptionCallback, set_dispatch_shared_ptr) {
+  int callback_count = 0;
+  auto shared_ptr_callback = [&callback_count](
+    const std::shared_ptr<test_msgs::msg::Empty>) {
+      callback_count++;
+    };
+
+  any_subscription_callback_.set(shared_ptr_callback);
+  EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_));
+  EXPECT_EQ(callback_count, 1);
+
+  // Can't convert ConstSharedPtr to SharedPtr
+  EXPECT_THROW(
+    any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_),
+    std::runtime_error);
+  EXPECT_EQ(callback_count, 1);
+
+  // Promotes Unique into SharedPtr
+  EXPECT_NO_THROW(
+    any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_));
+  EXPECT_EQ(callback_count, 2);
+}
+
+TEST_F(TestAnySubscriptionCallback, set_dispatch_shared_ptr_w_info) {
+  int callback_count = 0;
+  auto shared_ptr_w_info_callback = [&callback_count](
+    const std::shared_ptr<test_msgs::msg::Empty>, const rclcpp::MessageInfo &) {
+      callback_count++;
+    };
+
+  any_subscription_callback_.set(shared_ptr_w_info_callback);
+
+  EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_));
+  EXPECT_EQ(callback_count, 1);
+
+  // Can't convert ConstSharedPtr to SharedPtr
+  EXPECT_THROW(
+    any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_),
+    std::runtime_error);
+  EXPECT_EQ(callback_count, 1);
+
+  // Promotes Unique into SharedPtr
+  EXPECT_NO_THROW(
+    any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_));
+  EXPECT_EQ(callback_count, 2);
+}
+
+TEST_F(TestAnySubscriptionCallback, set_dispatch_const_shared_ptr) {
+  int callback_count = 0;
+  auto const_shared_ptr_callback = [&callback_count](
+    std::shared_ptr<const test_msgs::msg::Empty>) {
+      callback_count++;
+    };
+
+  any_subscription_callback_.set(const_shared_ptr_callback);
+
+  // Ok to promote shared_ptr to ConstSharedPtr
+  EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_));
+  EXPECT_EQ(callback_count, 1);
+
+  EXPECT_NO_THROW(
+    any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_));
+  EXPECT_EQ(callback_count, 2);
+
+  // Not allowed to convert unique_ptr to const shared_ptr
+  EXPECT_THROW(
+    any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_),
+    std::runtime_error);
+  EXPECT_EQ(callback_count, 2);
+}
+
+TEST_F(TestAnySubscriptionCallback, set_dispatch_const_shared_ptr_w_info) {
+  int callback_count = 0;
+  auto const_shared_ptr_callback = [&callback_count](
+    std::shared_ptr<const test_msgs::msg::Empty>, const rclcpp::MessageInfo &) {
+      callback_count++;
+    };
+
+  any_subscription_callback_.set(
+    std::move(const_shared_ptr_callback));
+
+  // Ok to promote shared_ptr to ConstSharedPtr
+  EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_));
+  EXPECT_EQ(callback_count, 1);
+
+  EXPECT_NO_THROW(
+    any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_));
+  EXPECT_EQ(callback_count, 2);
+
+  // Not allowed to convert unique_ptr to const shared_ptr
+  EXPECT_THROW(
+    any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_),
+    std::runtime_error);
+  EXPECT_EQ(callback_count, 2);
+}
+
+TEST_F(TestAnySubscriptionCallback, set_dispatch_unique_ptr) {
+  int callback_count = 0;
+  auto unique_ptr_callback = [&callback_count](
+    std::unique_ptr<test_msgs::msg::Empty>) {
+      callback_count++;
+    };
+
+  any_subscription_callback_.set(unique_ptr_callback);
+
+  // Message is copied into unique_ptr
+  EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_));
+  EXPECT_EQ(callback_count, 1);
+
+  EXPECT_THROW(
+    any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_),
+    std::runtime_error);
+  EXPECT_EQ(callback_count, 1);
+
+  // Unique_ptr is_moved
+  EXPECT_NO_THROW(
+    any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_));
+  EXPECT_EQ(callback_count, 2);
+}
+
+TEST_F(TestAnySubscriptionCallback, set_dispatch_unique_ptr_w_info) {
+  int callback_count = 0;
+  auto unique_ptr_callback = [&callback_count](
+    std::unique_ptr<test_msgs::msg::Empty>, const rclcpp::MessageInfo &) {
+      callback_count++;
+    };
+
+  any_subscription_callback_.set(unique_ptr_callback);
+
+  // Message is copied into unique_ptr
+  EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_));
+  EXPECT_EQ(callback_count, 1);
+
+  EXPECT_THROW(
+    any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_),
+    std::runtime_error);
+  EXPECT_EQ(callback_count, 1);
+
+  // Unique_ptr is_moved
+  EXPECT_NO_THROW(
+    any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_));
+  EXPECT_EQ(callback_count, 2);
+}

rclcpp/test/rclcpp/test_client.cpp

@@ -0,0 +1,330 @@
+// Copyright 2017 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 <string>
+#include <memory>
+#include <utility>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "rcl_interfaces/srv/list_parameters.hpp"
+
+#include "../mocking_utils/patch.hpp"
+
+#include "test_msgs/srv/empty.hpp"
+
+class TestClient : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+  }
+
+  void TearDown()
+  {
+    node.reset();
+  }
+
+  rclcpp::Node::SharedPtr node;
+};
+
+class TestClientSub : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+    subnode = node->create_sub_node("sub_ns");
+  }
+
+  void TearDown()
+  {
+    node.reset();
+  }
+
+  rclcpp::Node::SharedPtr node;
+  rclcpp::Node::SharedPtr subnode;
+};
+
+/*
+   Testing client construction and destruction.
+ */
+TEST_F(TestClient, construction_and_destruction) {
+  using rcl_interfaces::srv::ListParameters;
+  {
+    auto client = node->create_client<ListParameters>("service");
+  }
+
+  {
+    ASSERT_THROW(
+    {
+      auto client = node->create_client<ListParameters>("invalid_service?");
+    }, rclcpp::exceptions::InvalidServiceNameError);
+  }
+}
+
+TEST_F(TestClient, construction_with_free_function) {
+  {
+    auto client = rclcpp::create_client<rcl_interfaces::srv::ListParameters>(
+      node->get_node_base_interface(),
+      node->get_node_graph_interface(),
+      node->get_node_services_interface(),
+      "service",
+      rmw_qos_profile_services_default,
+      nullptr);
+  }
+  {
+    ASSERT_THROW(
+    {
+      auto client = rclcpp::create_client<rcl_interfaces::srv::ListParameters>(
+        node->get_node_base_interface(),
+        node->get_node_graph_interface(),
+        node->get_node_services_interface(),
+        "invalid_?service",
+        rmw_qos_profile_services_default,
+        nullptr);
+    }, rclcpp::exceptions::InvalidServiceNameError);
+  }
+}
+
+TEST_F(TestClient, construct_with_rcl_error) {
+  {
+    // reset() is not necessary for this exception, but handles unused return value warning
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_client_init, RCL_RET_ERROR);
+    EXPECT_THROW(
+      node->create_client<test_msgs::srv::Empty>("service").reset(),
+      rclcpp::exceptions::RCLError);
+  }
+  {
+    // reset() is required for this one
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_client_fini, RCL_RET_ERROR);
+    EXPECT_NO_THROW(node->create_client<test_msgs::srv::Empty>("service").reset());
+  }
+}
+
+TEST_F(TestClient, wait_for_service) {
+  const std::string service_name = "service";
+  auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+  EXPECT_FALSE(client->wait_for_service(std::chrono::nanoseconds(0)));
+  EXPECT_FALSE(client->wait_for_service(std::chrono::milliseconds(10)));
+
+  auto callback = [](
+    const test_msgs::srv::Empty::Request::SharedPtr,
+    test_msgs::srv::Empty::Response::SharedPtr) {};
+
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(service_name, std::move(callback));
+
+  EXPECT_TRUE(client->wait_for_service(std::chrono::nanoseconds(-1)));
+  EXPECT_TRUE(client->service_is_ready());
+}
+
+/*
+   Testing client construction and destruction for subnodes.
+ */
+TEST_F(TestClientSub, construction_and_destruction) {
+  using rcl_interfaces::srv::ListParameters;
+  {
+    auto client = subnode->create_client<ListParameters>("service");
+    EXPECT_STREQ(client->get_service_name(), "/ns/sub_ns/service");
+  }
+
+  {
+    ASSERT_THROW(
+    {
+      auto client = node->create_client<ListParameters>("invalid_service?");
+    }, rclcpp::exceptions::InvalidServiceNameError);
+  }
+}
+
+class TestClientWithServer : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("node", "ns");
+
+    auto callback = [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {};
+
+    service = node->create_service<test_msgs::srv::Empty>(service_name, std::move(callback));
+  }
+
+  ::testing::AssertionResult SendEmptyRequestAndWait(
+    std::chrono::milliseconds timeout = std::chrono::milliseconds(1000))
+  {
+    using SharedFuture = rclcpp::Client<test_msgs::srv::Empty>::SharedFuture;
+
+    auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+    if (!client->wait_for_service()) {
+      return ::testing::AssertionFailure() << "Waiting for service failed";
+    }
+
+    auto request = std::make_shared<test_msgs::srv::Empty::Request>();
+    bool received_response = false;
+    ::testing::AssertionResult request_result = ::testing::AssertionSuccess();
+    auto callback = [&received_response, &request_result](SharedFuture future_response) {
+        if (nullptr == future_response.get()) {
+          request_result = ::testing::AssertionFailure() << "Future response was null";
+        }
+        received_response = true;
+      };
+
+    auto future = client->async_send_request(request, std::move(callback));
+
+    auto start = std::chrono::steady_clock::now();
+    while (!received_response &&
+      (std::chrono::steady_clock::now() - start) < timeout)
+    {
+      rclcpp::spin_some(node);
+    }
+
+    if (!received_response) {
+      return ::testing::AssertionFailure() << "Waiting for response timed out";
+    }
+
+    return request_result;
+  }
+
+  std::shared_ptr<rclcpp::Node> node;
+  std::shared_ptr<rclcpp::Service<test_msgs::srv::Empty>> service;
+  const std::string service_name{"empty_service"};
+};
+
+TEST_F(TestClientWithServer, async_send_request) {
+  EXPECT_TRUE(SendEmptyRequestAndWait());
+}
+
+TEST_F(TestClientWithServer, async_send_request_callback_with_request) {
+  using SharedFutureWithRequest =
+    rclcpp::Client<test_msgs::srv::Empty>::SharedFutureWithRequest;
+
+  auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+  ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1)));
+
+  auto request = std::make_shared<test_msgs::srv::Empty::Request>();
+  bool received_response = false;
+  auto callback = [&request, &received_response](SharedFutureWithRequest future) {
+      auto request_response_pair = future.get();
+      EXPECT_EQ(request, request_response_pair.first);
+      EXPECT_NE(nullptr, request_response_pair.second);
+      received_response = true;
+    };
+  auto future = client->async_send_request(request, std::move(callback));
+
+  auto start = std::chrono::steady_clock::now();
+  while (!received_response &&
+    (std::chrono::steady_clock::now() - start) < std::chrono::seconds(1))
+  {
+    rclcpp::spin_some(node);
+  }
+  EXPECT_TRUE(received_response);
+}
+
+TEST_F(TestClientWithServer, async_send_request_rcl_send_request_error) {
+  // Checking rcl_send_request in rclcpp::Client::async_send_request()
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_request, RCL_RET_ERROR);
+  EXPECT_THROW(SendEmptyRequestAndWait(), rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestClientWithServer, async_send_request_rcl_service_server_is_available_error) {
+  {
+    // Checking rcl_service_server_is_available in rclcpp::ClientBase::service_is_ready
+    auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_service_server_is_available, RCL_RET_NODE_INVALID);
+    EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError);
+  }
+  {
+    // Checking rcl_service_server_is_available exception in rclcpp::ClientBase::service_is_ready
+    auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_service_server_is_available, RCL_RET_ERROR);
+    EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError);
+  }
+  {
+    // Checking rcl_service_server_is_available exception in rclcpp::ClientBase::service_is_ready
+    auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_service_server_is_available, RCL_RET_ERROR);
+    EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError);
+  }
+}
+
+TEST_F(TestClientWithServer, take_response) {
+  auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+  ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1)));
+  auto request = std::make_shared<test_msgs::srv::Empty::Request>();
+  auto request_header = client->create_request_header();
+  test_msgs::srv::Empty::Response response;
+
+  client->async_send_request(request);
+  EXPECT_FALSE(client->take_response(response, *request_header.get()));
+
+  {
+    // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_take_response, RCL_RET_OK);
+    EXPECT_TRUE(client->take_response(response, *request_header.get()));
+  }
+  {
+    // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_take_response, RCL_RET_CLIENT_TAKE_FAILED);
+    EXPECT_FALSE(client->take_response(response, *request_header.get()));
+  }
+  {
+    // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_take_response, RCL_RET_ERROR);
+    EXPECT_THROW(
+      client->take_response(response, *request_header.get()),
+      rclcpp::exceptions::RCLError);
+  }
+}

rclcpp/test/rclcpp/test_create_subscription.cpp

@@ -0,0 +1,67 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <memory>
+
+#include "rclcpp/create_subscription.hpp"
+#include "rclcpp/node.hpp"
+#include "test_msgs/msg/empty.hpp"
+#include "test_msgs/msg/empty.h"
+
+using namespace std::chrono_literals;
+
+class TestCreateSubscription : public ::testing::Test
+{
+public:
+  void SetUp() override
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void TearDown() override
+  {
+    rclcpp::shutdown();
+  }
+};
+
+TEST_F(TestCreateSubscription, create) {
+  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+  const rclcpp::QoS qos(10);
+  auto options = rclcpp::SubscriptionOptions();
+  auto callback = [](const test_msgs::msg::Empty::SharedPtr) {};
+  auto subscription =
+    rclcpp::create_subscription<test_msgs::msg::Empty>(node, "topic_name", qos, callback, options);
+
+  ASSERT_NE(nullptr, subscription);
+  EXPECT_STREQ("/ns/topic_name", subscription->get_topic_name());
+}
+
+TEST_F(TestCreateSubscription, create_with_statistics) {
+  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+  const rclcpp::QoS qos(10);
+  auto options = rclcpp::SubscriptionOptions();
+  options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable;
+  options.topic_stats_options.publish_topic = "topic_statistics";
+  options.topic_stats_options.publish_period = 5min;
+
+  auto callback = [](const test_msgs::msg::Empty::SharedPtr) {};
+  auto subscription =
+    rclcpp::create_subscription<test_msgs::msg::Empty>(node, "topic_name", qos, callback, options);
+
+  ASSERT_NE(nullptr, subscription);
+  EXPECT_STREQ("/ns/topic_name", subscription->get_topic_name());
+}

rclcpp/test/rclcpp/test_create_timer.cpp

@@ -0,0 +1,118 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <atomic>
+#include <chrono>
+#include <memory>
+
+#include "node_interfaces/node_wrapper.hpp"
+#include "rclcpp/create_timer.hpp"
+#include "rclcpp/executors.hpp"
+#include "rclcpp/node.hpp"
+
+using namespace std::chrono_literals;
+
+TEST(TestCreateTimer, timer_executes)
+{
+  rclcpp::init(0, nullptr);
+  auto node = std::make_shared<rclcpp::Node>("test_create_timer_node");
+
+  std::atomic<bool> got_callback{false};
+
+  rclcpp::TimerBase::SharedPtr timer;
+  timer = rclcpp::create_timer(
+    node,
+    node->get_clock(),
+    rclcpp::Duration(0ms),
+    [&got_callback, &timer]() {
+      got_callback = true;
+      timer->cancel();
+    });
+
+  rclcpp::spin_some(node);
+
+  ASSERT_TRUE(got_callback);
+  rclcpp::shutdown();
+}
+
+TEST(TestCreateTimer, call_with_node_wrapper_compiles)
+{
+  rclcpp::init(0, nullptr);
+  NodeWrapper node("test_create_timer_call_with_node_wrapper_compiles");
+
+  rclcpp::TimerBase::SharedPtr timer;
+  timer = rclcpp::create_timer(
+    node,
+    node.get_node_clock_interface()->get_clock(),
+    rclcpp::Duration(0ms),
+    []() {});
+  rclcpp::shutdown();
+}
+
+TEST(TestCreateTimer, call_wall_timer_with_bad_arguments)
+{
+  rclcpp::init(0, nullptr);
+  NodeWrapper node("test_create_wall_timers_with_bad_arguments");
+  auto callback = []() {};
+  rclcpp::CallbackGroup::SharedPtr group = nullptr;
+  auto node_interface =
+    rclcpp::node_interfaces::get_node_base_interface(node).get();
+  auto timers_interface =
+    rclcpp::node_interfaces::get_node_timers_interface(node).get();
+
+  // Negative period
+  EXPECT_THROW(
+    rclcpp::create_wall_timer(-1ms, callback, group, node_interface, timers_interface),
+    std::invalid_argument);
+
+  // Very negative period
+  constexpr auto nanoseconds_min = std::chrono::nanoseconds::min();
+  EXPECT_THROW(
+    rclcpp::create_wall_timer(
+      nanoseconds_min, callback, group, node_interface, timers_interface),
+    std::invalid_argument);
+
+  // Period must be less than nanoseconds::max()
+  constexpr auto nanoseconds_max = std::chrono::nanoseconds::min();
+  EXPECT_THROW(
+    rclcpp::create_wall_timer(
+      nanoseconds_max, callback, group, node_interface, timers_interface),
+    std::invalid_argument);
+
+  EXPECT_NO_THROW(
+    rclcpp::create_wall_timer(
+      nanoseconds_max - 1us, callback, group, node_interface, timers_interface));
+
+  EXPECT_NO_THROW(
+    rclcpp::create_wall_timer(0ms, callback, group, node_interface, timers_interface));
+
+  // Period must be less than nanoseconds::max()
+  constexpr auto hours_max = std::chrono::hours::max();
+  EXPECT_THROW(
+    rclcpp::create_wall_timer(hours_max, callback, group, node_interface, timers_interface),
+    std::invalid_argument);
+
+  // node_interface is null
+  EXPECT_THROW(
+    rclcpp::create_wall_timer(1ms, callback, group, nullptr, timers_interface),
+    std::invalid_argument);
+
+  // timers_interface is null
+  EXPECT_THROW(
+    rclcpp::create_wall_timer(1ms, callback, group, node_interface, nullptr),
+    std::invalid_argument);
+  rclcpp::shutdown();
+}

rclcpp/test/rclcpp/test_duration.cpp

@@ -0,0 +1,292 @@
+// Copyright 2017 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 <algorithm>
+#include <chrono>
+#include <limits>
+#include <string>
+
+#include "rcl/error_handling.h"
+#include "rcl/time.h"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/duration.hpp"
+
+#include "../utils/rclcpp_gtest_macros.hpp"
+
+using namespace std::chrono_literals;
+
+class TestDuration : public ::testing::Test
+{
+};
+
+TEST_F(TestDuration, operators) {
+  rclcpp::Duration old(1, 0);
+  rclcpp::Duration young(2, 0);
+
+  EXPECT_TRUE(old < young);
+  EXPECT_TRUE(young > old);
+  EXPECT_TRUE(old <= young);
+  EXPECT_TRUE(young >= old);
+  EXPECT_FALSE(young == old);
+  EXPECT_TRUE(young != old);
+
+  rclcpp::Duration add = old + young;
+  EXPECT_EQ(add.nanoseconds(), old.nanoseconds() + young.nanoseconds());
+  EXPECT_EQ(add, old + young);
+
+  rclcpp::Duration sub = young - old;
+  EXPECT_EQ(sub.nanoseconds(), young.nanoseconds() - old.nanoseconds());
+  EXPECT_EQ(sub, young - old);
+
+  rclcpp::Duration scale = old * 3;
+  EXPECT_EQ(scale.nanoseconds(), old.nanoseconds() * 3);
+
+  rclcpp::Duration time = rclcpp::Duration(0, 0);
+  rclcpp::Duration copy_constructor_duration(time);
+  rclcpp::Duration assignment_op_duration = rclcpp::Duration(1, 0);
+  (void)assignment_op_duration;
+  assignment_op_duration = time;
+
+  EXPECT_TRUE(time == copy_constructor_duration);
+  EXPECT_TRUE(time == assignment_op_duration);
+}
+
+TEST_F(TestDuration, chrono_overloads) {
+  int64_t ns = 123456789l;
+  auto chrono_ns = std::chrono::nanoseconds(ns);
+  auto d1 = rclcpp::Duration(ns);
+  auto d2 = rclcpp::Duration(chrono_ns);
+  auto d3 = rclcpp::Duration(123456789ns);
+  EXPECT_EQ(d1, d2);
+  EXPECT_EQ(d1, d3);
+  EXPECT_EQ(d2, d3);
+
+  // check non-nanosecond durations
+  std::chrono::milliseconds chrono_ms(100);
+  auto d4 = rclcpp::Duration(chrono_ms);
+  EXPECT_EQ(chrono_ms, d4.to_chrono<std::chrono::nanoseconds>());
+  std::chrono::duration<double, std::chrono::seconds::period> chrono_float_seconds(3.14);
+  auto d5 = rclcpp::Duration(chrono_float_seconds);
+  EXPECT_EQ(chrono_float_seconds, d5.to_chrono<decltype(chrono_float_seconds)>());
+}
+
+TEST_F(TestDuration, overflows) {
+  rclcpp::Duration max(std::numeric_limits<rcl_duration_value_t>::max());
+  rclcpp::Duration min(std::numeric_limits<rcl_duration_value_t>::min());
+
+  rclcpp::Duration one(1);
+  rclcpp::Duration negative_one(-1);
+
+  EXPECT_THROW(max + one, std::overflow_error);
+  EXPECT_THROW(min - one, std::underflow_error);
+  EXPECT_THROW(negative_one + min, std::underflow_error);
+  EXPECT_THROW(negative_one - max, std::underflow_error);
+
+  rclcpp::Duration base_d = max * 0.3;
+  EXPECT_THROW(base_d * 4, std::overflow_error);
+  EXPECT_THROW(base_d * (-4), std::underflow_error);
+
+  rclcpp::Duration base_d_neg = max * (-0.3);
+  EXPECT_THROW(base_d_neg * (-4), std::overflow_error);
+  EXPECT_THROW(base_d_neg * 4, std::underflow_error);
+}
+
+TEST_F(TestDuration, negative_duration) {
+  rclcpp::Duration assignable_duration = rclcpp::Duration(0) - rclcpp::Duration(5, 0);
+
+  {
+    // avoid windows converting a literal number less than -INT_MAX to unsigned int C4146
+    int64_t expected_value = -5000;
+    expected_value *= 1000 * 1000;
+    EXPECT_EQ(expected_value, assignable_duration.nanoseconds());
+  }
+
+  {
+    builtin_interfaces::msg::Duration duration_msg;
+    duration_msg.sec = -4;
+    duration_msg.nanosec = 250000000;
+
+    assignable_duration = duration_msg;
+    // avoid windows converting a literal number less than -INT_MAX to unsigned int C4146
+    int64_t expected_value = -3750;
+    expected_value *= 1000 * 1000;
+    EXPECT_EQ(expected_value, assignable_duration.nanoseconds());
+  }
+}
+
+TEST_F(TestDuration, maximum_duration) {
+  rclcpp::Duration max_duration = rclcpp::Duration::max();
+  rclcpp::Duration max(std::numeric_limits<int32_t>::max(), 999999999);
+
+  EXPECT_EQ(max_duration, max);
+}
+
+static const int64_t HALF_SEC_IN_NS = 500 * 1000 * 1000;
+static const int64_t ONE_SEC_IN_NS = 1000 * 1000 * 1000;
+static const int64_t ONE_AND_HALF_SEC_IN_NS = 3 * HALF_SEC_IN_NS;
+static const int64_t MAX_NANOSECONDS = std::numeric_limits<int64_t>::max();
+
+TEST_F(TestDuration, from_seconds) {
+  EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration::from_seconds(0.0));
+  EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration::from_seconds(0));
+  EXPECT_EQ(rclcpp::Duration(1, HALF_SEC_IN_NS), rclcpp::Duration::from_seconds(1.5));
+  EXPECT_EQ(rclcpp::Duration(-ONE_AND_HALF_SEC_IN_NS), rclcpp::Duration::from_seconds(-1.5));
+}
+
+TEST_F(TestDuration, std_chrono_constructors) {
+  EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration(0.0s));
+  EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration(0s));
+  EXPECT_EQ(rclcpp::Duration(1, HALF_SEC_IN_NS), rclcpp::Duration(1.5s));
+  EXPECT_EQ(rclcpp::Duration(-1, 0), rclcpp::Duration(-1s));
+}
+
+TEST_F(TestDuration, conversions) {
+  {
+    const rclcpp::Duration duration(HALF_SEC_IN_NS);
+    const auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
+    EXPECT_EQ(duration_msg.sec, 0);
+    EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS);
+    EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), HALF_SEC_IN_NS);
+
+    const auto rmw_time = duration.to_rmw_time();
+    EXPECT_EQ(rmw_time.sec, 0u);
+    EXPECT_EQ(rmw_time.nsec, static_cast<uint64_t>(HALF_SEC_IN_NS));
+
+    const auto chrono_duration = duration.to_chrono<std::chrono::nanoseconds>();
+    EXPECT_EQ(chrono_duration.count(), HALF_SEC_IN_NS);
+  }
+
+  {
+    const rclcpp::Duration duration(ONE_SEC_IN_NS);
+    const auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
+    EXPECT_EQ(duration_msg.sec, 1);
+    EXPECT_EQ(duration_msg.nanosec, 0u);
+    EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), ONE_SEC_IN_NS);
+
+    const auto rmw_time = duration.to_rmw_time();
+    EXPECT_EQ(rmw_time.sec, 1u);
+    EXPECT_EQ(rmw_time.nsec, 0u);
+
+    const auto chrono_duration = duration.to_chrono<std::chrono::nanoseconds>();
+    EXPECT_EQ(chrono_duration.count(), ONE_SEC_IN_NS);
+  }
+
+  {
+    const rclcpp::Duration duration(ONE_AND_HALF_SEC_IN_NS);
+    auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
+    EXPECT_EQ(duration_msg.sec, 1);
+    EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS);
+    EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), ONE_AND_HALF_SEC_IN_NS);
+
+    auto rmw_time = duration.to_rmw_time();
+    EXPECT_EQ(rmw_time.sec, 1u);
+    EXPECT_EQ(rmw_time.nsec, static_cast<uint64_t>(HALF_SEC_IN_NS));
+
+    auto chrono_duration = duration.to_chrono<std::chrono::nanoseconds>();
+    EXPECT_EQ(chrono_duration.count(), ONE_AND_HALF_SEC_IN_NS);
+  }
+
+  {
+    rclcpp::Duration duration(-HALF_SEC_IN_NS);
+    auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
+    EXPECT_EQ(duration_msg.sec, -1);
+    EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS);
+    EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), -HALF_SEC_IN_NS);
+
+    EXPECT_THROW(duration.to_rmw_time(), std::runtime_error);
+
+    auto chrono_duration = duration.to_chrono<std::chrono::nanoseconds>();
+    EXPECT_EQ(chrono_duration.count(), -HALF_SEC_IN_NS);
+  }
+
+  {
+    rclcpp::Duration duration(-ONE_SEC_IN_NS);
+    auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
+    EXPECT_EQ(duration_msg.sec, -1);
+    EXPECT_EQ(duration_msg.nanosec, 0u);
+    EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), -ONE_SEC_IN_NS);
+
+    EXPECT_THROW(duration.to_rmw_time(), std::runtime_error);
+
+    auto chrono_duration = duration.to_chrono<std::chrono::nanoseconds>();
+    EXPECT_EQ(chrono_duration.count(), -ONE_SEC_IN_NS);
+  }
+
+  {
+    rclcpp::Duration duration(-ONE_AND_HALF_SEC_IN_NS);
+    auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
+    EXPECT_EQ(duration_msg.sec, -2);
+    EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS);
+    EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), -ONE_AND_HALF_SEC_IN_NS);
+
+    EXPECT_THROW(duration.to_rmw_time(), std::runtime_error);
+
+    auto chrono_duration = duration.to_chrono<std::chrono::nanoseconds>();
+    EXPECT_EQ(chrono_duration.count(), -ONE_AND_HALF_SEC_IN_NS);
+  }
+
+  {
+    auto duration = rclcpp::Duration(MAX_NANOSECONDS);
+
+    const auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
+    EXPECT_EQ(duration_msg.sec, std::numeric_limits<int32_t>::max());
+    EXPECT_EQ(duration_msg.nanosec, std::numeric_limits<uint32_t>::max());
+
+    auto rmw_time = duration.to_rmw_time();
+    EXPECT_EQ(rmw_time.sec, 9223372036u);
+    EXPECT_EQ(rmw_time.nsec, 854775807u);
+
+    auto chrono_duration = duration.to_chrono<std::chrono::nanoseconds>();
+    EXPECT_EQ(chrono_duration.count(), MAX_NANOSECONDS);
+  }
+
+  {
+    auto duration = rclcpp::Duration(-MAX_NANOSECONDS);
+
+    const auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
+    EXPECT_EQ(duration_msg.sec, std::numeric_limits<int32_t>::min());
+    EXPECT_EQ(duration_msg.nanosec, 0u);
+
+    EXPECT_THROW(duration.to_rmw_time(), std::runtime_error);
+
+    auto chrono_duration = duration.to_chrono<std::chrono::nanoseconds>();
+    EXPECT_EQ(chrono_duration.count(), -MAX_NANOSECONDS);
+  }
+}
+
+TEST_F(TestDuration, test_some_constructors) {
+  builtin_interfaces::msg::Duration duration_msg;
+  duration_msg.sec = 1;
+  duration_msg.nanosec = 1000;
+  rclcpp::Duration duration_from_msg(duration_msg);
+  EXPECT_EQ(RCL_S_TO_NS(1) + 1000, duration_from_msg.nanoseconds());
+
+  rcl_duration_t duration_struct;
+  duration_struct.nanoseconds = 4000;
+  rclcpp::Duration duration_from_struct(duration_struct);
+  EXPECT_EQ(4000, duration_from_struct.nanoseconds());
+}
+
+TEST_F(TestDuration, test_some_exceptions) {
+  rclcpp::Duration test_duration(0u);
+  RCLCPP_EXPECT_THROW_EQ(
+    test_duration = rclcpp::Duration(INT64_MAX) - rclcpp::Duration(-1),
+    std::overflow_error("duration subtraction leads to int64_t overflow"));
+  RCLCPP_EXPECT_THROW_EQ(
+    test_duration = test_duration * (std::numeric_limits<double>::infinity()),
+    std::runtime_error("abnormal scale in rclcpp::Duration"));
+}

rclcpp/test/rclcpp/test_executor.cpp

@@ -0,0 +1,357 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <memory>
+#include <stdexcept>
+#include <string>
+
+#include "rclcpp/executor.hpp"
+#include "rclcpp/memory_strategy.hpp"
+#include "rclcpp/strategies/allocator_memory_strategy.hpp"
+
+#include "../mocking_utils/patch.hpp"
+#include "../utils/rclcpp_gtest_macros.hpp"
+
+// This file tests the abstract rclcpp::Executor class.  For tests of the concrete classes
+// that implement this class, please see the test/rclcpp/executors subdirectory.
+
+class DummyExecutor : public rclcpp::Executor
+{
+public:
+  DummyExecutor()
+  : rclcpp::Executor()
+  {
+  }
+
+  void spin() override
+  {
+  }
+
+  void spin_nanoseconds(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node)
+  {
+    spin_node_once_nanoseconds(node, std::chrono::milliseconds(100));
+  }
+
+  rclcpp::memory_strategy::MemoryStrategy * memory_strategy_ptr()
+  {
+    return memory_strategy_.get();
+  }
+
+  rclcpp::CallbackGroup::SharedPtr local_get_group_by_timer(rclcpp::TimerBase::SharedPtr timer)
+  {
+    return get_group_by_timer(timer);
+  }
+};
+
+class TestExecutor : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+};
+
+// Required for mocking_utils below
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, >)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >)
+
+TEST_F(TestExecutor, constructor_bad_guard_condition_init) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_guard_condition_init, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    new DummyExecutor(),
+    std::runtime_error(
+      "Failed to create interrupt guard condition in Executor constructor: error not set"));
+}
+
+TEST_F(TestExecutor, constructor_bad_wait_set_init) {
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_init, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    new DummyExecutor(),
+    std::runtime_error("Failed to create wait set in Executor constructor: error not set"));
+}
+
+TEST_F(TestExecutor, spin_node_once_nanoseconds) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool timer_fired = false;
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&timer_fired]() {timer_fired = true;});
+
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  EXPECT_FALSE(timer_fired);
+  dummy.spin_nanoseconds(node->get_node_base_interface());
+  EXPECT_TRUE(timer_fired);
+}
+
+TEST_F(TestExecutor, spin_node_some) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool timer_fired = false;
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&timer_fired]() {timer_fired = true;});
+
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  EXPECT_FALSE(timer_fired);
+  dummy.spin_node_some(node);
+  EXPECT_TRUE(timer_fired);
+}
+
+TEST_F(TestExecutor, spin_some_in_spin_some) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool spin_some_in_spin_some = false;
+  auto timer =
+    node->create_wall_timer(
+    std::chrono::milliseconds(1), [&]() {
+      try {
+        dummy.spin_some(std::chrono::milliseconds(1));
+      } catch (const std::runtime_error & err) {
+        if (err.what() == std::string("spin_some() called while already spinning")) {
+          spin_some_in_spin_some = true;
+        }
+      }
+    });
+
+  dummy.add_node(node);
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  EXPECT_FALSE(spin_some_in_spin_some);
+  dummy.spin_some(std::chrono::milliseconds(1));
+  EXPECT_TRUE(spin_some_in_spin_some);
+}
+
+TEST_F(TestExecutor, spin_some_elapsed) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool timer_called = false;
+  auto timer =
+    node->create_wall_timer(
+    std::chrono::milliseconds(1), [&]() {
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+      timer_called = true;
+    });
+
+  dummy.add_node(node);
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  dummy.spin_some(std::chrono::milliseconds(1));
+
+  ASSERT_TRUE(timer_called);
+}
+
+TEST_F(TestExecutor, spin_once_in_spin_once) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool spin_once_in_spin_once = false;
+  auto timer =
+    node->create_wall_timer(
+    std::chrono::milliseconds(1), [&]() {
+      try {
+        dummy.spin_once(std::chrono::milliseconds(1));
+      } catch (const std::runtime_error & err) {
+        if (err.what() == std::string("spin_once() called while already spinning")) {
+          spin_once_in_spin_once = true;
+        }
+      }
+    });
+
+  dummy.add_node(node);
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  EXPECT_FALSE(spin_once_in_spin_once);
+  dummy.spin_once(std::chrono::milliseconds(1));
+  EXPECT_TRUE(spin_once_in_spin_once);
+}
+
+TEST_F(TestExecutor, cancel_failed_trigger_guard_condition) {
+  DummyExecutor dummy;
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.cancel(),
+    std::runtime_error("Failed to trigger guard condition in cancel: error not set"));
+}
+
+TEST_F(TestExecutor, set_memory_strategy_nullptr) {
+  DummyExecutor dummy;
+
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.set_memory_strategy(nullptr),
+    std::runtime_error("Received NULL memory strategy in executor."));
+}
+
+TEST_F(TestExecutor, set_memory_strategy) {
+  DummyExecutor dummy;
+  rclcpp::memory_strategy::MemoryStrategy::SharedPtr strategy =
+    std::make_shared<
+    rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>();
+
+  dummy.set_memory_strategy(strategy);
+  EXPECT_EQ(dummy.memory_strategy_ptr(), strategy.get());
+}
+
+TEST_F(TestExecutor, spin_once_failed_trigger_guard_condition) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {});
+
+  dummy.add_node(node);
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.spin_once(std::chrono::milliseconds(1)),
+    std::runtime_error(
+      "Failed to trigger guard condition from execute_any_executable: error not set"));
+}
+
+TEST_F(TestExecutor, spin_some_fail_wait_set_clear) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {});
+
+  dummy.add_node(node);
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.spin_some(std::chrono::milliseconds(1)),
+    std::runtime_error("Couldn't clear wait set: error not set"));
+}
+
+TEST_F(TestExecutor, spin_some_fail_wait_set_resize) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {});
+
+  dummy.add_node(node);
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_resize, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.spin_some(std::chrono::milliseconds(1)),
+    std::runtime_error("Couldn't resize the wait set: error not set"));
+}
+
+TEST_F(TestExecutor, spin_some_fail_add_handles_to_wait_set) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {});
+
+  dummy.add_node(node);
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait_set_add_subscription,
+    RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.spin_some(std::chrono::milliseconds(1)),
+    std::runtime_error("Couldn't fill wait set"));
+}
+
+TEST_F(TestExecutor, spin_some_fail_wait) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {});
+
+  dummy.add_node(node);
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.spin_some(std::chrono::milliseconds(1)),
+    std::runtime_error("rcl_wait() failed: error not set"));
+}
+
+TEST_F(TestExecutor, get_group_by_timer_nullptr) {
+  DummyExecutor dummy;
+  ASSERT_EQ(nullptr, dummy.local_get_group_by_timer(nullptr));
+}
+
+TEST_F(TestExecutor, get_group_by_timer) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group);
+  dummy.add_node(node);
+
+  ASSERT_EQ(cb_group.get(), dummy.local_get_group_by_timer(timer).get());
+}
+
+TEST_F(TestExecutor, get_group_by_timer_with_deleted_group) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group);
+  dummy.add_node(node);
+
+  cb_group.reset();
+
+  ASSERT_EQ(nullptr, dummy.local_get_group_by_timer(timer).get());
+}
+
+TEST_F(TestExecutor, spin_node_once_base_interface) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool spin_called = false;
+  auto timer =
+    node->create_wall_timer(
+    std::chrono::milliseconds(1), [&]() {
+      spin_called = true;
+    });
+
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  EXPECT_FALSE(spin_called);
+  dummy.spin_node_once(node->get_node_base_interface());
+  EXPECT_TRUE(spin_called);
+}
+
+TEST_F(TestExecutor, spin_node_once_node) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool spin_called = false;
+  auto timer =
+    node->create_wall_timer(
+    std::chrono::milliseconds(1), [&]() {
+      spin_called = true;
+    });
+
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  EXPECT_FALSE(spin_called);
+  dummy.spin_node_once(node);
+  EXPECT_TRUE(spin_called);
+}

rclcpp/test/rclcpp/test_expand_topic_or_service_name.cpp

@@ -0,0 +1,244 @@
+// Copyright 2017 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 <stdexcept>
+
+#include "rcl/expand_topic_name.h"
+#include "rcl/validate_topic_name.h"
+#include "rmw/validate_full_topic_name.h"
+#include "rmw/validate_namespace.h"
+#include "rmw/validate_node_name.h"
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
+
+#include "../mocking_utils/patch.hpp"
+#include "../utils/rclcpp_gtest_macros.hpp"
+
+/*
+   Testing expand_topic_or_service_name.
+ */
+TEST(TestExpandTopicOrServiceName, normal) {
+  using rclcpp::expand_topic_or_service_name;
+  {
+    ASSERT_EQ("/ns/chatter", expand_topic_or_service_name("chatter", "node", "/ns"));
+  }
+}
+
+/*
+   Testing exceptions of expand_topic_or_service_name.
+ */
+TEST(TestExpandTopicOrServiceName, exceptions) {
+  using rclcpp::expand_topic_or_service_name;
+  {
+    ASSERT_THROW(
+    {
+      expand_topic_or_service_name("chatter", "invalid_node?", "/ns");
+    }, rclcpp::exceptions::InvalidNodeNameError);
+  }
+
+  {
+    ASSERT_THROW(
+    {
+      expand_topic_or_service_name("chatter", "node", "/invalid_ns?");
+    }, rclcpp::exceptions::InvalidNamespaceError);
+  }
+
+  {
+    ASSERT_THROW(
+    {
+      expand_topic_or_service_name("chatter/42invalid", "node", "/ns");
+    }, rclcpp::exceptions::InvalidTopicNameError);
+  }
+
+  {
+    ASSERT_THROW(
+    {
+      // this one will only fail on the "full" topic name validation check
+      expand_topic_or_service_name("chatter/{ns}/invalid", "node", "/ns");
+    }, rclcpp::exceptions::InvalidTopicNameError);
+  }
+
+  {
+    ASSERT_THROW(
+    {
+      // is_service = true
+      expand_topic_or_service_name("chatter/42invalid", "node", "/ns", true);
+    }, rclcpp::exceptions::InvalidServiceNameError);
+  }
+
+  {
+    ASSERT_THROW(
+    {
+      // is_service = true
+      // this one will only fail on the "full" topic name validation check
+      expand_topic_or_service_name("chatter/{ns}/invalid", "node", "/ns", true);
+    }, rclcpp::exceptions::InvalidServiceNameError);
+  }
+}
+
+// Required for mocking_utils below
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >)
+
+TEST(TestExpandTopicOrServiceName, rcutils_string_map_init_fail_bad_alloc) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcutils_string_map_init, RCUTILS_RET_BAD_ALLOC);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::bad_alloc());
+}
+
+TEST(TestExpandTopicOrServiceName, rcutils_string_map_init_fail_other) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcutils_string_map_init, RCUTILS_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::runtime_error("error not set"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcl_get_default_topic_name_substitution_fail) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_default_topic_name_substitutions, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::runtime_error("error not set"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcl_get_default_topic_name_substitution_and_map_fini_fail) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_default_topic_name_substitutions, RCL_RET_ERROR);
+  auto mock2 = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcutils_string_map_fini, RCUTILS_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::runtime_error("error not set"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcutils_string_map_fini_fail_bad_alloc) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcutils_string_map_fini, RCUTILS_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::runtime_error("error not set"));
+}
+
+TEST(TestExpandTopicOrServiceName, rmw_valid_full_topic_name_fail_invalid_argument) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_full_topic_name, RMW_RET_INVALID_ARGUMENT);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLInvalidArgument(
+      RCL_RET_INVALID_ARGUMENT, rcl_get_error_state(), "failed to validate full topic name"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcl_expand_topic_name_fail) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_TOPIC_NAME_INVALID);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::runtime_error("topic name unexpectedly valid"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcl_validate_topic_name_fail) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_TOPIC_NAME_INVALID);
+  auto mock2 = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_validate_topic_name, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, rcl_get_error_state(), "failed to validate full topic name"));
+}
+
+TEST(TestExpandTopicOrServiceName, rmw_validate_node_name_fail_invalid_argument) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAME);
+  auto mock2 = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_node_name, RMW_RET_INVALID_ARGUMENT);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLInvalidArgument(
+      RCL_RET_INVALID_ARGUMENT, rcl_get_error_state(), "failed to validate node name"));
+}
+
+TEST(TestExpandTopicOrServiceName, rmw_validate_node_name_fail_other) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAME);
+  auto mock2 = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_node_name, RMW_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, rcl_get_error_state(), "failed to validate node name"));
+}
+
+TEST(TestExpandTopicOrServiceName, rmw_validate_namespace_fail_invalid_argument) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAMESPACE);
+  auto mock2 = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_namespace, RMW_RET_INVALID_ARGUMENT);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLInvalidArgument(
+      RCL_RET_INVALID_ARGUMENT, rcl_get_error_state(), "failed to validate namespace"));
+}
+
+TEST(TestExpandTopicOrServiceName, rmw_validate_namespace_fail_other) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAMESPACE);
+  auto mock2 = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_namespace, RMW_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, rcl_get_error_state(), "failed to validate namespace"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcl_expand_topic_name_fail_other) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLError(RCL_RET_ERROR, rcl_get_error_state(), "error not set"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcl_expand_topic_name_fail_invalid_node_name) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAME);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::runtime_error("invalid rcl node name but valid rmw node name"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcl_expand_topic_name_fail_invalid_node_namespace) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAMESPACE);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::runtime_error("invalid rcl namespace but valid rmw namespace"));
+}
+
+TEST(TestExpandTopicOrServiceName, rmw_validate_full_topic_name_fail_other) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_full_topic_name, RMW_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, rcl_get_error_state(), "failed to validate full topic name"));
+}