Force segfault to test ament_cmake_test code

* correct use of move semantics

Signed-off-by: Knese Karsten <karsten@openrobotics.org>

* more tests

Signed-off-by: Knese Karsten <karsten@openrobotics.org>

* make error message more exact

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

* use std::exchange

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

* fix typo

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

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

@@ -9,35 +9,35 @@
 
   <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>

rclcpp/resource/get_interface.hpp.em

@@ -0,0 +1,111 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+@{
+uppercase_interface_name = interface_name.upper()
+}@
+
+#ifndef RCLCPP__NODE_INTERFACES__GET_@(uppercase_interface_name)_HPP_
+#define RCLCPP__NODE_INTERFACES__GET_@(uppercase_interface_name)_HPP_
+
+#include <memory>
+#include <utility>
+#include <type_traits>
+
+#include "rcpputils/pointer_traits.hpp"
+
+#include "rclcpp/node_interfaces/@(interface_name).hpp"
+#include "rclcpp/node_interfaces/@(interface_name)_traits.hpp"
+
+@{
+interface_typename = ''.join([part.capitalize() for part in interface_name.split('_')])
+}@
+
+/// This header provides the get_@(interface_name)() template function.
+/**
+ * This function is useful for getting the @(interface_typename) pointer from
+ * various kinds of Node-like classes.
+ *
+ * It's able to get a std::shared_ptr to a @(interface_typename) so long as the class
+ * has a method called ``get_@(interface_name)()`` which returns one.
+ */
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+namespace detail
+{
+
+// If NodeType has a method called get_@(interface_name)() which returns a shared pointer.
+template<
+  typename NodeType,
+  typename std::enable_if<has_@(interface_name)<
+    typename rcpputils::remove_pointer<NodeType>::type
+  >::value, int>::type = 0
+>
+std::shared_ptr<rclcpp::node_interfaces::@(interface_typename)>
+get_@(interface_name)_from_pointer(NodeType node_pointer)
+{
+  if (!node_pointer) {
+    throw std::invalid_argument("node cannot be nullptr");
+  }
+  return node_pointer->get_@(interface_name)();
+}
+
+}  // namespace detail
+
+/// Get the @(interface_typename) as a shared pointer from a pointer to a "Node like" object.
+template<
+  typename NodeType,
+  typename std::enable_if<
+    rcpputils::is_pointer<NodeType>::value, int
+  >::type = 0
+>
+inline
+std::shared_ptr<rclcpp::node_interfaces::@(interface_typename)>
+get_@(interface_name)(NodeType && node)
+{
+  // Forward pointers to detail implementation directly.
+  return detail::get_@(interface_name)_from_pointer(node);
+}
+
+/// Get the @(interface_typename) as a shared pointer from a "Node like" object.
+template<
+  typename NodeType,
+  typename std::enable_if<
+    !rcpputils::is_pointer<NodeType>::value, int
+  >::type = 0
+>
+inline
+std::shared_ptr<rclcpp::node_interfaces::@(interface_typename)>
+get_@(interface_name)(NodeType && node)
+{
+  // Forward references to detail implementation as a pointer.
+  return detail::get_@(interface_name)_from_pointer(&node);
+}
+
+/// Keep the @(interface_typename) a shared pointer.
+inline
+std::shared_ptr<rclcpp::node_interfaces::@(interface_typename)>
+get_@(interface_name)(
+  std::shared_ptr<rclcpp::node_interfaces::@(interface_typename)> & node_interface)
+{
+  return node_interface;
+}
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__GET_@(uppercase_interface_name)_HPP_