0.9.0

* Increasing test coverage of rclcpp_action

Signed-off-by: Stephen Brawner <brawner@gmail.com>

* PR Fixup

Signed-off-by: Stephen Brawner <brawner@gmail.com>

* PR Fixup

Signed-off-by: Stephen Brawner <brawner@gmail.com>

* PR Fixup

Signed-off-by: Stephen Brawner <brawner@gmail.com>

* Increasing test coverage of rclcpp_action

Signed-off-by: Stephen Brawner <brawner@gmail.com>

* PR Fixup

Signed-off-by: Stephen Brawner <brawner@gmail.com>

* PR Fixup

Signed-off-by: Stephen Brawner <brawner@gmail.com>

* PR Fixup

Signed-off-by: Stephen Brawner <brawner@gmail.com>

* Fix warnings

Signed-off-by: Stephen Brawner <brawner@gmail.com>

README.md

@@ -0,0 +1,17 @@
+# rclcpp
+
+This repository contains the source code for the ROS Client Library for C++ package, included with a standard install of any ROS 2 distro.
+
+rclcpp provides the standard C++ API for interacting with ROS 2.
+
+## Usage
+
+`#include "rclcpp/rclcpp.hpp"` allows use of the most common elements of the ROS 2 system.
+
+Visit the [rclcpp API documentation](http://docs.ros2.org/eloquent/api/rclcpp/) for a complete list of its main components.
+
+### 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/)
+contain some examples of rclcpp APIs in use.

rclcpp/CHANGELOG.rst

@@ -2,6 +2,89 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.9.0 (2020-04-29)
+------------------
+* Serialized message move constructor (`#1097 <https://github.com/ros2/rclcpp/issues/1097>`_)
+* Enforce a precedence for wildcard matching in parameter overrides. (`#1094 <https://github.com/ros2/rclcpp/issues/1094>`_)
+* Add serialized_message.hpp header (`#1095 <https://github.com/ros2/rclcpp/issues/1095>`_)
+* Add received message age metric to topic statistics (`#1080 <https://github.com/ros2/rclcpp/issues/1080>`_)
+* Deprecate redundant namespaces (`#1083 <https://github.com/ros2/rclcpp/issues/1083>`_)
+* Export targets in addition to include directories / libraries (`#1088 <https://github.com/ros2/rclcpp/issues/1088>`_)
+* Ensure logging is initialized just once (`#998 <https://github.com/ros2/rclcpp/issues/998>`_)
+* Adapt subscription traits to rclcpp::SerializedMessage (`#1092 <https://github.com/ros2/rclcpp/issues/1092>`_)
+* Protect subscriber_statistics_collectors\_ with a mutex (`#1084 <https://github.com/ros2/rclcpp/issues/1084>`_)
+* Remove unused test variable (`#1087 <https://github.com/ros2/rclcpp/issues/1087>`_)
+* Use serialized message (`#1081 <https://github.com/ros2/rclcpp/issues/1081>`_)
+* Integrate topic statistics (`#1072 <https://github.com/ros2/rclcpp/issues/1072>`_)
+* Fix rclcpp interface traits test (`#1086 <https://github.com/ros2/rclcpp/issues/1086>`_)
+* Generate node interfaces' getters and traits (`#1069 <https://github.com/ros2/rclcpp/issues/1069>`_)
+* Use composition for serialized message (`#1082 <https://github.com/ros2/rclcpp/issues/1082>`_)
+* Dnae adas/serialized message (`#1075 <https://github.com/ros2/rclcpp/issues/1075>`_)
+* Reflect changes in rclcpp API (`#1079 <https://github.com/ros2/rclcpp/issues/1079>`_)
+* Fix build regression (`#1078 <https://github.com/ros2/rclcpp/issues/1078>`_)
+* Add NodeDefault option for enabling topic statistics (`#1074 <https://github.com/ros2/rclcpp/issues/1074>`_)
+* Topic Statistics: Add SubscriptionTopicStatistics class (`#1050 <https://github.com/ros2/rclcpp/issues/1050>`_)
+* Add SubscriptionOptions for topic statistics (`#1057 <https://github.com/ros2/rclcpp/issues/1057>`_)
+* Remove warning message from failing to register default callback (`#1067 <https://github.com/ros2/rclcpp/issues/1067>`_)
+* Create a default warning for qos incompatibility (`#1051 <https://github.com/ros2/rclcpp/issues/1051>`_)
+* Add WaitSet class and modify entities to work without executor (`#1047 <https://github.com/ros2/rclcpp/issues/1047>`_)
+* Include what you use (`#1059 <https://github.com/ros2/rclcpp/issues/1059>`_)
+* Rename rosidl_generator_cpp namespace to rosidl_runtime_cpp (`#1060 <https://github.com/ros2/rclcpp/issues/1060>`_)
+* Changed rosidl_generator_c/cpp to rosidl_runtime_c/cpp (`#1014 <https://github.com/ros2/rclcpp/issues/1014>`_)
+* Use constexpr for endpoint type name (`#1055 <https://github.com/ros2/rclcpp/issues/1055>`_)
+* Add InvalidParameterTypeException (`#1027 <https://github.com/ros2/rclcpp/issues/1027>`_)
+* Support for ON_REQUESTED_INCOMPATIBLE_QOS and ON_OFFERED_INCOMPATIBLE_QOS events (`#924 <https://github.com/ros2/rclcpp/issues/924>`_)
+* Fixup clang warning (`#1040 <https://github.com/ros2/rclcpp/issues/1040>`_)
+* Adding a "static" single threaded executor (`#1034 <https://github.com/ros2/rclcpp/issues/1034>`_)
+* Add equality operators for QoS profile (`#1032 <https://github.com/ros2/rclcpp/issues/1032>`_)
+* Remove extra vertical whitespace (`#1030 <https://github.com/ros2/rclcpp/issues/1030>`_)
+* Switch IntraProcessMessage to test_msgs/Empty (`#1017 <https://github.com/ros2/rclcpp/issues/1017>`_)
+* Add new type of exception that may be thrown during creation of publisher/subscription (`#1026 <https://github.com/ros2/rclcpp/issues/1026>`_)
+* Don't check lifespan on publisher QoS (`#1002 <https://github.com/ros2/rclcpp/issues/1002>`_)
+* Fix get_parameter_tyeps of AsyncPrameterClient results are always empty (`#1019 <https://github.com/ros2/rclcpp/issues/1019>`_)
+* Cleanup node interfaces includes (`#1016 <https://github.com/ros2/rclcpp/issues/1016>`_)
+* Add ifdefs to remove tracing-related calls if tracing is disabled (`#1001 <https://github.com/ros2/rclcpp/issues/1001>`_)
+* Include missing header in node_graph.cpp (`#994 <https://github.com/ros2/rclcpp/issues/994>`_)
+* Add missing includes of logging.hpp (`#995 <https://github.com/ros2/rclcpp/issues/995>`_)
+* Zero initialize publisher GID in subscription intra process callback (`#1011 <https://github.com/ros2/rclcpp/issues/1011>`_)
+* Removed ament_cmake dependency (`#989 <https://github.com/ros2/rclcpp/issues/989>`_)
+* Switch to using new rcutils_strerror (`#993 <https://github.com/ros2/rclcpp/issues/993>`_)
+* Ensure all rclcpp::Clock accesses are thread-safe
+* Use a PIMPL for rclcpp::Clock implementation
+* Replace rmw_implementation for rmw dependency in package.xml (`#990 <https://github.com/ros2/rclcpp/issues/990>`_)
+* Add missing service callback registration tracepoint (`#986 <https://github.com/ros2/rclcpp/issues/986>`_)
+* Rename rmw_topic_endpoint_info_array count to size (`#996 <https://github.com/ros2/rclcpp/issues/996>`_)
+* Implement functions to get publisher and subcription informations like QoS policies from topic name (`#960 <https://github.com/ros2/rclcpp/issues/960>`_)
+* Code style only: wrap after open parenthesis if not in one line (`#977 <https://github.com/ros2/rclcpp/issues/977>`_)
+* Accept taking an rvalue ref future in spin_until_future_complete (`#971 <https://github.com/ros2/rclcpp/issues/971>`_)
+* Allow node clock use in logging macros (`#969 <https://github.com/ros2/rclcpp/issues/969>`_) (`#970 <https://github.com/ros2/rclcpp/issues/970>`_)
+* Change order of deprecated and visibility attributes (`#968 <https://github.com/ros2/rclcpp/issues/968>`_)
+* Deprecated is_initialized() (`#967 <https://github.com/ros2/rclcpp/issues/967>`_)
+* Don't specify calling convention in std::_Binder template (`#952 <https://github.com/ros2/rclcpp/issues/952>`_)
+* Added missing include to logging.hpp (`#964 <https://github.com/ros2/rclcpp/issues/964>`_)
+* Assigning make_shared result to variables in test (`#963 <https://github.com/ros2/rclcpp/issues/963>`_)
+* Fix unused parameter warning (`#962 <https://github.com/ros2/rclcpp/issues/962>`_)
+* Stop retaining ownership of the rcl context in GraphListener (`#946 <https://github.com/ros2/rclcpp/issues/946>`_)
+* Clear sub contexts when starting another init-shutdown cycle (`#947 <https://github.com/ros2/rclcpp/issues/947>`_)
+* Avoid possible UB in Clock jump callbacks (`#954 <https://github.com/ros2/rclcpp/issues/954>`_)
+* Handle unknown global ROS arguments (`#951 <https://github.com/ros2/rclcpp/issues/951>`_)
+* Mark get_clock() as override to fix clang warnings (`#939 <https://github.com/ros2/rclcpp/issues/939>`_)
+* Create node clock calls const (try 2) (`#922 <https://github.com/ros2/rclcpp/issues/922>`_)
+* Fix asserts on shared_ptr::use_count; expects long, got uint32 (`#936 <https://github.com/ros2/rclcpp/issues/936>`_)
+* Use absolute topic name for parameter events (`#929 <https://github.com/ros2/rclcpp/issues/929>`_)
+* Add enable_rosout into NodeOptions. (`#900 <https://github.com/ros2/rclcpp/issues/900>`_)
+* Removing "virtual", adding "override" keywords (`#897 <https://github.com/ros2/rclcpp/issues/897>`_)
+* Use weak_ptr to store context in GraphListener (`#906 <https://github.com/ros2/rclcpp/issues/906>`_)
+* Complete published event message when declaring a parameter (`#928 <https://github.com/ros2/rclcpp/issues/928>`_)
+* Fix duration.cpp lint error (`#930 <https://github.com/ros2/rclcpp/issues/930>`_)
+* Intra-process subscriber should use RMW actual qos. (ros2`#913 <https://github.com/ros2/rclcpp/issues/913>`_) (`#914 <https://github.com/ros2/rclcpp/issues/914>`_)
+* Type conversions fixes (`#901 <https://github.com/ros2/rclcpp/issues/901>`_)
+* Add override keyword to functions
+* Remove unnecessary virtual keywords
+* Only check for new work once in spin_some (`#471 <https://github.com/ros2/rclcpp/issues/471>`_) (`#844 <https://github.com/ros2/rclcpp/issues/844>`_)
+* Add addition/subtraction assignment operators to Time (`#748 <https://github.com/ros2/rclcpp/issues/748>`_)
+* Contributors: Alberto Soragna, Alejandro Hernández Cordero, Barry Xu, Chris Lalancette, Christophe Bedard, Claire Wang, Dan Rose, DensoADAS, Devin Bonnie, Dino Hüllmann, Dirk Thomas, DongheeYe, Emerson Knapp, Ivan Santiago Paunovic, Jacob Perron, Jaison Titus, Karsten Knese, Matt Schickler, Miaofei Mei, Michel Hidalgo, Mikael Arguedas, Monika Idzik, Prajakta Gokhale, Roger Strain, Scott K Logan, Sean Kelly, Stephen Brawner, Steven Macenski, Steven! Ragnarök, Todd Malsbary, Tomoya Fujita, William Woodall, Zachary Michaels
+
 0.8.3 (2019-11-19)
 ------------------
 

rclcpp/CMakeLists.txt

@@ -4,16 +4,19 @@ project(rclcpp)
 
 find_package(ament_cmake_ros REQUIRED)
 find_package(builtin_interfaces REQUIRED)
+find_package(libstatistics_collector REQUIRED)
 find_package(rcl REQUIRED)
 find_package(rcl_interfaces REQUIRED)
 find_package(rcl_yaml_param_parser REQUIRED)
 find_package(rcpputils REQUIRED)
+find_package(rcutils REQUIRED)
 find_package(rmw REQUIRED)
-find_package(rmw_implementation REQUIRED)
 find_package(rosgraph_msgs REQUIRED)
-find_package(rosidl_generator_cpp REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(rosidl_runtime_cpp REQUIRED)
 find_package(rosidl_typesupport_c REQUIRED)
 find_package(rosidl_typesupport_cpp REQUIRED)
+find_package(statistics_msgs REQUIRED)
 find_package(tracetools REQUIRED)
 
 # Default to C++14
@@ -24,8 +27,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-include_directories(include)
-
 set(${PROJECT_NAME}_SRCS
   src/rclcpp/any_executable.cpp
   src/rclcpp/callback_group.cpp
@@ -36,20 +37,27 @@ set(${PROJECT_NAME}_SRCS
   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
+  src/rclcpp/detail/utilities.cpp
   src/rclcpp/duration.cpp
   src/rclcpp/event.cpp
-  src/rclcpp/exceptions.cpp
+  src/rclcpp/exceptions/exceptions.cpp
+  src/rclcpp/executable_list.cpp
   src/rclcpp/executor.cpp
   src/rclcpp/executors.cpp
   src/rclcpp/expand_topic_or_service_name.cpp
   src/rclcpp/executors/multi_threaded_executor.cpp
   src/rclcpp/executors/single_threaded_executor.cpp
+  src/rclcpp/executors/static_executor_entities_collector.cpp
+  src/rclcpp/executors/static_single_threaded_executor.cpp
+  src/rclcpp/future_return_code.cpp
   src/rclcpp/graph_listener.cpp
+  src/rclcpp/guard_condition.cpp
   src/rclcpp/init_options.cpp
   src/rclcpp/intra_process_manager.cpp
   src/rclcpp/logger.cpp
   src/rclcpp/memory_strategies.cpp
   src/rclcpp/memory_strategy.cpp
+  src/rclcpp/message_info.cpp
   src/rclcpp/node.cpp
   src/rclcpp/node_options.cpp
   src/rclcpp/node_interfaces/node_base.cpp
@@ -71,6 +79,8 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/publisher_base.cpp
   src/rclcpp/qos.cpp
   src/rclcpp/qos_event.cpp
+  src/rclcpp/serialization.cpp
+  src/rclcpp/serialized_message.cpp
   src/rclcpp/service.cpp
   src/rclcpp/signal_handler.cpp
   src/rclcpp/subscription_base.cpp
@@ -80,6 +90,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/timer.cpp
   src/rclcpp/type_support.cpp
   src/rclcpp/utilities.cpp
+  src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp
   src/rclcpp/waitable.cpp
 )
 
@@ -90,32 +101,83 @@ configure_file(
   COPYONLY
 )
 # generate header with logging macros
-set(python_code
+set(python_code_logging
   "import em"
   "em.invoke(['-o', 'include/rclcpp/logging.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/logging.hpp.em'])")
-string(REPLACE ";" "$<SEMICOLON>" python_code "${python_code}")
+string(REPLACE ";" "$<SEMICOLON>" python_code_logging "${python_code_logging}")
 add_custom_command(OUTPUT include/rclcpp/logging.hpp
   COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp"
-  COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_code}"
+  COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_code_logging}"
   DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/logging.hpp.em.watch"
   COMMENT "Expanding logging.hpp.em"
   VERBATIM
 )
 list(APPEND ${PROJECT_NAME}_SRCS
   include/rclcpp/logging.hpp)
-include_directories("${CMAKE_CURRENT_BINARY_DIR}/include")
+
+file(GLOB interface_files "include/rclcpp/node_interfaces/node_*_interface.hpp")
+foreach(interface_file ${interface_files})
+  get_filename_component(interface_name ${interface_file} NAME_WE)
+
+  # "watch" template for changes
+  configure_file(
+    "resource/interface_traits.hpp.em"
+    "${CMAKE_CURRENT_BINARY_DIR}/${interface_name}_traits.hpp.em.watch"
+    COPYONLY
+  )
+  set(python_${interface_name}_traits
+    "import em"
+    "em.invoke(['-D', 'interface_name = \\'${interface_name}\\'', '-o', 'include/rclcpp/node_interfaces/${interface_name}_traits.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/interface_traits.hpp.em'])")
+  string(REPLACE ";" "$<SEMICOLON>" python_${interface_name}_traits "${python_${interface_name}_traits}")
+  add_custom_command(OUTPUT include/rclcpp/node_interfaces/${interface_name}_traits.hpp
+    COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp/node_interfaces"
+    COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_${interface_name}_traits}"
+    DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/${interface_name}_traits.hpp.em.watch"
+    COMMENT "Expanding interface_traits.hpp.em into ${interface_name}_traits.hpp"
+    VERBATIM
+  )
+  list(APPEND ${PROJECT_NAME}_SRCS
+    include/rclcpp/node_interfaces/${interface_name}_traits.hpp)
+
+  # "watch" template for changes
+  configure_file(
+    "resource/get_interface.hpp.em"
+    "get_${interface_name}.hpp.em.watch"
+    COPYONLY
+  )
+  set(python_get_${interface_name}
+    "import em"
+    "em.invoke(['-D', 'interface_name = \\'${interface_name}\\'', '-o', 'include/rclcpp/node_interfaces/get_${interface_name}.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/get_interface.hpp.em'])")
+  string(REPLACE ";" "$<SEMICOLON>" python_get_${interface_name} "${python_get_${interface_name}}")
+  add_custom_command(OUTPUT include/rclcpp/node_interfaces/get_${interface_name}.hpp
+    COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp/node_interfaces"
+    COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_get_${interface_name}}"
+    DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/get_${interface_name}.hpp.em.watch"
+    COMMENT "Expanding get_interface.hpp.em into get_${interface_file}.hpp"
+    VERBATIM
+  )
+  list(APPEND ${PROJECT_NAME}_SRCS
+    include/rclcpp/node_interfaces/get_${interface_name}.hpp)
+endforeach()
 
 add_library(${PROJECT_NAME}
   ${${PROJECT_NAME}_SRCS})
+target_include_directories(${PROJECT_NAME} PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>"
+  "$<INSTALL_INTERFACE:include>")
 # specific order: dependents before dependencies
 ament_target_dependencies(${PROJECT_NAME}
+  "libstatistics_collector"
   "rcl"
   "rcl_yaml_param_parser"
   "rcpputils"
+  "rcutils"
   "builtin_interfaces"
   "rosgraph_msgs"
   "rosidl_typesupport_cpp"
-  "rosidl_generator_cpp"
+  "rosidl_runtime_cpp"
+  "statistics_msgs"
   "tracetools"
 )
 
@@ -125,7 +187,7 @@ target_compile_definitions(${PROJECT_NAME}
   PRIVATE "RCLCPP_BUILDING_LIBRARY")
 
 install(
-  TARGETS ${PROJECT_NAME}
+  TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -134,16 +196,19 @@ install(
 # specific order: dependents before dependencies
 ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})
+ament_export_targets(${PROJECT_NAME})
 
-ament_export_dependencies(ament_cmake)
+ament_export_dependencies(libstatistics_collector)
 ament_export_dependencies(rcl)
 ament_export_dependencies(rcpputils)
+ament_export_dependencies(rcutils)
 ament_export_dependencies(builtin_interfaces)
 ament_export_dependencies(rosgraph_msgs)
 ament_export_dependencies(rosidl_typesupport_cpp)
 ament_export_dependencies(rosidl_typesupport_c)
-ament_export_dependencies(rosidl_generator_cpp)
+ament_export_dependencies(rosidl_runtime_cpp)
 ament_export_dependencies(rcl_yaml_param_parser)
+ament_export_dependencies(statistics_msgs)
 ament_export_dependencies(tracetools)
 
 if(BUILD_TESTING)
@@ -159,12 +224,20 @@ if(BUILD_TESTING)
 
   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_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_client ${PROJECT_NAME})
@@ -175,7 +248,7 @@ if(BUILD_TESTING)
       "rcl_interfaces"
       "rmw"
       "rcl"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_create_timer ${PROJECT_NAME})
@@ -186,17 +259,18 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_expand_topic_or_service_name
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "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_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
   endif()
@@ -206,7 +280,7 @@ if(BUILD_TESTING)
       "rcl"
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_intra_process_manager ${PROJECT_NAME})
@@ -216,7 +290,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_ring_buffer_implementation
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_ring_buffer_implementation ${PROJECT_NAME})
@@ -226,7 +300,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_intra_process_buffer
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_intra_process_buffer ${PROJECT_NAME})
@@ -244,8 +318,9 @@ if(BUILD_TESTING)
       "rcl_interfaces"
       "rcpputils"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
+      "test_msgs"
     )
     target_link_libraries(test_node ${PROJECT_NAME})
   endif()
@@ -282,7 +357,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_node_global_args
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_node_global_args ${PROJECT_NAME})
@@ -304,7 +379,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_parameter_events_filter
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_parameter_events_filter ${PROJECT_NAME})
@@ -314,7 +389,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_parameter
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_parameter ${PROJECT_NAME})
@@ -326,10 +401,10 @@ if(BUILD_TESTING)
   ament_add_gtest(test_publisher test/test_publisher.cpp)
   if(TARGET test_publisher)
     ament_target_dependencies(test_publisher
-      "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
+      "test_msgs"
     )
     target_link_libraries(test_publisher ${PROJECT_NAME})
   endif()
@@ -338,17 +413,37 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_publisher_subscription_count_api
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "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_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_rate
@@ -364,12 +459,21 @@ if(BUILD_TESTING)
       ${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_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_service ${PROJECT_NAME})
@@ -379,8 +483,9 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_subscription
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
+      "test_msgs"
     )
     target_link_libraries(test_subscription ${PROJECT_NAME})
   endif()
@@ -389,8 +494,9 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_subscription_publisher_count_api
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
+      "test_msgs"
     )
     target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME})
   endif()
@@ -400,6 +506,7 @@ if(BUILD_TESTING)
       "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)
@@ -483,6 +590,14 @@ if(BUILD_TESTING)
     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)
@@ -491,6 +606,43 @@ if(BUILD_TESTING)
     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

rclcpp/include/rclcpp/any_executable.hpp

@@ -29,8 +29,6 @@
 
 namespace rclcpp
 {
-namespace executor
-{
 
 struct AnyExecutable
 {
@@ -47,10 +45,15 @@ struct AnyExecutable
   rclcpp::ClientBase::SharedPtr client;
   rclcpp::Waitable::SharedPtr waitable;
   // These are used to keep the scope on the containing items
-  rclcpp::callback_group::CallbackGroup::SharedPtr callback_group;
+  rclcpp::CallbackGroup::SharedPtr callback_group;
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base;
 };
 
+namespace executor
+{
+
+using AnyExecutable [[deprecated("use rclcpp::AnyExecutable instead")]] = AnyExecutable;
+
 }  // namespace executor
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/any_service_callback.hpp

@@ -24,6 +24,7 @@
 #include "rclcpp/visibility_control.hpp"
 #include "rmw/types.h"
 #include "tracetools/tracetools.h"
+#include "tracetools/utils.hpp"
 
 namespace rclcpp
 {
@@ -98,6 +99,23 @@ public:
     }
     TRACEPOINT(callback_end, (const void *)this);
   }
+
+  void register_callback_for_tracing()
+  {
+#ifndef TRACETOOLS_DISABLED
+    if (shared_ptr_callback_) {
+      TRACEPOINT(
+        rclcpp_callback_register,
+        (const void *)this,
+        get_symbol(shared_ptr_callback_));
+    } else if (shared_ptr_with_request_header_callback_) {
+      TRACEPOINT(
+        rclcpp_callback_register,
+        (const void *)this,
+        get_symbol(shared_ptr_with_request_header_callback_));
+    }
+#endif  // TRACETOOLS_DISABLED
+  }
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -25,6 +25,7 @@
 
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/function_traits.hpp"
+#include "rclcpp/message_info.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "tracetools/tracetools.h"
 #include "tracetools/utils.hpp"
@@ -43,13 +44,13 @@ class AnySubscriptionCallback
 
   using SharedPtrCallback = std::function<void (const std::shared_ptr<MessageT>)>;
   using SharedPtrWithInfoCallback =
-    std::function<void (const std::shared_ptr<MessageT>, const rmw_message_info_t &)>;
+    std::function<void (const std::shared_ptr<MessageT>, const rclcpp::MessageInfo &)>;
   using ConstSharedPtrCallback = std::function<void (const std::shared_ptr<const MessageT>)>;
   using ConstSharedPtrWithInfoCallback =
-    std::function<void (const std::shared_ptr<const MessageT>, const rmw_message_info_t &)>;
+    std::function<void (const std::shared_ptr<const MessageT>, const rclcpp::MessageInfo &)>;
   using UniquePtrCallback = std::function<void (MessageUniquePtr)>;
   using UniquePtrWithInfoCallback =
-    std::function<void (MessageUniquePtr, const rmw_message_info_t &)>;
+    std::function<void (MessageUniquePtr, const rclcpp::MessageInfo &)>;
 
   SharedPtrCallback shared_ptr_callback_;
   SharedPtrWithInfoCallback shared_ptr_with_info_callback_;
@@ -155,7 +156,7 @@ public:
   }
 
   void dispatch(
-    std::shared_ptr<MessageT> message, const rmw_message_info_t & message_info)
+    std::shared_ptr<MessageT> message, const rclcpp::MessageInfo & message_info)
   {
     TRACEPOINT(callback_start, (const void *)this, false);
     if (shared_ptr_callback_) {
@@ -181,7 +182,7 @@ public:
   }
 
   void dispatch_intra_process(
-    ConstMessageSharedPtr message, const rmw_message_info_t & message_info)
+    ConstMessageSharedPtr message, const rclcpp::MessageInfo & message_info)
   {
     TRACEPOINT(callback_start, (const void *)this, true);
     if (const_shared_ptr_callback_) {
@@ -204,7 +205,7 @@ public:
   }
 
   void dispatch_intra_process(
-    MessageUniquePtr message, const rmw_message_info_t & message_info)
+    MessageUniquePtr message, const rclcpp::MessageInfo & message_info)
   {
     TRACEPOINT(callback_start, (const void *)this, true);
     if (shared_ptr_callback_) {
@@ -234,6 +235,7 @@ public:
 
   void register_callback_for_tracing()
   {
+#ifndef TRACETOOLS_DISABLED
     if (shared_ptr_callback_) {
       TRACEPOINT(
         rclcpp_callback_register,
@@ -255,6 +257,7 @@ public:
         (const void *)this,
         get_symbol(unique_ptr_with_info_callback_));
     }
+#endif  // TRACETOOLS_DISABLED
   }
 
 private:

rclcpp/include/rclcpp/callback_group.hpp

@@ -40,9 +40,6 @@ class NodeTopics;
 class NodeWaitables;
 }  // namespace node_interfaces
 
-namespace callback_group
-{
-
 enum class CallbackGroupType
 {
   MutuallyExclusive,
@@ -162,6 +159,12 @@ private:
   }
 };
 
+namespace callback_group
+{
+
+using CallbackGroupType [[deprecated("use rclcpp::CallbackGroupType instead")]] = CallbackGroupType;
+using CallbackGroup [[deprecated("use rclcpp::CallbackGroup instead")]] = CallbackGroup;
+
 }  // namespace callback_group
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/client.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__CLIENT_HPP_
 #define RCLCPP__CLIENT_HPP_
 
+#include <atomic>
 #include <future>
 #include <map>
 #include <memory>
@@ -62,6 +63,27 @@ public:
   RCLCPP_PUBLIC
   virtual ~ClientBase();
 
+  /// Take the next response for this client as a type erased pointer.
+  /**
+   * The type erased pointer allows for this method to be used in a type
+   * agnostic way along with ClientBase::create_response(),
+   * ClientBase::create_request_header(), and ClientBase::handle_response().
+   * The typed version of this can be used if the Service type is known,
+   * \sa Client::take_response().
+   *
+   * \param[out] response_out The type erased pointer to a Service Response into
+   *   which the middleware will copy the response being taken.
+   * \param[out] request_header_out The request header to be filled by the
+   *   middleware when taking, and which can be used to associte the response
+   *   to a specific request.
+   * \returns true if the response was taken, otherwise false.
+   * \throws rclcpp::exceptions::RCLError based exceptions if the underlying
+   *   rcl function fail.
+   */
+  RCLCPP_PUBLIC
+  bool
+  take_type_erased_response(void * response_out, rmw_request_id_t & request_header_out);
+
   RCLCPP_PUBLIC
   const char *
   get_service_name() const;
@@ -93,6 +115,20 @@ public:
   virtual void handle_response(
     std::shared_ptr<rmw_request_id_t> request_header, std::shared_ptr<void> response) = 0;
 
+  /// Exchange the "in use by wait set" state for this client.
+  /**
+   * This is used to ensure this client is not used by multiple
+   * wait sets at the same time.
+   *
+   * \param[in] in_use_state the new state to exchange into the state, true
+   *   indicates it is now in use by a wait set, and false is that it is no
+   *   longer in use by a wait set.
+   * \returns the previous state.
+   */
+  RCLCPP_PUBLIC
+  bool
+  exchange_in_use_by_wait_set_state(bool in_use_state);
+
 protected:
   RCLCPP_DISABLE_COPY(ClientBase)
 
@@ -113,6 +149,8 @@ protected:
   std::shared_ptr<rclcpp::Context> context_;
 
   std::shared_ptr<rcl_client_t> client_handle_;
+
+  std::atomic<bool> in_use_by_wait_set_{false};
 };
 
 template<typename ServiceT>
@@ -171,6 +209,25 @@ public:
   {
   }
 
+  /// Take the next response for this client.
+  /**
+   * \sa ClientBase::take_type_erased_response().
+   *
+   * \param[out] response_out The reference to a Service Response into
+   *   which the middleware will copy the response being taken.
+   * \param[out] request_header_out The request header to be filled by the
+   *   middleware when taking, and which can be used to associte the response
+   *   to a specific request.
+   * \returns true if the response was taken, otherwise false.
+   * \throws rclcpp::exceptions::RCLError based exceptions if the underlying
+   *   rcl function fail.
+   */
+  bool
+  take_response(typename ServiceT::Response & response_out, rmw_request_id_t & request_header_out)
+  {
+    return this->take_type_erased_response(&response_out, request_header_out);
+  }
+
   std::shared_ptr<void>
   create_response() override
   {

rclcpp/include/rclcpp/clock.hpp

@@ -16,6 +16,8 @@
 #define RCLCPP__CLOCK_HPP_
 
 #include <functional>
+#include <memory>
+#include <mutex>
 
 #include "rclcpp/macros.hpp"
 #include "rclcpp/time.hpp"
@@ -95,6 +97,10 @@ public:
   rcl_clock_type_t
   get_clock_type() const noexcept;
 
+  RCLCPP_PUBLIC
+  std::mutex &
+  get_clock_mutex() noexcept;
+
   // Add a callback to invoke if the jump threshold is exceeded.
   /**
    * These callback functions must remain valid as long as the
@@ -132,10 +138,10 @@ private:
     bool before_jump,
     void * user_data);
 
-  /// Internal storage backed by rcl
-  rcl_clock_t rcl_clock_;
-  friend TimeSource;  /// Allow TimeSource to access the rcl_clock_ datatype.
-  rcl_allocator_t allocator_;
+  /// Private internal storage
+  class Impl;
+
+  std::shared_ptr<Impl> impl_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/context.hpp

@@ -159,7 +159,7 @@ public:
    *
    * \param[in] reason the description of why shutdown happened
    * \return true if shutdown was successful, false if context was already shutdown
-   * \throw various exceptions derived from RCLErrorBase, if rcl_shutdown fails
+   * \throw various exceptions derived from rclcpp::exceptions::RCLError, if rcl_shutdown fails
    */
   RCLCPP_PUBLIC
   virtual
@@ -338,6 +338,9 @@ private:
   rclcpp::InitOptions init_options_;
   std::string shutdown_reason_;
 
+  // Keep shared ownership of global logging configure mutex.
+  std::shared_ptr<std::mutex> logging_configure_mutex_;
+
   std::unordered_map<std::type_index, std::shared_ptr<void>> sub_contexts_;
   // This mutex is recursive so that the constructor of a sub context may
   // attempt to acquire another sub context.

rclcpp/include/rclcpp/contexts/default_context.hpp

@@ -22,8 +22,6 @@ namespace rclcpp
 {
 namespace contexts
 {
-namespace default_context
-{
 
 class DefaultContext : public rclcpp::Context
 {
@@ -38,6 +36,21 @@ RCLCPP_PUBLIC
 DefaultContext::SharedPtr
 get_global_default_context();
 
+namespace default_context
+{
+
+using DefaultContext
+[[deprecated("use rclcpp::contexts::DefaultContext instead")]] = DefaultContext;
+
+[[deprecated("use rclcpp::contexts::get_global_default_context() instead")]]
+RCLCPP_PUBLIC
+inline
+DefaultContext::SharedPtr
+get_global_default_context()
+{
+  return rclcpp::contexts::get_global_default_context();
+}
+
 }  // namespace default_context
 }  // namespace contexts
 }  // namespace rclcpp

rclcpp/include/rclcpp/create_client.hpp

@@ -35,7 +35,7 @@ create_client(
   std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
   const std::string & service_name,
   const rmw_qos_profile_t & qos_profile,
-  rclcpp::callback_group::CallbackGroup::SharedPtr group)
+  rclcpp::CallbackGroup::SharedPtr group)
 {
   rcl_client_options_t options = rcl_client_get_default_options();
   options.qos = qos_profile;

rclcpp/include/rclcpp/create_service.hpp

@@ -37,7 +37,7 @@ create_service(
   const std::string & service_name,
   CallbackT && callback,
   const rmw_qos_profile_t & qos_profile,
-  rclcpp::callback_group::CallbackGroup::SharedPtr group)
+  rclcpp::CallbackGroup::SharedPtr group)
 {
   rclcpp::AnyServiceCallback<ServiceT> any_service_callback;
   any_service_callback.set(std::forward<CallbackT>(callback));

rclcpp/include/rclcpp/create_subscription.hpp

@@ -15,15 +15,25 @@
 #ifndef RCLCPP__CREATE_SUBSCRIPTION_HPP_
 #define RCLCPP__CREATE_SUBSCRIPTION_HPP_
 
+#include <chrono>
+#include <functional>
 #include <memory>
 #include <string>
 #include <utility>
 
+#include "rclcpp/detail/resolve_enable_topic_statistics.hpp"
+
+#include "rclcpp/node_interfaces/get_node_timers_interface.hpp"
 #include "rclcpp/node_interfaces/get_node_topics_interface.hpp"
+#include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
+
+#include "rclcpp/create_timer.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/subscription_factory.hpp"
 #include "rclcpp/subscription_options.hpp"
-#include "rclcpp/qos.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp"
 #include "rmw/qos_profiles.h"
 
 namespace rclcpp
@@ -64,10 +74,46 @@ create_subscription(
   using rclcpp::node_interfaces::get_node_topics_interface;
   auto node_topics = get_node_topics_interface(std::forward<NodeT>(node));
 
+  std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>>
+  subscription_topic_stats = nullptr;
+
+  if (rclcpp::detail::resolve_enable_topic_statistics(
+      options,
+      *node_topics->get_node_base_interface()))
+  {
+    std::shared_ptr<Publisher<statistics_msgs::msg::MetricsMessage>> publisher =
+      create_publisher<statistics_msgs::msg::MetricsMessage>(
+      node,
+      options.topic_stats_options.publish_topic,
+      qos);
+
+    subscription_topic_stats = std::make_shared<
+      rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>
+      >(node_topics->get_node_base_interface()->get_name(), publisher);
+
+    auto sub_call_back = [subscription_topic_stats]() {
+        subscription_topic_stats->publish_message();
+      };
+
+    auto node_timer_interface = node_topics->get_node_timers_interface();
+
+    auto timer = create_wall_timer(
+      std::chrono::duration_cast<std::chrono::nanoseconds>(
+        options.topic_stats_options.publish_period),
+      sub_call_back,
+      options.callback_group,
+      node_topics->get_node_base_interface(),
+      node_timer_interface
+    );
+
+    subscription_topic_stats->set_publisher_timer(timer);
+  }
+
   auto factory = rclcpp::create_subscription_factory<MessageT>(
     std::forward<CallbackT>(callback),
     options,
-    msg_mem_strat
+    msg_mem_strat,
+    subscription_topic_stats
   );
 
   auto sub = node_topics->create_subscription(topic_name, factory, qos);

rclcpp/include/rclcpp/create_timer.hpp

@@ -15,6 +15,8 @@
 #ifndef RCLCPP__CREATE_TIMER_HPP_
 #define RCLCPP__CREATE_TIMER_HPP_
 
+#include <chrono>
+#include <exception>
 #include <memory>
 #include <string>
 #include <utility>
@@ -32,12 +34,12 @@ namespace rclcpp
 template<typename CallbackT>
 typename rclcpp::TimerBase::SharedPtr
 create_timer(
-  node_interfaces::NodeBaseInterface * node_base,
-  node_interfaces::NodeTimersInterface * node_timers,
+  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+  std::shared_ptr<node_interfaces::NodeTimersInterface> node_timers,
   rclcpp::Clock::SharedPtr clock,
   rclcpp::Duration period,
   CallbackT && callback,
-  rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr)
+  rclcpp::CallbackGroup::SharedPtr group = nullptr)
 {
   auto timer = rclcpp::GenericTimer<CallbackT>::make_shared(
     clock,
@@ -57,7 +59,7 @@ create_timer(
   rclcpp::Clock::SharedPtr clock,
   rclcpp::Duration period,
   CallbackT && callback,
-  rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr)
+  rclcpp::CallbackGroup::SharedPtr group = nullptr)
 {
   return create_timer(
     rclcpp::node_interfaces::get_node_base_interface(node),
@@ -68,6 +70,46 @@ create_timer(
     group);
 }
 
+/// Convenience method to create a timer with node resources.
+/**
+ *
+ * \tparam DurationRepT
+ * \tparam DurationT
+ * \tparam CallbackT
+ * \param period period to exectute callback
+ * \param callback callback to execute via the timer period
+ * \param group
+ * \param node_base
+ * \param node_timers
+ * \return
+ * \throws std::invalid argument if either node_base or node_timers
+ * are null
+ */
+template<typename DurationRepT, typename DurationT, typename CallbackT>
+typename rclcpp::WallTimer<CallbackT>::SharedPtr
+create_wall_timer(
+  std::chrono::duration<DurationRepT, DurationT> period,
+  CallbackT callback,
+  rclcpp::CallbackGroup::SharedPtr group,
+  node_interfaces::NodeBaseInterface * node_base,
+  node_interfaces::NodeTimersInterface * node_timers)
+{
+  if (node_base == nullptr) {
+    throw std::invalid_argument{"input node_base cannot be null"};
+  }
+
+  if (node_timers == nullptr) {
+    throw std::invalid_argument{"input node_timers cannot be null"};
+  }
+
+  auto timer = rclcpp::WallTimer<CallbackT>::make_shared(
+    std::chrono::duration_cast<std::chrono::nanoseconds>(period),
+    std::move(callback),
+    node_base->get_context());
+  node_timers->add_timer(timer, group);
+  return timer;
+}
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__CREATE_TIMER_HPP_

rclcpp/include/rclcpp/detail/resolve_enable_topic_statistics.hpp

@@ -0,0 +1,53 @@
+// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved.
+//
+// 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__RESOLVE_ENABLE_TOPIC_STATISTICS_HPP_
+#define RCLCPP__DETAIL__RESOLVE_ENABLE_TOPIC_STATISTICS_HPP_
+
+#include <stdexcept>
+
+#include "rclcpp/topic_statistics_state.hpp"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+/// Return whether or not topic statistics is enabled, resolving "NodeDefault" if needed.
+template<typename OptionsT, typename NodeBaseT>
+bool
+resolve_enable_topic_statistics(const OptionsT & options, const NodeBaseT & node_base)
+{
+  bool topic_stats_enabled;
+  switch (options.topic_stats_options.state) {
+    case TopicStatisticsState::Enable:
+      topic_stats_enabled = true;
+      break;
+    case TopicStatisticsState::Disable:
+      topic_stats_enabled = false;
+      break;
+    case TopicStatisticsState::NodeDefault:
+      topic_stats_enabled = node_base.get_enable_topic_statistics_default();
+      break;
+    default:
+      throw std::runtime_error("Unrecognized EnableTopicStatistics value");
+  }
+
+  return topic_stats_enabled;
+}
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__RESOLVE_ENABLE_TOPIC_STATISTICS_HPP_

rclcpp/include/rclcpp/detail/utilities.hpp

@@ -0,0 +1,40 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__DETAIL__UTILITIES_HPP_
+#define RCLCPP__DETAIL__UTILITIES_HPP_
+
+#include "rclcpp/detail/utilities.hpp"
+
+#include <string>
+#include <vector>
+
+#include "rcl/allocator.h"
+#include "rcl/arguments.h"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+std::vector<std::string>
+get_unparsed_ros_arguments(
+  int argc, char const * const argv[],
+  rcl_arguments_t * arguments,
+  rcl_allocator_t allocator);
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__UTILITIES_HPP_

rclcpp/include/rclcpp/exceptions.hpp

@@ -15,246 +15,6 @@
 #ifndef RCLCPP__EXCEPTIONS_HPP_
 #define RCLCPP__EXCEPTIONS_HPP_
 
-#include <stdexcept>
-#include <string>
-#include <vector>
-
-#include "rcl/error_handling.h"
-#include "rcl/types.h"
-#include "rclcpp/visibility_control.hpp"
-
-#include "rcpputils/join.hpp"
-
-namespace rclcpp
-{
-namespace exceptions
-{
-
-/// Thrown when a method is trying to use a node, but it is invalid.
-class InvalidNodeError : public std::runtime_error
-{
-public:
-  InvalidNodeError()
-  : std::runtime_error("node is invalid") {}
-};
-
-/// Thrown when a any kind of name (node, namespace, topic, etc.) is invalid.
-class NameValidationError : public std::invalid_argument
-{
-public:
-  NameValidationError(
-    const char * name_type_,
-    const char * name_,
-    const char * error_msg_,
-    size_t invalid_index_)
-  : std::invalid_argument(format_error(name_type_, name_, error_msg_, invalid_index_)),
-    name_type(name_type_), name(name_), error_msg(error_msg_), invalid_index(invalid_index_)
-  {}
-
-  static std::string
-  format_error(
-    const char * name_type,
-    const char * name,
-    const char * error_msg,
-    size_t invalid_index);
-
-  const std::string name_type;
-  const std::string name;
-  const std::string error_msg;
-  const size_t invalid_index;
-};
-
-/// Thrown when a node name is invalid.
-class InvalidNodeNameError : public NameValidationError
-{
-public:
-  InvalidNodeNameError(const char * node_name, const char * error_msg, size_t invalid_index)
-  : NameValidationError("node name", node_name, error_msg, invalid_index)
-  {}
-};
-
-/// Thrown when a node namespace is invalid.
-class InvalidNamespaceError : public NameValidationError
-{
-public:
-  InvalidNamespaceError(const char * namespace_, const char * error_msg, size_t invalid_index)
-  : NameValidationError("namespace", namespace_, error_msg, invalid_index)
-  {}
-};
-
-/// Thrown when a topic name is invalid.
-class InvalidTopicNameError : public NameValidationError
-{
-public:
-  InvalidTopicNameError(const char * namespace_, const char * error_msg, size_t invalid_index)
-  : NameValidationError("topic name", namespace_, error_msg, invalid_index)
-  {}
-};
-
-/// Thrown when a service name is invalid.
-class InvalidServiceNameError : public NameValidationError
-{
-public:
-  InvalidServiceNameError(const char * namespace_, const char * error_msg, size_t invalid_index)
-  : NameValidationError("service name", namespace_, error_msg, invalid_index)
-  {}
-};
-
-/// 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
- * to reset the error.
- *
- * \param ret the return code for the current error state
- * \param prefix string to prefix to the error if applicable (not all errors have custom messages)
- * \param error_state error state to create exception from, if nullptr rcl_get_error_state is used
- * \param reset_error function to be called before throwing which whill clear the error state
- * \throws std::invalid_argument if ret is RCL_RET_OK
- * \throws std::runtime_error if the rcl_get_error_state returns 0
- * \throws RCLErrorBase some child class exception based on ret
- */
-/* *INDENT-OFF* */  // Uncrustify cannot yet understand [[noreturn]] properly
-RCLCPP_PUBLIC
-void
-throw_from_rcl_error [[noreturn]] (
-  rcl_ret_t ret,
-  const std::string & prefix = "",
-  const rcl_error_state_t * error_state = nullptr,
-  void (* reset_error)() = rcl_reset_error);
-/* *INDENT-ON* */
-
-class RCLErrorBase
-{
-public:
-  RCLCPP_PUBLIC
-  RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state);
-  virtual ~RCLErrorBase() {}
-
-  rcl_ret_t ret;
-  std::string message;
-  std::string file;
-  size_t line;
-  std::string formatted_message;
-};
-
-/// Created when the return code does not match one of the other specialized exceptions.
-class RCLError : public RCLErrorBase, public std::runtime_error
-{
-public:
-  RCLCPP_PUBLIC
-  RCLError(rcl_ret_t ret, const rcl_error_state_t * error_state, const std::string & prefix);
-  RCLCPP_PUBLIC
-  RCLError(const RCLErrorBase & base_exc, const std::string & prefix);
-};
-
-/// Created when the ret is RCL_RET_BAD_ALLOC.
-class RCLBadAlloc : public RCLErrorBase, public std::bad_alloc
-{
-public:
-  RCLCPP_PUBLIC
-  RCLBadAlloc(rcl_ret_t ret, const rcl_error_state_t * error_state);
-  RCLCPP_PUBLIC
-  explicit RCLBadAlloc(const RCLErrorBase & base_exc);
-};
-
-/// Created when the ret is RCL_RET_INVALID_ARGUMENT.
-class RCLInvalidArgument : public RCLErrorBase, public std::invalid_argument
-{
-public:
-  RCLCPP_PUBLIC
-  RCLInvalidArgument(
-    rcl_ret_t ret,
-    const rcl_error_state_t * error_state,
-    const std::string & prefix);
-  RCLCPP_PUBLIC
-  RCLInvalidArgument(const RCLErrorBase & base_exc, const std::string & prefix);
-};
-
-/// Created when the ret is RCL_RET_INVALID_ROS_ARGS.
-class RCLInvalidROSArgsError : public RCLErrorBase, public std::runtime_error
-{
-public:
-  RCLCPP_PUBLIC
-  RCLInvalidROSArgsError(
-    rcl_ret_t ret, const rcl_error_state_t * error_state, const std::string & prefix);
-  RCLCPP_PUBLIC
-  RCLInvalidROSArgsError(const RCLErrorBase & base_exc, const std::string & prefix);
-};
-
-/// Thrown when unparsed ROS specific arguments are found.
-class UnknownROSArgsError : public std::runtime_error
-{
-public:
-  explicit UnknownROSArgsError(std::vector<std::string> && unknown_ros_args_in)
-  : std::runtime_error(
-      "found unknown ROS arguments: '" + rcpputils::join(unknown_ros_args_in, "', '") + "'"),
-    unknown_ros_args(unknown_ros_args_in)
-  {
-  }
-
-  const std::vector<std::string> unknown_ros_args;
-};
-
-/// Thrown when an invalid rclcpp::Event object or SharedPtr is encountered.
-class InvalidEventError : public std::runtime_error
-{
-public:
-  InvalidEventError()
-  : std::runtime_error("event is invalid") {}
-};
-
-/// Thrown when an unregistered rclcpp::Event is encountered where a registered one was expected.
-class EventNotRegisteredError : public std::runtime_error
-{
-public:
-  EventNotRegisteredError()
-  : std::runtime_error("event already registered") {}
-};
-
-/// Thrown if passed parameters are inconsistent or invalid
-class InvalidParametersException : public std::runtime_error
-{
-public:
-  // Inherit constructors from runtime_error.
-  using std::runtime_error::runtime_error;
-};
-
-/// Thrown if passed parameter value is invalid.
-class InvalidParameterValueException : public std::runtime_error
-{
-  // Inherit constructors from runtime_error.
-  using std::runtime_error::runtime_error;
-};
-
-/// Thrown if parameter is already declared.
-class ParameterAlreadyDeclaredException : public std::runtime_error
-{
-  // Inherit constructors from runtime_error.
-  using std::runtime_error::runtime_error;
-};
-
-/// Thrown if parameter is not declared, e.g. either set or get was called without first declaring.
-class ParameterNotDeclaredException : public std::runtime_error
-{
-  // Inherit constructors from runtime_error.
-  using std::runtime_error::runtime_error;
-};
-
-/// Thrown if parameter is immutable and therefore cannot be undeclared.
-class ParameterImmutableException : public std::runtime_error
-{
-  // Inherit constructors from runtime_error.
-  using std::runtime_error::runtime_error;
-};
-
-/// Thrown if parameter is modified while in a set callback.
-class ParameterModifiedInCallbackException : public std::runtime_error
-{
-  // Inherit constructors from runtime_error.
-  using std::runtime_error::runtime_error;
-};
-
-}  // namespace exceptions
-}  // namespace rclcpp
+#include "rclcpp/exceptions/exceptions.hpp"
 
 #endif  // RCLCPP__EXCEPTIONS_HPP_

rclcpp/include/rclcpp/exceptions/exceptions.hpp

@@ -0,0 +1,279 @@
+// Copyright 2016 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__EXCEPTIONS__EXCEPTIONS_HPP_
+#define RCLCPP__EXCEPTIONS__EXCEPTIONS_HPP_
+
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include "rcl/error_handling.h"
+#include "rcl/types.h"
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcpputils/join.hpp"
+
+namespace rclcpp
+{
+namespace exceptions
+{
+
+/// Thrown when a method is trying to use a node, but it is invalid.
+class InvalidNodeError : public std::runtime_error
+{
+public:
+  InvalidNodeError()
+  : std::runtime_error("node is invalid") {}
+};
+
+/// Thrown when a any kind of name (node, namespace, topic, etc.) is invalid.
+class NameValidationError : public std::invalid_argument
+{
+public:
+  NameValidationError(
+    const char * name_type_,
+    const char * name_,
+    const char * error_msg_,
+    size_t invalid_index_)
+  : std::invalid_argument(format_error(name_type_, name_, error_msg_, invalid_index_)),
+    name_type(name_type_), name(name_), error_msg(error_msg_), invalid_index(invalid_index_)
+  {}
+
+  static std::string
+  format_error(
+    const char * name_type,
+    const char * name,
+    const char * error_msg,
+    size_t invalid_index);
+
+  const std::string name_type;
+  const std::string name;
+  const std::string error_msg;
+  const size_t invalid_index;
+};
+
+/// Thrown when a node name is invalid.
+class InvalidNodeNameError : public NameValidationError
+{
+public:
+  InvalidNodeNameError(const char * node_name, const char * error_msg, size_t invalid_index)
+  : NameValidationError("node name", node_name, error_msg, invalid_index)
+  {}
+};
+
+/// Thrown when a node namespace is invalid.
+class InvalidNamespaceError : public NameValidationError
+{
+public:
+  InvalidNamespaceError(const char * namespace_, const char * error_msg, size_t invalid_index)
+  : NameValidationError("namespace", namespace_, error_msg, invalid_index)
+  {}
+};
+
+/// Thrown when a topic name is invalid.
+class InvalidTopicNameError : public NameValidationError
+{
+public:
+  InvalidTopicNameError(const char * namespace_, const char * error_msg, size_t invalid_index)
+  : NameValidationError("topic name", namespace_, error_msg, invalid_index)
+  {}
+};
+
+/// Thrown when a service name is invalid.
+class InvalidServiceNameError : public NameValidationError
+{
+public:
+  InvalidServiceNameError(const char * namespace_, const char * error_msg, size_t invalid_index)
+  : NameValidationError("service name", namespace_, error_msg, invalid_index)
+  {}
+};
+
+/// 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
+ * to reset the error.
+ *
+ * \param ret the return code for the current error state
+ * \param prefix string to prefix to the error if applicable (not all errors have custom messages)
+ * \param error_state error state to create exception from, if nullptr rcl_get_error_state is used
+ * \param reset_error function to be called before throwing which whill clear the error state
+ * \throws std::invalid_argument if ret is RCL_RET_OK
+ * \throws std::runtime_error if the rcl_get_error_state returns 0
+ * \throws RCLErrorBase some child class exception based on ret
+ */
+/* *INDENT-OFF* */  // Uncrustify cannot yet understand [[noreturn]] properly
+RCLCPP_PUBLIC
+void
+throw_from_rcl_error [[noreturn]] (
+  rcl_ret_t ret,
+  const std::string & prefix = "",
+  const rcl_error_state_t * error_state = nullptr,
+  void (* reset_error)() = rcl_reset_error);
+/* *INDENT-ON* */
+
+class RCLErrorBase
+{
+public:
+  RCLCPP_PUBLIC
+  RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state);
+  virtual ~RCLErrorBase() {}
+
+  rcl_ret_t ret;
+  std::string message;
+  std::string file;
+  size_t line;
+  std::string formatted_message;
+};
+
+/// Created when the return code does not match one of the other specialized exceptions.
+class RCLError : public RCLErrorBase, public std::runtime_error
+{
+public:
+  RCLCPP_PUBLIC
+  RCLError(rcl_ret_t ret, const rcl_error_state_t * error_state, const std::string & prefix);
+  RCLCPP_PUBLIC
+  RCLError(const RCLErrorBase & base_exc, const std::string & prefix);
+};
+
+/// Created when the ret is RCL_RET_BAD_ALLOC.
+class RCLBadAlloc : public RCLErrorBase, public std::bad_alloc
+{
+public:
+  RCLCPP_PUBLIC
+  RCLBadAlloc(rcl_ret_t ret, const rcl_error_state_t * error_state);
+  RCLCPP_PUBLIC
+  explicit RCLBadAlloc(const RCLErrorBase & base_exc);
+};
+
+/// Created when the ret is RCL_RET_INVALID_ARGUMENT.
+class RCLInvalidArgument : public RCLErrorBase, public std::invalid_argument
+{
+public:
+  RCLCPP_PUBLIC
+  RCLInvalidArgument(
+    rcl_ret_t ret,
+    const rcl_error_state_t * error_state,
+    const std::string & prefix);
+  RCLCPP_PUBLIC
+  RCLInvalidArgument(const RCLErrorBase & base_exc, const std::string & prefix);
+};
+
+/// Created when the ret is RCL_RET_INVALID_ROS_ARGS.
+class RCLInvalidROSArgsError : public RCLErrorBase, public std::runtime_error
+{
+public:
+  RCLCPP_PUBLIC
+  RCLInvalidROSArgsError(
+    rcl_ret_t ret, const rcl_error_state_t * error_state, const std::string & prefix);
+  RCLCPP_PUBLIC
+  RCLInvalidROSArgsError(const RCLErrorBase & base_exc, const std::string & prefix);
+};
+
+/// Thrown when unparsed ROS specific arguments are found.
+class UnknownROSArgsError : public std::runtime_error
+{
+public:
+  explicit UnknownROSArgsError(std::vector<std::string> && unknown_ros_args_in)
+  : std::runtime_error(
+      "found unknown ROS arguments: '" + rcpputils::join(unknown_ros_args_in, "', '") + "'"),
+    unknown_ros_args(unknown_ros_args_in)
+  {
+  }
+
+  const std::vector<std::string> unknown_ros_args;
+};
+
+/// Thrown when an invalid rclcpp::Event object or SharedPtr is encountered.
+class InvalidEventError : public std::runtime_error
+{
+public:
+  InvalidEventError()
+  : std::runtime_error("event is invalid") {}
+};
+
+/// Thrown when an unregistered rclcpp::Event is encountered where a registered one was expected.
+class EventNotRegisteredError : public std::runtime_error
+{
+public:
+  EventNotRegisteredError()
+  : std::runtime_error("event already registered") {}
+};
+
+/// Thrown if passed parameters are inconsistent or invalid
+class InvalidParametersException : public std::runtime_error
+{
+public:
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
+/// Thrown if passed parameter value is invalid.
+class InvalidParameterValueException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
+/// Thrown if requested parameter type is invalid.
+/**
+ * Essentially the same as rclcpp::ParameterTypeException, but with parameter
+ * name in the error message.
+ */
+class InvalidParameterTypeException : public std::runtime_error
+{
+public:
+  /// Construct an instance.
+  /**
+   * \param[in] name the name of the parameter.
+   * \param[in] message custom exception message.
+   */
+  RCLCPP_PUBLIC
+  InvalidParameterTypeException(const std::string & name, const std::string message)
+  : std::runtime_error("parameter '" + name + "' has invalid type: " + message)
+  {}
+};
+
+/// Thrown if parameter is already declared.
+class ParameterAlreadyDeclaredException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
+/// Thrown if parameter is not declared, e.g. either set or get was called without first declaring.
+class ParameterNotDeclaredException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
+/// Thrown if parameter is immutable and therefore cannot be undeclared.
+class ParameterImmutableException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
+/// Thrown if parameter is modified while in a set callback.
+class ParameterModifiedInCallbackException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
+}  // namespace exceptions
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXCEPTIONS__EXCEPTIONS_HPP_

rclcpp/include/rclcpp/executor.hpp

@@ -30,6 +30,8 @@
 #include "rcl/wait.h"
 
 #include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/executor_options.hpp"
+#include "rclcpp/future_return_code.hpp"
 #include "rclcpp/memory_strategies.hpp"
 #include "rclcpp/memory_strategy.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
@@ -42,48 +44,6 @@ namespace rclcpp
 // Forward declaration is used in convenience method signature.
 class Node;
 
-namespace executor
-{
-
-/// Return codes to be used with spin_until_future_complete.
-/**
- * SUCCESS: The future is complete and can be accessed with "get" without blocking.
- *          This does not indicate that the operation succeeded; "get" may still throw an exception.
- * INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error.
- * TIMEOUT: Spinning timed out.
- */
-enum class FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT};
-
-RCLCPP_PUBLIC
-std::ostream &
-operator<<(std::ostream & os, const FutureReturnCode & future_return_code);
-
-RCLCPP_PUBLIC
-std::string
-to_string(const FutureReturnCode & future_return_code);
-
-///
-/**
- * Options to be passed to the executor constructor.
- */
-struct ExecutorArgs
-{
-  ExecutorArgs()
-  : memory_strategy(memory_strategies::create_default_strategy()),
-    context(rclcpp::contexts::default_context::get_global_default_context()),
-    max_conditions(0)
-  {}
-
-  memory_strategy::MemoryStrategy::SharedPtr memory_strategy;
-  std::shared_ptr<rclcpp::Context> context;
-  size_t max_conditions;
-};
-
-static inline ExecutorArgs create_default_executor_arguments()
-{
-  return ExecutorArgs();
-}
-
 /// Coordinate the order and timing of available communication tasks.
 /**
  * Executor provides spin functions (including spin_node_once and spin_some).
@@ -100,9 +60,11 @@ public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Executor)
 
   /// Default constructor.
-  // \param[in] ms The memory strategy to be used with this executor.
+  /**
+   * \param[in] options Options used to configure the executor.
+   */
   RCLCPP_PUBLIC
-  explicit Executor(const ExecutorArgs & args = ExecutorArgs());
+  explicit Executor(const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
 
   /// Default destructor.
   RCLCPP_PUBLIC
@@ -223,7 +185,7 @@ public:
   template<typename ResponseT, typename TimeRepT = int64_t, typename TimeT = std::milli>
   FutureReturnCode
   spin_until_future_complete(
-    std::shared_future<ResponseT> & future,
+    const std::shared_future<ResponseT> & future,
     std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
   {
     // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
@@ -323,10 +285,10 @@ protected:
 
   RCLCPP_PUBLIC
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
-  get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group);
+  get_node_by_group(rclcpp::CallbackGroup::SharedPtr group);
 
   RCLCPP_PUBLIC
-  rclcpp::callback_group::CallbackGroup::SharedPtr
+  rclcpp::CallbackGroup::SharedPtr
   get_group_by_timer(rclcpp::TimerBase::SharedPtr timer);
 
   RCLCPP_PUBLIC
@@ -357,13 +319,17 @@ protected:
   /// The context associated with this executor.
   std::shared_ptr<rclcpp::Context> context_;
 
-private:
   RCLCPP_DISABLE_COPY(Executor)
 
   std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
   std::list<const rcl_guard_condition_t *> guard_conditions_;
 };
 
+namespace executor
+{
+
+using Executor [[deprecated("use rclcpp::Executor instead")]] = rclcpp::Executor;
+
 }  // namespace executor
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/executor_options.hpp

@@ -0,0 +1,57 @@
+// Copyright 2014-2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__EXECUTOR_OPTIONS_HPP_
+#define RCLCPP__EXECUTOR_OPTIONS_HPP_
+
+#include "rclcpp/context.hpp"
+#include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/memory_strategies.hpp"
+#include "rclcpp/memory_strategy.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Options to be passed to the executor constructor.
+struct ExecutorOptions
+{
+  ExecutorOptions()
+  : memory_strategy(rclcpp::memory_strategies::create_default_strategy()),
+    context(rclcpp::contexts::get_global_default_context()),
+    max_conditions(0)
+  {}
+
+  rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy;
+  rclcpp::Context::SharedPtr context;
+  size_t max_conditions;
+};
+
+namespace executor
+{
+
+using ExecutorArgs [[deprecated("use rclcpp::ExecutorOptions instead")]] = ExecutorOptions;
+
+[[deprecated("use rclcpp::ExecutorOptions() instead")]]
+inline
+rclcpp::ExecutorOptions
+create_default_executor_arguments()
+{
+  return rclcpp::ExecutorOptions();
+}
+
+}  // namespace executor
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXECUTOR_OPTIONS_HPP_

rclcpp/include/rclcpp/executors.hpp

@@ -20,6 +20,7 @@
 
 #include "rclcpp/executors/multi_threaded_executor.hpp"
 #include "rclcpp/executors/single_threaded_executor.hpp"
+#include "rclcpp/executors/static_single_threaded_executor.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -66,11 +67,11 @@ using rclcpp::executors::SingleThreadedExecutor;
  * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
  */
 template<typename ResponseT, typename TimeRepT = int64_t, typename TimeT = std::milli>
-rclcpp::executor::FutureReturnCode
+rclcpp::FutureReturnCode
 spin_node_until_future_complete(
-  rclcpp::executor::Executor & executor,
+  rclcpp::Executor & executor,
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  std::shared_future<ResponseT> & future,
+  const std::shared_future<ResponseT> & future,
   std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
@@ -83,11 +84,11 @@ spin_node_until_future_complete(
 
 template<typename NodeT = rclcpp::Node, typename ResponseT, typename TimeRepT = int64_t,
   typename TimeT = std::milli>
-rclcpp::executor::FutureReturnCode
+rclcpp::FutureReturnCode
 spin_node_until_future_complete(
-  rclcpp::executor::Executor & executor,
+  rclcpp::Executor & executor,
   std::shared_ptr<NodeT> node_ptr,
-  std::shared_future<ResponseT> & future,
+  const std::shared_future<ResponseT> & future,
   std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   return rclcpp::executors::spin_node_until_future_complete(
@@ -100,10 +101,10 @@ spin_node_until_future_complete(
 }  // namespace executors
 
 template<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::milli>
-rclcpp::executor::FutureReturnCode
+rclcpp::FutureReturnCode
 spin_until_future_complete(
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  std::shared_future<FutureT> & future,
+  const std::shared_future<FutureT> & future,
   std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   rclcpp::executors::SingleThreadedExecutor executor;
@@ -112,10 +113,10 @@ spin_until_future_complete(
 
 template<typename NodeT = rclcpp::Node, typename FutureT, typename TimeRepT = int64_t,
   typename TimeT = std::milli>
-rclcpp::executor::FutureReturnCode
+rclcpp::FutureReturnCode
 spin_until_future_complete(
   std::shared_ptr<NodeT> node_ptr,
-  std::shared_future<FutureT> & future,
+  const std::shared_future<FutureT> & future,
   std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout);

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -32,7 +32,7 @@ namespace rclcpp
 namespace executors
 {
 
-class MultiThreadedExecutor : public executor::Executor
+class MultiThreadedExecutor : public rclcpp::Executor
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(MultiThreadedExecutor)
@@ -45,14 +45,14 @@ public:
    * This is useful for reproducing some bugs related to taking work more than
    * once.
    *
-   * \param args common arguments for all executors
+   * \param options common options for all executors
    * \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
    */
   RCLCPP_PUBLIC
   MultiThreadedExecutor(
-    const executor::ExecutorArgs & args = executor::ExecutorArgs(),
+    const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions(),
     size_t number_of_threads = 0,
     bool yield_before_execute = false,
     std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
@@ -62,7 +62,7 @@ public:
 
   RCLCPP_PUBLIC
   void
-  spin();
+  spin() override;
 
   RCLCPP_PUBLIC
   size_t

rclcpp/include/rclcpp/executors/single_threaded_executor.hpp

@@ -35,28 +35,34 @@ namespace rclcpp
 namespace executors
 {
 
-/// Single-threaded executor implementation
-// This is the default executor created by rclcpp::spin.
-class SingleThreadedExecutor : public executor::Executor
+/// Single-threaded executor implementation.
+/**
+ * This is the default executor created by rclcpp::spin.
+ */
+class SingleThreadedExecutor : public rclcpp::Executor
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(SingleThreadedExecutor)
 
   /// Default constructor. See the default constructor for Executor.
   RCLCPP_PUBLIC
-  SingleThreadedExecutor(
-    const executor::ExecutorArgs & args = executor::ExecutorArgs());
+  explicit SingleThreadedExecutor(
+    const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
 
-  /// Default destrcutor.
+  /// Default destructor.
   RCLCPP_PUBLIC
   virtual ~SingleThreadedExecutor();
 
   /// Single-threaded implementation of spin.
-  // This function will block until work comes in, execute it, and keep blocking.
-  // It will only be interrupt by a CTRL-C (managed by the global signal handler).
+  /**
+   * This function will block until work comes in, execute it, and then repeat
+   * the process until canceled.
+   * It may be interrupt by a call to rclcpp::Executor::cancel() or by ctrl-c
+   * if the associated context is configured to shutdown on SIGINT.
+   */
   RCLCPP_PUBLIC
   void
-  spin();
+  spin() override;
 
 private:
   RCLCPP_DISABLE_COPY(SingleThreadedExecutor)

rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp

@@ -0,0 +1,168 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_
+#define RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_
+
+#include <chrono>
+#include <list>
+#include <memory>
+
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
+
+#include "rclcpp/experimental/executable_list.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/memory_strategy.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace executors
+{
+
+class StaticExecutorEntitiesCollector final
+  : public rclcpp::Waitable,
+  public std::enable_shared_from_this<StaticExecutorEntitiesCollector>
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(StaticExecutorEntitiesCollector)
+
+  // Constructor
+  RCLCPP_PUBLIC
+  StaticExecutorEntitiesCollector() = default;
+
+  // Destructor
+  ~StaticExecutorEntitiesCollector();
+
+  RCLCPP_PUBLIC
+  void
+  init(
+    rcl_wait_set_t * p_wait_set,
+    rclcpp::memory_strategy::MemoryStrategy::SharedPtr & memory_strategy,
+    rcl_guard_condition_t * executor_guard_condition);
+
+  RCLCPP_PUBLIC
+  void
+  execute() override;
+
+  RCLCPP_PUBLIC
+  void
+  fill_memory_strategy();
+
+  RCLCPP_PUBLIC
+  void
+  fill_executable_list();
+
+  /// Function to reallocate space for entities in the wait set.
+  RCLCPP_PUBLIC
+  void
+  prepare_wait_set();
+
+  /// Function to add_handles_to_wait_set and wait for work and
+  // block until the wait set is ready or until the timeout has been exceeded.
+  RCLCPP_PUBLIC
+  void
+  refresh_wait_set(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+
+  RCLCPP_PUBLIC
+  bool
+  add_to_wait_set(rcl_wait_set_t * wait_set) override;
+
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_ready_guard_conditions() override;
+
+  RCLCPP_PUBLIC
+  void
+  add_node(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+  RCLCPP_PUBLIC
+  bool
+  remove_node(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+  /// Complete all available queued work without blocking.
+  /**
+   * This function checks if after the guard condition was triggered
+   * (or a spurious wakeup happened) we are really ready to execute
+   * i.e. re-collect entities
+   */
+  RCLCPP_PUBLIC
+  bool
+  is_ready(rcl_wait_set_t * wait_set) override;
+
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_timers() {return exec_list_.number_of_timers;}
+
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_subscriptions() {return exec_list_.number_of_subscriptions;}
+
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_services() {return exec_list_.number_of_services;}
+
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_clients() {return exec_list_.number_of_clients;}
+
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_waitables() {return exec_list_.number_of_waitables;}
+
+  RCLCPP_PUBLIC
+  rclcpp::SubscriptionBase::SharedPtr
+  get_subscription(size_t i) {return exec_list_.subscription[i];}
+
+  RCLCPP_PUBLIC
+  rclcpp::TimerBase::SharedPtr
+  get_timer(size_t i) {return exec_list_.timer[i];}
+
+  RCLCPP_PUBLIC
+  rclcpp::ServiceBase::SharedPtr
+  get_service(size_t i) {return exec_list_.service[i];}
+
+  RCLCPP_PUBLIC
+  rclcpp::ClientBase::SharedPtr
+  get_client(size_t i) {return exec_list_.client[i];}
+
+  RCLCPP_PUBLIC
+  rclcpp::Waitable::SharedPtr
+  get_waitable(size_t i) {return exec_list_.waitable[i];}
+
+private:
+  /// Nodes guard conditions which trigger this waitable
+  std::list<const rcl_guard_condition_t *> guard_conditions_;
+
+  /// Memory strategy: an interface for handling user-defined memory allocation strategies.
+  rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;
+
+  /// List of weak nodes registered in the static executor
+  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
+
+  /// Wait set for managing entities that the rmw layer waits on.
+  rcl_wait_set_t * p_wait_set_ = nullptr;
+
+  /// Executable list: timers, subscribers, clients, services and waitables
+  rclcpp::experimental::ExecutableList exec_list_;
+};
+
+}  // namespace executors
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_

rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp

@@ -0,0 +1,200 @@
+// Copyright 2019 Nobleo Technology
+//
+// 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__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_
+#define RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_
+
+#include <cassert>
+#include <cstdlib>
+#include <memory>
+#include <vector>
+#include <string>
+
+#include "rmw/rmw.h"
+
+#include "rclcpp/executor.hpp"
+#include "rclcpp/executors/static_executor_entities_collector.hpp"
+#include "rclcpp/experimental/executable_list.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/memory_strategies.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/rate.hpp"
+#include "rclcpp/utilities.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace executors
+{
+
+/// Static executor implementation
+/**
+ * This executor is a static version of the original single threaded executor.
+ * It's static because it doesn't reconstruct the executable list for every iteration.
+ * All nodes, callbackgroups, timers, subscriptions etc. are created before
+ * spin() is called, and modified only when an entity is added/removed to/from a node.
+ *
+ * To run this executor instead of SingleThreadedExecutor replace:
+ * rclcpp::executors::SingleThreadedExecutor exec;
+ * by
+ * rclcpp::executors::StaticSingleThreadedExecutor exec;
+ * in your source code and spin node(s) in the following way:
+ * exec.add_node(node);
+ * exec.spin();
+ * exec.remove_node(node);
+ */
+class StaticSingleThreadedExecutor : public rclcpp::Executor
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(StaticSingleThreadedExecutor)
+
+  /// Default constructor. See the default constructor for Executor.
+  RCLCPP_PUBLIC
+  explicit StaticSingleThreadedExecutor(
+    const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
+
+  /// Default destrcutor.
+  RCLCPP_PUBLIC
+  virtual ~StaticSingleThreadedExecutor();
+
+  /// Static executor implementation of spin.
+  // This function will block until work comes in, execute it, and keep blocking.
+  // It will only be interrupt by a CTRL-C (managed by the global signal handler).
+  RCLCPP_PUBLIC
+  void
+  spin() override;
+
+  /// Add a node to the executor.
+  /**
+   * An executor can have zero or more nodes which provide work during `spin` functions.
+   * \param[in] node_ptr Shared pointer to the node to be added.
+   * \param[in] notify True to trigger the interrupt guard condition during this function. If
+   * the executor is blocked at the rmw layer while waiting for work and it is notified that a new
+   * node was added, it will wake up.
+   */
+  RCLCPP_PUBLIC
+  void
+  add_node(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    bool notify = true) override;
+
+  /// Convenience function which takes Node and forwards NodeBaseInterface.
+  RCLCPP_PUBLIC
+  void
+  add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
+
+  /// Remove a node from the executor.
+  /**
+   * \param[in] node_ptr Shared pointer to the node to remove.
+   * \param[in] notify True to trigger the interrupt guard condition and wake up the executor.
+   * This is useful if the last node was removed from the executor while the executor was blocked
+   * waiting for work in another thread, because otherwise the executor would never be notified.
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_node(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    bool notify = true) override;
+
+  /// Convenience function which takes Node and forwards NodeBaseInterface.
+  RCLCPP_PUBLIC
+  void
+  remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
+
+  /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
+  /**
+   * \param[in] future The future to wait on. If this function returns SUCCESS, the future can be
+   *   accessed without blocking (though it may still throw an exception).
+   * \param[in] timeout Optional timeout parameter, which gets passed to
+   *    Executor::execute_ready_executables.
+   *   `-1` is block forever, `0` is non-blocking.
+   *   If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return
+   *   code.
+   * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
+   *
+   *  Example usage:
+   *  rclcpp::executors::StaticSingleThreadedExecutor exec;
+   *  // ... other part of code like creating node
+   *  // define future
+   *  exec.add_node(node);
+   *  exec.spin_until_future_complete(future);
+   */
+  template<typename ResponseT, typename TimeRepT = int64_t, typename TimeT = std::milli>
+  rclcpp::FutureReturnCode
+  spin_until_future_complete(
+    std::shared_future<ResponseT> & future,
+    std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
+  {
+    std::future_status status = future.wait_for(std::chrono::seconds(0));
+    if (status == std::future_status::ready) {
+      return rclcpp::FutureReturnCode::SUCCESS;
+    }
+
+    auto end_time = std::chrono::steady_clock::now();
+    std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+      timeout);
+    if (timeout_ns > std::chrono::nanoseconds::zero()) {
+      end_time += timeout_ns;
+    }
+    std::chrono::nanoseconds timeout_left = timeout_ns;
+
+    entities_collector_ = std::make_shared<StaticExecutorEntitiesCollector>();
+    entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_);
+
+    while (rclcpp::ok(this->context_)) {
+      // Do one set of work.
+      entities_collector_->refresh_wait_set(timeout_left);
+      execute_ready_executables();
+      // 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) {
+        return rclcpp::FutureReturnCode::SUCCESS;
+      }
+      // If the original timeout is < 0, then this is blocking, never TIMEOUT.
+      if (timeout_ns < std::chrono::nanoseconds::zero()) {
+        continue;
+      }
+      // Otherwise check if we still have time to wait, return TIMEOUT if not.
+      auto now = std::chrono::steady_clock::now();
+      if (now >= end_time) {
+        return rclcpp::FutureReturnCode::TIMEOUT;
+      }
+      // Subtract the elapsed time from the original timeout.
+      timeout_left = std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - now);
+    }
+
+    // The future did not complete before ok() returned false, return INTERRUPTED.
+    return rclcpp::FutureReturnCode::INTERRUPTED;
+  }
+
+protected:
+  /// Check which executables in ExecutableList struct are ready from wait_set and execute them.
+  /**
+   * \param[in] exec_list Structure that can hold subscriptionbases, timerbases, etc
+   * \param[in] timeout Optional timeout parameter.
+   */
+  RCLCPP_PUBLIC
+  void
+  execute_ready_executables();
+
+private:
+  RCLCPP_DISABLE_COPY(StaticSingleThreadedExecutor)
+
+  StaticExecutorEntitiesCollector::SharedPtr entities_collector_;
+};
+
+}  // namespace executors
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_

rclcpp/include/rclcpp/experimental/executable_list.hpp

@@ -0,0 +1,92 @@
+// Copyright 2019 Nobleo Technology
+//
+// 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__EXPERIMENTAL__EXECUTABLE_LIST_HPP_
+#define RCLCPP__EXPERIMENTAL__EXECUTABLE_LIST_HPP_
+
+#include <memory>
+#include <vector>
+
+#include "rclcpp/client.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+
+/// This class contains subscriptionbase, timerbase, etc. which can be used to run callbacks.
+class ExecutableList final
+{
+public:
+  RCLCPP_PUBLIC
+  ExecutableList();
+
+  RCLCPP_PUBLIC
+  ~ExecutableList();
+
+  RCLCPP_PUBLIC
+  void
+  clear();
+
+  RCLCPP_PUBLIC
+  void
+  add_subscription(rclcpp::SubscriptionBase::SharedPtr subscription);
+
+  RCLCPP_PUBLIC
+  void
+  add_timer(rclcpp::TimerBase::SharedPtr timer);
+
+  RCLCPP_PUBLIC
+  void
+  add_service(rclcpp::ServiceBase::SharedPtr service);
+
+  RCLCPP_PUBLIC
+  void
+  add_client(rclcpp::ClientBase::SharedPtr client);
+
+  RCLCPP_PUBLIC
+  void
+  add_waitable(rclcpp::Waitable::SharedPtr waitable);
+
+  // Vector containing the SubscriptionBase of all the subscriptions added to the executor.
+  std::vector<rclcpp::SubscriptionBase::SharedPtr> subscription;
+  // Contains the count of added subscriptions
+  size_t number_of_subscriptions;
+  // Vector containing the TimerBase of all the timers added to the executor.
+  std::vector<rclcpp::TimerBase::SharedPtr> timer;
+  // Contains the count of added timers
+  size_t number_of_timers;
+  // Vector containing the ServiceBase of all the services added to the executor.
+  std::vector<rclcpp::ServiceBase::SharedPtr> service;
+  // Contains the count of added services
+  size_t number_of_services;
+  // Vector containing the ClientBase of all the clients added to the executor.
+  std::vector<rclcpp::ClientBase::SharedPtr> client;
+  // Contains the count of added clients
+  size_t number_of_clients;
+  // Vector containing all the waitables added to the executor.
+  std::vector<rclcpp::Waitable::SharedPtr> waitable;
+  // Contains the count of added waitables
+  size_t number_of_waitables;
+};
+
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__EXECUTABLE_LIST_HPP_

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

@@ -227,12 +227,10 @@ 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>(shared_msg,
-        sub_ids.take_shared_subscriptions);
+      this->template add_shared_msg_to_buffers<MessageT>(
+        shared_msg, sub_ids.take_shared_subscriptions);
       this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter>(
-        std::move(message),
-        sub_ids.take_ownership_subscriptions,
-        allocator);
+        std::move(message), sub_ids.take_ownership_subscriptions, allocator);
     }
   }
 
@@ -265,8 +263,8 @@ 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>(shared_msg,
-          sub_ids.take_shared_subscriptions);
+        this->template add_shared_msg_to_buffers<MessageT>(
+          shared_msg, sub_ids.take_shared_subscriptions);
       }
       return shared_msg;
     } else {

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -97,7 +97,9 @@ public:
     // The callback object gets copied, so if registration is done too early/before this point
     // (e.g. in `AnySubscriptionCallback::set()`), its address won't match any address used later
     // in subsequent tracepoints.
+#ifndef TRACETOOLS_DISABLED
     any_callback_.register_callback_for_tracing();
+#endif
   }
 
   bool
@@ -152,6 +154,7 @@ private:
   execute_impl()
   {
     rmw_message_info_t msg_info;
+    msg_info.publisher_gid = {0, {0}};
     msg_info.from_intra_process = true;
 
     if (any_callback_.use_take_shared_method()) {

rclcpp/include/rclcpp/function_traits.hpp

@@ -88,8 +88,7 @@ struct function_traits<std::_Bind<ReturnTypeT(ClassT::*(FArgs ...))(Args ...)>>
 struct function_traits<std::_Bind<std::_Mem_fn<ReturnTypeT (ClassT::*)(Args ...)>(FArgs ...)>>
 #elif defined _MSC_VER  // MS Visual Studio
 struct function_traits<
-  std::_Binder<std::_Unforced, ReturnTypeT(__cdecl ClassT::*)(Args ...), FArgs ...>
->
+  std::_Binder<std::_Unforced, ReturnTypeT (ClassT::*)(Args ...), FArgs ...>>
 #else
 #error "Unsupported C++ compiler / standard library"
 #endif
@@ -106,8 +105,7 @@ struct function_traits<std::_Bind<ReturnTypeT(ClassT::*(FArgs ...))(Args ...) co
 struct function_traits<std::_Bind<std::_Mem_fn<ReturnTypeT (ClassT::*)(Args ...) const>(FArgs ...)>>
 #elif defined _MSC_VER  // MS Visual Studio
 struct function_traits<
-  std::_Binder<std::_Unforced, ReturnTypeT(__cdecl ClassT::*)(Args ...) const, FArgs ...>
->
+  std::_Binder<std::_Unforced, ReturnTypeT (ClassT::*)(Args ...) const, FArgs ...>>
 #else
 #error "Unsupported C++ compiler / standard library"
 #endif
@@ -121,7 +119,7 @@ struct function_traits<std::__bind<ReturnTypeT( &)(Args ...), FArgs ...>>
 #elif defined __GLIBCXX__  // glibc++ (GNU C++)
 struct function_traits<std::_Bind<ReturnTypeT(*(FArgs ...))(Args ...)>>
 #elif defined _MSC_VER  // MS Visual Studio
-struct function_traits<std::_Binder<std::_Unforced, ReturnTypeT(__cdecl &)(Args ...), FArgs ...>>
+struct function_traits<std::_Binder<std::_Unforced, ReturnTypeT( &)(Args ...), FArgs ...>>
 #else
 #error "Unsupported C++ compiler / standard library"
 #endif

rclcpp/include/rclcpp/future_return_code.hpp

@@ -0,0 +1,61 @@
+// Copyright 2014 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__FUTURE_RETURN_CODE_HPP_
+#define RCLCPP__FUTURE_RETURN_CODE_HPP_
+
+#include <iostream>
+#include <string>
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Return codes to be used with spin_until_future_complete.
+/**
+ * SUCCESS: The future is complete and can be accessed with "get" without blocking.
+ *          This does not indicate that the operation succeeded; "get" may still throw an exception.
+ * INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error.
+ * TIMEOUT: Spinning timed out.
+ */
+enum class FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT};
+
+/// Stream operator for FutureReturnCode.
+RCLCPP_PUBLIC
+std::ostream &
+operator<<(std::ostream & os, const FutureReturnCode & future_return_code);
+
+/// String conversion function for FutureReturnCode.
+RCLCPP_PUBLIC
+std::string
+to_string(const FutureReturnCode & future_return_code);
+
+namespace executor
+{
+
+using FutureReturnCode [[deprecated("use rclcpp::FutureReturnCode instead")]] = FutureReturnCode;
+
+[[deprecated("use rclcpp::to_string(const rclcpp::FutureReturnCode &) instead")]]
+inline
+std::string
+to_string(const rclcpp::FutureReturnCode & future_return_code)
+{
+  return rclcpp::to_string(future_return_code);
+}
+
+}  // namespace executor
+}  // namespace rclcpp
+
+#endif  // RCLCPP__FUTURE_RETURN_CODE_HPP_

rclcpp/include/rclcpp/graph_listener.hpp

@@ -165,7 +165,7 @@ private:
   void
   __shutdown(bool);
 
-  rclcpp::Context::SharedPtr parent_context_;
+  rclcpp::Context::WeakPtr parent_context_;
 
   std::thread listener_thread_;
   bool is_started_;
@@ -177,7 +177,6 @@ private:
   std::vector<rclcpp::node_interfaces::NodeGraphInterface *> node_graph_interfaces_;
 
   rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
-  std::shared_ptr<rcl_context_t> interrupt_guard_condition_context_;
   rcl_guard_condition_t * shutdown_guard_condition_;
   rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
 };

rclcpp/include/rclcpp/guard_condition.hpp

@@ -0,0 +1,100 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__GUARD_CONDITION_HPP_
+#define RCLCPP__GUARD_CONDITION_HPP_
+
+#include <atomic>
+
+#include "rcl/guard_condition.h"
+
+#include "rclcpp/context.hpp"
+#include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// A condition that can be waited on in a single wait set and asynchronously triggered.
+class GuardCondition
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(GuardCondition)
+
+  // TODO(wjwwood): support custom allocator, maybe restrict to polymorphic allocator
+  /// Construct the guard condition, optionally specifying which Context to use.
+  /**
+   * \param[in] context Optional custom context to be used.
+   *   Defaults to using the global default context singleton.
+   *   Shared ownership of the context is held with the guard condition until
+   *   destruction.
+   * \throws std::invalid_argument if the context is nullptr.
+   * \throws rclcpp::exceptions::RCLError based exceptions when underlying
+   *   rcl functions fail.
+   */
+  RCLCPP_PUBLIC
+  explicit GuardCondition(
+    rclcpp::Context::SharedPtr context =
+    rclcpp::contexts::get_global_default_context());
+
+  RCLCPP_PUBLIC
+  virtual
+  ~GuardCondition();
+
+  /// Return the context used when creating this guard condition.
+  RCLCPP_PUBLIC
+  rclcpp::Context::SharedPtr
+  get_context() const;
+
+  /// Return the underlying rcl guard condition structure.
+  RCLCPP_PUBLIC
+  const rcl_guard_condition_t &
+  get_rcl_guard_condition() const;
+
+  /// Notify the wait set waiting on this condition, if any, that the condition had been met.
+  /**
+   * This function is thread-safe, and may be called concurrently with waiting
+   * on this guard condition in a wait set.
+   *
+   * \throws rclcpp::exceptions::RCLError based exceptions when underlying
+   *   rcl functions fail.
+   */
+  RCLCPP_PUBLIC
+  void
+  trigger();
+
+  /// Exchange the "in use by wait set" state for this guard condition.
+  /**
+   * This is used to ensure this guard condition is not used by multiple
+   * wait sets at the same time.
+   *
+   * \param[in] in_use_state the new state to exchange into the state, true
+   *   indicates it is now in use by a wait set, and false is that it is no
+   *   longer in use by a wait set.
+   * \returns the previous state.
+   */
+  RCLCPP_PUBLIC
+  bool
+  exchange_in_use_by_wait_set_state(bool in_use_state);
+
+protected:
+  rclcpp::Context::SharedPtr context_;
+  rcl_guard_condition_t rcl_guard_condition_;
+  std::atomic<bool> in_use_by_wait_set_{false};
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__GUARD_CONDITION_HPP_

rclcpp/include/rclcpp/init_options.hpp

@@ -42,6 +42,16 @@ public:
   RCLCPP_PUBLIC
   InitOptions(const InitOptions & other);
 
+  /// Return `true` if logging should be initialized when `rclcpp::Context::init` is called.
+  RCLCPP_PUBLIC
+  bool
+  auto_initialize_logging() const;
+
+  /// Set flag indicating if logging should be initialized or not.
+  RCLCPP_PUBLIC
+  InitOptions &
+  auto_initialize_logging(bool initialize_logging);
+
   /// Assignment operator.
   RCLCPP_PUBLIC
   InitOptions &
@@ -62,6 +72,7 @@ protected:
 
 private:
   std::unique_ptr<rcl_init_options_t> init_options_;
+  bool initialize_logging_{true};
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -56,6 +56,7 @@ public:
   virtual size_t number_of_guard_conditions() const = 0;
   virtual size_t number_of_waitables() const = 0;
 
+  virtual void add_waitable_handle(const rclcpp::Waitable::SharedPtr & waitable) = 0;
   virtual bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) = 0;
   virtual void clear_handles() = 0;
   virtual void remove_null_handles(rcl_wait_set_t * wait_set) = 0;
@@ -66,27 +67,27 @@ public:
 
   virtual void
   get_next_subscription(
-    rclcpp::executor::AnyExecutable & any_exec,
+    rclcpp::AnyExecutable & any_exec,
     const WeakNodeList & weak_nodes) = 0;
 
   virtual void
   get_next_service(
-    rclcpp::executor::AnyExecutable & any_exec,
+    rclcpp::AnyExecutable & any_exec,
     const WeakNodeList & weak_nodes) = 0;
 
   virtual void
   get_next_client(
-    rclcpp::executor::AnyExecutable & any_exec,
+    rclcpp::AnyExecutable & any_exec,
     const WeakNodeList & weak_nodes) = 0;
 
   virtual void
   get_next_timer(
-    rclcpp::executor::AnyExecutable & any_exec,
+    rclcpp::AnyExecutable & any_exec,
     const WeakNodeList & weak_nodes) = 0;
 
   virtual void
   get_next_waitable(
-    rclcpp::executor::AnyExecutable & any_exec,
+    rclcpp::AnyExecutable & any_exec,
     const WeakNodeList & weak_nodes) = 0;
 
   virtual rcl_allocator_t
@@ -114,30 +115,30 @@ public:
 
   static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
   get_node_by_group(
-    rclcpp::callback_group::CallbackGroup::SharedPtr group,
+    rclcpp::CallbackGroup::SharedPtr group,
     const WeakNodeList & weak_nodes);
 
-  static rclcpp::callback_group::CallbackGroup::SharedPtr
+  static rclcpp::CallbackGroup::SharedPtr
   get_group_by_subscription(
     rclcpp::SubscriptionBase::SharedPtr subscription,
     const WeakNodeList & weak_nodes);
 
-  static rclcpp::callback_group::CallbackGroup::SharedPtr
+  static rclcpp::CallbackGroup::SharedPtr
   get_group_by_service(
     rclcpp::ServiceBase::SharedPtr service,
     const WeakNodeList & weak_nodes);
 
-  static rclcpp::callback_group::CallbackGroup::SharedPtr
+  static rclcpp::CallbackGroup::SharedPtr
   get_group_by_client(
     rclcpp::ClientBase::SharedPtr client,
     const WeakNodeList & weak_nodes);
 
-  static rclcpp::callback_group::CallbackGroup::SharedPtr
+  static rclcpp::CallbackGroup::SharedPtr
   get_group_by_timer(
     rclcpp::TimerBase::SharedPtr timer,
     const WeakNodeList & weak_nodes);
 
-  static rclcpp::callback_group::CallbackGroup::SharedPtr
+  static rclcpp::CallbackGroup::SharedPtr
   get_group_by_waitable(
     rclcpp::Waitable::SharedPtr waitable,
     const WeakNodeList & weak_nodes);

rclcpp/include/rclcpp/message_info.hpp

@@ -0,0 +1,52 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__MESSAGE_INFO_HPP_
+#define RCLCPP__MESSAGE_INFO_HPP_
+
+#include "rmw/types.h"
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Additional meta data about messages taken from subscriptions.
+class RCLCPP_PUBLIC MessageInfo
+{
+public:
+  /// Default empty constructor.
+  MessageInfo() = default;
+
+  /// Conversion constructor, which is intentionally not marked as explicit.
+  // cppcheck-suppress noExplicitConstructor
+  MessageInfo(const rmw_message_info_t & rmw_message_info);  // NOLINT(runtime/explicit)
+
+  virtual ~MessageInfo();
+
+  /// Return the message info as the underlying rmw message info type.
+  const rmw_message_info_t &
+  get_rmw_message_info() const;
+
+  /// Return the message info as the underlying rmw message info type.
+  rmw_message_info_t &
+  get_rmw_message_info();
+
+private:
+  rmw_message_info_t rmw_message_info_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__MESSAGE_INFO_HPP_

rclcpp/include/rclcpp/message_memory_strategy.hpp

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

rclcpp/include/rclcpp/node.hpp

@@ -135,12 +135,12 @@ public:
 
   /// Create and return a callback group.
   RCLCPP_PUBLIC
-  rclcpp::callback_group::CallbackGroup::SharedPtr
-  create_callback_group(rclcpp::callback_group::CallbackGroupType group_type);
+  rclcpp::CallbackGroup::SharedPtr
+  create_callback_group(rclcpp::CallbackGroupType group_type);
 
   /// Return the list of callback groups in the node.
   RCLCPP_PUBLIC
-  const std::vector<rclcpp::callback_group::CallbackGroup::WeakPtr> &
+  const std::vector<rclcpp::CallbackGroup::WeakPtr> &
   get_callback_groups() const;
 
   /// Create and return a Publisher.
@@ -227,7 +227,7 @@ public:
   create_wall_timer(
     std::chrono::duration<DurationRepT, DurationT> period,
     CallbackT callback,
-    rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
+    rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
   /* Create and return a Client. */
   template<typename ServiceT>
@@ -235,7 +235,7 @@ public:
   create_client(
     const std::string & service_name,
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default,
-    rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
+    rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
   /* Create and return a Service. */
   template<typename ServiceT, typename CallbackT>
@@ -244,7 +244,7 @@ public:
     const std::string & service_name,
     CallbackT && callback,
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default,
-    rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
+    rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
   /// Declare and initialize a parameter, return the effective value.
   /**
@@ -301,7 +301,7 @@ public:
    *
    * If the type of the default value, and therefore also the type of return
    * value, differs from the initial value provided in the node options, then
-   * a rclcpp::ParameterTypeException may be thrown.
+   * a rclcpp::exceptions::InvalidParameterTypeException may be thrown.
    * To avoid this, use the declare_parameter() method which returns an
    * rclcpp::ParameterValue instead.
    *
@@ -867,6 +867,58 @@ public:
   size_t
   count_subscribers(const std::string & topic_name) const;
 
+  /// Return the topic endpoint information about publishers on a given topic.
+  /**
+   * The returned parameter is a list of topic endpoint information, where each item will contain
+   * the node name, node namespace, topic type, endpoint type, topic endpoint's GID, and its QoS
+   * profile.
+   *
+   * When the `no_mangle` parameter is `true`, the provided `topic_name` should be a valid topic
+   * name for the middleware (useful when combining ROS with native middleware (e.g. DDS) apps).
+   * When the `no_mangle` parameter is `false`, the provided `topic_name` should follow
+   * ROS topic name conventions.
+   *
+   * `topic_name` may be a relative, private, or fully qualified topic name.
+   * A relative or private topic will be expanded using this node's namespace and name.
+   * The queried `topic_name` is not remapped.
+   *
+   * \param[in] topic_name the topic_name on which to find the publishers.
+   * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name,
+   *   otherwise it should be a valid ROS topic name. Defaults to `false`.
+   * \return a list of TopicEndpointInfo representing all the publishers on this topic.
+   * \throws InvalidTopicNameError if the given topic_name is invalid.
+   * \throws std::runtime_error if internal error happens.
+   */
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_publishers_info_by_topic(const std::string & topic_name, bool no_mangle = false) const;
+
+  /// Return the topic endpoint information about subscriptions on a given topic.
+  /**
+   * The returned parameter is a list of topic endpoint information, where each item will contain
+   * the node name, node namespace, topic type, endpoint type, topic endpoint's GID, and its QoS
+   * profile.
+   *
+   * When the `no_mangle` parameter is `true`, the provided `topic_name` should be a valid topic
+   * name for the middleware (useful when combining ROS with native middleware (e.g. DDS) apps).
+   * When the `no_mangle` parameter is `false`, the provided `topic_name` should follow
+   * ROS topic name conventions.
+   *
+   * `topic_name` may be a relative, private, or fully qualified topic name.
+   * A relative or private topic will be expanded using this node's namespace and name.
+   * The queried `topic_name` is not remapped.
+   *
+   * \param[in] topic_name the topic_name on which to find the subscriptions.
+   * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name,
+   *   otherwise it should be a valid ROS topic name. Defaults to `false`.
+   * \return a list of TopicEndpointInfo representing all the subscriptions on this topic.
+   * \throws InvalidTopicNameError if the given topic_name is invalid.
+   * \throws std::runtime_error if internal error happens.
+   */
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_subscriptions_info_by_topic(const std::string & topic_name, bool no_mangle = false) const;
+
   /// Return a graph event, which will be set anytime a graph change occurs.
   /* The graph Event object is a loan which must be returned.
    * The Event object is scoped and therefore to return the loan just let it go
@@ -894,9 +946,13 @@ public:
   rclcpp::Clock::SharedPtr
   get_clock();
 
+  RCLCPP_PUBLIC
+  rclcpp::Clock::ConstSharedPtr
+  get_clock() const;
+
   RCLCPP_PUBLIC
   Time
-  now();
+  now() const;
 
   /// Return the Node's internal NodeBaseInterface implementation.
   RCLCPP_PUBLIC
@@ -1086,7 +1142,7 @@ private:
 
   RCLCPP_PUBLIC
   bool
-  group_in_node(callback_group::CallbackGroup::SharedPtr group);
+  group_in_node(CallbackGroup::SharedPtr group);
 
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;

rclcpp/include/rclcpp/node_impl.hpp

@@ -19,6 +19,7 @@
 #include <rmw/rmw.h>
 
 #include <algorithm>
+#include <chrono>
 #include <cstdlib>
 #include <iostream>
 #include <limits>
@@ -33,15 +34,16 @@
 #include "rcl/publisher.h"
 #include "rcl/subscription.h"
 
-#include "rcl_interfaces/msg/intra_process_message.hpp"
-
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/create_client.hpp"
 #include "rclcpp/create_publisher.hpp"
 #include "rclcpp/create_service.hpp"
+#include "rclcpp/create_timer.hpp"
 #include "rclcpp/create_subscription.hpp"
+#include "rclcpp/detail/resolve_enable_topic_statistics.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/qos.hpp"
+#include "rclcpp/timer.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -107,14 +109,14 @@ typename rclcpp::WallTimer<CallbackT>::SharedPtr
 Node::create_wall_timer(
   std::chrono::duration<DurationRepT, DurationT> period,
   CallbackT callback,
-  rclcpp::callback_group::CallbackGroup::SharedPtr group)
+  rclcpp::CallbackGroup::SharedPtr group)
 {
-  auto timer = rclcpp::WallTimer<CallbackT>::make_shared(
-    std::chrono::duration_cast<std::chrono::nanoseconds>(period),
+  return rclcpp::create_wall_timer(
+    period,
     std::move(callback),
-    this->node_base_->get_context());
-  node_timers_->add_timer(timer, group);
-  return timer;
+    group,
+    this->node_base_.get(),
+    this->node_timers_.get());
 }
 
 template<typename ServiceT>
@@ -122,7 +124,7 @@ typename Client<ServiceT>::SharedPtr
 Node::create_client(
   const std::string & service_name,
   const rmw_qos_profile_t & qos_profile,
-  rclcpp::callback_group::CallbackGroup::SharedPtr group)
+  rclcpp::CallbackGroup::SharedPtr group)
 {
   return rclcpp::create_client<ServiceT>(
     node_base_,
@@ -139,7 +141,7 @@ Node::create_service(
   const std::string & service_name,
   CallbackT && callback,
   const rmw_qos_profile_t & qos_profile,
-  rclcpp::callback_group::CallbackGroup::SharedPtr group)
+  rclcpp::CallbackGroup::SharedPtr group)
 {
   return rclcpp::create_service<ServiceT, CallbackT>(
     node_base_,
@@ -158,12 +160,16 @@ Node::declare_parameter(
   const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor,
   bool ignore_override)
 {
-  return this->declare_parameter(
-    name,
-    rclcpp::ParameterValue(default_value),
-    parameter_descriptor,
-    ignore_override
-  ).get<ParameterT>();
+  try {
+    return this->declare_parameter(
+      name,
+      rclcpp::ParameterValue(default_value),
+      parameter_descriptor,
+      ignore_override
+    ).get<ParameterT>();
+  } catch (const ParameterTypeException & ex) {
+    throw exceptions::InvalidParameterTypeException(name, ex.what());
+  }
 }
 
 template<typename ParameterT>

rclcpp/include/rclcpp/node_interfaces/get_node_base_interface.hpp

@@ -1,149 +0,0 @@
-// Copyright 2019 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef RCLCPP__NODE_INTERFACES__GET_NODE_BASE_INTERFACE_HPP_
-#define RCLCPP__NODE_INTERFACES__GET_NODE_BASE_INTERFACE_HPP_
-
-#include <memory>
-#include <utility>
-#include <type_traits>
-
-#include "rclcpp/node_interfaces/node_base_interface.hpp"
-
-/// This header provides the get_node_base_interface() template function.
-/**
- * This function is useful for getting the NodeBaseInterface pointer from
- * various kinds of Node-like classes.
- *
- * It's able to get the NodeBaseInterface pointer so long as the class
- * has a method called ``get_node_base_interface()`` which returns
- * either a pointer (const or not) to a NodeBaseInterface or a
- * std::shared_ptr to a NodeBaseInterface.
- */
-
-namespace rclcpp
-{
-namespace node_interfaces
-{
-
-namespace detail
-{
-
-// This is a meta-programming checker for if a given Node-like object has a
-// getter called get_node_base_interface() which returns various types,
-// e.g. const pointer or a shared pointer.
-template<typename NodeType, typename ReturnType>
-struct has_get_node_base_interface
-{
-private:
-  template<typename T>
-  static constexpr
-  auto
-  check(T *)->typename std::is_same<
-    decltype(std::declval<T>().get_node_base_interface()),
-    ReturnType
-  >::type;
-
-  template<typename>
-  static constexpr
-  std::false_type
-  check(...);
-
-public:
-  using type = decltype(check<NodeType>(nullptr));
-  static constexpr bool value = type::value;
-};
-
-// If NodeType is a pointer to NodeBaseInterface already (just normal function overload).
-inline
-rclcpp::node_interfaces::NodeBaseInterface *
-get_node_base_interface_from_pointer(rclcpp::node_interfaces::NodeBaseInterface * pointer)
-{
-  return pointer;
-}
-
-// If NodeType has a method called get_node_base_interface() which returns a shared pointer.
-template<
-  typename NodeType,
-  typename std::enable_if<has_get_node_base_interface<
-    typename std::remove_pointer<NodeType>::type,
-    std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>
-  >::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeBaseInterface *
-get_node_base_interface_from_pointer(NodeType node_pointer)
-{
-  return node_pointer->get_node_base_interface().get();
-}
-
-// If NodeType has a method called get_node_base_interface() which returns a pointer.
-template<
-  typename NodeType,
-  typename std::enable_if<has_get_node_base_interface<
-    typename std::remove_pointer<NodeType>::type,
-    rclcpp::node_interfaces::NodeBaseInterface *
-  >::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeBaseInterface *
-get_node_base_interface_from_pointer(NodeType node_pointer)
-{
-  return node_pointer->get_node_base_interface();
-}
-
-// Forward shared_ptr's to const node pointer signatures.
-template<
-  typename NodeType,
-  typename std::enable_if<std::is_same<
-    NodeType,
-    typename std::shared_ptr<typename std::remove_pointer<NodeType>::type::element_type> *
-  >::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeBaseInterface *
-get_node_base_interface_from_pointer(NodeType node_shared_pointer)
-{
-  return get_node_base_interface_from_pointer(node_shared_pointer->get());
-}
-
-}  // namespace detail
-
-/// Get the NodeBaseInterface as a pointer from a pointer to a "Node like" object.
-template<
-  typename NodeType,
-  typename std::enable_if<std::is_pointer<NodeType>::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeBaseInterface *
-get_node_base_interface(NodeType node_pointer)
-{
-  // Forward pointers to detail implmentation directly.
-  return detail::get_node_base_interface_from_pointer(node_pointer);
-}
-
-/// Get the NodeBaseInterface as a pointer from a "Node like" object.
-template<
-  typename NodeType,
-  typename std::enable_if<
-    !std::is_pointer<typename std::remove_reference<NodeType>::type>::value, int
-  >::type = 0
->
-rclcpp::node_interfaces::NodeBaseInterface *
-get_node_base_interface(NodeType && node_reference)
-{
-  // Forward references to detail implmentation as a pointer.
-  return detail::get_node_base_interface_from_pointer(&node_reference);
-}
-
-}  // namespace node_interfaces
-}  // namespace rclcpp
-
-#endif  // RCLCPP__NODE_INTERFACES__GET_NODE_BASE_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/get_node_timers_interface.hpp

@@ -1,149 +0,0 @@
-// Copyright 2019 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef RCLCPP__NODE_INTERFACES__GET_NODE_TIMERS_INTERFACE_HPP_
-#define RCLCPP__NODE_INTERFACES__GET_NODE_TIMERS_INTERFACE_HPP_
-
-#include <memory>
-#include <utility>
-#include <type_traits>
-
-#include "rclcpp/node_interfaces/node_timers_interface.hpp"
-
-/// This header provides the get_node_timers_interface() template function.
-/**
- * This function is useful for getting the NodeTimersInterface pointer from
- * various kinds of Node-like classes.
- *
- * It's able to get the NodeTimersInterface pointer so long as the class
- * has a method called ``get_node_timers_interface()`` which returns
- * either a pointer (const or not) to a NodeTimersInterface or a
- * std::shared_ptr to a NodeTimersInterface.
- */
-
-namespace rclcpp
-{
-namespace node_interfaces
-{
-
-namespace detail
-{
-
-// This is a meta-programming checker for if a given Node-like object has a
-// getter called get_node_timers_interface() which returns various types,
-// e.g. const pointer or a shared pointer.
-template<typename NodeType, typename ReturnType>
-struct has_get_node_timers_interface
-{
-private:
-  template<typename T>
-  static constexpr
-  auto
-  check(T *)->typename std::is_same<
-    decltype(std::declval<T>().get_node_timers_interface()),
-    ReturnType
-  >::type;
-
-  template<typename>
-  static constexpr
-  std::false_type
-  check(...);
-
-public:
-  using type = decltype(check<NodeType>(nullptr));
-  static constexpr bool value = type::value;
-};
-
-// If NodeType is a pointer to NodeTimersInterface already (just normal function overload).
-inline
-rclcpp::node_interfaces::NodeTimersInterface *
-get_node_timers_interface_from_pointer(rclcpp::node_interfaces::NodeTimersInterface * pointer)
-{
-  return pointer;
-}
-
-// If NodeType has a method called get_node_timers_interface() which returns a shared pointer.
-template<
-  typename NodeType,
-  typename std::enable_if<has_get_node_timers_interface<
-    typename std::remove_pointer<NodeType>::type,
-    std::shared_ptr<rclcpp::node_interfaces::NodeTimersInterface>
-  >::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeTimersInterface *
-get_node_timers_interface_from_pointer(NodeType node_pointer)
-{
-  return node_pointer->get_node_timers_interface().get();
-}
-
-// If NodeType has a method called get_node_timers_interface() which returns a pointer.
-template<
-  typename NodeType,
-  typename std::enable_if<has_get_node_timers_interface<
-    typename std::remove_pointer<NodeType>::type,
-    rclcpp::node_interfaces::NodeTimersInterface *
-  >::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeTimersInterface *
-get_node_timers_interface_from_pointer(NodeType node_pointer)
-{
-  return node_pointer->get_node_timers_interface();
-}
-
-// Forward shared_ptr's to const node pointer signatures.
-template<
-  typename NodeType,
-  typename std::enable_if<std::is_same<
-    NodeType,
-    typename std::shared_ptr<typename std::remove_pointer<NodeType>::type::element_type> *
-  >::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeTimersInterface *
-get_node_timers_interface_from_pointer(NodeType node_shared_pointer)
-{
-  return get_node_timers_interface_from_pointer(node_shared_pointer->get());
-}
-
-}  // namespace detail
-
-/// Get the NodeTimersInterface as a pointer from a pointer to a "Node like" object.
-template<
-  typename NodeType,
-  typename std::enable_if<std::is_pointer<NodeType>::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeTimersInterface *
-get_node_timers_interface(NodeType node_pointer)
-{
-  // Forward pointers to detail implmentation directly.
-  return detail::get_node_timers_interface_from_pointer(node_pointer);
-}
-
-/// Get the NodeTimersInterface as a pointer from a "Node like" object.
-template<
-  typename NodeType,
-  typename std::enable_if<
-    !std::is_pointer<typename std::remove_reference<NodeType>::type>::value, int
-  >::type = 0
->
-rclcpp::node_interfaces::NodeTimersInterface *
-get_node_timers_interface(NodeType && node_reference)
-{
-  // Forward references to detail implmentation as a pointer.
-  return detail::get_node_timers_interface_from_pointer(&node_reference);
-}
-
-}  // namespace node_interfaces
-}  // namespace rclcpp
-
-#endif  // RCLCPP__NODE_INTERFACES__GET_NODE_TIMERS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/get_node_topics_interface.hpp

@@ -1,149 +0,0 @@
-// Copyright 2019 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef RCLCPP__NODE_INTERFACES__GET_NODE_TOPICS_INTERFACE_HPP_
-#define RCLCPP__NODE_INTERFACES__GET_NODE_TOPICS_INTERFACE_HPP_
-
-#include <memory>
-#include <utility>
-#include <type_traits>
-
-#include "rclcpp/node_interfaces/node_topics_interface.hpp"
-
-/// This header provides the get_node_topics_interface() template function.
-/**
- * This function is useful for getting the NodeTopicsInterface pointer from
- * various kinds of Node-like classes.
- *
- * It's able to get the NodeTopicsInterface pointer so long as the class
- * has a method called ``get_node_topics_interface()`` which returns
- * either a pointer (const or not) to a NodeTopicsInterface or a
- * std::shared_ptr to a NodeTopicsInterface.
- */
-
-namespace rclcpp
-{
-namespace node_interfaces
-{
-
-namespace detail
-{
-
-// This is a meta-programming checker for if a given Node-like object has a
-// getter called get_node_topics_interface() which returns various types,
-// e.g. const pointer or a shared pointer.
-template<typename NodeType, typename ReturnType>
-struct has_get_node_topics_interface
-{
-private:
-  template<typename T>
-  static constexpr
-  auto
-  check(T *)->typename std::is_same<
-    decltype(std::declval<T>().get_node_topics_interface()),
-    ReturnType
-  >::type;
-
-  template<typename>
-  static constexpr
-  std::false_type
-  check(...);
-
-public:
-  using type = decltype(check<NodeType>(nullptr));
-  static constexpr bool value = type::value;
-};
-
-// If NodeType is a pointer to NodeTopicsInterface already (just normal function overload).
-inline
-rclcpp::node_interfaces::NodeTopicsInterface *
-get_node_topics_interface_from_pointer(rclcpp::node_interfaces::NodeTopicsInterface * pointer)
-{
-  return pointer;
-}
-
-// If NodeType has a method called get_node_topics_interface() which returns a shared pointer.
-template<
-  typename NodeType,
-  typename std::enable_if<has_get_node_topics_interface<
-    typename std::remove_pointer<NodeType>::type,
-    std::shared_ptr<rclcpp::node_interfaces::NodeTopicsInterface>
-  >::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeTopicsInterface *
-get_node_topics_interface_from_pointer(NodeType node_pointer)
-{
-  return node_pointer->get_node_topics_interface().get();
-}
-
-// If NodeType has a method called get_node_topics_interface() which returns a pointer.
-template<
-  typename NodeType,
-  typename std::enable_if<has_get_node_topics_interface<
-    typename std::remove_pointer<NodeType>::type,
-    rclcpp::node_interfaces::NodeTopicsInterface *
-  >::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeTopicsInterface *
-get_node_topics_interface_from_pointer(NodeType node_pointer)
-{
-  return node_pointer->get_node_topics_interface();
-}
-
-// Forward shared_ptr's to const node pointer signatures.
-template<
-  typename NodeType,
-  typename std::enable_if<std::is_same<
-    NodeType,
-    typename std::shared_ptr<typename std::remove_pointer<NodeType>::type::element_type> *
-  >::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeTopicsInterface *
-get_node_topics_interface_from_pointer(NodeType node_shared_pointer)
-{
-  return get_node_topics_interface_from_pointer(node_shared_pointer->get());
-}
-
-}  // namespace detail
-
-/// Get the NodeTopicsInterface as a pointer from a pointer to a "Node like" object.
-template<
-  typename NodeType,
-  typename std::enable_if<std::is_pointer<NodeType>::value, int>::type = 0
->
-rclcpp::node_interfaces::NodeTopicsInterface *
-get_node_topics_interface(NodeType node_pointer)
-{
-  // Forward pointers to detail implmentation directly.
-  return detail::get_node_topics_interface_from_pointer(node_pointer);
-}
-
-/// Get the NodeTopicsInterface as a pointer from a "Node like" object.
-template<
-  typename NodeType,
-  typename std::enable_if<
-    !std::is_pointer<typename std::remove_reference<NodeType>::type>::value, int
-  >::type = 0
->
-rclcpp::node_interfaces::NodeTopicsInterface *
-get_node_topics_interface(NodeType && node_reference)
-{
-  // Forward references to detail implmentation as a pointer.
-  return detail::get_node_topics_interface_from_pointer(&node_reference);
-}
-
-}  // namespace node_interfaces
-}  // namespace rclcpp
-
-#endif  // RCLCPP__NODE_INTERFACES__GET_NODE_TOPICS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -23,7 +23,6 @@
 #include "rclcpp/context.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
-#include "rclcpp/node_options.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -43,107 +42,112 @@ public:
     const std::string & namespace_,
     rclcpp::Context::SharedPtr context,
     const rcl_node_options_t & rcl_node_options,
-    bool use_intra_process_default);
+    bool use_intra_process_default,
+    bool enable_topic_statistics_default);
 
   RCLCPP_PUBLIC
   virtual
   ~NodeBase();
 
   RCLCPP_PUBLIC
-  virtual
+
   const char *
-  get_name() const;
+  get_name() const override;
 
   RCLCPP_PUBLIC
-  virtual
+
   const char *
-  get_namespace() const;
+  get_namespace() const override;
 
   RCLCPP_PUBLIC
-  virtual
+
   const char *
-  get_fully_qualified_name() const;
+  get_fully_qualified_name() const override;
 
   RCLCPP_PUBLIC
-  virtual
+
   rclcpp::Context::SharedPtr
-  get_context();
+  get_context() override;
 
   RCLCPP_PUBLIC
-  virtual
+
   rcl_node_t *
-  get_rcl_node_handle();
+  get_rcl_node_handle() override;
 
   RCLCPP_PUBLIC
-  virtual
+
   const rcl_node_t *
-  get_rcl_node_handle() const;
+  get_rcl_node_handle() const override;
 
   RCLCPP_PUBLIC
-  virtual
+
   std::shared_ptr<rcl_node_t>
-  get_shared_rcl_node_handle();
+  get_shared_rcl_node_handle() override;
 
   RCLCPP_PUBLIC
-  virtual
+
   std::shared_ptr<const rcl_node_t>
-  get_shared_rcl_node_handle() const;
+  get_shared_rcl_node_handle() const override;
 
   RCLCPP_PUBLIC
-  virtual
+
   bool
-  assert_liveliness() const;
+  assert_liveliness() const override;
 
   RCLCPP_PUBLIC
-  virtual
-  rclcpp::callback_group::CallbackGroup::SharedPtr
-  create_callback_group(rclcpp::callback_group::CallbackGroupType group_type);
+
+  rclcpp::CallbackGroup::SharedPtr
+  create_callback_group(rclcpp::CallbackGroupType group_type) override;
 
   RCLCPP_PUBLIC
-  virtual
-  rclcpp::callback_group::CallbackGroup::SharedPtr
-  get_default_callback_group();
+
+  rclcpp::CallbackGroup::SharedPtr
+  get_default_callback_group() override;
 
   RCLCPP_PUBLIC
-  virtual
+
   bool
-  callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group);
+  callback_group_in_node(rclcpp::CallbackGroup::SharedPtr group) override;
 
   RCLCPP_PUBLIC
-  virtual
-  const std::vector<rclcpp::callback_group::CallbackGroup::WeakPtr> &
-  get_callback_groups() const;
+
+  const std::vector<rclcpp::CallbackGroup::WeakPtr> &
+  get_callback_groups() const override;
 
   RCLCPP_PUBLIC
-  virtual
+
   std::atomic_bool &
-  get_associated_with_executor_atomic();
+  get_associated_with_executor_atomic() override;
 
   RCLCPP_PUBLIC
-  virtual
+
   rcl_guard_condition_t *
-  get_notify_guard_condition();
+  get_notify_guard_condition() override;
 
   RCLCPP_PUBLIC
-  virtual
+
   std::unique_lock<std::recursive_mutex>
-  acquire_notify_guard_condition_lock() const;
+  acquire_notify_guard_condition_lock() const override;
 
   RCLCPP_PUBLIC
-  virtual
+
   bool
-  get_use_intra_process_default() const;
+  get_use_intra_process_default() const override;
+
+  bool
+  get_enable_topic_statistics_default() const override;
 
 private:
   RCLCPP_DISABLE_COPY(NodeBase)
 
   rclcpp::Context::SharedPtr context_;
   bool use_intra_process_default_;
+  bool enable_topic_statistics_default_;
 
   std::shared_ptr<rcl_node_t> node_handle_;
 
-  rclcpp::callback_group::CallbackGroup::SharedPtr default_callback_group_;
-  std::vector<rclcpp::callback_group::CallbackGroup::WeakPtr> callback_groups_;
+  rclcpp::CallbackGroup::SharedPtr default_callback_group_;
+  std::vector<rclcpp::CallbackGroup::WeakPtr> callback_groups_;
 
   std::atomic_bool associated_with_executor_;
 

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -111,25 +111,25 @@ public:
   /// Create and return a callback group.
   RCLCPP_PUBLIC
   virtual
-  rclcpp::callback_group::CallbackGroup::SharedPtr
-  create_callback_group(rclcpp::callback_group::CallbackGroupType group_type) = 0;
+  rclcpp::CallbackGroup::SharedPtr
+  create_callback_group(rclcpp::CallbackGroupType group_type) = 0;
 
   /// Return the default callback group.
   RCLCPP_PUBLIC
   virtual
-  rclcpp::callback_group::CallbackGroup::SharedPtr
+  rclcpp::CallbackGroup::SharedPtr
   get_default_callback_group() = 0;
 
   /// Return true if the given callback group is associated with this node.
   RCLCPP_PUBLIC
   virtual
   bool
-  callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0;
+  callback_group_in_node(rclcpp::CallbackGroup::SharedPtr group) = 0;
 
   /// Return list of callback groups associated with this node.
   RCLCPP_PUBLIC
   virtual
-  const std::vector<rclcpp::callback_group::CallbackGroup::WeakPtr> &
+  const std::vector<rclcpp::CallbackGroup::WeakPtr> &
   get_callback_groups() const = 0;
 
   /// Return the atomic bool which is used to ensure only one executor is used.
@@ -161,6 +161,12 @@ public:
   virtual
   bool
   get_use_intra_process_default() const = 0;
+
+  /// Return the default preference for enabling topic statistics collection.
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  get_enable_topic_statistics_default() const = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_clock.hpp

@@ -15,14 +15,14 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_
 
-#include "rclcpp/callback_group.hpp"
 #include "rclcpp/clock.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/node_interfaces/node_clock_interface.hpp"
-#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
+#include "rclcpp/node_interfaces/node_graph_interface.hpp"
+#include "rclcpp/node_interfaces/node_logging_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
-#include "rclcpp/time_source.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -50,9 +50,13 @@ public:
 
   /// Get a clock which will be kept up to date by the node.
   RCLCPP_PUBLIC
-  virtual
   rclcpp::Clock::SharedPtr
-  get_clock();
+  get_clock() override;
+
+  /// Get a clock which will be kept up to date by the node.
+  RCLCPP_PUBLIC
+  rclcpp::Clock::ConstSharedPtr
+  get_clock() const override;
 
 private:
   RCLCPP_DISABLE_COPY(NodeClock)

rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp

@@ -15,7 +15,6 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_
 
-#include "rclcpp/callback_group.hpp"
 #include "rclcpp/clock.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -40,6 +39,12 @@ public:
   virtual
   rclcpp::Clock::SharedPtr
   get_clock() = 0;
+
+  /// Get a const ROS clock which will be kept up to date by the node.
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::Clock::ConstSharedPtr
+  get_clock() const = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

@@ -30,6 +30,7 @@
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rmw/topic_endpoint_info_array.h"
 
 namespace rclcpp
 {
@@ -56,66 +57,78 @@ public:
   ~NodeGraph();
 
   RCLCPP_PUBLIC
-  virtual
+
   std::map<std::string, std::vector<std::string>>
-  get_topic_names_and_types(bool no_demangle = false) const;
+  get_topic_names_and_types(bool no_demangle = false) const override;
 
   RCLCPP_PUBLIC
-  virtual
+
   std::map<std::string, std::vector<std::string>>
-  get_service_names_and_types() const;
+  get_service_names_and_types() const override;
 
   RCLCPP_PUBLIC
-  virtual
+
   std::vector<std::string>
-  get_node_names() const;
+  get_node_names() const override;
 
   RCLCPP_PUBLIC
-  virtual
+
   std::vector<std::pair<std::string, std::string>>
-  get_node_names_and_namespaces() const;
+  get_node_names_and_namespaces() const override;
 
   RCLCPP_PUBLIC
-  virtual
+
   size_t
-  count_publishers(const std::string & topic_name) const;
+  count_publishers(const std::string & topic_name) const override;
 
   RCLCPP_PUBLIC
-  virtual
+
   size_t
-  count_subscribers(const std::string & topic_name) const;
+  count_subscribers(const std::string & topic_name) const override;
 
   RCLCPP_PUBLIC
-  virtual
+
   const rcl_guard_condition_t *
-  get_graph_guard_condition() const;
+  get_graph_guard_condition() const override;
 
   RCLCPP_PUBLIC
-  virtual
+
   void
-  notify_graph_change();
+  notify_graph_change() override;
 
   RCLCPP_PUBLIC
-  virtual
+
   void
-  notify_shutdown();
+  notify_shutdown() override;
 
   RCLCPP_PUBLIC
-  virtual
+
   rclcpp::Event::SharedPtr
-  get_graph_event();
+  get_graph_event() override;
 
   RCLCPP_PUBLIC
-  virtual
+
   void
   wait_for_graph_change(
     rclcpp::Event::SharedPtr event,
-    std::chrono::nanoseconds timeout);
+    std::chrono::nanoseconds timeout) override;
 
   RCLCPP_PUBLIC
-  virtual
+
   size_t
-  count_graph_users();
+  count_graph_users() override;
+
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_publishers_info_by_topic(
+    const std::string & topic_name,
+    bool no_mangle = false) const override;
+
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_subscriptions_info_by_topic(
+    const std::string & topic_name,
+    bool no_mangle = false) const override;
 
 private:
   RCLCPP_DISABLE_COPY(NodeGraph)

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

@@ -15,20 +15,120 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_
 
+#include <algorithm>
+#include <array>
 #include <chrono>
 #include <map>
 #include <string>
 #include <utility>
 #include <vector>
 
+#include "rcl/graph.h"
 #include "rcl/guard_condition.h"
 
 #include "rclcpp/event.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
+
+enum class EndpointType
+{
+  Invalid = RMW_ENDPOINT_INVALID,
+  Publisher = RMW_ENDPOINT_PUBLISHER,
+  Subscription = RMW_ENDPOINT_SUBSCRIPTION
+};
+
+/**
+ * Struct that contains topic endpoint information like the associated node name, node namespace,
+ * topic type, endpoint type, endpoint GID, and its QoS.
+ */
+class TopicEndpointInfo
+{
+public:
+  /// Construct a TopicEndpointInfo from a rcl_topic_endpoint_info_t.
+  RCLCPP_PUBLIC
+  explicit TopicEndpointInfo(const rcl_topic_endpoint_info_t & info)
+  : node_name_(info.node_name),
+    node_namespace_(info.node_namespace),
+    topic_type_(info.topic_type),
+    endpoint_type_(static_cast<rclcpp::EndpointType>(info.endpoint_type)),
+    qos_profile_({info.qos_profile.history, info.qos_profile.depth}, info.qos_profile)
+  {
+    std::copy(info.endpoint_gid, info.endpoint_gid + RMW_GID_STORAGE_SIZE, endpoint_gid_.begin());
+  }
+
+  /// Get a mutable reference to the node name.
+  RCLCPP_PUBLIC
+  std::string &
+  node_name();
+
+  /// Get a const reference to the node name.
+  RCLCPP_PUBLIC
+  const std::string &
+  node_name() const;
+
+  /// Get a mutable reference to the node namespace.
+  RCLCPP_PUBLIC
+  std::string &
+  node_namespace();
+
+  /// Get a const reference to the node namespace.
+  RCLCPP_PUBLIC
+  const std::string &
+  node_namespace() const;
+
+  /// Get a mutable reference to the topic type string.
+  RCLCPP_PUBLIC
+  std::string &
+  topic_type();
+
+  /// Get a const reference to the topic type string.
+  RCLCPP_PUBLIC
+  const std::string &
+  topic_type() const;
+
+  /// Get a mutable reference to the topic endpoint type.
+  RCLCPP_PUBLIC
+  rclcpp::EndpointType &
+  endpoint_type();
+
+  /// Get a const reference to the topic endpoint type.
+  RCLCPP_PUBLIC
+  const rclcpp::EndpointType &
+  endpoint_type() const;
+
+  /// Get a mutable reference to the GID of the topic endpoint.
+  RCLCPP_PUBLIC
+  std::array<uint8_t, RMW_GID_STORAGE_SIZE> &
+  endpoint_gid();
+
+  /// Get a const reference to the GID of the topic endpoint.
+  RCLCPP_PUBLIC
+  const std::array<uint8_t, RMW_GID_STORAGE_SIZE> &
+  endpoint_gid() const;
+
+  /// Get a mutable reference to the QoS profile of the topic endpoint.
+  RCLCPP_PUBLIC
+  rclcpp::QoS &
+  qos_profile();
+
+  /// Get a const reference to the QoS profile of the topic endpoint.
+  RCLCPP_PUBLIC
+  const rclcpp::QoS &
+  qos_profile() const;
+
+private:
+  std::string node_name_;
+  std::string node_namespace_;
+  std::string topic_type_;
+  rclcpp::EndpointType endpoint_type_;
+  std::array<uint8_t, RMW_GID_STORAGE_SIZE> endpoint_gid_;
+  rclcpp::QoS qos_profile_;
+};
+
 namespace node_interfaces
 {
 
@@ -150,6 +250,24 @@ public:
   virtual
   size_t
   count_graph_users() = 0;
+
+  /// Return the topic endpoint information about publishers on a given topic.
+  /**
+   * \sa rclcpp::Node::get_publishers_info_by_topic
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_publishers_info_by_topic(const std::string & topic_name, bool no_mangle = false) const = 0;
+
+  /// Return the topic endpoint information about subscriptions on a given topic.
+  /**
+   * \sa rclcpp::Node::get_subscriptions_info_by_topic
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_subscriptions_info_by_topic(const std::string & topic_name, bool no_mangle = false) const = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_logging.hpp

@@ -42,14 +42,14 @@ public:
   ~NodeLogging();
 
   RCLCPP_PUBLIC
-  virtual
+
   rclcpp::Logger
-  get_logger() const;
+  get_logger() const override;
 
   RCLCPP_PUBLIC
-  virtual
+
   const char *
-  get_logger_name() const;
+  get_logger_name() const override;
 
 private:
   RCLCPP_DISABLE_COPY(NodeLogging)

rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp

@@ -19,7 +19,6 @@
 
 #include "rclcpp/logger.hpp"
 #include "rclcpp/macros.hpp"
-#include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -28,6 +28,7 @@
 
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_logging_interface.hpp"
 #include "rclcpp/node_interfaces/node_parameters_interface.hpp"
 #include "rclcpp/node_interfaces/node_services_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"

rclcpp/include/rclcpp/node_interfaces/node_services.hpp

@@ -42,18 +42,18 @@ public:
   ~NodeServices();
 
   RCLCPP_PUBLIC
-  virtual
+
   void
   add_client(
     rclcpp::ClientBase::SharedPtr client_base_ptr,
-    rclcpp::callback_group::CallbackGroup::SharedPtr group);
+    rclcpp::CallbackGroup::SharedPtr group) override;
 
   RCLCPP_PUBLIC
-  virtual
+
   void
   add_service(
     rclcpp::ServiceBase::SharedPtr service_base_ptr,
-    rclcpp::callback_group::CallbackGroup::SharedPtr group);
+    rclcpp::CallbackGroup::SharedPtr group) override;
 
 private:
   RCLCPP_DISABLE_COPY(NodeServices)

rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp

@@ -41,14 +41,14 @@ public:
   void
   add_client(
     rclcpp::ClientBase::SharedPtr client_base_ptr,
-    rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0;
+    rclcpp::CallbackGroup::SharedPtr group) = 0;
 
   RCLCPP_PUBLIC
   virtual
   void
   add_service(
     rclcpp::ServiceBase::SharedPtr service_base_ptr,
-    rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0;
+    rclcpp::CallbackGroup::SharedPtr group) = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp

@@ -15,12 +15,13 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_HPP_
 
-#include "rclcpp/callback_group.hpp"
-#include "rclcpp/clock.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/node_interfaces/node_clock_interface.hpp"
+#include "rclcpp/node_interfaces/node_graph_interface.hpp"
+#include "rclcpp/node_interfaces/node_logging_interface.hpp"
 #include "rclcpp/node_interfaces/node_parameters_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
 #include "rclcpp/node_interfaces/node_time_source_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
 #include "rclcpp/time_source.hpp"

rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp

@@ -15,8 +15,6 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_
 
-#include "rclcpp/callback_group.hpp"
-#include "rclcpp/clock.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/visibility_control.hpp"
 

rclcpp/include/rclcpp/node_interfaces/node_timers.hpp

@@ -42,11 +42,11 @@ public:
 
   /// Add a timer to the node.
   RCLCPP_PUBLIC
-  virtual
+
   void
   add_timer(
     rclcpp::TimerBase::SharedPtr timer,
-    rclcpp::callback_group::CallbackGroup::SharedPtr callback_group);
+    rclcpp::CallbackGroup::SharedPtr callback_group) override;
 
 private:
   RCLCPP_DISABLE_COPY(NodeTimers)

rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp

@@ -41,7 +41,7 @@ public:
   void
   add_timer(
     rclcpp::TimerBase::SharedPtr timer,
-    rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0;
+    rclcpp::CallbackGroup::SharedPtr callback_group) = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_topics.hpp

@@ -22,6 +22,7 @@
 
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
 #include "rclcpp/publisher.hpp"
 #include "rclcpp/publisher_factory.hpp"
@@ -39,7 +40,9 @@ public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTopicsInterface)
 
   RCLCPP_PUBLIC
-  explicit NodeTopics(rclcpp::node_interfaces::NodeBaseInterface * node_base);
+  NodeTopics(
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    rclcpp::node_interfaces::NodeTimersInterface * node_timers);
 
   RCLCPP_PUBLIC
   ~NodeTopics() override;
@@ -55,7 +58,7 @@ public:
   void
   add_publisher(
     rclcpp::PublisherBase::SharedPtr publisher,
-    rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) override;
+    rclcpp::CallbackGroup::SharedPtr callback_group) override;
 
   RCLCPP_PUBLIC
   rclcpp::SubscriptionBase::SharedPtr
@@ -68,16 +71,21 @@ public:
   void
   add_subscription(
     rclcpp::SubscriptionBase::SharedPtr subscription,
-    rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) override;
+    rclcpp::CallbackGroup::SharedPtr callback_group) override;
 
   RCLCPP_PUBLIC
   rclcpp::node_interfaces::NodeBaseInterface *
   get_node_base_interface() const override;
 
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeTimersInterface *
+  get_node_timers_interface() const override;
+
 private:
   RCLCPP_DISABLE_COPY(NodeTopics)
 
   rclcpp::node_interfaces::NodeBaseInterface * node_base_;
+  rclcpp::node_interfaces::NodeTimersInterface * node_timers_;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp

@@ -22,7 +22,10 @@
 #include "rcl/publisher.h"
 #include "rcl/subscription.h"
 
+#include "rclcpp/callback_group.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/publisher.hpp"
 #include "rclcpp/publisher_factory.hpp"
 #include "rclcpp/subscription.hpp"
@@ -57,7 +60,7 @@ public:
   void
   add_publisher(
     rclcpp::PublisherBase::SharedPtr publisher,
-    rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0;
+    rclcpp::CallbackGroup::SharedPtr callback_group) = 0;
 
   RCLCPP_PUBLIC
   virtual
@@ -72,12 +75,17 @@ public:
   void
   add_subscription(
     rclcpp::SubscriptionBase::SharedPtr subscription,
-    rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0;
+    rclcpp::CallbackGroup::SharedPtr callback_group) = 0;
 
   RCLCPP_PUBLIC
   virtual
   rclcpp::node_interfaces::NodeBaseInterface *
   get_node_base_interface() const = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::node_interfaces::NodeTimersInterface *
+  get_node_timers_interface() const = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp

@@ -16,7 +16,6 @@
 #define RCLCPP__NODE_INTERFACES__NODE_WAITABLES_HPP_
 
 #include "rclcpp/callback_group.hpp"
-#include "rclcpp/client.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/node_interfaces/node_waitables_interface.hpp"
@@ -42,18 +41,18 @@ public:
   ~NodeWaitables();
 
   RCLCPP_PUBLIC
-  virtual
+
   void
   add_waitable(
     rclcpp::Waitable::SharedPtr waitable_base_ptr,
-    rclcpp::callback_group::CallbackGroup::SharedPtr group);
+    rclcpp::CallbackGroup::SharedPtr group) override;
 
   RCLCPP_PUBLIC
-  virtual
+
   void
   remove_waitable(
     rclcpp::Waitable::SharedPtr waitable_ptr,
-    rclcpp::callback_group::CallbackGroup::SharedPtr group) noexcept;
+    rclcpp::CallbackGroup::SharedPtr group) noexcept override;
 
 private:
   RCLCPP_DISABLE_COPY(NodeWaitables)

rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp

@@ -40,7 +40,7 @@ public:
   void
   add_waitable(
     rclcpp::Waitable::SharedPtr waitable_ptr,
-    rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0;
+    rclcpp::CallbackGroup::SharedPtr group) = 0;
 
   /// \note this function should not throw because it may be called in destructors
   RCLCPP_PUBLIC
@@ -48,7 +48,7 @@ public:
   void
   remove_waitable(
     rclcpp::Waitable::SharedPtr waitable_ptr,
-    rclcpp::callback_group::CallbackGroup::SharedPtr group) noexcept = 0;
+    rclcpp::CallbackGroup::SharedPtr group) noexcept = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_options.hpp

@@ -38,11 +38,12 @@ public:
   /**
    * Default values for the node options:
    *
-   *   - context = rclcpp::contexts::default_context::get_global_default_context()
+   *   - context = rclcpp::contexts::get_global_default_context()
    *   - arguments = {}
    *   - parameter_overrides = {}
    *   - use_global_arguments = true
    *   - use_intra_process_comms = false
+   *   - enable_topic_statistics = false
    *   - start_parameter_services = true
    *   - start_parameter_event_publisher = true
    *   - parameter_event_qos = rclcpp::ParameterEventQoS
@@ -152,6 +153,22 @@ public:
   NodeOptions &
   use_global_arguments(bool use_global_arguments);
 
+  /// Return the enable_rosout flag.
+  RCLCPP_PUBLIC
+  bool
+  enable_rosout() const;
+
+  /// Set the enable_rosout flag, return this for parameter idiom.
+  /**
+   * If false this will cause the node not to use rosout logging.
+   *
+   * Defaults to true for now, as there are still some cases where it is
+   * desirable.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  enable_rosout(bool enable_rosout);
+
   /// Return the use_intra_process_comms flag.
   RCLCPP_PUBLIC
   bool
@@ -171,6 +188,23 @@ public:
   NodeOptions &
   use_intra_process_comms(bool use_intra_process_comms);
 
+  /// Return the enable_topic_statistics flag.
+  RCLCPP_PUBLIC
+  bool
+  enable_topic_statistics() const;
+
+  /// Set the enable_topic_statistics flag, return this for parameter idiom.
+  /**
+   * If true, topic statistics collection and publication will be enabled
+   * for all subscriptions.
+   * This can be used to override the global topic statistics setting.
+   *
+   * Defaults to false.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  enable_topic_statistics(bool enable_topic_statistics);
+
   /// Return the start_parameter_services flag.
   RCLCPP_PUBLIC
   bool
@@ -304,7 +338,7 @@ private:
   // documentation in this class.
 
   rclcpp::Context::SharedPtr context_ {
-    rclcpp::contexts::default_context::get_global_default_context()};
+    rclcpp::contexts::get_global_default_context()};
 
   std::vector<std::string> arguments_ {};
 
@@ -312,8 +346,12 @@ private:
 
   bool use_global_arguments_ {true};
 
+  bool enable_rosout_ {true};
+
   bool use_intra_process_comms_ {false};
 
+  bool enable_topic_statistics_ {false};
+
   bool start_parameter_services_ {true};
 
   bool start_parameter_event_publisher_ {true};

rclcpp/include/rclcpp/parameter.hpp

@@ -22,6 +22,7 @@
 #include <vector>
 
 #include "rcl_interfaces/msg/parameter.hpp"
+#include "rclcpp/exceptions.hpp"
 #include "rclcpp/parameter_value.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -221,8 +222,12 @@ template<typename T>
 decltype(auto)
 Parameter::get_value() const
 {
-  // use the helper to specialize for the ParameterValue and Parameter cases.
-  return detail::get_value_helper<T>(this);
+  try {
+    // use the helper to specialize for the ParameterValue and Parameter cases.
+    return detail::get_value_helper<T>(this);
+  } catch (const ParameterTypeException & ex) {
+    throw exceptions::InvalidParameterTypeException(this->name_, ex.what());
+  }
 }
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/parameter_client.hpp

@@ -54,21 +54,21 @@ public:
     const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
     const std::string & remote_node_name = "",
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters,
-    rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
+    rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
   RCLCPP_PUBLIC
   AsyncParametersClient(
     const rclcpp::Node::SharedPtr node,
     const std::string & remote_node_name = "",
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters,
-    rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
+    rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
   RCLCPP_PUBLIC
   AsyncParametersClient(
     rclcpp::Node * node,
     const std::string & remote_node_name = "",
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters,
-    rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
+    rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
   RCLCPP_PUBLIC
   std::shared_future<std::vector<rclcpp::Parameter>>
@@ -205,7 +205,7 @@ public:
 
   RCLCPP_PUBLIC
   SyncParametersClient(
-    rclcpp::executor::Executor::SharedPtr executor,
+    rclcpp::Executor::SharedPtr executor,
     rclcpp::Node::SharedPtr node,
     const std::string & remote_node_name = "",
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
@@ -218,14 +218,14 @@ public:
 
   RCLCPP_PUBLIC
   SyncParametersClient(
-    rclcpp::executor::Executor::SharedPtr executor,
+    rclcpp::Executor::SharedPtr executor,
     rclcpp::Node * node,
     const std::string & remote_node_name = "",
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
   RCLCPP_PUBLIC
   SyncParametersClient(
-    rclcpp::executor::Executor::SharedPtr executor,
+    rclcpp::Executor::SharedPtr executor,
     const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
     const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
     const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
@@ -271,10 +271,11 @@ public:
   {
     return get_parameter_impl(
       parameter_name,
-      std::function<T()>([&parameter_name]() -> T
-      {
-        throw std::runtime_error("Parameter '" + parameter_name + "' is not set");
-      })
+      std::function<T()>(
+        [&parameter_name]() -> T
+        {
+          throw std::runtime_error("Parameter '" + parameter_name + "' is not set");
+        })
     );
   }
 
@@ -338,7 +339,7 @@ public:
   }
 
 private:
-  rclcpp::executor::Executor::SharedPtr executor_;
+  rclcpp::Executor::SharedPtr executor_;
   const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_;
   AsyncParametersClient::SharedPtr async_parameters_client_;
 };

rclcpp/include/rclcpp/publisher.hpp

@@ -84,7 +84,22 @@ public:
         options_.event_callbacks.liveliness_callback,
         RCL_PUBLISHER_LIVELINESS_LOST);
     }
-
+    if (options_.event_callbacks.incompatible_qos_callback) {
+      this->add_event_handler(
+        options_.event_callbacks.incompatible_qos_callback,
+        RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
+    } else if (options_.use_default_callbacks) {
+      // Register default callback when not specified
+      try {
+        this->add_event_handler(
+          [this](QOSOfferedIncompatibleQoSInfo & info) {
+            this->default_incompatible_qos_callback(info);
+          },
+          RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
+      } catch (UnsupportedEventTypeException & /*exc*/) {
+        // pass
+      }
+    }
     // Setup continues in the post construction method, post_init_setup().
   }
 
@@ -202,6 +217,12 @@ public:
     return this->do_serialized_publish(&serialized_msg);
   }
 
+  void
+  publish(const SerializedMessage & serialized_msg)
+  {
+    return this->do_serialized_publish(&serialized_msg.get_rcl_serialized_message());
+  }
+
   /// Publish an instance of a LoanedMessage.
   /**
    * When publishing a loaned message, the memory for this ROS message will be deallocated

rclcpp/include/rclcpp/publisher_base.hpp

@@ -208,6 +208,9 @@ protected:
     event_handlers_.emplace_back(handler);
   }
 
+  RCLCPP_PUBLIC
+  void default_incompatible_qos_callback(QOSOfferedIncompatibleQoSInfo & info) const;
+
   std::shared_ptr<rcl_node_t> rcl_node_handle_;
 
   rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher();

rclcpp/include/rclcpp/publisher_options.hpp

@@ -30,10 +30,7 @@
 namespace rclcpp
 {
 
-namespace callback_group
-{
 class CallbackGroup;
-}  // namespace callback_group
 
 /// Non-templated part of PublisherOptionsWithAllocator<Allocator>.
 struct PublisherOptionsBase
@@ -44,8 +41,11 @@ struct PublisherOptionsBase
   /// Callbacks for various events related to publishers.
   PublisherEventCallbacks event_callbacks;
 
+  /// Whether or not to use default callbacks when user doesn't supply any in event_callbacks
+  bool use_default_callbacks = true;
+
   /// Callback group in which the waitable items from the publisher should be placed.
-  std::shared_ptr<rclcpp::callback_group::CallbackGroup> callback_group;
+  std::shared_ptr<rclcpp::CallbackGroup> callback_group;
 
   /// Optional RMW implementation specific payload to be used during creation of the publisher.
   std::shared_ptr<rclcpp::detail::RMWImplementationSpecificPublisherPayload>

rclcpp/include/rclcpp/qos.hpp

@@ -15,15 +15,19 @@
 #ifndef RCLCPP__QOS_HPP_
 #define RCLCPP__QOS_HPP_
 
-#include <rmw/qos_profiles.h>
-#include <rmw/types.h>
+#include <string>
 
 #include "rclcpp/duration.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rmw/incompatible_qos_events_statuses.h"
+#include "rmw/qos_profiles.h"
+#include "rmw/types.h"
 
 namespace rclcpp
 {
 
+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.
 struct RCLCPP_PUBLIC QoSInitialization
 {
@@ -151,6 +155,12 @@ private:
   rmw_qos_profile_t rmw_qos_profile_;
 };
 
+/// Check if two QoS profiles are exactly equal in all policy values.
+RCLCPP_PUBLIC
+bool operator==(const QoS & left, const QoS & right);
+RCLCPP_PUBLIC
+bool operator!=(const QoS & left, const QoS & right);
+
 class RCLCPP_PUBLIC SensorDataQoS : public QoS
 {
 public:

rclcpp/include/rclcpp/qos_event.hpp

@@ -16,8 +16,10 @@
 #define RCLCPP__QOS_EVENT_HPP_
 
 #include <functional>
+#include <string>
 
 #include "rcl/error_handling.h"
+#include "rmw/incompatible_qos_events_statuses.h"
 
 #include "rcutils/logging_macros.h"
 
@@ -32,17 +34,23 @@ using QOSDeadlineRequestedInfo = rmw_requested_deadline_missed_status_t;
 using QOSDeadlineOfferedInfo = rmw_offered_deadline_missed_status_t;
 using QOSLivelinessChangedInfo = rmw_liveliness_changed_status_t;
 using QOSLivelinessLostInfo = rmw_liveliness_lost_status_t;
+using QOSOfferedIncompatibleQoSInfo = rmw_offered_qos_incompatible_event_status_t;
+using QOSRequestedIncompatibleQoSInfo = rmw_requested_qos_incompatible_event_status_t;
 
 using QOSDeadlineRequestedCallbackType = std::function<void (QOSDeadlineRequestedInfo &)>;
 using QOSDeadlineOfferedCallbackType = std::function<void (QOSDeadlineOfferedInfo &)>;
 using QOSLivelinessChangedCallbackType = std::function<void (QOSLivelinessChangedInfo &)>;
 using QOSLivelinessLostCallbackType = std::function<void (QOSLivelinessLostInfo &)>;
+using QOSOfferedIncompatibleQoSCallbackType = std::function<void (QOSOfferedIncompatibleQoSInfo &)>;
+using QOSRequestedIncompatibleQoSCallbackType =
+  std::function<void (QOSRequestedIncompatibleQoSInfo &)>;
 
 /// Contains callbacks for various types of events a Publisher can receive from the middleware.
 struct PublisherEventCallbacks
 {
   QOSDeadlineOfferedCallbackType deadline_callback;
   QOSLivelinessLostCallbackType liveliness_callback;
+  QOSOfferedIncompatibleQoSCallbackType incompatible_qos_callback;
 };
 
 /// Contains callbacks for non-message events that a Subscription can receive from the middleware.
@@ -50,6 +58,22 @@ struct SubscriptionEventCallbacks
 {
   QOSDeadlineRequestedCallbackType deadline_callback;
   QOSLivelinessChangedCallbackType liveliness_callback;
+  QOSRequestedIncompatibleQoSCallbackType incompatible_qos_callback;
+};
+
+class UnsupportedEventTypeException : public exceptions::RCLErrorBase, public std::runtime_error
+{
+public:
+  RCLCPP_PUBLIC
+  UnsupportedEventTypeException(
+    rcl_ret_t ret,
+    const rcl_error_state_t * error_state,
+    const std::string & prefix);
+
+  RCLCPP_PUBLIC
+  UnsupportedEventTypeException(
+    const exceptions::RCLErrorBase & base_exc,
+    const std::string & prefix);
 };
 
 class QOSEventHandlerBase : public Waitable
@@ -93,7 +117,13 @@ public:
     event_handle_ = rcl_get_zero_initialized_event();
     rcl_ret_t ret = init_func(&event_handle_, parent_handle, event_type);
     if (ret != RCL_RET_OK) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "could not create event");
+      if (ret == RCL_RET_UNSUPPORTED) {
+        UnsupportedEventTypeException exc(ret, rcl_get_error_state(), "Failed to initialize event");
+        rcl_reset_error();
+        throw exc;
+      } else {
+        rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to initialize event");
+      }
     }
   }
 

rclcpp/include/rclcpp/rclcpp.hpp

@@ -81,7 +81,7 @@
  *   - rclcpp/executors/multi_threaded_executor.hpp
  * - CallbackGroups (mechanism for enforcing concurrency rules for callbacks):
  *   - rclcpp::Node::create_callback_group()
- *   - rclcpp::callback_group::CallbackGroup
+ *   - rclcpp::CallbackGroup
  *   - rclcpp/callback_group.hpp
  *
  * Additionally, there are some methods for introspecting the ROS graph:
@@ -143,6 +143,7 @@
 #include <memory>
 
 #include "rclcpp/executors.hpp"
+#include "rclcpp/guard_condition.hpp"
 #include "rclcpp/logging.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/parameter.hpp"
@@ -152,6 +153,7 @@
 #include "rclcpp/time.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/wait_set.hpp"
 #include "rclcpp/waitable.hpp"
 
 #endif  // RCLCPP__RCLCPP_HPP_

rclcpp/include/rclcpp/serialization.hpp

@@ -0,0 +1,98 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__SERIALIZATION_HPP_
+#define RCLCPP__SERIALIZATION_HPP_
+
+#include <memory>
+#include <stdexcept>
+#include <string>
+
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcl/types.h"
+
+#include "rosidl_runtime_c/message_type_support_struct.h"
+
+#include "rosidl_typesupport_cpp/message_type_support.hpp"
+
+namespace rclcpp
+{
+
+class SerializedMessage;
+
+namespace serialization_traits
+{
+// trait to check if type is the object oriented serialized message
+template<typename T>
+struct is_serialized_message_class : std::false_type
+{};
+
+template<>
+struct is_serialized_message_class<SerializedMessage>: std::true_type
+{};
+}  // namespace serialization_traits
+
+/// Interface to (de)serialize a message
+class RCLCPP_PUBLIC_TYPE SerializationBase
+{
+public:
+  /// Constructor of SerializationBase
+  /**
+   * \param[in] type_support handle for the message type support
+   * to be used for serialization and deserialization.
+   */
+  explicit SerializationBase(const rosidl_message_type_support_t * type_support);
+
+  /// Destructor of SerializationBase
+  virtual ~SerializationBase() = default;
+
+  /// Serialize a ROS2 message to a serialized stream
+  /**
+   * \param[in] message The ROS2 message which is read and serialized by rmw.
+   * \param[out] serialized_message The serialized message.
+   */
+  void serialize_message(
+    const void * ros_message, SerializedMessage * serialized_message) const;
+
+  /// 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.
+   */
+  void deserialize_message(
+    const SerializedMessage * serialized_message, void * ros_message) const;
+
+private:
+  const rosidl_message_type_support_t * type_support_;
+};
+
+/// Default implementation to (de)serialize a message by using rmw_(de)serialize
+template<typename MessageT>
+class Serialization : public SerializationBase
+{
+public:
+  /// Constructor of Serialization
+  Serialization()
+  : SerializationBase(rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>())
+  {
+    static_assert(
+      !serialization_traits::is_serialized_message_class<MessageT>::value,
+      "Serialization of serialized message to serialized message is not possible.");
+  }
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__SERIALIZATION_HPP_

rclcpp/include/rclcpp/serialized_message.hpp

@@ -0,0 +1,121 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__SERIALIZED_MESSAGE_HPP_
+#define RCLCPP__SERIALIZED_MESSAGE_HPP_
+
+#include "rcl/allocator.h"
+#include "rcl/types.h"
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Object oriented version of rcl_serialized_message_t with destructor to avoid memory leaks
+class RCLCPP_PUBLIC_TYPE SerializedMessage
+{
+public:
+  /// Default constructor for a SerializedMessage
+  /**
+   * Default constructs a serialized message and initalizes it
+   * with initial capacity of 0.
+   * The allocator defaults to `rcl_get_default_allocator()`.
+   *
+   * \param[in] allocator The allocator to be used for the initialization.
+   */
+  explicit SerializedMessage(
+    const rcl_allocator_t & allocator = rcl_get_default_allocator());
+
+  /// Default constructor for a SerializedMessage
+  /**
+   * Default constructs a serialized message and initalizes it
+   * with the provided capacity.
+   * The allocator defaults to `rcl_get_default_allocator()`.
+   *
+   * \param[in] initial_capacity The amount of memory to be allocated.
+   * \param[in] allocator The allocator to be used for the initialization.
+   */
+  SerializedMessage(
+    size_t initial_capacity,
+    const rcl_allocator_t & allocator = rcl_get_default_allocator());
+
+  /// Copy Constructor for a SerializedMessage
+  SerializedMessage(const SerializedMessage & other);
+
+  /// Constructor for a SerializedMessage from a rcl_serialized_message_t
+  explicit SerializedMessage(const rcl_serialized_message_t & other);
+
+  /// Move Constructor for a SerializedMessage
+  SerializedMessage(SerializedMessage && other);
+
+  /// Constructor for a SerializedMessage from a moved rcl_serialized_message_t
+  explicit SerializedMessage(rcl_serialized_message_t && other);
+
+  /// Copy assignment operator
+  SerializedMessage & operator=(const SerializedMessage & other);
+
+  /// Copy assignment operator from a rcl_serialized_message_t
+  SerializedMessage & operator=(const rcl_serialized_message_t & other);
+
+  /// Move assignment operator
+  SerializedMessage & operator=(SerializedMessage && other);
+
+  /// Move assignment operator from a rcl_serialized_message_t
+  SerializedMessage & operator=(rcl_serialized_message_t && other);
+
+  /// Destructor for a SerializedMessage
+  virtual ~SerializedMessage();
+
+  /// Get the underlying rcl_serialized_t handle
+  rcl_serialized_message_t & get_rcl_serialized_message();
+
+  // Get a const handle to the underlying rcl_serialized_message_t
+  const rcl_serialized_message_t & get_rcl_serialized_message() const;
+
+  /// Get the size of the serialized data buffer
+  /**
+   * Note, this is different from the actual amount of allocated memory.
+   * This can be obtained via a call to `capacity`.
+   */
+  size_t size() const;
+
+  /// Get the size of allocated memory for the data buffer
+  /**
+   * Note, this is different from the amount of content in the buffer.
+   * This can be obtained via a call to `size`.
+   */
+  size_t capacity() const;
+
+  /// Allocate memory in the data buffer
+  /**
+   * The data buffer of the underlying rcl_serialized_message_t will be resized.
+   * This might change the data layout and invalidates all pointers to the data.
+   */
+  void reserve(size_t capacity);
+
+  /// Release the underlying rcl_serialized_message_t
+  /**
+   * The memory (i.e. the data buffer) of the serialized message will no longer
+   * be managed by this instance and the memory won't be deallocated on destruction.
+   */
+  rcl_serialized_message_t release_rcl_serialized_message();
+
+private:
+  rcl_serialized_message_t serialized_message_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__SERIALIZED_MESSAGE_HPP_

rclcpp/include/rclcpp/service.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__SERVICE_HPP_
 #define RCLCPP__SERVICE_HPP_
 
+#include <atomic>
 #include <functional>
 #include <iostream>
 #include <memory>
@@ -44,8 +45,7 @@ public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase)
 
   RCLCPP_PUBLIC
-  explicit ServiceBase(
-    std::shared_ptr<rcl_node_t> node_handle);
+  explicit ServiceBase(std::shared_ptr<rcl_node_t> node_handle);
 
   RCLCPP_PUBLIC
   virtual ~ServiceBase();
@@ -62,12 +62,53 @@ public:
   std::shared_ptr<const rcl_service_t>
   get_service_handle() const;
 
-  virtual std::shared_ptr<void> create_request() = 0;
-  virtual std::shared_ptr<rmw_request_id_t> create_request_header() = 0;
-  virtual void handle_request(
+  /// Take the next request from the service as a type erased pointer.
+  /**
+   * This type erased version of \sa Service::take_request() is useful when
+   * using the service in a type agnostic way with methods like
+   * ServiceBase::create_request(), ServiceBase::create_request_header(), and
+   * ServiceBase::handle_request().
+   *
+   * \param[out] request_out The type erased pointer to a service request object
+   *   into which the middleware will copy the taken request.
+   * \param[out] request_id_out The output id for the request which can be used
+   *   to associate response with this request in the future.
+   * \returns true if the request was taken, otherwise false.
+   * \throws rclcpp::exceptions::RCLError based exceptions if the underlying
+   *   rcl calls fail.
+   */
+  RCLCPP_PUBLIC
+  bool
+  take_type_erased_request(void * request_out, rmw_request_id_t & request_id_out);
+
+  virtual
+  std::shared_ptr<void>
+  create_request() = 0;
+
+  virtual
+  std::shared_ptr<rmw_request_id_t>
+  create_request_header() = 0;
+
+  virtual
+  void
+  handle_request(
     std::shared_ptr<rmw_request_id_t> request_header,
     std::shared_ptr<void> request) = 0;
 
+  /// Exchange the "in use by wait set" state for this service.
+  /**
+   * This is used to ensure this service is not used by multiple
+   * wait sets at the same time.
+   *
+   * \param[in] in_use_state the new state to exchange into the state, true
+   *   indicates it is now in use by a wait set, and false is that it is no
+   *   longer in use by a wait set.
+   * \returns the previous state.
+   */
+  RCLCPP_PUBLIC
+  bool
+  exchange_in_use_by_wait_set_state(bool in_use_state);
+
 protected:
   RCLCPP_DISABLE_COPY(ServiceBase)
 
@@ -83,6 +124,8 @@ protected:
 
   std::shared_ptr<rcl_service_t> service_handle_;
   bool owns_rcl_handle_ = true;
+
+  std::atomic<bool> in_use_by_wait_set_{false};
 };
 
 template<typename ServiceT>
@@ -159,6 +202,9 @@ public:
       rclcpp_service_callback_added,
       (const void *)get_service_handle().get(),
       (const void *)&any_callback_);
+#ifndef TRACETOOLS_DISABLED
+    any_callback_.register_callback_for_tracing();
+#endif
   }
 
   Service(
@@ -181,6 +227,9 @@ public:
       rclcpp_service_callback_added,
       (const void *)get_service_handle().get(),
       (const void *)&any_callback_);
+#ifndef TRACETOOLS_DISABLED
+    any_callback_.register_callback_for_tracing();
+#endif
   }
 
   Service(
@@ -205,6 +254,9 @@ public:
       rclcpp_service_callback_added,
       (const void *)get_service_handle().get(),
       (const void *)&any_callback_);
+#ifndef TRACETOOLS_DISABLED
+    any_callback_.register_callback_for_tracing();
+#endif
   }
 
   Service() = delete;
@@ -213,36 +265,63 @@ public:
   {
   }
 
-  std::shared_ptr<void> create_request()
+  /// Take the next request from the service.
+  /**
+   * \sa ServiceBase::take_type_erased_request().
+   *
+   * \param[out] request_out The reference to a service request object
+   *   into which the middleware will copy the taken request.
+   * \param[out] request_id_out The output id for the request which can be used
+   *   to associate response with this request in the future.
+   * \returns true if the request was taken, otherwise false.
+   * \throws rclcpp::exceptions::RCLError based exceptions if the underlying
+   *   rcl calls fail.
+   */
+  bool
+  take_request(typename ServiceT::Request & request_out, rmw_request_id_t & request_id_out)
   {
-    return std::shared_ptr<void>(new typename ServiceT::Request());
+    return this->take_type_erased_request(&request_out, request_id_out);
   }
 
-  std::shared_ptr<rmw_request_id_t> create_request_header()
+  std::shared_ptr<void>
+  create_request() override
   {
-    // TODO(wjwwood): This should probably use rmw_request_id's allocator.
-    //                (since it is a C type)
-    return std::shared_ptr<rmw_request_id_t>(new rmw_request_id_t);
+    return std::make_shared<typename ServiceT::Request>();
   }
 
-  void handle_request(
+  std::shared_ptr<rmw_request_id_t>
+  create_request_header() override
+  {
+    return std::make_shared<rmw_request_id_t>();
+  }
+
+  void
+  handle_request(
     std::shared_ptr<rmw_request_id_t> request_header,
-    std::shared_ptr<void> request)
+    std::shared_ptr<void> request) override
   {
     auto typed_request = std::static_pointer_cast<typename ServiceT::Request>(request);
-    auto response = std::shared_ptr<typename ServiceT::Response>(new typename ServiceT::Response);
+    auto response = std::make_shared<typename ServiceT::Response>();
     any_callback_.dispatch(request_header, typed_request, response);
-    send_response(request_header, response);
+    send_response(*request_header, *response);
   }
 
-  void send_response(
+  [[deprecated("use the send_response() which takes references instead of shared pointers")]]
+  void
+  send_response(
     std::shared_ptr<rmw_request_id_t> req_id,
     std::shared_ptr<typename ServiceT::Response> response)
   {
-    rcl_ret_t status = rcl_send_response(get_service_handle().get(), req_id.get(), response.get());
+    send_response(*req_id, *response);
+  }
 
-    if (status != RCL_RET_OK) {
-      rclcpp::exceptions::throw_from_rcl_error(status, "failed to send response");
+  void
+  send_response(rmw_request_id_t & req_id, typename ServiceT::Response & response)
+  {
+    rcl_ret_t ret = rcl_send_response(get_service_handle().get(), &req_id, &response);
+
+    if (ret != RCL_RET_OK) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send response");
     }
   }
 

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -61,7 +61,7 @@ public:
     allocator_ = std::make_shared<VoidAlloc>();
   }
 
-  void add_guard_condition(const rcl_guard_condition_t * guard_condition)
+  void add_guard_condition(const rcl_guard_condition_t * guard_condition) override
   {
     for (const auto & existing_guard_condition : guard_conditions_) {
       if (existing_guard_condition == guard_condition) {
@@ -71,7 +71,7 @@ public:
     guard_conditions_.push_back(guard_condition);
   }
 
-  void remove_guard_condition(const rcl_guard_condition_t * guard_condition)
+  void remove_guard_condition(const rcl_guard_condition_t * guard_condition) override
   {
     for (auto it = guard_conditions_.begin(); it != guard_conditions_.end(); ++it) {
       if (*it == guard_condition) {
@@ -81,7 +81,7 @@ public:
     }
   }
 
-  void clear_handles()
+  void clear_handles() override
   {
     subscription_handles_.clear();
     service_handles_.clear();
@@ -90,7 +90,7 @@ public:
     waitable_handles_.clear();
   }
 
-  virtual void remove_null_handles(rcl_wait_set_t * wait_set)
+  void remove_null_handles(rcl_wait_set_t * wait_set) override
   {
     // TODO(jacobperron): Check if wait set sizes are what we expect them to be?
     //                    e.g. wait_set->size_of_clients == client_handles_.size()
@@ -150,7 +150,7 @@ public:
     );
   }
 
-  bool collect_entities(const WeakNodeList & weak_nodes)
+  bool collect_entities(const WeakNodeList & weak_nodes) override
   {
     bool has_invalid_weak_nodes = false;
     for (auto & weak_node : weak_nodes) {
@@ -169,20 +169,23 @@ public:
             subscription_handles_.push_back(subscription->get_subscription_handle());
             return false;
           });
-
-        group->find_service_ptrs_if([this](const rclcpp::ServiceBase::SharedPtr & service) {
+        group->find_service_ptrs_if(
+          [this](const rclcpp::ServiceBase::SharedPtr & service) {
             service_handles_.push_back(service->get_service_handle());
             return false;
           });
-        group->find_client_ptrs_if([this](const rclcpp::ClientBase::SharedPtr & client) {
+        group->find_client_ptrs_if(
+          [this](const rclcpp::ClientBase::SharedPtr & client) {
             client_handles_.push_back(client->get_client_handle());
             return false;
           });
-        group->find_timer_ptrs_if([this](const rclcpp::TimerBase::SharedPtr & timer) {
+        group->find_timer_ptrs_if(
+          [this](const rclcpp::TimerBase::SharedPtr & timer) {
             timer_handles_.push_back(timer->get_timer_handle());
             return false;
           });
-        group->find_waitable_ptrs_if([this](const rclcpp::Waitable::SharedPtr & waitable) {
+        group->find_waitable_ptrs_if(
+          [this](const rclcpp::Waitable::SharedPtr & waitable) {
             waitable_handles_.push_back(waitable);
             return false;
           });
@@ -191,7 +194,15 @@ public:
     return has_invalid_weak_nodes;
   }
 
-  bool add_handles_to_wait_set(rcl_wait_set_t * wait_set)
+  void add_waitable_handle(const rclcpp::Waitable::SharedPtr & waitable) override
+  {
+    if (nullptr == waitable) {
+      throw std::runtime_error("waitable object unexpectedly nullptr");
+    }
+    waitable_handles_.push_back(waitable);
+  }
+
+  bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) override
   {
     for (auto subscription : subscription_handles_) {
       if (rcl_wait_set_add_subscription(wait_set, subscription.get(), NULL) != RCL_RET_OK) {
@@ -250,10 +261,10 @@ public:
     return true;
   }
 
-  virtual void
+  void
   get_next_subscription(
-    executor::AnyExecutable & any_exec,
-    const WeakNodeList & weak_nodes)
+    rclcpp::AnyExecutable & any_exec,
+    const WeakNodeList & weak_nodes) override
   {
     auto it = subscription_handles_.begin();
     while (it != subscription_handles_.end()) {
@@ -285,10 +296,10 @@ public:
     }
   }
 
-  virtual void
+  void
   get_next_service(
-    executor::AnyExecutable & any_exec,
-    const WeakNodeList & weak_nodes)
+    rclcpp::AnyExecutable & any_exec,
+    const WeakNodeList & weak_nodes) override
   {
     auto it = service_handles_.begin();
     while (it != service_handles_.end()) {
@@ -320,8 +331,8 @@ public:
     }
   }
 
-  virtual void
-  get_next_client(executor::AnyExecutable & any_exec, const WeakNodeList & weak_nodes)
+  void
+  get_next_client(rclcpp::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) override
   {
     auto it = client_handles_.begin();
     while (it != client_handles_.end()) {
@@ -353,10 +364,10 @@ public:
     }
   }
 
-  virtual void
+  void
   get_next_timer(
-    executor::AnyExecutable & any_exec,
-    const WeakNodeList & weak_nodes)
+    rclcpp::AnyExecutable & any_exec,
+    const WeakNodeList & weak_nodes) override
   {
     auto it = timer_handles_.begin();
     while (it != timer_handles_.end()) {
@@ -388,8 +399,8 @@ public:
     }
   }
 
-  virtual void
-  get_next_waitable(executor::AnyExecutable & any_exec, const WeakNodeList & weak_nodes)
+  void
+  get_next_waitable(rclcpp::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) override
   {
     auto it = waitable_handles_.begin();
     while (it != waitable_handles_.end()) {
@@ -421,12 +432,12 @@ public:
     }
   }
 
-  virtual rcl_allocator_t get_allocator()
+  rcl_allocator_t get_allocator() override
   {
     return rclcpp::allocator::get_rcl_allocator<void *, VoidAlloc>(*allocator_.get());
   }
 
-  size_t number_of_ready_subscriptions() const
+  size_t number_of_ready_subscriptions() const override
   {
     size_t number_of_subscriptions = subscription_handles_.size();
     for (auto waitable : waitable_handles_) {
@@ -435,7 +446,7 @@ public:
     return number_of_subscriptions;
   }
 
-  size_t number_of_ready_services() const
+  size_t number_of_ready_services() const override
   {
     size_t number_of_services = service_handles_.size();
     for (auto waitable : waitable_handles_) {
@@ -444,7 +455,7 @@ public:
     return number_of_services;
   }
 
-  size_t number_of_ready_events() const
+  size_t number_of_ready_events() const override
   {
     size_t number_of_events = 0;
     for (auto waitable : waitable_handles_) {
@@ -453,7 +464,7 @@ public:
     return number_of_events;
   }
 
-  size_t number_of_ready_clients() const
+  size_t number_of_ready_clients() const override
   {
     size_t number_of_clients = client_handles_.size();
     for (auto waitable : waitable_handles_) {
@@ -462,7 +473,7 @@ public:
     return number_of_clients;
   }
 
-  size_t number_of_guard_conditions() const
+  size_t number_of_guard_conditions() const override
   {
     size_t number_of_guard_conditions = guard_conditions_.size();
     for (auto waitable : waitable_handles_) {
@@ -471,7 +482,7 @@ public:
     return number_of_guard_conditions;
   }
 
-  size_t number_of_ready_timers() const
+  size_t number_of_ready_timers() const override
   {
     size_t number_of_timers = timer_handles_.size();
     for (auto waitable : waitable_handles_) {
@@ -480,7 +491,7 @@ public:
     return number_of_timers;
   }
 
-  size_t number_of_waitables() const
+  size_t number_of_waitables() const override
   {
     return waitable_handles_.size();
   }

rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp

@@ -17,6 +17,8 @@
 
 #include <memory>
 
+#include "rosidl_runtime_cpp/traits.hpp"
+
 #include "rclcpp/macros.hpp"
 #include "rclcpp/message_memory_strategy.hpp"
 #include "rclcpp/visibility_control.hpp"

rclcpp/include/rclcpp/subscription.hpp

@@ -18,6 +18,7 @@
 #include <rmw/error_handling.h>
 #include <rmw/rmw.h>
 
+#include <chrono>
 #include <functional>
 #include <iostream>
 #include <memory>
@@ -25,7 +26,6 @@
 #include <string>
 #include <utility>
 
-
 #include "rcl/error_handling.h"
 #include "rcl/subscription.h"
 
@@ -38,6 +38,7 @@
 #include "rclcpp/experimental/subscription_intra_process.hpp"
 #include "rclcpp/logging.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/message_info.hpp"
 #include "rclcpp/message_memory_strategy.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/subscription_base.hpp"
@@ -46,6 +47,7 @@
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rclcpp/waitable.hpp"
+#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp"
 #include "tracetools/tracetools.h"
 
 namespace rclcpp
@@ -74,6 +76,8 @@ public:
   using MessageDeleter = allocator::Deleter<MessageAllocator, CallbackMessageT>;
   using ConstMessageSharedPtr = std::shared_ptr<const CallbackMessageT>;
   using MessageUniquePtr = std::unique_ptr<CallbackMessageT, MessageDeleter>;
+  using SubscriptionTopicStatisticsSharedPtr =
+    std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>>;
 
   RCLCPP_SMART_PTR_DEFINITIONS(Subscription)
 
@@ -97,7 +101,8 @@ public:
     const rclcpp::QoS & qos,
     AnySubscriptionCallback<CallbackMessageT, AllocatorT> callback,
     const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
-    typename MessageMemoryStrategyT::SharedPtr message_memory_strategy)
+    typename MessageMemoryStrategyT::SharedPtr message_memory_strategy,
+    SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics = nullptr)
   : SubscriptionBase(
       node_base,
       type_support_handle,
@@ -118,40 +123,55 @@ public:
         options.event_callbacks.liveliness_callback,
         RCL_SUBSCRIPTION_LIVELINESS_CHANGED);
     }
+    if (options.event_callbacks.incompatible_qos_callback) {
+      this->add_event_handler(
+        options.event_callbacks.incompatible_qos_callback,
+        RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
+    } else if (options_.use_default_callbacks) {
+      // Register default callback when not specified
+      try {
+        this->add_event_handler(
+          [this](QOSRequestedIncompatibleQoSInfo & info) {
+            this->default_incompatible_qos_callback(info);
+          },
+          RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
+      } catch (UnsupportedEventTypeException & /*exc*/) {
+        // pass
+      }
+    }
 
     // Setup intra process publishing if requested.
     if (rclcpp::detail::resolve_use_intra_process(options, *node_base)) {
       using rclcpp::detail::resolve_intra_process_buffer_type;
 
       // Check if the QoS is compatible with intra-process.
-      if (qos.get_rmw_qos_profile().history == RMW_QOS_POLICY_HISTORY_KEEP_ALL) {
+      rmw_qos_profile_t qos_profile = get_actual_qos().get_rmw_qos_profile();
+      if (qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_ALL) {
         throw std::invalid_argument(
                 "intraprocess communication is not allowed with keep all history qos policy");
       }
-      if (qos.get_rmw_qos_profile().depth == 0) {
+      if (qos_profile.depth == 0) {
         throw std::invalid_argument(
                 "intraprocess communication is not allowed with 0 depth qos policy");
       }
-      if (qos.get_rmw_qos_profile().durability != RMW_QOS_POLICY_DURABILITY_VOLATILE) {
+      if (qos_profile.durability != RMW_QOS_POLICY_DURABILITY_VOLATILE) {
         throw std::invalid_argument(
                 "intraprocess communication allowed only with volatile durability");
       }
 
       // First create a SubscriptionIntraProcess which will be given to the intra-process manager.
       auto context = node_base->get_context();
-      auto subscription_intra_process = std::make_shared<
-        rclcpp::experimental::SubscriptionIntraProcess<
-          CallbackMessageT,
-          AllocatorT,
-          typename MessageUniquePtr::deleter_type
-        >>(
+      using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess<
+        CallbackMessageT,
+        AllocatorT,
+        typename MessageUniquePtr::deleter_type>;
+      auto subscription_intra_process = std::make_shared<SubscriptionIntraProcessT>(
         callback,
         options.get_allocator(),
         context,
-        this->get_topic_name(),    // important to get like this, as it has the fully-qualified name
-        qos.get_rmw_qos_profile(),
-        resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback)
-        );
+        this->get_topic_name(),  // important to get like this, as it has the fully-qualified name
+        qos_profile,
+        resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback));
       TRACEPOINT(
         rclcpp_subscription_init,
         (const void *)get_subscription_handle().get(),
@@ -164,6 +184,10 @@ public:
       this->setup_intra_process(intra_process_subscription_id, ipm);
     }
 
+    if (subscription_topic_statistics != nullptr) {
+      this->subscription_topic_statistics_ = std::move(subscription_topic_statistics);
+    }
+
     TRACEPOINT(
       rclcpp_subscription_init,
       (const void *)get_subscription_handle().get(),
@@ -175,11 +199,14 @@ public:
     // The callback object gets copied, so if registration is done too early/before this point
     // (e.g. in `AnySubscriptionCallback::set()`), its address won't match any address used later
     // in subsequent tracepoints.
+#ifndef TRACETOOLS_DISABLED
     any_callback_.register_callback_for_tracing();
+#endif
   }
 
   /// Called after construction to continue setup that requires shared_from_this().
-  void post_init_setup(
+  void
+  post_init_setup(
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
     const rclcpp::QoS & qos,
     const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options)
@@ -189,19 +216,32 @@ public:
     (void)options;
   }
 
-  /// Support dynamically setting the message memory strategy.
+  /// Take the next message from the inter-process subscription.
   /**
-   * Behavior may be undefined if called while the subscription could be executing.
-   * \param[in] message_memory_strategy Shared pointer to the memory strategy to set.
+   * Data may be taken (written) into the message_out and message_info_out even
+   * if false is returned.
+   * Specifically in the case of dropping redundant intra-process data, where
+   * data is received via both intra-process and inter-process (due to the
+   * underlying middleware being unabled to avoid this duplicate delivery) and
+   * so inter-process data from those intra-process publishers is ignored, but
+   * it has to be taken to know if it came from an intra-process publisher or
+   * not, and therefore could be dropped.
+   *
+   * \sa SubscriptionBase::take_type_erased()
+   *
+   * \param[out] message_out The message into which take will copy the data.
+   * \param[out] message_info_out The message info for the taken message.
+   * \returns true if data was taken and is valid, otherwise false
+   * \throws any rcl errors from rcl_take, \sa rclcpp::exceptions::throw_from_rcl_error()
    */
-  void set_message_memory_strategy(
-    typename message_memory_strategy::MessageMemoryStrategy<CallbackMessageT,
-    AllocatorT>::SharedPtr message_memory_strategy)
+  bool
+  take(CallbackMessageT & message_out, rclcpp::MessageInfo & message_info_out)
   {
-    message_memory_strategy_ = message_memory_strategy;
+    return this->take_type_erased(static_cast<void *>(&message_out), message_info_out);
   }
 
-  std::shared_ptr<void> create_message() override
+  std::shared_ptr<void>
+  create_message() override
   {
     /* The default message memory strategy provides a dynamically allocated message on each call to
      * create_message, though alternative memory strategies that re-use a preallocated message may be
@@ -210,26 +250,37 @@ public:
     return message_memory_strategy_->borrow_message();
   }
 
-  std::shared_ptr<rcl_serialized_message_t> create_serialized_message() override
+  std::shared_ptr<rclcpp::SerializedMessage>
+  create_serialized_message() override
   {
     return message_memory_strategy_->borrow_serialized_message();
   }
 
-  void handle_message(
-    std::shared_ptr<void> & message, const rmw_message_info_t & message_info) override
+  void
+  handle_message(
+    std::shared_ptr<void> & message,
+    const rclcpp::MessageInfo & message_info) override
   {
-    if (matches_any_intra_process_publishers(&message_info.publisher_gid)) {
+    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 = std::static_pointer_cast<CallbackMessageT>(message);
     any_callback_.dispatch(typed_message, message_info);
+
+    if (subscription_topic_statistics_) {
+      const auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(
+        std::chrono::system_clock::now());
+      const auto time = rclcpp::Time(nanos.time_since_epoch().count());
+      subscription_topic_statistics_->handle_message(*typed_message, time);
+    }
   }
 
   void
   handle_loaned_message(
-    void * loaned_message, const rmw_message_info_t & message_info) override
+    void * loaned_message,
+    const rclcpp::MessageInfo & message_info) override
   {
     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
@@ -240,18 +291,21 @@ public:
 
   /// Return the borrowed message.
   /** \param message message to be returned */
-  void return_message(std::shared_ptr<void> & message) override
+  void
+  return_message(std::shared_ptr<void> & message) override
   {
     auto typed_message = std::static_pointer_cast<CallbackMessageT>(message);
     message_memory_strategy_->return_message(typed_message);
   }
 
-  void return_serialized_message(std::shared_ptr<rcl_serialized_message_t> & message) override
+  void
+  return_serialized_message(std::shared_ptr<rclcpp::SerializedMessage> & message) override
   {
     message_memory_strategy_->return_serialized_message(message);
   }
 
-  bool use_take_shared_method() const
+  bool
+  use_take_shared_method() const
   {
     return any_callback_.use_take_shared_method();
   }
@@ -268,6 +322,8 @@ private:
   const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> options_;
   typename message_memory_strategy::MessageMemoryStrategy<CallbackMessageT, AllocatorT>::SharedPtr
     message_memory_strategy_;
+  /// Component which computes and publishes topic statistics for this subscriber
+  SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics_{nullptr};
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/subscription_base.hpp

@@ -15,9 +15,12 @@
 #ifndef RCLCPP__SUBSCRIPTION_BASE_HPP_
 #define RCLCPP__SUBSCRIPTION_BASE_HPP_
 
+#include <atomic>
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include <vector>
+#include <utility>
 
 #include "rcl/subscription.h"
 
@@ -27,8 +30,10 @@
 #include "rclcpp/experimental/intra_process_manager.hpp"
 #include "rclcpp/experimental/subscription_intra_process_base.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/message_info.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/qos_event.hpp"
+#include "rclcpp/serialized_message.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -110,6 +115,45 @@ public:
   rclcpp::QoS
   get_actual_qos() const;
 
+  /// Take the next inter-process message from the subscription as a type erased pointer.
+  /**
+   * \sa Subscription::take() for details on how this function works.
+   *
+   * The only difference is that it takes a type erased pointer rather than a
+   * reference to the exact message type.
+   *
+   * This type erased version facilitates using the subscriptions in a type
+   * agnostic way using SubscriptionBase::create_message() and
+   * SubscriptionBase::handle_message().
+   *
+   * \param[out] message_out The type erased message pointer into which take
+   *   will copy the data.
+   * \param[out] message_info_out The message info for the taken message.
+   * \returns true if data was taken and is valid, otherwise false
+   * \throws any rcl errors from rcl_take, \sa rclcpp::exceptions::throw_from_rcl_error()
+   */
+  RCLCPP_PUBLIC
+  bool
+  take_type_erased(void * message_out, rclcpp::MessageInfo & message_info_out);
+
+  /// Take the next inter-process message, in its serialized form, from the subscription.
+  /**
+   * For now, if data is taken (written) into the message_out and
+   * message_info_out then true will be returned.
+   * Unlike Subscription::take(), taking data serialized is not possible via
+   * intra-process for the time being, so it will not need to de-duplicate
+   * data in any case.
+   *
+   * \param[out] message_out The serialized message data structure used to
+   *   store the taken message.
+   * \param[out] message_info_out The message info for the taken message.
+   * \returns true if data was taken and is valid, otherwise false
+   * \throws any rcl errors from rcl_take, \sa rclcpp::exceptions::throw_from_rcl_error()
+   */
+  RCLCPP_PUBLIC
+  bool
+  take_serialized(rclcpp::SerializedMessage & message_out, rclcpp::MessageInfo & message_info_out);
+
   /// Borrow a new message.
   /** \return Shared pointer to the fresh message. */
   RCLCPP_PUBLIC
@@ -121,7 +165,7 @@ public:
   /** \return Shared pointer to a rcl_message_serialized_t. */
   RCLCPP_PUBLIC
   virtual
-  std::shared_ptr<rcl_serialized_message_t>
+  std::shared_ptr<rclcpp::SerializedMessage>
   create_serialized_message() = 0;
 
   /// Check if we need to handle the message, and execute the callback if we do.
@@ -132,12 +176,12 @@ public:
   RCLCPP_PUBLIC
   virtual
   void
-  handle_message(std::shared_ptr<void> & message, const rmw_message_info_t & message_info) = 0;
+  handle_message(std::shared_ptr<void> & message, const rclcpp::MessageInfo & message_info) = 0;
 
   RCLCPP_PUBLIC
   virtual
   void
-  handle_loaned_message(void * loaned_message, const rmw_message_info_t & message_info) = 0;
+  handle_loaned_message(void * loaned_message, const rclcpp::MessageInfo & message_info) = 0;
 
   /// Return the message borrowed in create_message.
   /** \param[in] message Shared pointer to the returned message. */
@@ -151,7 +195,7 @@ public:
   RCLCPP_PUBLIC
   virtual
   void
-  return_serialized_message(std::shared_ptr<rcl_serialized_message_t> & message) = 0;
+  return_serialized_message(std::shared_ptr<rclcpp::SerializedMessage> & message) = 0;
 
   RCLCPP_PUBLIC
   const rosidl_message_type_support_t &
@@ -193,6 +237,23 @@ public:
   rclcpp::Waitable::SharedPtr
   get_intra_process_waitable() const;
 
+  /// Exchange state of whether or not a part of the subscription is used by a wait set.
+  /**
+   * Used to ensure parts of the subscription are not used with multiple wait
+   * sets simultaneously.
+   *
+   * \param[in] pointer_to_subscription_part address of a subscription part
+   * \param[in] in_use_state the new state to exchange, true means "now in use",
+   *   and false means "no longer in use".
+   * \returns the current "in use" state.
+   * \throws std::invalid_argument If pointer_to_subscription_part is nullptr.
+   * \throws std::runtime_error If the pointer given is not a pointer to one of
+   *   the parts of the subscription which can be used with a wait set.
+   */
+  RCLCPP_PUBLIC
+  bool
+  exchange_in_use_by_wait_set_state(void * pointer_to_subscription_part, bool in_use_state);
+
 protected:
   template<typename EventCallbackT>
   void
@@ -205,9 +266,13 @@ protected:
       rcl_subscription_event_init,
       get_subscription_handle().get(),
       event_type);
+    qos_events_in_use_by_wait_set_.insert(std::make_pair(handler.get(), false));
     event_handlers_.emplace_back(handler);
   }
 
+  RCLCPP_PUBLIC
+  void default_incompatible_qos_callback(QOSRequestedIncompatibleQoSInfo & info) const;
+
   RCLCPP_PUBLIC
   bool
   matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const;
@@ -229,6 +294,11 @@ private:
 
   rosidl_message_type_support_t type_support_;
   bool is_serialized_;
+
+  std::atomic<bool> subscription_in_use_by_wait_set_{false};
+  std::atomic<bool> intra_process_subscription_waitable_in_use_by_wait_set_{false};
+  std::unordered_map<rclcpp::QOSEventHandlerBase *,
+    std::atomic<bool>> qos_events_in_use_by_wait_set_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/subscription_factory.hpp

@@ -32,6 +32,7 @@
 #include "rclcpp/subscription_options.hpp"
 #include "rclcpp/subscription_traits.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp"
 
 namespace rclcpp
 {
@@ -78,7 +79,10 @@ SubscriptionFactory
 create_subscription_factory(
   CallbackT && callback,
   const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
-  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat)
+  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat,
+  std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>>
+  subscription_topic_stats = nullptr
+)
 {
   auto allocator = options.get_allocator();
 
@@ -88,7 +92,7 @@ create_subscription_factory(
 
   SubscriptionFactory factory {
     // factory function that creates a MessageT specific SubscriptionT
-    [options, msg_mem_strat, any_subscription_callback](
+    [options, msg_mem_strat, any_subscription_callback, subscription_topic_stats](
       rclcpp::node_interfaces::NodeBaseInterface * node_base,
       const std::string & topic_name,
       const rclcpp::QoS & qos
@@ -104,7 +108,8 @@ create_subscription_factory(
         qos,
         any_subscription_callback,
         options,
-        msg_mem_strat);
+        msg_mem_strat,
+        subscription_topic_stats);
       // This is used for setting up things like intra process comms which
       // require this->shared_from_this() which cannot be called from
       // the constructor.

rclcpp/include/rclcpp/subscription_options.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__SUBSCRIPTION_OPTIONS_HPP_
 #define RCLCPP__SUBSCRIPTION_OPTIONS_HPP_
 
+#include <chrono>
 #include <memory>
 #include <string>
 #include <vector>
@@ -25,6 +26,7 @@
 #include "rclcpp/intra_process_setting.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/qos_event.hpp"
+#include "rclcpp/topic_statistics_state.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -36,11 +38,14 @@ struct SubscriptionOptionsBase
   /// Callbacks for events related to this subscription.
   SubscriptionEventCallbacks event_callbacks;
 
+  /// Whether or not to use default callbacks when user doesn't supply any in event_callbacks
+  bool use_default_callbacks = true;
+
   /// True to ignore local publications.
   bool ignore_local_publications = false;
 
   /// The callback group for this subscription. NULL to use the default callback group.
-  rclcpp::callback_group::CallbackGroup::SharedPtr callback_group = nullptr;
+  rclcpp::CallbackGroup::SharedPtr callback_group = nullptr;
 
   /// Setting to explicitly set intraprocess communications.
   IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault;
@@ -51,6 +56,21 @@ struct SubscriptionOptionsBase
   /// Optional RMW implementation specific payload to be used during creation of the subscription.
   std::shared_ptr<rclcpp::detail::RMWImplementationSpecificSubscriptionPayload>
   rmw_implementation_payload = nullptr;
+
+  // Options to configure topic statistics collector in the subscription.
+  struct TopicStatisticsOptions
+  {
+    // Enable and disable topic statistics calculation and publication. Defaults to disabled.
+    TopicStatisticsState state = TopicStatisticsState::NodeDefault;
+
+    // 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.
+    std::chrono::milliseconds publish_period{std::chrono::seconds(1)};
+  };
+
+  TopicStatisticsOptions topic_stats_options;
 };
 
 /// Structure containing optional configuration for Subscriptions.
@@ -101,7 +121,6 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
 };
 
 using SubscriptionOptions = SubscriptionOptionsWithAllocator<std::allocator<void>>;
-
 }  // namespace rclcpp
 
 #endif  // RCLCPP__SUBSCRIPTION_OPTIONS_HPP_

rclcpp/include/rclcpp/subscription_traits.hpp

@@ -18,6 +18,8 @@
 #include <memory>
 
 #include "rclcpp/function_traits.hpp"
+#include "rclcpp/serialized_message.hpp"
+#include "rclcpp/subscription_options.hpp"
 #include "rcl/types.h"
 
 namespace rclcpp
@@ -41,11 +43,11 @@ struct is_serialized_subscription_argument : std::false_type
 {};
 
 template<>
-struct is_serialized_subscription_argument<rcl_serialized_message_t>: std::true_type
+struct is_serialized_subscription_argument<SerializedMessage>: std::true_type
 {};
 
 template<>
-struct is_serialized_subscription_argument<std::shared_ptr<rcl_serialized_message_t>>
+struct is_serialized_subscription_argument<std::shared_ptr<SerializedMessage>>
   : std::true_type
 {};
 
@@ -75,6 +77,7 @@ struct extract_message_type<std::unique_ptr<MessageT, Deleter>>: extract_message
 
 template<
   typename CallbackT,
+  typename AllocatorT = std::allocator<void>,
   // Do not attempt if CallbackT is an integer (mistaken for depth)
   typename = std::enable_if_t<!std::is_integral<
     std::remove_cv_t<std::remove_reference_t<CallbackT>>>::value>,
@@ -85,6 +88,10 @@ template<
   // Do not attempt if CallbackT is a rmw_qos_profile_t (mistaken for qos profile)
   typename = std::enable_if_t<!std::is_same<
     rmw_qos_profile_t,
+    std::remove_cv_t<std::remove_reference_t<CallbackT>>>::value>,
+  // Do not attempt if CallbackT is a rclcpp::SubscriptionOptionsWithAllocator
+  typename = std::enable_if_t<!std::is_same<
+    rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>,
     std::remove_cv_t<std::remove_reference_t<CallbackT>>>::value>
 >
 struct has_message_type : extract_message_type<

rclcpp/include/rclcpp/subscription_wait_set_mask.hpp

@@ -0,0 +1,37 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__SUBSCRIPTION_WAIT_SET_MASK_HPP_
+#define RCLCPP__SUBSCRIPTION_WAIT_SET_MASK_HPP_
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Options used to determine what parts of a subscription get added to or removed from a wait set.
+class RCLCPP_PUBLIC SubscriptionWaitSetMask
+{
+public:
+  /// If true, include the actual subscription.
+  bool include_subscription = true;
+  /// If true, include any events attached to the subscription.
+  bool include_events = true;
+  /// If true, include the waitable used to handle intra process communication.
+  bool include_intra_process_waitable = true;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__SUBSCRIPTION_WAIT_SET_MASK_HPP_

rclcpp/include/rclcpp/time.hpp

@@ -98,6 +98,14 @@ public:
   Time
   operator-(const rclcpp::Duration & rhs) const;
 
+  RCLCPP_PUBLIC
+  Time &
+  operator+=(const rclcpp::Duration & rhs);
+
+  RCLCPP_PUBLIC
+  Time &
+  operator-=(const rclcpp::Duration & rhs);
+
   RCLCPP_PUBLIC
   rcl_time_point_value_t
   nanoseconds() const;

rclcpp/include/rclcpp/timer.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__TIMER_HPP_
 #define RCLCPP__TIMER_HPP_
 
+#include <atomic>
 #include <chrono>
 #include <functional>
 #include <memory>
@@ -64,7 +65,7 @@ public:
   /**
    * \return true if the timer has been cancelled, false otherwise
    * \throws std::runtime_error if the rcl_get_error_state returns 0
-   * \throws RCLErrorBase some child class exception based on ret
+   * \throws rclcpp::exceptions::RCLError some child class exception based on ret
    */
   RCLCPP_PUBLIC
   bool
@@ -101,9 +102,25 @@ public:
   RCLCPP_PUBLIC
   bool is_ready();
 
+  /// Exchange the "in use by wait set" state for this timer.
+  /**
+   * This is used to ensure this timer is not used by multiple
+   * wait sets at the same time.
+   *
+   * \param[in] in_use_state the new state to exchange into the state, true
+   *   indicates it is now in use by a wait set, and false is that it is no
+   *   longer in use by a wait set.
+   * \returns the previous state.
+   */
+  RCLCPP_PUBLIC
+  bool
+  exchange_in_use_by_wait_set_state(bool in_use_state);
+
 protected:
   Clock::SharedPtr clock_;
   std::shared_ptr<rcl_timer_t> timer_handle_;
+
+  std::atomic<bool> in_use_by_wait_set_{false};
 };
 
 

rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp

@@ -0,0 +1,243 @@
+// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved.
+//
+// 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__TOPIC_STATISTICS__SUBSCRIPTION_TOPIC_STATISTICS_HPP_
+#define RCLCPP__TOPIC_STATISTICS__SUBSCRIPTION_TOPIC_STATISTICS_HPP_
+
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "libstatistics_collector/collector/generate_statistics_message.hpp"
+#include "libstatistics_collector/moving_average_statistics/types.hpp"
+#include "libstatistics_collector/topic_statistics_collector/constants.hpp"
+#include "libstatistics_collector/topic_statistics_collector/received_message_age.hpp"
+#include "libstatistics_collector/topic_statistics_collector/received_message_period.hpp"
+
+#include "rcl/time.h"
+#include "rclcpp/time.hpp"
+#include "rclcpp/publisher.hpp"
+#include "rclcpp/timer.hpp"
+
+#include "statistics_msgs/msg/metrics_message.hpp"
+
+namespace rclcpp
+{
+namespace topic_statistics
+{
+
+constexpr const char kDefaultPublishTopicName[]{"/statistics"};
+constexpr const std::chrono::milliseconds kDefaultPublishingPeriod{std::chrono::seconds(1)};
+
+using libstatistics_collector::collector::GenerateStatisticMessage;
+using statistics_msgs::msg::MetricsMessage;
+using libstatistics_collector::moving_average_statistics::StatisticData;
+
+/**
+ * Class used to collect, measure, and publish topic statistics data. Current statistics
+ * supported for subscribers are received message age and received message period.
+ *
+ * \tparam CallbackMessageT the subscribed message type
+ */
+template<typename CallbackMessageT>
+class SubscriptionTopicStatistics
+{
+  using TopicStatsCollector =
+    libstatistics_collector::topic_statistics_collector::TopicStatisticsCollector<
+    CallbackMessageT>;
+  using ReceivedMessageAge =
+    libstatistics_collector::topic_statistics_collector::ReceivedMessageAgeCollector<
+    CallbackMessageT>;
+  using ReceivedMessagePeriod =
+    libstatistics_collector::topic_statistics_collector::ReceivedMessagePeriodCollector<
+    CallbackMessageT>;
+
+public:
+  /// Construct a SubscriptionTopicStatistics object.
+  /**
+   * This object wraps utilities, defined in libstatistics_collector, to collect,
+   * measure, and publish topic statistics data. This throws an invalid_argument
+   * if the input publisher is null.
+   *
+   * \param node_name the name of the node, which created this instance, in order to denote
+   * topic source
+   * \param publisher instance constructed by the node in order to publish statistics data.
+   * This class owns the publisher.
+   */
+  SubscriptionTopicStatistics(
+    const std::string & node_name,
+    rclcpp::Publisher<statistics_msgs::msg::MetricsMessage>::SharedPtr publisher)
+  : node_name_(node_name),
+    publisher_(std::move(publisher))
+  {
+    // TODO(dbbonnie): ros-tooling/aws-roadmap/issues/226, received message age
+
+    if (nullptr == publisher_) {
+      throw std::invalid_argument("publisher pointer is nullptr");
+    }
+
+    bring_up();
+  }
+
+  virtual ~SubscriptionTopicStatistics()
+  {
+    tear_down();
+  }
+
+  /// Handle a message received by the subscription to collect statistics.
+  /**
+   * This method acquires a lock to prevent race conditions to collectors list.
+   *
+   * \param received_message the message received by the subscription
+   * \param now_nanoseconds current time in nanoseconds
+   */
+  virtual void handle_message(
+    const CallbackMessageT & received_message,
+    const rclcpp::Time now_nanoseconds) const
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    for (const auto & collector : subscriber_statistics_collectors_) {
+      collector->OnMessageReceived(received_message, now_nanoseconds.nanoseconds());
+    }
+  }
+
+  /// Set the timer used to publish statistics messages.
+  /**
+   * \param measurement_timer the timer to fire the publisher, created by the node
+   */
+  void set_publisher_timer(rclcpp::TimerBase::SharedPtr publisher_timer)
+  {
+    publisher_timer_ = publisher_timer;
+  }
+
+  /// Publish a populated MetricsStatisticsMessage.
+  /**
+   * This method acquires a lock to prevent race conditions to collectors list.
+   */
+  virtual void publish_message()
+  {
+    std::vector<MetricsMessage> msgs;
+    rclcpp::Time window_end{get_current_nanoseconds_since_epoch()};
+
+    {
+      std::lock_guard<std::mutex> lock(mutex_);
+      for (auto & collector : subscriber_statistics_collectors_) {
+        const auto collected_stats = collector->GetStatisticsResults();
+
+        auto message = libstatistics_collector::collector::GenerateStatisticMessage(
+          node_name_,
+          collector->GetMetricName(),
+          collector->GetMetricUnit(),
+          window_start_,
+          window_end,
+          collected_stats);
+        msgs.push_back(message);
+      }
+    }
+
+    for (auto & msg : msgs) {
+      publisher_->publish(msg);
+    }
+    window_start_ = window_end;
+  }
+
+protected:
+  /// Return a vector of all the currently collected data.
+  /**
+   * This method acquires a lock to prevent race conditions to collectors list.
+   *
+   * \return a vector of all the collected data
+   */
+  std::vector<StatisticData> get_current_collector_data() const
+  {
+    std::vector<StatisticData> data;
+    std::lock_guard<std::mutex> lock(mutex_);
+    for (const auto & collector : subscriber_statistics_collectors_) {
+      data.push_back(collector->GetStatisticsResults());
+    }
+    return data;
+  }
+
+private:
+  /// Construct and start all collectors and set window_start_.
+  /**
+   * This method acquires a lock to prevent race conditions to collectors list.
+   */
+  void bring_up()
+  {
+    auto received_message_age = std::make_unique<ReceivedMessageAge>();
+    received_message_age->Start();
+    subscriber_statistics_collectors_.emplace_back(std::move(received_message_age));
+
+    auto received_message_period = std::make_unique<ReceivedMessagePeriod>();
+    received_message_period->Start();
+    {
+      std::lock_guard<std::mutex> lock(mutex_);
+      subscriber_statistics_collectors_.emplace_back(std::move(received_message_period));
+    }
+
+    window_start_ = rclcpp::Time(get_current_nanoseconds_since_epoch());
+  }
+
+  /// Stop all collectors, clear measurements, stop publishing timer, and reset publisher.
+  /**
+   * This method acquires a lock to prevent race conditions to collectors list.
+   */
+  void tear_down()
+  {
+    {
+      std::lock_guard<std::mutex> lock(mutex_);
+      for (auto & collector : subscriber_statistics_collectors_) {
+        collector->Stop();
+      }
+
+      subscriber_statistics_collectors_.clear();
+    }
+
+    if (publisher_timer_) {
+      publisher_timer_->cancel();
+      publisher_timer_.reset();
+    }
+
+    publisher_.reset();
+  }
+
+  /// Return the current nanoseconds (count) since epoch.
+  /**
+   * \return the current nanoseconds (count) since epoch
+   */
+  int64_t get_current_nanoseconds_since_epoch() const
+  {
+    const auto now = std::chrono::system_clock::now();
+    return std::chrono::duration_cast<std::chrono::nanoseconds>(now.time_since_epoch()).count();
+  }
+
+  /// Mutex to protect the subsequence vectors
+  mutable std::mutex mutex_;
+  /// Collection of statistics collectors
+  std::vector<std::unique_ptr<TopicStatsCollector>> subscriber_statistics_collectors_{};
+  /// Node name used to generate topic statistics messages to be published
+  const std::string node_name_;
+  /// Publisher, created by the node, used to publish topic statistics messages
+  rclcpp::Publisher<statistics_msgs::msg::MetricsMessage>::SharedPtr publisher_;
+  /// Timer which fires the publisher
+  rclcpp::TimerBase::SharedPtr publisher_timer_;
+  /// The start of the collection window, used in the published topic statistics message
+  rclcpp::Time window_start_;
+};
+}  // namespace topic_statistics
+}  // namespace rclcpp
+
+#endif  // RCLCPP__TOPIC_STATISTICS__SUBSCRIPTION_TOPIC_STATISTICS_HPP_

rclcpp/include/rclcpp/topic_statistics_state.hpp

@@ -0,0 +1,35 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__TOPIC_STATISTICS_STATE_HPP_
+#define RCLCPP__TOPIC_STATISTICS_STATE_HPP_
+
+namespace rclcpp
+{
+
+/// Represent the state of topic statistics collector.
+/// Used as argument in create_subscriber.
+enum class TopicStatisticsState
+{
+  /// Explicitly enable topic statistics at subscription level.
+  Enable,
+  /// Explicitly disable topic statistics at subscription level.
+  Disable,
+  /// Take topic statistics state from the node.
+  NodeDefault
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__TOPIC_STATISTICS_STATE_HPP_

rclcpp/include/rclcpp/type_support_decl.hpp

@@ -15,8 +15,8 @@
 #ifndef RCLCPP__TYPE_SUPPORT_DECL_HPP_
 #define RCLCPP__TYPE_SUPPORT_DECL_HPP_
 
-#include "rosidl_generator_cpp/message_type_support_decl.hpp"
-#include "rosidl_generator_cpp/service_type_support_decl.hpp"
+#include "rosidl_runtime_cpp/message_type_support_decl.hpp"
+#include "rosidl_runtime_cpp/service_type_support_decl.hpp"
 
 #include "rosidl_typesupport_cpp/message_type_support.hpp"
 #include "rosidl_typesupport_cpp/service_type_support.hpp"

rclcpp/include/rclcpp/utilities.hpp

@@ -45,7 +45,7 @@ namespace rclcpp
 /// Initialize communications via the rmw implementation and set up a global signal handler.
 /**
  * Initializes the global context which is accessible via the function
- * rclcpp::contexts::default_context::get_global_default_context().
+ * rclcpp::contexts::get_global_default_context().
  * Also, installs the global signal handlers with the function
  * rclcpp::install_signal_handlers().
  *
@@ -121,7 +121,7 @@ init_and_remove_ros_arguments(
  * \param[in] argv Argument vector.
  * \returns Members of the argument vector that are not ROS arguments.
  * \throws anything throw_from_rcl_error can throw
- * \throws rclcpp::exceptions::RCLErrorBase if the parsing fails
+ * \throws rclcpp::exceptions::RCLError if the parsing fails
  */
 RCLCPP_PUBLIC
 std::vector<std::string>
@@ -153,6 +153,7 @@ ok(rclcpp::Context::SharedPtr context = nullptr);
  * \param[in] context Check for initialization of this Context.
  * \return true if the context is initialized, and false otherwise
  */
+[[deprecated("use the function ok() instead, which has the same usage.")]]
 RCLCPP_PUBLIC
 bool
 is_initialized(rclcpp::Context::SharedPtr context = nullptr);

rclcpp/include/rclcpp/wait_result.hpp

@@ -0,0 +1,155 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAIT_RESULT_HPP_
+#define RCLCPP__WAIT_RESULT_HPP_
+
+#include <cassert>
+#include <functional>
+#include <stdexcept>
+
+#include "rcl/wait.h"
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/wait_result_kind.hpp"
+
+namespace rclcpp
+{
+
+// TODO(wjwwood): the union-like design of this class could be replaced with
+//   std::variant, when we have access to that...
+/// Interface for introspecting a wait set after waiting on it.
+/**
+ * This class:
+ *
+ *   - provides the result of waiting, i.e. ready, timeout, or empty, and
+ *   - holds the ownership of the entities of the wait set, if needed, and
+ *   - provides the necessary information for iterating over the wait set.
+ *
+ * This class is only valid as long as the wait set which created it is valid,
+ * and it must be deleted before the wait set is deleted, as it contains a
+ * back reference to the wait set.
+ *
+ * An instance of this, which is returned from rclcpp::WaitSetTemplate::wait(),
+ * will cause the wait set to keep ownership of the entities because it only
+ * holds a reference to the sequences of them, rather than taking a copy.
+ * Also, in the thread-safe case, an instance of this will cause the wait set,
+ * to block calls which modify the sequences of the entities, e.g. add/remove
+ * guard condition or subscription methods.
+ *
+ * \tparam WaitSetT The wait set type which created this class.
+ */
+template<class WaitSetT>
+class WaitResult final
+{
+public:
+  /// Create WaitResult from a "ready" result.
+  /**
+   * \param[in] wait_set A reference to the wait set, which this class
+   *   will keep for the duration of its lifetime.
+   */
+  static
+  WaitResult
+  from_ready_wait_result_kind(WaitSetT & wait_set)
+  {
+    return WaitResult(WaitResultKind::Ready, wait_set);
+  }
+
+  /// Create WaitResult from a "timeout" result.
+  static
+  WaitResult
+  from_timeout_wait_result_kind()
+  {
+    return WaitResult(WaitResultKind::Timeout);
+  }
+
+  /// Create WaitResult from a "empty" result.
+  static
+  WaitResult
+  from_empty_wait_result_kind()
+  {
+    return WaitResult(WaitResultKind::Empty);
+  }
+
+  /// Return the kind of the WaitResult.
+  WaitResultKind
+  kind() const
+  {
+    return wait_result_kind_;
+  }
+
+  /// Return the rcl wait set.
+  const WaitSetT &
+  get_wait_set() const
+  {
+    if (this->kind() != WaitResultKind::Ready) {
+      throw std::runtime_error("cannot access wait set when the result was not ready");
+    }
+    // This should never happen, defensive (and debug mode) check only.
+    assert(wait_set_pointer_);
+    return *wait_set_pointer_;
+  }
+
+  /// Return the rcl wait set.
+  WaitSetT &
+  get_wait_set()
+  {
+    if (this->kind() != WaitResultKind::Ready) {
+      throw std::runtime_error("cannot access wait set when the result was not ready");
+    }
+    // This should never happen, defensive (and debug mode) check only.
+    assert(wait_set_pointer_);
+    return *wait_set_pointer_;
+  }
+
+  WaitResult(WaitResult && other) noexcept
+  : wait_result_kind_(other.wait_result_kind_),
+    wait_set_pointer_(std::exchange(other.wait_set_pointer_, nullptr))
+  {}
+
+  ~WaitResult()
+  {
+    if (wait_set_pointer_) {
+      wait_set_pointer_->wait_result_release();
+    }
+  }
+
+private:
+  RCLCPP_DISABLE_COPY(WaitResult)
+
+  explicit WaitResult(WaitResultKind wait_result_kind)
+  : wait_result_kind_(wait_result_kind)
+  {
+    // Should be enforced by the static factory methods on this class.
+    assert(WaitResultKind::Ready != wait_result_kind);
+  }
+
+  WaitResult(WaitResultKind wait_result_kind, WaitSetT & wait_set)
+  : wait_result_kind_(wait_result_kind),
+    wait_set_pointer_(&wait_set)
+  {
+    // Should be enforced by the static factory methods on this class.
+    assert(WaitResultKind::Ready == wait_result_kind);
+    // Secure thread-safety (if provided) and shared ownership (if needed).
+    wait_set_pointer_->wait_result_acquire();
+  }
+
+  const WaitResultKind wait_result_kind_;
+
+  WaitSetT * wait_set_pointer_ = nullptr;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_RESULT_HPP_

rclcpp/include/rclcpp/wait_result_kind.hpp

@@ -0,0 +1,33 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAIT_RESULT_KIND_HPP_
+#define RCLCPP__WAIT_RESULT_KIND_HPP_
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Represents the various kinds of results from waiting on a wait set.
+enum RCLCPP_PUBLIC WaitResultKind
+{
+  Ready,  //<! Kind used when something in the wait set was ready.
+  Timeout,  //<! Kind used when the wait resulted in a timeout.
+  Empty,  //<! Kind used when trying to wait on an empty wait set.
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_RESULT_KIND_HPP_

rclcpp/include/rclcpp/wait_set.hpp

@@ -0,0 +1,106 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAIT_SET_HPP_
+#define RCLCPP__WAIT_SET_HPP_
+
+#include <memory>
+
+#include "rcl/wait.h"
+
+#include "rclcpp/guard_condition.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/wait_set_policies/dynamic_storage.hpp"
+#include "rclcpp/wait_set_policies/sequential_synchronization.hpp"
+#include "rclcpp/wait_set_policies/static_storage.hpp"
+#include "rclcpp/wait_set_policies/thread_safe_synchronization.hpp"
+#include "rclcpp/wait_set_template.hpp"
+
+namespace rclcpp
+{
+
+/// Most common user configuration of a WaitSet, which is dynamic but not thread-safe.
+/**
+ * This wait set allows you to add and remove items dynamically, and it will
+ * automatically remove items that are let out of scope each time wait() or
+ * prune_destroyed_entities() is called.
+ *
+ * It will not, however, provide thread-safety for adding and removing entities
+ * while waiting.
+ *
+ * \sa rclcpp::WaitSetTemplate for API documentation
+ */
+using WaitSet = rclcpp::WaitSetTemplate<
+  rclcpp::wait_set_policies::SequentialSynchronization,
+  rclcpp::wait_set_policies::DynamicStorage
+>;
+
+/// WaitSet configuration which does not allow changes after construction.
+/**
+ * This wait set requires that you specify all entities at construction, and
+ * prevents you from calling the typical add and remove functions.
+ * It also requires that you specify how many of each item there will be as a
+ * template argument.
+ *
+ * It will share ownership of the entities until destroyed, therefore it will
+ * prevent the destruction of entities so long as the wait set exists, even if
+ * the user lets their copy of the shared pointer to the entity go out of scope.
+ *
+ * Since the wait set cannot be mutated, it does not need to be thread-safe.
+ *
+ * \sa rclcpp::WaitSetTemplate for API documentation
+ */
+template<
+  std::size_t NumberOfSubscriptions,
+  std::size_t NumberOfGuardCondtions,
+  std::size_t NumberOfTimers,
+  std::size_t NumberOfClients,
+  std::size_t NumberOfServices,
+  std::size_t NumberOfWaitables
+>
+using StaticWaitSet = rclcpp::WaitSetTemplate<
+  rclcpp::wait_set_policies::SequentialSynchronization,
+  rclcpp::wait_set_policies::StaticStorage<
+    NumberOfSubscriptions,
+    NumberOfGuardCondtions,
+    NumberOfTimers,
+    NumberOfClients,
+    NumberOfServices,
+    NumberOfWaitables
+  >
+>;
+
+/// Like WaitSet, this configuration is dynamic, but is also thread-safe.
+/**
+ * This wait set allows you to add and remove items dynamically, and it will
+ * automatically remove items that are let out of scope each time wait() or
+ * prune_destroyed_entities() is called.
+ *
+ * It will also ensure that adding and removing items explicitly from the
+ * wait set is done in a thread-safe way, protecting against concurrent add and
+ * deletes, as well as add and deletes during a wait().
+ * This thread-safety comes at some overhead and the use of thread
+ * synchronization primitives.
+ *
+ * \sa rclcpp::WaitSetTemplate for API documentation
+ */
+using ThreadSafeWaitSet = rclcpp::WaitSetTemplate<
+  rclcpp::wait_set_policies::ThreadSafeSynchronization,
+  rclcpp::wait_set_policies::DynamicStorage
+>;
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_SET_HPP_

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

@@ -0,0 +1,412 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_
+#define RCLCPP__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_
+
+#include <memory>
+#include <stdexcept>
+#include <utility>
+
+#include "rcl/wait.h"
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/logging.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace wait_set_policies
+{
+namespace detail
+{
+
+/// Common structure for storage policies, which provides rcl wait set access.
+template<bool HasStrongOwnership>
+class StoragePolicyCommon
+{
+protected:
+  template<
+    class SubscriptionsIterable,
+    class GuardConditionsIterable,
+    class ExtraGuardConditionsIterable,
+    class TimersIterable,
+    class ClientsIterable,
+    class ServicesIterable,
+    class WaitablesIterable
+  >
+  explicit
+  StoragePolicyCommon(
+    const SubscriptionsIterable & subscriptions,
+    const GuardConditionsIterable & guard_conditions,
+    const ExtraGuardConditionsIterable & extra_guard_conditions,
+    const TimersIterable & timers,
+    const ClientsIterable & clients,
+    const ServicesIterable & services,
+    const WaitablesIterable & waitables,
+    rclcpp::Context::SharedPtr context
+  )
+  : rcl_wait_set_(rcl_get_zero_initialized_wait_set()), context_(context)
+  {
+    // Check context is not nullptr.
+    if (nullptr == context) {
+      throw std::invalid_argument("context is nullptr");
+    }
+    // Accumulate total contributions from waitables.
+    size_t subscriptions_from_waitables = 0;
+    size_t guard_conditions_from_waitables = 0;
+    size_t timers_from_waitables = 0;
+    size_t clients_from_waitables = 0;
+    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();
+      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();
+      clients_from_waitables += waitable.get_number_of_ready_clients();
+      services_from_waitables += waitable.get_number_of_ready_services();
+      events_from_waitables += waitable.get_number_of_ready_events();
+    }
+    // Initialize wait set using initial inputs.
+    rcl_ret_t ret = rcl_wait_set_init(
+      &rcl_wait_set_,
+      subscriptions.size() + subscriptions_from_waitables,
+      guard_conditions.size() + extra_guard_conditions.size() + guard_conditions_from_waitables,
+      timers.size() + timers_from_waitables,
+      clients.size() + clients_from_waitables,
+      services.size() + services_from_waitables,
+      events_from_waitables,
+      context_->get_rcl_context().get(),
+      // TODO(wjwwood): support custom allocator, maybe restrict to polymorphic allocator
+      rcl_get_default_allocator());
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret);
+    }
+
+    // (Re)build the wait set for the first time.
+    this->storage_rebuild_rcl_wait_set_with_sets(
+      subscriptions,
+      guard_conditions,
+      extra_guard_conditions,
+      timers,
+      clients,
+      services,
+      waitables);
+  }
+
+  ~StoragePolicyCommon()
+  {
+    rcl_ret_t ret = rcl_wait_set_fini(&rcl_wait_set_);
+    if (RCL_RET_OK != ret) {
+      try {
+        rclcpp::exceptions::throw_from_rcl_error(ret);
+      } catch (const std::exception & exception) {
+        RCLCPP_ERROR(
+          rclcpp::get_logger("rclcpp"),
+          "Error in destruction of rcl wait set: %s", exception.what());
+      }
+    }
+  }
+
+  template<class EntityT>
+  std::pair<void *, EntityT *>
+  get_raw_pointer_from_smart_pointer(const std::shared_ptr<EntityT> & shared_pointer)
+  {
+    return {nullptr, shared_pointer.get()};
+  }
+
+  template<class EntityT>
+  std::pair<std::shared_ptr<EntityT>, EntityT *>
+  get_raw_pointer_from_smart_pointer(const std::weak_ptr<EntityT> & weak_pointer)
+  {
+    auto shared_pointer = weak_pointer.lock();
+    return {shared_pointer, shared_pointer.get()};
+  }
+
+  /// Rebuild the wait set, preparing it for the next wait call.
+  /**
+   * The wait set is rebuilt by:
+   *
+   *   - resizing the wait set if needed,
+   *   - clearing the wait set if not already done by resizing, and
+   *   - re-adding the entities.
+   */
+  template<
+    class SubscriptionsIterable,
+    class GuardConditionsIterable,
+    class ExtraGuardConditionsIterable,
+    class TimersIterable,
+    class ClientsIterable,
+    class ServicesIterable,
+    class WaitablesIterable
+  >
+  void
+  storage_rebuild_rcl_wait_set_with_sets(
+    const SubscriptionsIterable & subscriptions,
+    const GuardConditionsIterable & guard_conditions,
+    const ExtraGuardConditionsIterable & extra_guard_conditions,
+    const TimersIterable & timers,
+    const ClientsIterable & clients,
+    const ServicesIterable & services,
+    const WaitablesIterable & waitables
+  )
+  {
+    bool was_resized = false;
+    // Resize the wait set, but only if it needs to be.
+    if (needs_resize_) {
+      // Resizing with rcl_wait_set_resize() is a no-op if nothing has changed,
+      // but tracking the need to resize in this class avoids an unnecessary
+      // library call (rcl is most likely a separate shared library) each wait
+      // loop.
+      // Also, since static storage wait sets will never need resizing, so it
+      // avoids completely redundant calls to this function in that case.
+      // Accumulate total contributions from waitables.
+      size_t subscriptions_from_waitables = 0;
+      size_t guard_conditions_from_waitables = 0;
+      size_t timers_from_waitables = 0;
+      size_t clients_from_waitables = 0;
+      size_t services_from_waitables = 0;
+      size_t events_from_waitables = 0;
+      for (const auto & waitable_entry : waitables) {
+        auto waitable_ptr_pair = get_raw_pointer_from_smart_pointer(waitable_entry.waitable);
+        if (nullptr == waitable_ptr_pair.second) {
+          // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
+          if (HasStrongOwnership) {
+            // This will not happen in fixed sized storage, as it holds
+            // shared ownership the whole time and is never in need of pruning.
+            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();
+        clients_from_waitables += waitable.get_number_of_ready_clients();
+        services_from_waitables += waitable.get_number_of_ready_services();
+        events_from_waitables += waitable.get_number_of_ready_events();
+      }
+      rcl_ret_t ret = rcl_wait_set_resize(
+        &rcl_wait_set_,
+        subscriptions.size() + subscriptions_from_waitables,
+        guard_conditions.size() + extra_guard_conditions.size() + guard_conditions_from_waitables,
+        timers.size() + timers_from_waitables,
+        clients.size() + clients_from_waitables,
+        services.size() + services_from_waitables,
+        events_from_waitables
+      );
+      if (RCL_RET_OK != ret) {
+        rclcpp::exceptions::throw_from_rcl_error(ret);
+      }
+      was_resized = true;
+      // Assumption: the calling code ensures this function is not called
+      // concurrently with functions that set this variable to true, either
+      // with documentation (as is the case for the SequentialSychronization
+      // policy), or with synchronization primitives (as is the case with
+      // the ThreadSafeSynchronization policy).
+      needs_resize_ = false;
+    }
+
+    // Now clear the wait set, but only if it was not resized, as resizing also
+    // clears the wait set.
+    if (!was_resized) {
+      rcl_ret_t ret = rcl_wait_set_clear(&rcl_wait_set_);
+      if (RCL_RET_OK != ret) {
+        rclcpp::exceptions::throw_from_rcl_error(ret);
+      }
+    }
+
+    // Add subscriptions.
+    for (const auto & subscription_entry : subscriptions) {
+      auto subscription_ptr_pair =
+        get_raw_pointer_from_smart_pointer(subscription_entry.subscription);
+      if (nullptr == subscription_ptr_pair.second) {
+        // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
+        if (HasStrongOwnership) {
+          // This will not happen in fixed sized storage, as it holds
+          // shared ownership the whole time and is never in need of pruning.
+          throw std::runtime_error("unexpected condition, fixed storage policy needs pruning");
+        }
+        // Flag for pruning.
+        needs_pruning_ = true;
+        continue;
+      }
+      rcl_ret_t ret = rcl_wait_set_add_subscription(
+        &rcl_wait_set_,
+        subscription_ptr_pair.second->get_subscription_handle().get(),
+        nullptr);
+      if (RCL_RET_OK != ret) {
+        rclcpp::exceptions::throw_from_rcl_error(ret);
+      }
+    }
+
+    // Setup common code to add guard_conditions.
+    auto add_guard_conditions =
+      [this](const auto & inner_guard_conditions)
+      {
+        for (const auto & guard_condition : inner_guard_conditions) {
+          auto guard_condition_ptr_pair = get_raw_pointer_from_smart_pointer(guard_condition);
+          if (nullptr == guard_condition_ptr_pair.second) {
+            // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
+            if (HasStrongOwnership) {
+              // This will not happen in fixed sized storage, as it holds
+              // shared ownership the whole time and is never in need of pruning.
+              throw std::runtime_error("unexpected condition, fixed storage policy needs pruning");
+            }
+            // Flag for pruning.
+            needs_pruning_ = true;
+            continue;
+          }
+          rcl_ret_t ret = rcl_wait_set_add_guard_condition(
+            &rcl_wait_set_,
+            &guard_condition_ptr_pair.second->get_rcl_guard_condition(),
+            nullptr);
+          if (RCL_RET_OK != ret) {
+            rclcpp::exceptions::throw_from_rcl_error(ret);
+          }
+        }
+      };
+
+    // Add guard conditions.
+    add_guard_conditions(guard_conditions);
+
+    // Add extra guard conditions.
+    add_guard_conditions(extra_guard_conditions);
+
+    // Add timers.
+    for (const auto & timer : timers) {
+      auto timer_ptr_pair = get_raw_pointer_from_smart_pointer(timer);
+      if (nullptr == timer_ptr_pair.second) {
+        // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
+        if (HasStrongOwnership) {
+          // This will not happen in fixed sized storage, as it holds
+          // shared ownership the whole time and is never in need of pruning.
+          throw std::runtime_error("unexpected condition, fixed storage policy needs pruning");
+        }
+        // Flag for pruning.
+        needs_pruning_ = true;
+        continue;
+      }
+      rcl_ret_t ret = rcl_wait_set_add_timer(
+        &rcl_wait_set_,
+        timer_ptr_pair.second->get_timer_handle().get(),
+        nullptr);
+      if (RCL_RET_OK != ret) {
+        rclcpp::exceptions::throw_from_rcl_error(ret);
+      }
+    }
+
+    // Add clients.
+    for (const auto & client : clients) {
+      auto client_ptr_pair = get_raw_pointer_from_smart_pointer(client);
+      if (nullptr == client_ptr_pair.second) {
+        // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
+        if (HasStrongOwnership) {
+          // This will not happen in fixed sized storage, as it holds
+          // shared ownership the whole time and is never in need of pruning.
+          throw std::runtime_error("unexpected condition, fixed storage policy needs pruning");
+        }
+        // Flag for pruning.
+        needs_pruning_ = true;
+        continue;
+      }
+      rcl_ret_t ret = rcl_wait_set_add_client(
+        &rcl_wait_set_,
+        client_ptr_pair.second->get_client_handle().get(),
+        nullptr);
+      if (RCL_RET_OK != ret) {
+        rclcpp::exceptions::throw_from_rcl_error(ret);
+      }
+    }
+
+    // Add services.
+    for (const auto & service : services) {
+      auto service_ptr_pair = get_raw_pointer_from_smart_pointer(service);
+      if (nullptr == service_ptr_pair.second) {
+        // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
+        if (HasStrongOwnership) {
+          // This will not happen in fixed sized storage, as it holds
+          // shared ownership the whole time and is never in need of pruning.
+          throw std::runtime_error("unexpected condition, fixed storage policy needs pruning");
+        }
+        // Flag for pruning.
+        needs_pruning_ = true;
+        continue;
+      }
+      rcl_ret_t ret = rcl_wait_set_add_service(
+        &rcl_wait_set_,
+        service_ptr_pair.second->get_service_handle().get(),
+        nullptr);
+      if (RCL_RET_OK != ret) {
+        rclcpp::exceptions::throw_from_rcl_error(ret);
+      }
+    }
+
+    // Add waitables.
+    for (auto & waitable_entry : waitables) {
+      auto waitable_ptr_pair = get_raw_pointer_from_smart_pointer(waitable_entry.waitable);
+      if (nullptr == waitable_ptr_pair.second) {
+        // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
+        if (HasStrongOwnership) {
+          // This will not happen in fixed sized storage, as it holds
+          // shared ownership the whole time and is never in need of pruning.
+          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;
+      bool successful = waitable.add_to_wait_set(&rcl_wait_set_);
+      if (!successful) {
+        throw std::runtime_error("waitable unexpectedly failed to be added to wait set");
+      }
+    }
+  }
+
+  const rcl_wait_set_t &
+  storage_get_rcl_wait_set() const
+  {
+    return rcl_wait_set_;
+  }
+
+  rcl_wait_set_t &
+  storage_get_rcl_wait_set()
+  {
+    return rcl_wait_set_;
+  }
+
+  void
+  storage_flag_for_resize()
+  {
+    needs_resize_ = true;
+  }
+
+  rcl_wait_set_t rcl_wait_set_;
+  rclcpp::Context::SharedPtr context_;
+
+  bool needs_pruning_ = false;
+  bool needs_resize_ = false;
+};
+
+}  // namespace detail
+}  // namespace wait_set_policies
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_

rclcpp/include/rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp

@@ -0,0 +1,72 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__SYNCHRONIZATION_POLICY_COMMON_HPP_
+#define RCLCPP__WAIT_SET_POLICIES__DETAIL__SYNCHRONIZATION_POLICY_COMMON_HPP_
+
+#include <chrono>
+#include <functional>
+
+namespace rclcpp
+{
+namespace wait_set_policies
+{
+namespace detail
+{
+
+/// Common structure for synchronization policies.
+class SynchronizationPolicyCommon
+{
+protected:
+  SynchronizationPolicyCommon() = default;
+  ~SynchronizationPolicyCommon() = default;
+
+  std::function<bool()>
+  create_loop_predicate(
+    std::chrono::nanoseconds time_to_wait_ns,
+    std::chrono::steady_clock::time_point start)
+  {
+    if (time_to_wait_ns >= std::chrono::nanoseconds(0)) {
+      // If time_to_wait_ns is >= 0 schedule against a deadline.
+      auto deadline = start + time_to_wait_ns;
+      return [deadline]() -> bool {return std::chrono::steady_clock::now() < deadline;};
+    } else {
+      // In the case of time_to_wait_ns < 0, just always return true to loop forever.
+      return []() -> bool {return true;};
+    }
+  }
+
+  std::chrono::nanoseconds
+  calculate_time_left_to_wait(
+    std::chrono::nanoseconds original_time_to_wait_ns,
+    std::chrono::steady_clock::time_point start)
+  {
+    std::chrono::nanoseconds time_left_to_wait;
+    if (original_time_to_wait_ns < std::chrono::nanoseconds(0)) {
+      time_left_to_wait = original_time_to_wait_ns;
+    } else {
+      time_left_to_wait = original_time_to_wait_ns - (std::chrono::steady_clock::now() - start);
+      if (time_left_to_wait < std::chrono::nanoseconds(0)) {
+        time_left_to_wait = std::chrono::nanoseconds(0);
+      }
+    }
+    return time_left_to_wait;
+  }
+};
+
+}  // namespace detail
+}  // namespace wait_set_policies
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__SYNCHRONIZATION_POLICY_COMMON_HPP_

rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp

@@ -0,0 +1,243 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__WRITE_PREFERRING_READ_WRITE_LOCK_HPP_
+#define RCLCPP__WAIT_SET_POLICIES__DETAIL__WRITE_PREFERRING_READ_WRITE_LOCK_HPP_
+
+#include <condition_variable>
+#include <functional>
+#include <mutex>
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace wait_set_policies
+{
+namespace detail
+{
+
+/// Writer-perferring read-write lock.
+/**
+ * This class is based on an implementation of a "write-preferring RW lock" as described in this
+ * wikipedia page:
+ *
+ * https://en.wikipedia.org/wiki/Readers%E2%80%93writer_lock#Using_a_condition_variable_and_a_mutex
+ *
+ * Copying here for posterity:
+ *
+ * \verbatim
+ *   For a write-preferring RW lock one can use two integer counters and one boolean flag:
+ *
+ *       num_readers_active: the number of readers that have acquired the lock (integer)
+ *       num_writers_waiting: the number of writers waiting for access (integer)
+ *       writer_active: whether a writer has acquired the lock (boolean).
+ *
+ *   Initially num_readers_active and num_writers_waiting are zero and writer_active is false.
+ *
+ *   The lock and release operations can be implemented as
+ *
+ *   Begin Read
+ *
+ *       Lock g
+ *       While num_writers_waiting > 0 or writer_active:
+ *           wait cond, g[a]
+ *       Increment num_readers_active
+ *       Unlock g.
+ *
+ *   End Read
+ *
+ *       Lock g
+ *       Decrement num_readers_active
+ *       If num_readers_active = 0:
+ *           Notify cond (broadcast)
+ *       Unlock g.
+ *
+ *   Begin Write
+ *
+ *       Lock g
+ *       Increment num_writers_waiting
+ *       While num_readers_active > 0 or writer_active is true:
+ *           wait cond, g
+ *       Decrement num_writers_waiting
+ *       Set writer_active to true
+ *       Unlock g.
+ *
+ *   End Write
+ *
+ *       Lock g
+ *       Set writer_active to false
+ *       Notify cond (broadcast)
+ *       Unlock g.
+ * \endverbatim
+ *
+ * It will prefer any waiting write calls to any waiting read calls, meaning
+ * that excessive write calls can starve read calls.
+ *
+ * This class diverges from that design in two important ways.
+ * First, it is a single reader, single writer version.
+ * Second, it allows for user defined code to be run after a writer enters the
+ * waiting state, and the purpose of this feature is to allow the user to
+ * interrupt any potentially long blocking read activities.
+ *
+ * Together these two features allow new waiting writers to not only ensure
+ * they get the lock before any queued readers, but also that it can safely
+ * interrupt read activities if needed, without allowing new read activities to
+ * start before it gains the lock.
+ *
+ * The first difference prevents the case that a multiple read activities occur
+ * at the same time but the writer can only reliably interrupt one of them.
+ * By preventing multiple read activities concurrently, this case is avoided.
+ * The second difference allows the user to define how to interrupt read
+ * activity that could be blocking the write activities that need to happen
+ * as soon as possible.
+ *
+ * To implement the differences, this class replaces the "num_readers_active"
+ * counter with a "reader_active" boolean.
+ * It also changes the "Begin Read" section from above, like this:
+ *
+ * \verbatim
+ *   Begin Read
+ *
+ *       Lock g
+ *       While num_writers_waiting > 0 or writer_active or reader_active:  // changed
+ *           wait cond, g[a]
+ *       Set reader_active to true  // changed
+ *       Unlock g.
+ * \endverbatim
+ *
+ * And changes the "End Read" section from above, like this:
+ *
+ * \verbatim
+ *   End Read
+ *
+ *       Lock g
+ *       Set reader_active to false  // changed
+ *       Notify cond (broadcast)  // changed, now unconditional
+ *       Unlock g.
+ * \endverbatim
+ *
+ * The "Begin Write" section is also updated as follows:
+ *
+ * \verbatim
+ *   Begin Write
+ *
+ *       Lock g
+ *       Increment num_writers_waiting
+ *       Call user defined enter_waiting function  // new
+ *       While reader_active is true or writer_active is true:  // changed
+ *           wait cond, g
+ *       Decrement num_writers_waiting
+ *       Set writer_active to true
+ *       Unlock g.
+ * \endverbatim
+ *
+ * The implementation uses a single condition variable, single lock, and several
+ * state variables.
+ *
+ * The typical use of this class is as follows:
+ *
+ *     class MyClass
+ *     {
+ *       WritePreferringReadWriteLock wprw_lock_;
+ *     public:
+ *       MyClass() {}
+ *       void do_some_reading()
+ *       {
+ *         using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
+ *         std::lock_guard<WritePreferringReadWriteLock::ReadMutex> lock(wprw_lock_.get_read_mutex());
+ *         // Do reading...
+ *       }
+ *       void do_some_writing()
+ *       {
+ *         using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
+ *         std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
+ *         // Do writing...
+ *       }
+ *     };
+ */
+class WritePreferringReadWriteLock final
+{
+public:
+  RCLCPP_PUBLIC
+  explicit WritePreferringReadWriteLock(std::function<void()> enter_waiting_function = nullptr);
+
+  /// Read mutex for the WritePreferringReadWriteLock.
+  /**
+   * Implements the "C++ named requirements: BasicLockable".
+   */
+  class RCLCPP_PUBLIC ReadMutex
+  {
+public:
+    void
+    lock();
+
+    void
+    unlock();
+
+protected:
+    explicit ReadMutex(WritePreferringReadWriteLock & parent_lock);
+
+    WritePreferringReadWriteLock & parent_lock_;
+
+    friend WritePreferringReadWriteLock;
+  };
+
+  /// Write mutex for the WritePreferringReadWriteLock.
+  /**
+   * Implements the "C++ named requirements: BasicLockable".
+   */
+  class RCLCPP_PUBLIC WriteMutex
+  {
+public:
+    void
+    lock();
+
+    void
+    unlock();
+
+protected:
+    explicit WriteMutex(WritePreferringReadWriteLock & parent_lock);
+
+    WritePreferringReadWriteLock & parent_lock_;
+
+    friend WritePreferringReadWriteLock;
+  };
+
+  /// Return read mutex which can be used with standard constructs like std::lock_guard.
+  RCLCPP_PUBLIC
+  ReadMutex &
+  get_read_mutex();
+
+  /// Return write mutex which can be used with standard constructs like std::lock_guard.
+  RCLCPP_PUBLIC
+  WriteMutex &
+  get_write_mutex();
+
+protected:
+  bool reader_active_ = false;
+  std::size_t number_of_writers_waiting_ = 0;
+  bool writer_active_ = false;
+  std::mutex mutex_;
+  std::condition_variable condition_variable_;
+  ReadMutex read_mutex_;
+  WriteMutex write_mutex_;
+  std::function<void()> enter_waiting_function_;
+};
+
+}  // namespace detail
+}  // namespace wait_set_policies
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__WRITE_PREFERRING_READ_WRITE_LOCK_HPP_

rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp

@@ -0,0 +1,470 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAIT_SET_POLICIES__DYNAMIC_STORAGE_HPP_
+#define RCLCPP__WAIT_SET_POLICIES__DYNAMIC_STORAGE_HPP_
+
+#include <algorithm>
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "rclcpp/client.hpp"
+#include "rclcpp/guard_condition.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/subscription_wait_set_mask.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/wait_set_policies/detail/storage_policy_common.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace wait_set_policies
+{
+
+/// WaitSet policy that provides dynamically sized storage.
+class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCommon<false>
+{
+protected:
+  using is_mutable = std::true_type;
+
+  class SubscriptionEntry
+  {
+    // (wjwwood): indent of 'public:' is weird, I know. uncrustify is dumb.
+
+public:
+    std::shared_ptr<rclcpp::SubscriptionBase> subscription;
+    rclcpp::SubscriptionWaitSetMask mask;
+
+    /// Conversion constructor, which is intentionally not marked explicit.
+    SubscriptionEntry(
+      const std::shared_ptr<rclcpp::SubscriptionBase> & subscription_in = nullptr,
+      const rclcpp::SubscriptionWaitSetMask & mask_in = {})
+    : subscription(subscription_in),
+      mask(mask_in)
+    {}
+
+    void
+    reset() noexcept
+    {
+      subscription.reset();
+    }
+  };
+  class WeakSubscriptionEntry
+  {
+public:
+    std::weak_ptr<rclcpp::SubscriptionBase> subscription;
+    rclcpp::SubscriptionWaitSetMask mask;
+
+    explicit WeakSubscriptionEntry(
+      const std::shared_ptr<rclcpp::SubscriptionBase> & subscription_in,
+      const rclcpp::SubscriptionWaitSetMask & mask_in) noexcept
+    : subscription(subscription_in),
+      mask(mask_in)
+    {}
+
+    explicit WeakSubscriptionEntry(const SubscriptionEntry & other)
+    : subscription(other.subscription),
+      mask(other.mask)
+    {}
+
+    std::shared_ptr<rclcpp::SubscriptionBase>
+    lock() const
+    {
+      return subscription.lock();
+    }
+
+    bool
+    expired() const noexcept
+    {
+      return subscription.expired();
+    }
+  };
+  using SequenceOfWeakSubscriptions = std::vector<WeakSubscriptionEntry>;
+  using SubscriptionsIterable = std::vector<SubscriptionEntry>;
+
+  using SequenceOfWeakGuardConditions = std::vector<std::weak_ptr<rclcpp::GuardCondition>>;
+  using GuardConditionsIterable = std::vector<std::shared_ptr<rclcpp::GuardCondition>>;
+
+  using SequenceOfWeakTimers = std::vector<std::weak_ptr<rclcpp::TimerBase>>;
+  using TimersIterable = std::vector<std::shared_ptr<rclcpp::TimerBase>>;
+
+  using SequenceOfWeakClients = std::vector<std::weak_ptr<rclcpp::ClientBase>>;
+  using ClientsIterable = std::vector<std::shared_ptr<rclcpp::ClientBase>>;
+
+  using SequenceOfWeakServices = std::vector<std::weak_ptr<rclcpp::ServiceBase>>;
+  using ServicesIterable = std::vector<std::shared_ptr<rclcpp::ServiceBase>>;
+
+  class WaitableEntry
+  {
+public:
+    std::shared_ptr<rclcpp::Waitable> waitable;
+    std::shared_ptr<void> associated_entity;
+
+    /// Conversion constructor, which is intentionally not marked explicit.
+    WaitableEntry(
+      const std::shared_ptr<rclcpp::Waitable> & waitable_in = nullptr,
+      const std::shared_ptr<void> & associated_entity_in = nullptr) noexcept
+    : waitable(waitable_in),
+      associated_entity(associated_entity_in)
+    {}
+
+    void
+    reset() noexcept
+    {
+      waitable.reset();
+      associated_entity.reset();
+    }
+  };
+  class WeakWaitableEntry
+  {
+public:
+    std::weak_ptr<rclcpp::Waitable> waitable;
+    std::weak_ptr<void> associated_entity;
+
+    explicit WeakWaitableEntry(
+      const std::shared_ptr<rclcpp::Waitable> & waitable_in,
+      const std::shared_ptr<void> & associated_entity_in) noexcept
+    : waitable(waitable_in),
+      associated_entity(associated_entity_in)
+    {}
+
+    explicit WeakWaitableEntry(const WaitableEntry & other)
+    : waitable(other.waitable),
+      associated_entity(other.associated_entity)
+    {}
+
+    std::shared_ptr<rclcpp::Waitable>
+    lock() const
+    {
+      return waitable.lock();
+    }
+
+    bool
+    expired() const noexcept
+    {
+      return waitable.expired();
+    }
+  };
+  using SequenceOfWeakWaitables = std::vector<WeakWaitableEntry>;
+  using WaitablesIterable = std::vector<WaitableEntry>;
+
+  template<class ArrayOfExtraGuardConditions>
+  explicit
+  DynamicStorage(
+    const SubscriptionsIterable & subscriptions,
+    const GuardConditionsIterable & guard_conditions,
+    const ArrayOfExtraGuardConditions & extra_guard_conditions,
+    const TimersIterable & timers,
+    const ClientsIterable & clients,
+    const ServicesIterable & services,
+    const WaitablesIterable & waitables,
+    rclcpp::Context::SharedPtr context
+  )
+  : StoragePolicyCommon(
+      subscriptions,
+      guard_conditions,
+      extra_guard_conditions,
+      timers,
+      clients,
+      services,
+      waitables,
+      context),
+    subscriptions_(subscriptions.cbegin(), subscriptions.cend()),
+    shared_subscriptions_(subscriptions_.size()),
+    guard_conditions_(guard_conditions.cbegin(), guard_conditions.cend()),
+    shared_guard_conditions_(guard_conditions_.size()),
+    timers_(timers.cbegin(), timers.cend()),
+    shared_timers_(timers_.size()),
+    clients_(clients.cbegin(), clients.cend()),
+    shared_clients_(clients_.size()),
+    services_(services.cbegin(), services.cend()),
+    shared_services_(services_.size()),
+    waitables_(waitables.cbegin(), waitables.cend()),
+    shared_waitables_(waitables_.size())
+  {}
+
+  ~DynamicStorage() = default;
+
+  template<class ArrayOfExtraGuardConditions>
+  void
+  storage_rebuild_rcl_wait_set(const ArrayOfExtraGuardConditions & extra_guard_conditions)
+  {
+    this->storage_rebuild_rcl_wait_set_with_sets(
+      subscriptions_,
+      guard_conditions_,
+      extra_guard_conditions,
+      timers_,
+      clients_,
+      services_,
+      waitables_
+    );
+  }
+
+  template<class EntityT, class SequenceOfEntitiesT>
+  static
+  bool
+  storage_has_entity(const EntityT & entity, const SequenceOfEntitiesT & entities)
+  {
+    return std::any_of(
+      entities.cbegin(),
+      entities.cend(),
+      [&entity](const auto & inner) {return &entity == inner.lock().get();});
+  }
+
+  template<class EntityT, class SequenceOfEntitiesT>
+  static
+  auto
+  storage_find_entity(const EntityT & entity, const SequenceOfEntitiesT & entities)
+  {
+    return std::find_if(
+      entities.cbegin(),
+      entities.cend(),
+      [&entity](const auto & inner) {return &entity == inner.lock().get();});
+  }
+
+  void
+  storage_add_subscription(std::shared_ptr<rclcpp::SubscriptionBase> && subscription)
+  {
+    if (this->storage_has_entity(*subscription, subscriptions_)) {
+      throw std::runtime_error("subscription already in wait set");
+    }
+    WeakSubscriptionEntry weak_entry{std::move(subscription), {}};
+    subscriptions_.push_back(std::move(weak_entry));
+    this->storage_flag_for_resize();
+  }
+
+  void
+  storage_remove_subscription(std::shared_ptr<rclcpp::SubscriptionBase> && subscription)
+  {
+    auto it = this->storage_find_entity(*subscription, subscriptions_);
+    if (subscriptions_.cend() == it) {
+      throw std::runtime_error("subscription not in wait set");
+    }
+    subscriptions_.erase(it);
+    this->storage_flag_for_resize();
+  }
+
+  void
+  storage_add_guard_condition(std::shared_ptr<rclcpp::GuardCondition> && guard_condition)
+  {
+    if (this->storage_has_entity(*guard_condition, guard_conditions_)) {
+      throw std::runtime_error("guard_condition already in wait set");
+    }
+    guard_conditions_.push_back(std::move(guard_condition));
+    this->storage_flag_for_resize();
+  }
+
+  void
+  storage_remove_guard_condition(std::shared_ptr<rclcpp::GuardCondition> && guard_condition)
+  {
+    auto it = this->storage_find_entity(*guard_condition, guard_conditions_);
+    if (guard_conditions_.cend() == it) {
+      throw std::runtime_error("guard_condition not in wait set");
+    }
+    guard_conditions_.erase(it);
+    this->storage_flag_for_resize();
+  }
+
+  void
+  storage_add_timer(std::shared_ptr<rclcpp::TimerBase> && timer)
+  {
+    if (this->storage_has_entity(*timer, timers_)) {
+      throw std::runtime_error("timer already in wait set");
+    }
+    timers_.push_back(std::move(timer));
+    this->storage_flag_for_resize();
+  }
+
+  void
+  storage_remove_timer(std::shared_ptr<rclcpp::TimerBase> && timer)
+  {
+    auto it = this->storage_find_entity(*timer, timers_);
+    if (timers_.cend() == it) {
+      throw std::runtime_error("timer not in wait set");
+    }
+    timers_.erase(it);
+    this->storage_flag_for_resize();
+  }
+
+  void
+  storage_add_client(std::shared_ptr<rclcpp::ClientBase> && client)
+  {
+    if (this->storage_has_entity(*client, clients_)) {
+      throw std::runtime_error("client already in wait set");
+    }
+    clients_.push_back(std::move(client));
+    this->storage_flag_for_resize();
+  }
+
+  void
+  storage_remove_client(std::shared_ptr<rclcpp::ClientBase> && client)
+  {
+    auto it = this->storage_find_entity(*client, clients_);
+    if (clients_.cend() == it) {
+      throw std::runtime_error("client not in wait set");
+    }
+    clients_.erase(it);
+    this->storage_flag_for_resize();
+  }
+
+  void
+  storage_add_service(std::shared_ptr<rclcpp::ServiceBase> && service)
+  {
+    if (this->storage_has_entity(*service, services_)) {
+      throw std::runtime_error("service already in wait set");
+    }
+    services_.push_back(std::move(service));
+    this->storage_flag_for_resize();
+  }
+
+  void
+  storage_remove_service(std::shared_ptr<rclcpp::ServiceBase> && service)
+  {
+    auto it = this->storage_find_entity(*service, services_);
+    if (services_.cend() == it) {
+      throw std::runtime_error("service not in wait set");
+    }
+    services_.erase(it);
+    this->storage_flag_for_resize();
+  }
+
+  void
+  storage_add_waitable(
+    std::shared_ptr<rclcpp::Waitable> && waitable,
+    std::shared_ptr<void> && associated_entity)
+  {
+    if (this->storage_has_entity(*waitable, waitables_)) {
+      throw std::runtime_error("waitable already in wait set");
+    }
+    WeakWaitableEntry weak_entry(std::move(waitable), std::move(associated_entity));
+    waitables_.push_back(std::move(weak_entry));
+    this->storage_flag_for_resize();
+  }
+
+  void
+  storage_remove_waitable(std::shared_ptr<rclcpp::Waitable> && waitable)
+  {
+    auto it = this->storage_find_entity(*waitable, waitables_);
+    if (waitables_.cend() == it) {
+      throw std::runtime_error("waitable not in wait set");
+    }
+    waitables_.erase(it);
+    this->storage_flag_for_resize();
+  }
+
+  // this is noexcept because:
+  //   - std::weak_ptr::expired is noexcept
+  //   - the erase-remove idiom is noexcept, since we're not using the ExecutionPolicy version
+  //   - std::vector::erase is noexcept if the assignment operator of T is also
+  //     - and, the operator= for std::weak_ptr is noexcept
+  void
+  storage_prune_deleted_entities() noexcept
+  {
+    // reusable (templated) lambda for removal predicate
+    auto p =
+      [](const auto & weak_ptr) {
+        // remove entries which have expired
+        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));
+  }
+
+  void
+  storage_acquire_ownerships()
+  {
+    if (++ownership_reference_counter_ > 1) {
+      // Avoid redundant locking.
+      return;
+    }
+    // Setup common locking function.
+    auto lock_all = [](const auto & weak_ptrs, auto & shared_ptrs) {
+        shared_ptrs.resize(weak_ptrs.size());
+        size_t index = 0;
+        for (const auto & weak_ptr : weak_ptrs) {
+          shared_ptrs[index++] = weak_ptr.lock();
+        }
+      };
+    // Lock all the weak pointers and hold them until released.
+    lock_all(guard_conditions_, shared_guard_conditions_);
+    lock_all(timers_, shared_timers_);
+    lock_all(clients_, shared_clients_);
+    lock_all(services_, shared_services_);
+
+    // We need a specialized version of this for waitables.
+    auto lock_all_waitables = [](const auto & weak_ptrs, auto & shared_ptrs) {
+        shared_ptrs.resize(weak_ptrs.size());
+        size_t index = 0;
+        for (const auto & weak_ptr : weak_ptrs) {
+          shared_ptrs[index++] = WaitableEntry{
+            weak_ptr.waitable.lock(),
+            weak_ptr.associated_entity.lock()};
+        }
+      };
+    lock_all_waitables(waitables_, shared_waitables_);
+  }
+
+  void
+  storage_release_ownerships()
+  {
+    if (--ownership_reference_counter_ > 0) {
+      // Avoid releasing ownership until reference count is 0.
+      return;
+    }
+    // "Unlock" all shared pointers by resetting them.
+    auto reset_all = [](auto & shared_ptrs) {
+        for (auto & shared_ptr : shared_ptrs) {
+          shared_ptr.reset();
+        }
+      };
+    reset_all(shared_guard_conditions_);
+    reset_all(shared_timers_);
+    reset_all(shared_clients_);
+    reset_all(shared_services_);
+    reset_all(shared_waitables_);
+  }
+
+  size_t ownership_reference_counter_ = 0;
+
+  SequenceOfWeakSubscriptions subscriptions_;
+  SubscriptionsIterable shared_subscriptions_;
+
+  SequenceOfWeakGuardConditions guard_conditions_;
+  GuardConditionsIterable shared_guard_conditions_;
+
+  SequenceOfWeakTimers timers_;
+  TimersIterable shared_timers_;
+
+  SequenceOfWeakClients clients_;
+  ClientsIterable shared_clients_;
+
+  SequenceOfWeakServices services_;
+  ServicesIterable shared_services_;
+
+  SequenceOfWeakWaitables waitables_;
+  WaitablesIterable shared_waitables_;
+};
+
+}  // namespace wait_set_policies
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_SET_POLICIES__DYNAMIC_STORAGE_HPP_

rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp

@@ -0,0 +1,317 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAIT_SET_POLICIES__SEQUENTIAL_SYNCHRONIZATION_HPP_
+#define RCLCPP__WAIT_SET_POLICIES__SEQUENTIAL_SYNCHRONIZATION_HPP_
+
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <utility>
+
+#include "rclcpp/client.hpp"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/guard_condition.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/subscription_wait_set_mask.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/wait_result.hpp"
+#include "rclcpp/wait_result_kind.hpp"
+#include "rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace wait_set_policies
+{
+
+/// WaitSet policy that explicitly provides no thread synchronization.
+class SequentialSynchronization : public detail::SynchronizationPolicyCommon
+{
+protected:
+  explicit SequentialSynchronization(rclcpp::Context::SharedPtr) {}
+  ~SequentialSynchronization() = default;
+
+  /// Return any "extra" guard conditions needed to implement the synchronization policy.
+  /**
+   * Since this policy provides no thread-safety, it also needs no extra guard
+   * conditions to implement it.
+   */
+  const std::array<std::shared_ptr<rclcpp::GuardCondition>, 0> &
+  get_extra_guard_conditions()
+  {
+    static const std::array<std::shared_ptr<rclcpp::GuardCondition>, 0> empty{};
+    return empty;
+  }
+
+  /// Add subscription without thread-safety.
+  /**
+   * Does not throw, but storage function may throw.
+   */
+  void
+  sync_add_subscription(
+    std::shared_ptr<rclcpp::SubscriptionBase> && subscription,
+    const rclcpp::SubscriptionWaitSetMask & mask,
+    std::function<
+      void(std::shared_ptr<rclcpp::SubscriptionBase>&&, const rclcpp::SubscriptionWaitSetMask &)
+    > add_subscription_function)
+  {
+    // Explicitly no thread synchronization.
+    add_subscription_function(std::move(subscription), mask);
+  }
+
+  /// Remove guard condition without thread-safety.
+  /**
+   * Does not throw, but storage function may throw.
+   */
+  void
+  sync_remove_subscription(
+    std::shared_ptr<rclcpp::SubscriptionBase> && subscription,
+    const rclcpp::SubscriptionWaitSetMask & mask,
+    std::function<
+      void(std::shared_ptr<rclcpp::SubscriptionBase>&&, const rclcpp::SubscriptionWaitSetMask &)
+    > remove_subscription_function)
+  {
+    // Explicitly no thread synchronization.
+    remove_subscription_function(std::move(subscription), mask);
+  }
+
+  /// Add guard condition without thread-safety.
+  /**
+   * Does not throw, but storage function may throw.
+   */
+  void
+  sync_add_guard_condition(
+    std::shared_ptr<rclcpp::GuardCondition> && guard_condition,
+    std::function<void(std::shared_ptr<rclcpp::GuardCondition>&&)> add_guard_condition_function)
+  {
+    // Explicitly no thread synchronization.
+    add_guard_condition_function(std::move(guard_condition));
+  }
+
+  /// Remove guard condition without thread-safety.
+  /**
+   * Does not throw, but storage function may throw.
+   */
+  void
+  sync_remove_guard_condition(
+    std::shared_ptr<rclcpp::GuardCondition> && guard_condition,
+    std::function<void(std::shared_ptr<rclcpp::GuardCondition>&&)> remove_guard_condition_function)
+  {
+    // Explicitly no thread synchronization.
+    remove_guard_condition_function(std::move(guard_condition));
+  }
+
+  /// Add timer without thread-safety.
+  /**
+   * Does not throw, but storage function may throw.
+   */
+  void
+  sync_add_timer(
+    std::shared_ptr<rclcpp::TimerBase> && timer,
+    std::function<void(std::shared_ptr<rclcpp::TimerBase>&&)> add_timer_function)
+  {
+    // Explicitly no thread synchronization.
+    add_timer_function(std::move(timer));
+  }
+
+  /// Remove timer without thread-safety.
+  /**
+   * Does not throw, but storage function may throw.
+   */
+  void
+  sync_remove_timer(
+    std::shared_ptr<rclcpp::TimerBase> && timer,
+    std::function<void(std::shared_ptr<rclcpp::TimerBase>&&)> remove_timer_function)
+  {
+    // Explicitly no thread synchronization.
+    remove_timer_function(std::move(timer));
+  }
+
+  /// Add client without thread-safety.
+  /**
+   * Does not throw, but storage function may throw.
+   */
+  void
+  sync_add_client(
+    std::shared_ptr<rclcpp::ClientBase> && client,
+    std::function<void(std::shared_ptr<rclcpp::ClientBase>&&)> add_client_function)
+  {
+    // Explicitly no thread synchronization.
+    add_client_function(std::move(client));
+  }
+
+  /// Remove client without thread-safety.
+  /**
+   * Does not throw, but storage function may throw.
+   */
+  void
+  sync_remove_client(
+    std::shared_ptr<rclcpp::ClientBase> && client,
+    std::function<void(std::shared_ptr<rclcpp::ClientBase>&&)> remove_client_function)
+  {
+    // Explicitly no thread synchronization.
+    remove_client_function(std::move(client));
+  }
+
+  /// Add service without thread-safety.
+  /**
+   * Does not throw, but storage function may throw.
+   */
+  void
+  sync_add_service(
+    std::shared_ptr<rclcpp::ServiceBase> && service,
+    std::function<void(std::shared_ptr<rclcpp::ServiceBase>&&)> add_service_function)
+  {
+    // Explicitly no thread synchronization.
+    add_service_function(std::move(service));
+  }
+
+  /// Remove service without thread-safety.
+  /**
+   * Does not throw, but storage function may throw.
+   */
+  void
+  sync_remove_service(
+    std::shared_ptr<rclcpp::ServiceBase> && service,
+    std::function<void(std::shared_ptr<rclcpp::ServiceBase>&&)> remove_service_function)
+  {
+    // Explicitly no thread synchronization.
+    remove_service_function(std::move(service));
+  }
+
+  /// Add waitable without thread-safety.
+  /**
+   * Does not throw, but storage function may throw.
+   */
+  void
+  sync_add_waitable(
+    std::shared_ptr<rclcpp::Waitable> && waitable,
+    std::shared_ptr<void> && associated_entity,
+    std::function<
+      void(std::shared_ptr<rclcpp::Waitable>&&, std::shared_ptr<void>&&)
+    > add_waitable_function)
+  {
+    // Explicitly no thread synchronization.
+    add_waitable_function(std::move(waitable), std::move(associated_entity));
+  }
+
+  /// Remove waitable without thread-safety.
+  /**
+   * Does not throw, but storage function may throw.
+   */
+  void
+  sync_remove_waitable(
+    std::shared_ptr<rclcpp::Waitable> && waitable,
+    std::function<void(std::shared_ptr<rclcpp::Waitable>&&)> remove_waitable_function)
+  {
+    // Explicitly no thread synchronization.
+    remove_waitable_function(std::move(waitable));
+  }
+
+  /// Prune deleted entities without thread-safety.
+  /**
+   * Does not throw, but storage function may throw.
+   */
+  void
+  sync_prune_deleted_entities(std::function<void()> prune_deleted_entities_function)
+  {
+    // Explicitly no thread synchronization.
+    prune_deleted_entities_function();
+  }
+
+  /// Implements wait without any thread-safety.
+  template<class WaitResultT>
+  WaitResultT
+  sync_wait(
+    std::chrono::nanoseconds time_to_wait_ns,
+    std::function<void()> rebuild_rcl_wait_set,
+    std::function<rcl_wait_set_t & ()> get_rcl_wait_set,
+    std::function<WaitResultT(WaitResultKind wait_result_kind)> create_wait_result)
+  {
+    // Assumption: this function assumes that some measure has been taken to
+    // ensure none of the entities being waited on by the wait set are allowed
+    // to go out of scope and therefore be deleted.
+    // In the case of the StaticStorage policy, this is ensured because it
+    // retains shared ownership of all entites for the duration of its own life.
+    // In the case of the DynamicStorage policy, this is ensured by the function
+    // which calls this function, by acquiring shared ownership of the entites
+    // for the duration of this function.
+
+    // Setup looping predicate.
+    auto start = std::chrono::steady_clock::now();
+    std::function<bool()> should_loop = this->create_loop_predicate(time_to_wait_ns, start);
+
+    // Wait until exit condition is met.
+    do {
+      // Rebuild the wait set.
+      // This will resize the wait set if needed, due to e.g. adding or removing
+      // entities since the last wait, but this should never occur in static
+      // storage wait sets since they cannot be changed after construction.
+      // This will also clear the wait set and re-add all the entities, which
+      // prepares it to be waited on again.
+      rebuild_rcl_wait_set();
+
+      rcl_wait_set_t & rcl_wait_set = get_rcl_wait_set();
+
+      // Wait unconditionally until timeout condition occurs since we assume
+      // there are no conditions that would require the wait to stop and reset,
+      // like asynchronously adding or removing an entity, i.e. explicitly
+      // providing no thread-safety.
+
+      // Calculate how much time there is left to wait, unless blocking indefinitely.
+      auto time_left_to_wait_ns = this->calculate_time_left_to_wait(time_to_wait_ns, start);
+
+      // Then wait for entities to become ready.
+      rcl_ret_t ret = rcl_wait(&rcl_wait_set, time_left_to_wait_ns.count());
+      if (RCL_RET_OK == ret) {
+        // Something has become ready in the wait set, and since this class
+        // did not add anything to it, it is a user entity that is ready.
+        return create_wait_result(WaitResultKind::Ready);
+      } else if (RCL_RET_TIMEOUT == ret) {
+        // The wait set timed out, exit the loop.
+        break;
+      } else if (RCL_RET_WAIT_SET_EMPTY == ret) {
+        // Wait set was empty, return Empty.
+        return create_wait_result(WaitResultKind::Empty);
+      } else {
+        // Some other error case, throw.
+        rclcpp::exceptions::throw_from_rcl_error(ret);
+      }
+    } while (should_loop());
+
+    // Wait did not result in ready items, return timeout.
+    return create_wait_result(WaitResultKind::Timeout);
+  }
+
+  void
+  sync_wait_result_acquire()
+  {
+    // Explicitly do nothing.
+  }
+
+  void
+  sync_wait_result_release()
+  {
+    // Explicitly do nothing.
+  }
+};
+
+}  // namespace wait_set_policies
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_SET_POLICIES__SEQUENTIAL_SYNCHRONIZATION_HPP_

rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp

@@ -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.
+
+#ifndef RCLCPP__WAIT_SET_POLICIES__STATIC_STORAGE_HPP_
+#define RCLCPP__WAIT_SET_POLICIES__STATIC_STORAGE_HPP_
+
+#include <array>
+#include <memory>
+
+#include "rclcpp/client.hpp"
+#include "rclcpp/guard_condition.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/subscription_wait_set_mask.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/wait_set_policies/detail/storage_policy_common.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace wait_set_policies
+{
+
+/// WaitSet policy that explicitly provides fixed sized storage only.
+/**
+ * Note the underlying rcl_wait_set_t is still dynamically allocated, but only
+ * once during construction, and deallocated once during destruction.
+ */
+template<
+  std::size_t NumberOfSubscriptions,
+  std::size_t NumberOfGuardCondtions,
+  std::size_t NumberOfTimers,
+  std::size_t NumberOfClients,
+  std::size_t NumberOfServices,
+  std::size_t NumberOfWaitables
+>
+class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCommon<true>
+{
+protected:
+  using is_mutable = std::false_type;
+
+  class SubscriptionEntry
+  {
+public:
+    std::shared_ptr<rclcpp::SubscriptionBase> subscription;
+    rclcpp::SubscriptionWaitSetMask mask;
+
+    /// Conversion constructor, which is intentionally not marked explicit.
+    SubscriptionEntry(
+      const std::shared_ptr<rclcpp::SubscriptionBase> & subscription_in = nullptr,
+      const rclcpp::SubscriptionWaitSetMask & mask_in = {})
+    : subscription(subscription_in),
+      mask(mask_in)
+    {}
+  };
+  using ArrayOfSubscriptions = std::array<
+    SubscriptionEntry,
+    NumberOfSubscriptions
+  >;
+  using SubscriptionsIterable = ArrayOfSubscriptions;
+
+  using ArrayOfGuardConditions = std::array<
+    std::shared_ptr<rclcpp::GuardCondition>,
+    NumberOfGuardCondtions
+  >;
+  using GuardConditionsIterable = ArrayOfGuardConditions;
+
+  using ArrayOfTimers = std::array<
+    std::shared_ptr<rclcpp::TimerBase>,
+    NumberOfTimers
+  >;
+  using TimersIterable = ArrayOfTimers;
+
+  using ArrayOfClients = std::array<
+    std::shared_ptr<rclcpp::ClientBase>,
+    NumberOfClients
+  >;
+  using ClientsIterable = ArrayOfClients;
+
+  using ArrayOfServices = std::array<
+    std::shared_ptr<rclcpp::ServiceBase>,
+    NumberOfServices
+  >;
+  using ServicesIterable = ArrayOfServices;
+
+  struct WaitableEntry
+  {
+    /// Conversion constructor, which is intentionally not marked explicit.
+    WaitableEntry(
+      const std::shared_ptr<rclcpp::Waitable> & waitable_in = nullptr,
+      const std::shared_ptr<void> & associated_entity_in = nullptr) noexcept
+    : waitable(waitable_in),
+      associated_entity(associated_entity_in)
+    {}
+
+    std::shared_ptr<rclcpp::Waitable> waitable;
+    std::shared_ptr<void> associated_entity;
+  };
+  using ArrayOfWaitables = std::array<
+    WaitableEntry,
+    NumberOfWaitables
+  >;
+  using WaitablesIterable = ArrayOfWaitables;
+
+  template<class ArrayOfExtraGuardConditions>
+  explicit
+  StaticStorage(
+    const ArrayOfSubscriptions & subscriptions,
+    const ArrayOfGuardConditions & guard_conditions,
+    const ArrayOfExtraGuardConditions & extra_guard_conditions,
+    const ArrayOfTimers & timers,
+    const ArrayOfClients & clients,
+    const ArrayOfServices & services,
+    const ArrayOfWaitables & waitables,
+    rclcpp::Context::SharedPtr context
+  )
+  : StoragePolicyCommon(
+      subscriptions,
+      guard_conditions,
+      extra_guard_conditions,
+      timers,
+      clients,
+      services,
+      waitables,
+      context),
+    subscriptions_(subscriptions),
+    guard_conditions_(guard_conditions),
+    timers_(timers),
+    clients_(clients),
+    services_(services),
+    waitables_(waitables)
+  {}
+
+  ~StaticStorage() = default;
+
+  template<class ArrayOfExtraGuardConditions>
+  void
+  storage_rebuild_rcl_wait_set(const ArrayOfExtraGuardConditions & extra_guard_conditions)
+  {
+    this->storage_rebuild_rcl_wait_set_with_sets(
+      subscriptions_,
+      guard_conditions_,
+      extra_guard_conditions,
+      timers_,
+      clients_,
+      services_,
+      waitables_
+    );
+  }
+
+  // storage_add_subscription() explicitly not declared here
+  // storage_remove_subscription() explicitly not declared here
+  // storage_add_guard_condition() explicitly not declared here
+  // storage_remove_guard_condition() explicitly not declared here
+  // storage_add_timer() explicitly not declared here
+  // storage_remove_timer() explicitly not declared here
+  // storage_add_client() explicitly not declared here
+  // storage_remove_client() explicitly not declared here
+  // storage_add_service() explicitly not declared here
+  // storage_remove_service() explicitly not declared here
+  // storage_add_waitable() explicitly not declared here
+  // storage_remove_waitable() explicitly not declared here
+  // storage_prune_deleted_entities() explicitly not declared here
+
+  void
+  storage_acquire_ownerships()
+  {
+    // Explicitly do nothing.
+  }
+
+  void
+  storage_release_ownerships()
+  {
+    // Explicitly do nothing.
+  }
+
+  const ArrayOfSubscriptions subscriptions_;
+  const ArrayOfGuardConditions guard_conditions_;
+  const ArrayOfTimers timers_;
+  const ArrayOfClients clients_;
+  const ArrayOfServices services_;
+  const ArrayOfWaitables waitables_;
+};
+
+}  // namespace wait_set_policies
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_SET_POLICIES__STATIC_STORAGE_HPP_

rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp

@@ -0,0 +1,379 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAIT_SET_POLICIES__THREAD_SAFE_SYNCHRONIZATION_HPP_
+#define RCLCPP__WAIT_SET_POLICIES__THREAD_SAFE_SYNCHRONIZATION_HPP_
+
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <utility>
+
+#include "rclcpp/client.hpp"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/guard_condition.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/subscription_wait_set_mask.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/wait_result.hpp"
+#include "rclcpp/wait_result_kind.hpp"
+#include "rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp"
+#include "rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace wait_set_policies
+{
+
+/// WaitSet policy that provides thread-safe synchronization for the wait set.
+/**
+ * This class uses a "write-preferring RW lock" so that adding items to, and
+ * removing items from, the wait set will take priority over reading, i.e.
+ * waiting.
+ * This is done since add and remove calls will interrupt the wait set anyways
+ * so it is wasteful to do "fair" locking when there are many add/remove
+ * operations queued up.
+ *
+ * There are some things to consider about the thread-safety provided by this
+ * policy.
+ * There are two categories of activities, reading and writing activities.
+ * The writing activities include all of the add and remove methods, as well as
+ * the prune_deleted_entities() method.
+ * The reading methods include the wait() method and keeping a WaitResult in
+ * scope.
+ * The reading and writing activities will not be run at the same time, and one
+ * will block the other.
+ * Therefore, if you are holding a WaitResult in scope, and try to add or
+ * remove an entity at the same time, they will block each other.
+ * The write activities will try to interrupt the wait() method by triggering
+ * a guard condition, but they have no way of causing the WaitResult to release
+ * its lock.
+ */
+class ThreadSafeSynchronization : public detail::SynchronizationPolicyCommon
+{
+protected:
+  explicit ThreadSafeSynchronization(rclcpp::Context::SharedPtr context)
+  : extra_guard_conditions_{{std::make_shared<rclcpp::GuardCondition>(context)}},
+    wprw_lock_([this]() {this->interrupt_waiting_wait_set();})
+  {}
+  ~ThreadSafeSynchronization() = default;
+
+  /// Return any "extra" guard conditions needed to implement the synchronization policy.
+  /**
+   * This policy has one guard condition which is used to interrupt the wait
+   * set when adding and removing entities.
+   */
+  const std::array<std::shared_ptr<rclcpp::GuardCondition>, 1> &
+  get_extra_guard_conditions()
+  {
+    return extra_guard_conditions_;
+  }
+
+  /// Interrupt any waiting wait set.
+  /**
+   * Used to interrupt the wait set when adding or removing items.
+   */
+  void
+  interrupt_waiting_wait_set()
+  {
+    extra_guard_conditions_[0]->trigger();
+  }
+
+  /// Add subscription.
+  void
+  sync_add_subscription(
+    std::shared_ptr<rclcpp::SubscriptionBase> && subscription,
+    const rclcpp::SubscriptionWaitSetMask & mask,
+    std::function<
+      void(std::shared_ptr<rclcpp::SubscriptionBase>&&, const rclcpp::SubscriptionWaitSetMask &)
+    > add_subscription_function)
+  {
+    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
+    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
+    add_subscription_function(std::move(subscription), mask);
+  }
+
+  /// Remove guard condition.
+  void
+  sync_remove_subscription(
+    std::shared_ptr<rclcpp::SubscriptionBase> && subscription,
+    const rclcpp::SubscriptionWaitSetMask & mask,
+    std::function<
+      void(std::shared_ptr<rclcpp::SubscriptionBase>&&, const rclcpp::SubscriptionWaitSetMask &)
+    > remove_subscription_function)
+  {
+    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
+    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
+    remove_subscription_function(std::move(subscription), mask);
+  }
+
+  /// Add guard condition.
+  void
+  sync_add_guard_condition(
+    std::shared_ptr<rclcpp::GuardCondition> && guard_condition,
+    std::function<void(std::shared_ptr<rclcpp::GuardCondition>&&)> add_guard_condition_function)
+  {
+    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
+    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
+    add_guard_condition_function(std::move(guard_condition));
+  }
+
+  /// Remove guard condition.
+  void
+  sync_remove_guard_condition(
+    std::shared_ptr<rclcpp::GuardCondition> && guard_condition,
+    std::function<void(std::shared_ptr<rclcpp::GuardCondition>&&)> remove_guard_condition_function)
+  {
+    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
+    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
+    remove_guard_condition_function(std::move(guard_condition));
+  }
+
+  /// Add timer.
+  void
+  sync_add_timer(
+    std::shared_ptr<rclcpp::TimerBase> && timer,
+    std::function<void(std::shared_ptr<rclcpp::TimerBase>&&)> add_timer_function)
+  {
+    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
+    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
+    add_timer_function(std::move(timer));
+  }
+
+  /// Remove timer.
+  void
+  sync_remove_timer(
+    std::shared_ptr<rclcpp::TimerBase> && timer,
+    std::function<void(std::shared_ptr<rclcpp::TimerBase>&&)> remove_timer_function)
+  {
+    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
+    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
+    remove_timer_function(std::move(timer));
+  }
+
+  /// Add client.
+  void
+  sync_add_client(
+    std::shared_ptr<rclcpp::ClientBase> && client,
+    std::function<void(std::shared_ptr<rclcpp::ClientBase>&&)> add_client_function)
+  {
+    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
+    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
+    add_client_function(std::move(client));
+  }
+
+  /// Remove client.
+  void
+  sync_remove_client(
+    std::shared_ptr<rclcpp::ClientBase> && client,
+    std::function<void(std::shared_ptr<rclcpp::ClientBase>&&)> remove_client_function)
+  {
+    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
+    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
+    remove_client_function(std::move(client));
+  }
+
+  /// Add service.
+  void
+  sync_add_service(
+    std::shared_ptr<rclcpp::ServiceBase> && service,
+    std::function<void(std::shared_ptr<rclcpp::ServiceBase>&&)> add_service_function)
+  {
+    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
+    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
+    add_service_function(std::move(service));
+  }
+
+  /// Remove service.
+  void
+  sync_remove_service(
+    std::shared_ptr<rclcpp::ServiceBase> && service,
+    std::function<void(std::shared_ptr<rclcpp::ServiceBase>&&)> remove_service_function)
+  {
+    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
+    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
+    remove_service_function(std::move(service));
+  }
+
+  /// Add waitable.
+  void
+  sync_add_waitable(
+    std::shared_ptr<rclcpp::Waitable> && waitable,
+    std::shared_ptr<void> && associated_entity,
+    std::function<
+      void(std::shared_ptr<rclcpp::Waitable>&&, std::shared_ptr<void>&&)
+    > add_waitable_function)
+  {
+    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
+    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
+    add_waitable_function(std::move(waitable), std::move(associated_entity));
+  }
+
+  /// Remove waitable.
+  void
+  sync_remove_waitable(
+    std::shared_ptr<rclcpp::Waitable> && waitable,
+    std::function<void(std::shared_ptr<rclcpp::Waitable>&&)> remove_waitable_function)
+  {
+    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
+    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
+    remove_waitable_function(std::move(waitable));
+  }
+
+  /// Prune deleted entities.
+  void
+  sync_prune_deleted_entities(std::function<void()> prune_deleted_entities_function)
+  {
+    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
+    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
+    prune_deleted_entities_function();
+  }
+
+  /// Implements wait.
+  template<class WaitResultT>
+  WaitResultT
+  sync_wait(
+    std::chrono::nanoseconds time_to_wait_ns,
+    std::function<void()> rebuild_rcl_wait_set,
+    std::function<rcl_wait_set_t & ()> get_rcl_wait_set,
+    std::function<WaitResultT(WaitResultKind wait_result_kind)> create_wait_result)
+  {
+    // Assumption: this function assumes that some measure has been taken to
+    // ensure none of the entities being waited on by the wait set are allowed
+    // to go out of scope and therefore be deleted.
+    // In the case of the StaticStorage policy, this is ensured because it
+    // retains shared ownership of all entites for the duration of its own life.
+    // In the case of the DynamicStorage policy, this is ensured by the function
+    // which calls this function, by acquiring shared ownership of the entites
+    // for the duration of this function.
+
+    // Setup looping predicate.
+    auto start = std::chrono::steady_clock::now();
+    std::function<bool()> should_loop = this->create_loop_predicate(time_to_wait_ns, start);
+
+    // Wait until exit condition is met.
+    do {
+      {
+        // We have to prevent the entity sets from being mutated while building
+        // the rcl wait set.
+        using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
+        std::lock_guard<WritePreferringReadWriteLock::ReadMutex> lock(wprw_lock_.get_read_mutex());
+        // Rebuild the wait set.
+        // This will resize the wait set if needed, due to e.g. adding or removing
+        // entities since the last wait, but this should never occur in static
+        // storage wait sets since they cannot be changed after construction.
+        // This will also clear the wait set and re-add all the entities, which
+        // prepares it to be waited on again.
+        rebuild_rcl_wait_set();
+      }
+
+      rcl_wait_set_t & rcl_wait_set = get_rcl_wait_set();
+
+      // Wait unconditionally until timeout condition occurs since we assume
+      // there are no conditions that would require the wait to stop and reset,
+      // like asynchronously adding or removing an entity, i.e. explicitly
+      // providing no thread-safety.
+
+      // Calculate how much time there is left to wait, unless blocking indefinitely.
+      auto time_left_to_wait_ns = this->calculate_time_left_to_wait(time_to_wait_ns, start);
+
+      // Then wait for entities to become ready.
+
+      // It is ok to wait while not having the lock acquired, because the state
+      // in the rcl wait set will not be updated until this method calls
+      // rebuild_rcl_wait_set().
+      rcl_ret_t ret = rcl_wait(&rcl_wait_set, time_left_to_wait_ns.count());
+      if (RCL_RET_OK == ret) {
+        // Something has become ready in the wait set, first check if it was
+        // the guard condition added by this class and/or a user defined guard condition.
+        const rcl_guard_condition_t * interrupt_guard_condition_ptr =
+          &(extra_guard_conditions_[0]->get_rcl_guard_condition());
+        bool was_interrupted_by_this_class = false;
+        bool any_user_guard_conditions_triggered = false;
+        for (size_t index = 0; index < rcl_wait_set.size_of_guard_conditions; ++index) {
+          const rcl_guard_condition_t * current = rcl_wait_set.guard_conditions[index];
+          if (nullptr != current) {
+            // Something is ready.
+            if (rcl_wait_set.guard_conditions[index] == interrupt_guard_condition_ptr) {
+              // This means that this class triggered a guard condition to interrupt this wait.
+              was_interrupted_by_this_class = true;
+            } else {
+              // This means it was a user guard condition.
+              any_user_guard_conditions_triggered = true;
+            }
+          }
+        }
+
+        if (!was_interrupted_by_this_class || any_user_guard_conditions_triggered) {
+          // In this case we know:
+          //   - something was ready
+          //   - it was either:
+          //     - not interrupted by this class, or
+          //     - maybe it was, but there were also user defined guard conditions.
+          //
+          // We cannot ignore user defined guard conditions, but we can ignore
+          // other kinds of user defined entities, because they will still be
+          // ready next time we wait, whereas guard conditions are cleared.
+          // Therefore we need to create a WaitResult and return it.
+
+          // The WaitResult will call sync_wait_result_acquire() and
+          // sync_wait_result_release() to ensure thread-safety by preventing
+          // the mutation of the entity sets while introspecting after waiting.
+          return create_wait_result(WaitResultKind::Ready);
+        }
+        // If we get here the we interrupted the wait set and there were no user
+        // guard conditions that needed to be handled.
+        // So we will loop and it will re-acquire the lock and rebuild the
+        // rcl wait set.
+      } else if (RCL_RET_TIMEOUT == ret) {
+        // The wait set timed out, exit the loop.
+        break;
+      } else if (RCL_RET_WAIT_SET_EMPTY == ret) {
+        // Wait set was empty, return Empty.
+        return create_wait_result(WaitResultKind::Empty);
+      } else {
+        // Some other error case, throw.
+        rclcpp::exceptions::throw_from_rcl_error(ret);
+      }
+    } while (should_loop());
+
+    // Wait did not result in ready items, return timeout.
+    return create_wait_result(WaitResultKind::Timeout);
+  }
+
+  void
+  sync_wait_result_acquire()
+  {
+    wprw_lock_.get_read_mutex().lock();
+  }
+
+  void
+  sync_wait_result_release()
+  {
+    wprw_lock_.get_read_mutex().unlock();
+  }
+
+protected:
+  std::array<std::shared_ptr<rclcpp::GuardCondition>, 1> extra_guard_conditions_;
+  rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock wprw_lock_;
+};
+
+}  // namespace wait_set_policies
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_SET_POLICIES__THREAD_SAFE_SYNCHRONIZATION_HPP_

rclcpp/include/rclcpp/wait_set_template.hpp

@@ -0,0 +1,743 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAIT_SET_TEMPLATE_HPP_
+#define RCLCPP__WAIT_SET_TEMPLATE_HPP_
+
+#include <chrono>
+#include <memory>
+#include <utility>
+
+#include "rcl/wait.h"
+
+#include "rclcpp/client.hpp"
+#include "rclcpp/context.hpp"
+#include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/guard_condition.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/scope_exit.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/subscription_wait_set_mask.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/wait_result.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+
+/// Encapsulates sets of waitable items which can be waited on as a group.
+/**
+ * This class uses the rcl_wait_set_t as storage, but it also helps manage the
+ * ownership of associated rclcpp types.
+ */
+template<class SynchronizationPolicy, class StoragePolicy>
+class WaitSetTemplate final : private SynchronizationPolicy, private StoragePolicy
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(WaitSetTemplate)
+
+  using typename StoragePolicy::SubscriptionEntry;
+  using typename StoragePolicy::WaitableEntry;
+
+  /// Construct a wait set with optional initial waitable entities and optional custom context.
+  /**
+   * For the waitables, they have additionally an "associated" entity, which
+   * you can read more about in the add and remove functions for those types
+   * in this class.
+   *
+   * \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] 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.
+   */
+  explicit
+  WaitSetTemplate(
+    const typename StoragePolicy::SubscriptionsIterable & subscriptions = {},
+    const typename StoragePolicy::GuardConditionsIterable & guard_conditions = {},
+    const typename StoragePolicy::TimersIterable & timers = {},
+    const typename StoragePolicy::ClientsIterable & clients = {},
+    const typename StoragePolicy::ServicesIterable & services = {},
+    const typename StoragePolicy::WaitablesIterable & waitables = {},
+    rclcpp::Context::SharedPtr context =
+    rclcpp::contexts::get_global_default_context())
+  : SynchronizationPolicy(context),
+    StoragePolicy(
+      subscriptions,
+      guard_conditions,
+      // this method comes from the SynchronizationPolicy
+      this->get_extra_guard_conditions(),
+      timers,
+      clients,
+      services,
+      waitables,
+      context)
+  {}
+
+  /// Return the internal rcl wait set object.
+  /**
+   * This method provides no thread-safety when accessing this structure.
+   * The state of this structure can be updated at anytime by methods like
+   * wait(), add_*(), remove_*(), etc.
+   */
+  const rcl_wait_set_t &
+  get_rcl_wait_set() const
+  {
+    // this method comes from the StoragePolicy
+    return this->storage_get_rcl_wait_set();
+  }
+
+  /// Add a subscription to this wait set.
+  /**
+   * \sa add_guard_condition() for details of how this method works.
+   *
+   * Additionally to the documentation for add_guard_condition, this method
+   * has a mask parameter which allows you to control which parts of the
+   * subscription is added to the wait set with this call.
+   * For example, you might want to include the actual subscription to this
+   * wait set, but add the intra-process waitable to another wait set.
+   * If intra-process is disabled, no error will occur, it will just be skipped.
+   *
+   * When introspecting after waiting, this subscription's shared pointer will
+   * be the Waitable's (intra-process or the QoS Events) "associated entity"
+   * pointer, for more easily figuring out which subscription which waitable
+   * goes with afterwards.
+   *
+   * \param[in] subscription Subscription to be added.
+   * \param[in] mask A class which controls which parts of the subscription to add.
+   * \throws std::invalid_argument if subscription is nullptr.
+   * \throws std::runtime_error if subscription has already been added or is
+   *   associated with another wait set.
+   * \throws exceptions based on the policies used.
+   */
+  void
+  add_subscription(
+    std::shared_ptr<rclcpp::SubscriptionBase> subscription,
+    rclcpp::SubscriptionWaitSetMask mask = {})
+  {
+    if (nullptr == subscription) {
+      throw std::invalid_argument("subscription is nullptr");
+    }
+    // this method comes from the SynchronizationPolicy
+    this->sync_add_subscription(
+      std::move(subscription),
+      mask,
+      [this](
+        std::shared_ptr<rclcpp::SubscriptionBase> && inner_subscription,
+        const rclcpp::SubscriptionWaitSetMask & mask)
+      {
+        // These methods comes from the StoragePolicy, and may not exist for
+        // fixed sized storage policies.
+        // It will throw if the subscription has already been added.
+        if (mask.include_subscription) {
+          auto local_subscription = inner_subscription;
+          bool already_in_use =
+          local_subscription->exchange_in_use_by_wait_set_state(local_subscription.get(), true);
+          if (already_in_use) {
+            throw std::runtime_error("subscription already associated with a wait set");
+          }
+          this->storage_add_subscription(std::move(local_subscription));
+        }
+        if (mask.include_events) {
+          for (auto event : inner_subscription->get_event_handlers()) {
+            auto local_subscription = inner_subscription;
+            bool already_in_use =
+            local_subscription->exchange_in_use_by_wait_set_state(event.get(), true);
+            if (already_in_use) {
+              throw std::runtime_error("subscription event already associated with a wait set");
+            }
+            this->storage_add_waitable(std::move(event), std::move(local_subscription));
+          }
+        }
+        if (mask.include_intra_process_waitable) {
+          auto local_subscription = inner_subscription;
+          auto waitable = inner_subscription->get_intra_process_waitable();
+          if (nullptr != waitable) {
+            bool already_in_use = local_subscription->exchange_in_use_by_wait_set_state(
+              waitable.get(),
+              true);
+            if (already_in_use) {
+              throw std::runtime_error(
+                "subscription intra-process waitable already associated with a wait set");
+            }
+            this->storage_add_waitable(
+              std::move(inner_subscription->get_intra_process_waitable()),
+              std::move(local_subscription));
+          }
+        }
+      });
+  }
+
+  /// Remove a subscription from this wait set.
+  /**
+   * \sa remove_guard_condition() for details of how this method works.
+   *
+   * Additionally to the documentation for add_guard_condition, this method
+   * has a mask parameter which allows you to control which parts of the
+   * subscription is added to the wait set with this call.
+   * You may remove items selectively from the wait set in a different order
+   * than they were added.
+   *
+   * \param[in] subscription Subscription to be removed.
+   * \param[in] mask A class which controls which parts of the subscription to remove.
+   * \throws std::invalid_argument if subscription is nullptr.
+   * \throws std::runtime_error if subscription is not part of the wait set.
+   * \throws exceptions based on the policies used.
+   */
+  void
+  remove_subscription(
+    std::shared_ptr<rclcpp::SubscriptionBase> subscription,
+    rclcpp::SubscriptionWaitSetMask mask = {})
+  {
+    if (nullptr == subscription) {
+      throw std::invalid_argument("subscription is nullptr");
+    }
+    // this method comes from the SynchronizationPolicy
+    this->sync_remove_subscription(
+      std::move(subscription),
+      mask,
+      [this](
+        std::shared_ptr<rclcpp::SubscriptionBase> && inner_subscription,
+        const rclcpp::SubscriptionWaitSetMask & mask)
+      {
+        // This method comes from the StoragePolicy, and it may not exist for
+        // fixed sized storage policies.
+        // It will throw if the subscription is not in the wait set.
+        if (mask.include_subscription) {
+          auto local_subscription = inner_subscription;
+          local_subscription->exchange_in_use_by_wait_set_state(local_subscription.get(), false);
+          this->storage_remove_subscription(std::move(local_subscription));
+        }
+        if (mask.include_events) {
+          for (auto event : inner_subscription->get_event_handlers()) {
+            auto local_subscription = inner_subscription;
+            local_subscription->exchange_in_use_by_wait_set_state(event.get(), false);
+            this->storage_remove_waitable(std::move(event));
+          }
+        }
+        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->storage_remove_waitable(std::move(local_waitable));
+          }
+        }
+      });
+  }
+
+  /// Add a guard condition to this wait set.
+  /**
+   * Guard condition is added to the wait set, and shared ownership is held
+   * while waiting.
+   * However, if between calls to wait() the guard condition's reference count
+   * goes to zero, it will be implicitly removed on the next call to wait().
+   *
+   * Except in the case of a fixed sized storage, where changes to the wait set
+   * cannot occur after construction, in which case it holds shared ownership
+   * at all times until the wait set is destroy, but this method also does not
+   * exist on a fixed sized wait set.
+   *
+   * This function may be thread-safe depending on the SynchronizationPolicy
+   * used with this class.
+   * Using the ThreadSafeWaitSetPolicy will ensure that wait() is interrupted
+   * and returns before this function adds the guard condition.
+   * Otherwise, it is not safe to call this function concurrently with wait().
+   *
+   * This function will not be enabled (will not be available) if the
+   * StoragePolicy does not allow editing of the wait set after initialization.
+   *
+   * \param[in] guard_condition Guard condition to be added.
+   * \throws std::invalid_argument if guard_condition is nullptr.
+   * \throws std::runtime_error if guard_condition has already been added or is
+   *   associated with another wait set.
+   * \throws exceptions based on the policies used.
+   */
+  void
+  add_guard_condition(std::shared_ptr<rclcpp::GuardCondition> guard_condition)
+  {
+    if (nullptr == guard_condition) {
+      throw std::invalid_argument("guard_condition is nullptr");
+    }
+    // this method comes from the SynchronizationPolicy
+    this->sync_add_guard_condition(
+      std::move(guard_condition),
+      [this](std::shared_ptr<rclcpp::GuardCondition> && inner_guard_condition) {
+        bool already_in_use = inner_guard_condition->exchange_in_use_by_wait_set_state(true);
+        if (already_in_use) {
+          throw std::runtime_error("guard condition already in use by another wait set");
+        }
+        // This method comes from the StoragePolicy, and it may not exist for
+        // fixed sized storage policies.
+        // It will throw if the guard condition has already been added.
+        this->storage_add_guard_condition(std::move(inner_guard_condition));
+      });
+  }
+
+  /// Remove a guard condition from this wait set.
+  /**
+   * Guard condition is removed from the wait set, and if needed the shared
+   * ownership is released.
+   *
+   * This function may be thread-safe depending on the SynchronizationPolicy
+   * used with this class.
+   * Using the ThreadSafeWaitSetPolicy will ensure that wait() is interrupted
+   * and returns before this function removes the guard condition.
+   * Otherwise, it is not safe to call this function concurrently with wait().
+   *
+   * This function will not be enabled (will not be available) if the
+   * StoragePolicy does not allow editing of the wait set after initialization.
+   *
+   * \param[in] guard_condition Guard condition to be removed.
+   * \throws std::invalid_argument if guard_condition is nullptr.
+   * \throws std::runtime_error if guard_condition is not part of the wait set.
+   * \throws exceptions based on the policies used.
+   */
+  void
+  remove_guard_condition(std::shared_ptr<rclcpp::GuardCondition> guard_condition)
+  {
+    if (nullptr == guard_condition) {
+      throw std::invalid_argument("guard_condition is nullptr");
+    }
+    // this method comes from the SynchronizationPolicy
+    this->sync_remove_guard_condition(
+      std::move(guard_condition),
+      [this](std::shared_ptr<rclcpp::GuardCondition> && inner_guard_condition) {
+        inner_guard_condition->exchange_in_use_by_wait_set_state(false);
+        // This method comes from the StoragePolicy, and it may not exist for
+        // fixed sized storage policies.
+        // It will throw if the guard condition is not in the wait set.
+        this->storage_remove_guard_condition(std::move(inner_guard_condition));
+      });
+  }
+
+  /// Add a timer to this wait set.
+  /**
+   * \sa add_guard_condition() for details of how this method works.
+   *
+   * \param[in] timer Timer to be added.
+   * \throws std::invalid_argument if timer is nullptr.
+   * \throws std::runtime_error if timer has already been added or is
+   *   associated with another wait set.
+   * \throws exceptions based on the policies used.
+   */
+  void
+  add_timer(std::shared_ptr<rclcpp::TimerBase> timer)
+  {
+    if (nullptr == timer) {
+      throw std::invalid_argument("timer is nullptr");
+    }
+    // this method comes from the SynchronizationPolicy
+    this->sync_add_timer(
+      std::move(timer),
+      [this](std::shared_ptr<rclcpp::TimerBase> && inner_timer) {
+        bool already_in_use = inner_timer->exchange_in_use_by_wait_set_state(true);
+        if (already_in_use) {
+          throw std::runtime_error("timer already in use by another wait set");
+        }
+        // This method comes from the StoragePolicy, and it may not exist for
+        // fixed sized storage policies.
+        // It will throw if the timer has already been added.
+        this->storage_add_timer(std::move(inner_timer));
+      });
+  }
+
+  /// Remove a timer from this wait set.
+  /**
+   * \sa remove_guard_condition() for details of how this method works.
+   *
+   * \param[in] timer Timer to be removed.
+   * \throws std::invalid_argument if timer is nullptr.
+   * \throws std::runtime_error if timer is not part of the wait set.
+   * \throws exceptions based on the policies used.
+   */
+  void
+  remove_timer(std::shared_ptr<rclcpp::TimerBase> timer)
+  {
+    if (nullptr == timer) {
+      throw std::invalid_argument("timer is nullptr");
+    }
+    // this method comes from the SynchronizationPolicy
+    this->sync_remove_timer(
+      std::move(timer),
+      [this](std::shared_ptr<rclcpp::TimerBase> && inner_timer) {
+        inner_timer->exchange_in_use_by_wait_set_state(false);
+        // This method comes from the StoragePolicy, and it may not exist for
+        // fixed sized storage policies.
+        // It will throw if the timer is not in the wait set.
+        this->storage_remove_timer(std::move(inner_timer));
+      });
+  }
+
+  /// Add a client to this wait set.
+  /**
+   * \sa add_guard_condition() for details of how this method works.
+   *
+   * \param[in] client Client to be added.
+   * \throws std::invalid_argument if client is nullptr.
+   * \throws std::runtime_error if client has already been added or is
+   *   associated with another wait set.
+   * \throws exceptions based on the policies used.
+   */
+  void
+  add_client(std::shared_ptr<rclcpp::ClientBase> client)
+  {
+    if (nullptr == client) {
+      throw std::invalid_argument("client is nullptr");
+    }
+    // this method comes from the SynchronizationPolicy
+    this->sync_add_client(
+      std::move(client),
+      [this](std::shared_ptr<rclcpp::ClientBase> && inner_client) {
+        bool already_in_use = inner_client->exchange_in_use_by_wait_set_state(true);
+        if (already_in_use) {
+          throw std::runtime_error("client already in use by another wait set");
+        }
+        // This method comes from the StoragePolicy, and it may not exist for
+        // fixed sized storage policies.
+        // It will throw if the client has already been added.
+        this->storage_add_client(std::move(inner_client));
+      });
+  }
+
+  /// Remove a client from this wait set.
+  /**
+   * \sa remove_guard_condition() for details of how this method works.
+   *
+   * \param[in] client Client to be removed.
+   * \throws std::invalid_argument if client is nullptr.
+   * \throws std::runtime_error if client is not part of the wait set.
+   * \throws exceptions based on the policies used.
+   */
+  void
+  remove_client(std::shared_ptr<rclcpp::ClientBase> client)
+  {
+    if (nullptr == client) {
+      throw std::invalid_argument("client is nullptr");
+    }
+    // this method comes from the SynchronizationPolicy
+    this->sync_remove_client(
+      std::move(client),
+      [this](std::shared_ptr<rclcpp::ClientBase> && inner_client) {
+        inner_client->exchange_in_use_by_wait_set_state(false);
+        // This method comes from the StoragePolicy, and it may not exist for
+        // fixed sized storage policies.
+        // It will throw if the client is not in the wait set.
+        this->storage_remove_client(std::move(inner_client));
+      });
+  }
+
+  /// Add a service to this wait set.
+  /**
+   * \sa add_guard_condition() for details of how this method works.
+   *
+   * \param[in] service Service to be added.
+   * \throws std::invalid_argument if service is nullptr.
+   * \throws std::runtime_error if service has already been added or is
+   *   associated with another wait set.
+   * \throws exceptions based on the policies used.
+   */
+  void
+  add_service(std::shared_ptr<rclcpp::ServiceBase> service)
+  {
+    if (nullptr == service) {
+      throw std::invalid_argument("service is nullptr");
+    }
+    // this method comes from the SynchronizationPolicy
+    this->sync_add_service(
+      std::move(service),
+      [this](std::shared_ptr<rclcpp::ServiceBase> && inner_service) {
+        bool already_in_use = inner_service->exchange_in_use_by_wait_set_state(true);
+        if (already_in_use) {
+          throw std::runtime_error("service already in use by another wait set");
+        }
+        // This method comes from the StoragePolicy, and it may not exist for
+        // fixed sized storage policies.
+        // It will throw if the service has already been added.
+        this->storage_add_service(std::move(inner_service));
+      });
+  }
+
+  /// Remove a service from this wait set.
+  /**
+   * \sa remove_guard_condition() for details of how this method works.
+   *
+   * \param[in] service Service to be removed.
+   * \throws std::invalid_argument if service is nullptr.
+   * \throws std::runtime_error if service is not part of the wait set.
+   * \throws exceptions based on the policies used.
+   */
+  void
+  remove_service(std::shared_ptr<rclcpp::ServiceBase> service)
+  {
+    if (nullptr == service) {
+      throw std::invalid_argument("service is nullptr");
+    }
+    // this method comes from the SynchronizationPolicy
+    this->sync_remove_service(
+      std::move(service),
+      [this](std::shared_ptr<rclcpp::ServiceBase> && inner_service) {
+        inner_service->exchange_in_use_by_wait_set_state(false);
+        // This method comes from the StoragePolicy, and it may not exist for
+        // fixed sized storage policies.
+        // It will throw if the service is not in the wait set.
+        this->storage_remove_service(std::move(inner_service));
+      });
+  }
+
+  /// Add a waitable to this wait set.
+  /**
+   * \sa add_guard_condition() for details of how this method works.
+   *
+   * Additionally, this function has an optional parameter which can be used to
+   * more quickly associate this waitable with an entity when it is ready, and
+   * so that the ownership maybe held in order to keep the waitable's parent in
+   * scope while waiting.
+   * If it is set to nullptr it will be ignored.
+   * The destruction of the associated entity's shared pointer will not cause
+   * the waitable to be removed, but it will cause the associated entity pointer
+   * to be nullptr when introspecting this waitable after waiting.
+   *
+   * Note that rclcpp::QOSEventHandlerBase are just a special case of
+   * rclcpp::Waitable and can be added with this function.
+   *
+   * \param[in] waitable Waitable to be added.
+   * \param[in] associated_entity Type erased shared pointer associated with the waitable.
+   *   This may be nullptr.
+   * \throws std::invalid_argument if waitable is nullptr.
+   * \throws std::runtime_error if waitable has already been added or is
+   *   associated with another wait set.
+   * \throws exceptions based on the policies used.
+   */
+  void
+  add_waitable(
+    std::shared_ptr<rclcpp::Waitable> waitable,
+    std::shared_ptr<void> associated_entity = nullptr)
+  {
+    if (nullptr == waitable) {
+      throw std::invalid_argument("waitable is nullptr");
+    }
+    // this method comes from the SynchronizationPolicy
+    this->sync_add_waitable(
+      std::move(waitable),
+      std::move(associated_entity),
+      [this](
+        std::shared_ptr<rclcpp::Waitable> && inner_waitable,
+        std::shared_ptr<void> && associated_entity)
+      {
+        bool already_in_use = inner_waitable->exchange_in_use_by_wait_set_state(true);
+        if (already_in_use) {
+          throw std::runtime_error("waitable already in use by another wait set");
+        }
+        // This method comes from the StoragePolicy, and it may not exist for
+        // fixed sized storage policies.
+        // It will throw if the waitable has already been added.
+        this->storage_add_waitable(std::move(inner_waitable), std::move(associated_entity));
+      });
+  }
+
+  /// Remove a waitable from this wait set.
+  /**
+   * \sa remove_guard_condition() for details of how this method works.
+   *
+   * \param[in] waitable Waitable to be removed.
+   * \throws std::invalid_argument if waitable is nullptr.
+   * \throws std::runtime_error if waitable is not part of the wait set.
+   * \throws exceptions based on the policies used.
+   */
+  void
+  remove_waitable(std::shared_ptr<rclcpp::Waitable> waitable)
+  {
+    if (nullptr == waitable) {
+      throw std::invalid_argument("waitable is nullptr");
+    }
+    // this method comes from the SynchronizationPolicy
+    this->sync_remove_waitable(
+      std::move(waitable),
+      [this](std::shared_ptr<rclcpp::Waitable> && inner_waitable) {
+        inner_waitable->exchange_in_use_by_wait_set_state(false);
+        // This method comes from the StoragePolicy, and it may not exist for
+        // fixed sized storage policies.
+        // It will throw if the waitable is not in the wait set.
+        this->storage_remove_waitable(std::move(inner_waitable));
+      });
+  }
+
+  /// Remove any destroyed entities from the wait set.
+  /**
+   * When the storage policy does not maintain shared ownership for the life
+   * of the wait set, e.g. the DynamicStorage policy, it is possible for an
+   * entity to go out of scope and be deleted without this wait set noticing.
+   * Therefore there are weak references in this wait set which need to be
+   * periodically cleared.
+   * This function performs that clean up.
+   *
+   * Since this involves removing entities from the wait set, and is only
+   * needed if the wait set does not keep ownership of the added entities, the
+   * storage policies which are static will not need this function and therefore
+   * do not provide this function.
+   *
+   * \throws exceptions based on the policies used.
+   */
+  void
+  prune_deleted_entities()
+  {
+    // this method comes from the SynchronizationPolicy
+    this->sync_prune_deleted_entities(
+      [this]() {
+        // This method comes from the StoragePolicy, and it may not exist for
+        // fixed sized storage policies.
+        this->storage_prune_deleted_entities();
+      });
+  }
+
+  /// Wait for any of the entities in the wait set to be ready, or a period of time to pass.
+  /**
+   * This function will return when either one of the entities within this wait
+   * set is ready, or a period of time has passed, which ever is first.
+   * The term "ready" means different things for different entities, but
+   * generally it means some condition is met asynchronously for which this
+   * function waits.
+   *
+   * This function can either wait for a period of time, do no waiting
+   * (non-blocking), or wait indefinitely, all based on the value of the
+   * time_to_wait parameter.
+   * Waiting is always measured against the std::chrono::steady_clock.
+   * If waiting indefinitely, the Timeout result is not possible.
+   * There is no "cancel wait" function on this class, but if you want to wait
+   * indefinitely but have a way to asynchronously interrupt this method, then
+   * you can use a dedicated rclcpp::GuardCondition for that purpose.
+   *
+   * This function will modify the internal rcl_wait_set_t, so introspecting
+   * the wait set during a call to wait is never safe.
+   * You should always wait, then introspect, and then, only when done
+   * introspecting, wait again.
+   *
+   * It may be thread-safe to add and remove entities to the wait set
+   * concurrently with this function, depending on the SynchronizationPolicy
+   * that is used.
+   * With the rclcpp::wait_set_policies::ThreadSafeSynchronization policy this
+   * function will stop waiting to allow add or remove of an entity, and then
+   * resume waiting, so long as the timeout has not been reached.
+   *
+   * \param[in] time_to_wait If > 0, time to wait for entities to be ready,
+   *   if == 0, check if anything is ready without blocking, or
+   *   if < 0, wait indefinitely until one of the items is ready.
+   *   Default is -1, so wait indefinitely.
+   * \returns Ready when one of the entities is ready, or
+   * \returns Timeout when the given time to wait is exceeded, not possible
+   *   when time_to_wait is < 0, or
+   * \returns Empty if the wait set is empty, avoiding the possibility of
+   *   waiting indefinitely on an empty wait set.
+   * \throws rclcpp::exceptions::RCLError on unhandled rcl errors
+   */
+  template<class Rep = int64_t, class Period = std::milli>
+  RCUTILS_WARN_UNUSED
+  WaitResult<WaitSetTemplate>
+  wait(std::chrono::duration<Rep, Period> time_to_wait = std::chrono::duration<Rep, Period>(-1))
+  {
+    auto time_to_wait_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(time_to_wait);
+
+    // ensure the ownership of the entities in the wait set is shared for the duration of wait
+    this->storage_acquire_ownerships();
+    RCLCPP_SCOPE_EXIT({this->storage_release_ownerships();});
+
+    // this method comes from the SynchronizationPolicy
+    return this->template sync_wait<WaitResult<WaitSetTemplate>>(
+      // pass along the time_to_wait duration as nanoseconds
+      time_to_wait_ns,
+      // this method provides the ability to rebuild the wait set, if needed
+      [this]() {
+        // This method comes from the StoragePolicy
+        this->storage_rebuild_rcl_wait_set(
+          // This method comes from the SynchronizationPolicy
+          this->get_extra_guard_conditions()
+        );
+      },
+      // this method provides access to the rcl wait set
+      [this]() -> rcl_wait_set_t & {
+        // This method comes from the StoragePolicy
+        return this->storage_get_rcl_wait_set();
+      },
+      // this method provides a way to create the WaitResult
+      [this](WaitResultKind wait_result_kind) -> WaitResult<WaitSetTemplate> {
+        // convert the result into a WaitResult
+        switch (wait_result_kind) {
+          case WaitResultKind::Ready:
+            return WaitResult<WaitSetTemplate>::from_ready_wait_result_kind(*this);
+          case WaitResultKind::Timeout:
+            return WaitResult<WaitSetTemplate>::from_timeout_wait_result_kind();
+          case WaitResultKind::Empty:
+            return WaitResult<WaitSetTemplate>::from_empty_wait_result_kind();
+          default:
+            auto msg = "unknown WaitResultKind with value: " + std::to_string(wait_result_kind);
+            throw std::runtime_error(msg);
+        }
+      }
+    );
+  }
+
+private:
+  // Add WaitResult type as a friend so it can call private methods for
+  // acquiring and releasing resources as the WaitResult is initialized and
+  // destructed, respectively.
+  friend WaitResult<WaitSetTemplate>;
+
+  /// Called by the WaitResult's constructor to place a hold on ownership and thread-safety.
+  /**
+   * Should only be called in pairs with wait_result_release().
+   *
+   * \throws std::runtime_error If called twice before wait_result_release().
+   */
+  void
+  wait_result_acquire()
+  {
+    if (wait_result_holding_) {
+      throw std::runtime_error("wait_result_acquire() called while already holding");
+    }
+    wait_result_holding_ = true;
+    // this method comes from the SynchronizationPolicy
+    this->sync_wait_result_acquire();
+    // this method comes from the StoragePolicy
+    this->storage_acquire_ownerships();
+  }
+
+  /// Called by the WaitResult's destructor to release resources.
+  /**
+   * Should only be called if wait_result_acquire() has been called.
+   *
+   * \throws std::runtime_error If called before wait_result_acquire().
+   */
+  void
+  wait_result_release()
+  {
+    if (!wait_result_holding_) {
+      throw std::runtime_error("wait_result_release() called while not holding");
+    }
+    wait_result_holding_ = false;
+    // this method comes from the StoragePolicy
+    this->storage_release_ownerships();
+    // this method comes from the SynchronizationPolicy
+    this->sync_wait_result_release();
+  }
+
+  bool wait_result_holding_ = false;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_SET_TEMPLATE_HPP_

rclcpp/include/rclcpp/waitable.hpp

@@ -15,6 +15,8 @@
 #ifndef RCLCPP__WAITABLE_HPP_
 #define RCLCPP__WAITABLE_HPP_
 
+#include <atomic>
+
 #include "rclcpp/macros.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -94,7 +96,6 @@ public:
   size_t
   get_number_of_ready_guard_conditions();
 
-  // TODO(jacobperron): smart pointer?
   /// Add the Waitable to a wait set.
   /**
    * \param[in] wait_set A handle to the wait set to add the Waitable to.
@@ -146,6 +147,23 @@ public:
   virtual
   void
   execute() = 0;
+
+  /// Exchange the "in use by wait set" state for this timer.
+  /**
+   * This is used to ensure this timer is not used by multiple
+   * wait sets at the same time.
+   *
+   * \param[in] in_use_state the new state to exchange into the state, true
+   *   indicates it is now in use by a wait set, and false is that it is no
+   *   longer in use by a wait set.
+   * \returns the previous state.
+   */
+  RCLCPP_PUBLIC
+  bool
+  exchange_in_use_by_wait_set_state(bool in_use_state);
+
+private:
+  std::atomic<bool> in_use_by_wait_set_{false};
 };  // class Waitable
 
 }  // namespace rclcpp

rclcpp/package.xml

@@ -2,42 +2,42 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp</name>
-  <version>0.8.3</version>
+  <version>0.9.0</version>
   <description>The ROS client library in C++.</description>
   <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
-  <build_export_depend>rmw</build_export_depend>
-
   <build_depend>builtin_interfaces</build_depend>
   <build_depend>rcl_interfaces</build_depend>
   <build_depend>rosgraph_msgs</build_depend>
-  <build_depend>rosidl_generator_cpp</build_depend>
+  <build_depend>rosidl_runtime_cpp</build_depend>
   <build_depend>rosidl_typesupport_c</build_depend>
   <build_depend>rosidl_typesupport_cpp</build_depend>
   <build_export_depend>builtin_interfaces</build_export_depend>
   <build_export_depend>rcl_interfaces</build_export_depend>
   <build_export_depend>rosgraph_msgs</build_export_depend>
-  <build_export_depend>rosidl_generator_cpp</build_export_depend>
+  <build_export_depend>rosidl_runtime_cpp</build_export_depend>
   <build_export_depend>rosidl_typesupport_c</build_export_depend>
   <build_export_depend>rosidl_typesupport_cpp</build_export_depend>
 
+  <depend>libstatistics_collector</depend>
   <depend>rcl</depend>
   <depend>rcl_yaml_param_parser</depend>
   <depend>rcpputils</depend>
-  <depend>rmw_implementation</depend>
+  <depend>rcutils</depend>
+  <depend>rmw</depend>
+  <depend>statistics_msgs</depend>
   <depend>tracetools</depend>
 
-  <exec_depend>ament_cmake</exec_depend>
-
   <test_depend>ament_cmake_gmock</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>rmw</test_depend>
   <test_depend>rmw_implementation_cmake</test_depend>
+  <test_depend>rosidl_default_generators</test_depend>
   <test_depend>test_msgs</test_depend>
 
   <export>