wip

Signed-off-by: William Woodall <william@osrfoundation.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,18 @@ 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_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
@@ -36,20 +38,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 +80,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 +91,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
 )
 
@@ -109,13 +121,16 @@ add_library(${PROJECT_NAME}
   ${${PROJECT_NAME}_SRCS})
 # 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"
 )
 
@@ -135,15 +150,17 @@ install(
 ament_export_include_directories(include)
 ament_export_libraries(${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)
@@ -164,7 +181,7 @@ if(BUILD_TESTING)
     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 +192,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,7 +203,7 @@ 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})
@@ -196,7 +213,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_function_traits
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
   endif()
@@ -206,7 +223,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 +233,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 +243,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 +261,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 +300,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 +322,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 +332,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 +344,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 +356,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 +402,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 +426,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 +437,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 +449,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)
@@ -491,6 +541,39 @@ 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)
+  if(TARGET test_subscription_topic_statistics)
+    ament_target_dependencies(test_subscription_topic_statistics
+      "libstatistics_collector"
+      "rcl_interfaces"
+      "rcutils"
+      "rmw"
+      "rosidl_runtime_cpp"
+      "rosidl_typesupport_cpp"
+      "statistics_msgs"
+      "test_msgs")
+    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

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_timer.hpp

@@ -37,7 +37,7 @@ create_timer(
   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 +57,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),

rclcpp/include/rclcpp/detail/resolve_enable_topic_statistics.hpp

@@ -0,0 +1,54 @@
+// 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");
+      break;
+  }
+
+  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

@@ -29,129 +29,241 @@
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
 
-#include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/executor_options.hpp"
+#include "rclcpp/executor_policies/timer_favoring_priority_queue.hpp"
+#include "rclcpp/future_return_code.hpp"
+#include "rclcpp/guard_condition.hpp"
 #include "rclcpp/memory_strategies.hpp"
 #include "rclcpp/memory_strategy.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/wait_set.hpp"
 
 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).
- * It coordinates the nodes and callback groups by looking for available work and completing it,
- * based on the threading or concurrency scheme provided by the subclass implementation.
- * An example of available work is executing a subscription callback, or a timer callback.
- * The executor structure allows for a decoupling of the communication graph and the execution
- * model.
- * See SingleThreadedExecutor and MultiThreadedExecutor for examples of execution paradigms.
- */
-class Executor
+/// Base class for Executor providing the common interface for adding items, spinning, etc.
+class ExecutorBase
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Executor)
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ExecutorBase)
 
   /// Default constructor.
-  // \param[in] ms The memory strategy to be used with this executor.
+  /**
+   * \param[in] options Options for the executor.
+   */
   RCLCPP_PUBLIC
-  explicit Executor(const ExecutorArgs & args = ExecutorArgs());
+  explicit ExecutorBase(const ExecutorOptions & options = ExecutorOptions());
 
   /// Default destructor.
   RCLCPP_PUBLIC
-  virtual ~Executor();
+  virtual ~ExecutorBase();
 
-  /// Do work periodically as it becomes available to us. Blocking call, may block indefinitely.
-  // It is up to the implementation of Executor to implement spin.
-  virtual void
+  /// Execution loop which waits for work, executes work, and repeats until canceled.
+  /**
+   * This will block, continuing to wait for work and then execute it, until
+   * canceled, either by the cancel() method or by the associated context being
+   * shutdown, either explicitly or due to a SIGINT (perhaps due to ctrl-c).
+   */
+  virtual
+  void
   spin() = 0;
 
-  /// Add a node to the executor.
+  /// Add all of a node's callback groups 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.
+   * Add all of the callback groups of a node to this executor.
+   *
+   * If any callback groups are associated with another executor, this method
+   * will throw a std::runtime_error.
+   *
+   * It will also trigger the interrupt guard condition which will cause the
+   * executor to wake up and consider the changes, then go back to waiting.
+   * Unless the notify parameter is passed false, in which case it will not
+   * interrupt the executor, and the changes may not be considered immediately.
+   *
+   * \param[in] node_ptr Shared pointer to the node which will have callback groups added.
+   * \param[in] notify If true, notfiy the executor of changes, otherwise do not.
+   * \throws std::runtime_error if any callback groups are associated with another executor.
+   */
+  template<class NodeT>
+  void
+  add_node(const std::shared_ptr<NodeT> & node_ptr, bool notify = true)
+  {
+    this->add_node(
+      node_ptr->get_node_base_interface(),
+      notify,
+      false  // raise on encountering already associated callback groups
+    );
+  }
+
+  /// Overload that takes the NodeBaseInterface directly.
+  template<class NodeT>
+  void
+  add_node(
+    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & node_ptr,
+    bool notify = true)
+  {
+    this->add_node(
+      node_ptr,
+      notify,
+      false  // raise on encountering already associated callback groups
+    );
+  }
+
+  /// Add all unassociated callback groups of the given node to this executor.
+  /**
+   * Same as add_node(), but instead of throwing if a callback group is already
+   * associated with another exector (already added to it) it will just ignore
+   * it rather than throwing.
+   *
+   * \param[in] node_ptr Shared pointer to the node which will have callback groups added.
+   * \param[in] notify If true, notfiy the executor of changes, otherwise do not.
+   */
+  template<class NodeT>
+  void
+  add_unassociated_callback_groups_from_node(
+    const std::shared_ptr<NodeT> & node_ptr,
+    bool notify = true)
+  {
+    this->add_node(
+      node_ptr->get_node_base_interface(),
+      notify,
+      true  // ignore already associated callback groups
+    );
+  }
+
+  /// Overload that takes the NodeBaseInterface directly.
+  template<class NodeT>
+  void
+  add_unassociated_callback_groups_from_node(
+    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & node_ptr,
+    bool notify = true)
+  {
+    this->add_node(
+      node_ptr,
+      notify,
+      true  // ignore already associated callback groups
+    );
+  }
+
+  /// Remove all of a node's callback groups from the executor.
+  /**
+   * Remove all of the callback groups of a node from this executor.
+   *
+   * It will also trigger the interrupt guard condition which will cause the
+   * executor to wake up and consider the changes, then go back to waiting.
+   * Unless the notify parameter is passed false, in which case it will not
+   * interrupt the executor, and the changes may not be considered immediately.
+   *
+   * \param[in] node Node which will have callback groups removed.
+   * \param[in] notify If true, notfiy the executor of changes, otherwise do not.
+   */
+  template<class NodeT>
+  void
+  remove_node(const NodeT & node, bool notify = true)
+  {
+    this->remove_node(*node.get_node_base_interface(), notify);
+  }
+
+  /// Overload that takes a shared pointer to the node.
+  /**
+   * This is kept for backwards compatibility from when executors shared
+   * ownership of Nodes.
+   */
+  template<class NodeT>
+  void
+  remove_node(const std::shared_ptr<NodeT> & node_ptr, bool notify = true)
+  {
+    this->remove_node(*node_ptr->get_node_base_interface(), notify);
+  }
+
+  /// Placeholder used to indicate that a method overload should not notify the executor.
+  struct DoNotNotify {};
+
+  /// Add a callback group to this executor.
+  /**
+   * If the given callback group is already associated with another executor,
+   * this method will throw a std::runtime_error.
+   *
+   * This overload of add_callback_group() will notify the executor so it will
+   * wake up if waiting and consider the changes.
+   *
+   * Weak ownership of the callback group is kept by the executor all of the
+   * time, but while waiting the weak ownership is periodically elevated to
+   * shared ownership.
+   * Therefore, if you let the callback group shared pointer go out of scope
+   * then it will stay in scope until this executor is done using it, at which
+   * point the callback group will be destructed and automatically removed from
+   * this executor in the next pass.
+   *
+   * \param[in] callback_group_ptr The callback group to be added.
+   * \throws std::runtime_error if the callback group is associated with another
+   *   executor already.
+   * \throws std::invalid_argument if the callback group pointer is nullptr.
    */
   RCLCPP_PUBLIC
-  virtual void
-  add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true);
+  virtual
+  void
+  add_callback_group(rclcpp::CallbackGroup::SharedPtr callback_group_ptr) = 0;
 
-  /// Convenience function which takes Node and forwards NodeBaseInterface.
-  RCLCPP_PUBLIC
-  virtual void
-  add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true);
-
-  /// Remove a node from the executor.
+  /// Add a callback group to this executor without notifying 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.
+   * The same as the other overload of add_callback_group(), except it does not
+   * notify the executor, so it will not wake up and these changes may not be
+   * considered immediately.
+   *
+   * Note, a bool with a default value would be preferable for controlling the
+   * notify behavior, and we're using it in the add/remove node above, but
+   * in order to keep this function virtual, and to avoid using default values
+   * in conjunction with virtual methods, we use an overload instead, in the
+   * spirit of std::nothrow_t, e.g.:
+   *   https://en.cppreference.com/w/cpp/memory/new/nothrow
    */
   RCLCPP_PUBLIC
-  virtual void
-  remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true);
+  virtual
+  void
+  add_callback_group(rclcpp::CallbackGroup::SharedPtr callback_group_ptr, DoNotNotify) = 0;
 
-  /// Convenience function which takes Node and forwards NodeBaseInterface.
+  /// Remove a callback group from this executor.
+  /**
+   * If the given callback group is not associated with this executor, this
+   * method will throw a std::runtime_error.
+   *
+   * This overload of add_callback_group() will notify the executor so it will
+   * wake up if waiting and consider the changes.
+   *
+   * \param[in] callback_group The callback group to be removed.
+   * \throws std::runtime_error if the callback group is not associated with
+   *   this executor.
+   * \throws std::invalid_argument if the callback group pointer is nullptr.
+   */
   RCLCPP_PUBLIC
-  virtual void
-  remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true);
+  virtual
+  void
+  remove_callback_group(const rclcpp::CallbackGroup & callback_group) = 0;
+
+  /// Remove a callback group from this executor without notifying the executor.
+  /**
+   * The same as the other overload of remove_callback_group(), except it does not
+   * notify the executor, so it will not wake up and these changes may not be
+   * considered immediately.
+   *
+   * See add_callback_group() for a note about the use of DoNotNotify.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  remove_callback_group(const rclcpp::CallbackGroup & callback_group, DoNotNotify) = 0;
 
   /// Add a node to executor, execute the next available unit of work, and remove the node.
   /**
    * \param[in] node Shared pointer to the node to add.
-   * \param[in] timeout How long to wait for work to become available. Negative values cause
-   * spin_node_once to block indefinitely (the default behavior). A timeout of 0 causes this
-   * function to be non-blocking.
+   * \param[in] timeout How long to wait for work to become available.
+   *   Negative values cause spin_node_once to block indefinitely (the default
+   *   behavior).
+   *   A timeout of 0 causes this function to be non-blocking.
    */
   template<typename RepT = int64_t, typename T = std::milli>
   void
@@ -166,7 +278,7 @@ public:
   }
 
   /// Convenience function which takes Node and forwards NodeBaseInterface.
-  template<typename NodeT = rclcpp::Node, typename RepT = int64_t, typename T = std::milli>
+  template<typename NodeT, typename RepT = int64_t, typename T = std::milli>
   void
   spin_node_once(
     std::shared_ptr<NodeT> node,
@@ -180,16 +292,14 @@ public:
 
   /// Add a node, complete all immediately available work, and remove the node.
   /**
-   * \param[in] node Shared pointer to the node to add.
+   * \param[in] node Shared pointer to the node to spin some.
    */
-  RCLCPP_PUBLIC
+  template<class NodeT>
   void
-  spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node);
-
-  /// Convenience function which takes Node and forwards NodeBaseInterface.
-  RCLCPP_PUBLIC
-  void
-  spin_node_some(std::shared_ptr<rclcpp::Node> node);
+  spin_node_some(const std::shared_ptr<NodeT> & node)
+  {
+    this->spin_node_some(node->get_node_base_interface());
+  }
 
   /// Complete all available queued work without blocking.
   /**
@@ -203,12 +313,14 @@ public:
    * been exceeded.
    */
   RCLCPP_PUBLIC
-  virtual void
-  spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0));
+  virtual
+  void
+  spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)) = 0;
 
   RCLCPP_PUBLIC
-  virtual void
-  spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+  virtual
+  void
+  spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)) = 0;
 
   /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
   /**
@@ -223,7 +335,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
@@ -270,51 +382,79 @@ public:
   }
 
   /// Cancel any running spin* function, causing it to return.
-  /* This function can be called asynchonously from any thread. */
-  RCLCPP_PUBLIC
-  void
-  cancel();
-
-  /// Support dynamic switching of the memory strategy.
   /**
-   * Switching the memory strategy while the executor is spinning in another threading could have
-   * unintended consequences.
-   * \param[in] memory_strategy Shared pointer to the memory strategy to set.
+   * This function can be called asynchronously from any thread.
    */
   RCLCPP_PUBLIC
+  virtual
   void
-  set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy);
+  cancel() = 0;
 
 protected:
+  /// Implementation of add_node().
+  /**
+   * \param[in] node_ptr The node which will have its callback groups added.
+   * \param[in] notify If true, the executor is interrupted to consider the
+   *   changes, otherwise it is not interrupted.
+   * \param[in] ignore_associated_callback_groups If true, then when a callback
+   *   group which is already been added to another executor is encountered
+   *   it will be ignored, if false then std::runtime_error is thrown instead.
+   * \throws std::runtime_error if ignore_associated_callback_groups is false
+   *   and a callback group which is already associated with another executor
+   *   is encountered.
+   * \throws std::invalid_argument if node_ptr is nullptr.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_node(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    bool notify,
+    bool ignore_associated_callback_groups) = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) = 0;
+
   RCLCPP_PUBLIC
   void
   spin_node_once_nanoseconds(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
     std::chrono::nanoseconds timeout);
 
+  RCLCPP_PUBLIC
+  virtual
+  void
+  spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node);
+
   /// Find the next available executable and do the work associated with it.
   /** \param[in] any_exec Union structure that can hold any executable type (timer, subscription,
    * service, client).
    */
   RCLCPP_PUBLIC
   void
-  execute_any_executable(AnyExecutable & any_exec);
+  execute_any_executable(rclcpp::AnyExecutable & any_exec);
 
   RCLCPP_PUBLIC
-  static void
+  static
+  void
   execute_subscription(
     rclcpp::SubscriptionBase::SharedPtr subscription);
 
   RCLCPP_PUBLIC
-  static void
+  static
+  void
   execute_timer(rclcpp::TimerBase::SharedPtr timer);
 
   RCLCPP_PUBLIC
-  static void
+  static
+  void
   execute_service(rclcpp::ServiceBase::SharedPtr service);
 
   RCLCPP_PUBLIC
-  static void
+  static
+  void
   execute_client(rclcpp::ClientBase::SharedPtr client);
 
   RCLCPP_PUBLIC
@@ -322,48 +462,103 @@ protected:
   wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
   RCLCPP_PUBLIC
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
-  get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group);
-
-  RCLCPP_PUBLIC
-  rclcpp::callback_group::CallbackGroup::SharedPtr
+  rclcpp::CallbackGroup::SharedPtr
   get_group_by_timer(rclcpp::TimerBase::SharedPtr timer);
 
   RCLCPP_PUBLIC
   bool
-  get_next_ready_executable(AnyExecutable & any_executable);
+  get_next_ready_executable(rclcpp::AnyExecutable & any_executable);
 
   RCLCPP_PUBLIC
   bool
   get_next_executable(
-    AnyExecutable & any_executable,
+    rclcpp::AnyExecutable & any_executable,
     std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
+  RCLCPP_DISABLE_COPY(ExecutorBase)
+
   /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins.
   std::atomic_bool spinning;
 
   /// Guard condition for signaling the rmw layer to wake up for special events.
-  rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
+  rclcpp::GuardCondition interrupt_guard_condition_;
+  // rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
 
-  /// Wait set for managing entities that the rmw layer waits on.
-  rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
+  // /// Wait set for managing entities that the rmw layer waits on.
+  // rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
 
-  // Mutex to protect the subsequent memory_strategy_.
-  std::mutex memory_strategy_mutex_;
+  // // Mutex to protect the subsequent memory_strategy_.
+  // std::mutex memory_strategy_mutex_;
 
-  /// The memory strategy: an interface for handling user-defined memory allocation strategies.
-  memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;
+  // /// The memory strategy: an interface for handling user-defined memory allocation strategies.
+  // memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;
 
   /// The context associated with this executor.
-  std::shared_ptr<rclcpp::Context> context_;
+  rclcpp::Context::SharedPtr context_;
 
-private:
-  RCLCPP_DISABLE_COPY(Executor)
+  // std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
+  // std::list<const rcl_guard_condition_t *> guard_conditions_;
+  std::vector<rclcpp::CallbackGroup::WeakPtr> weak_guard_conditions_;
 
-  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
-  std::list<const rcl_guard_condition_t *> guard_conditions_;
 };
 
+/// Template class which serves as the foundation of actual executors.
+/**
+ * This class combines the wait set, scheduling policy, and the ExecutorBase
+ * class, and implements all of the pure virtual functions of ExecutorBase
+ * making it a concrete class.
+ */
+template<class WaitSetT, class SchedulingPolicy>
+class ExecutorTemplate : public ExecutorBase, public SchedulingPolicy
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ExecutorTemplate)
+
+  /// Default constructor.
+  /**
+   * \param[in] options Options for the executor.
+   */
+  explicit ExecutorTemplate(const ExecutorOptions & options = ExecutorOptions())
+  : ExecutorBase(options), SchedulingPolicy(options), WaitSetT(options.context)
+  {}
+
+  /// Default virtual destructor.
+  RCLCPP_PUBLIC
+  virtual
+  ~ExecutorTemplate() = default;
+
+protected:
+  WaitSetT wait_set_;
+};
+
+/// Executor concept which waits for work and coordinates execution of user callbacks.
+/**
+ * Executor provides spin functions (including spin_node_once and spin_some).
+ * It coordinates the nodes and callback groups by looking for available work
+ * and completing it, based on the threading or concurrency scheme provided by
+ * the subclass implementation.
+ * An example of available work is executing a subscription callback, or a
+ * timer callback.
+ * The executor structure allows for a decoupling of the communication graph
+ * and the execution model.
+ * See SingleThreadedExecutor and MultiThreadedExecutor for examples of
+ * different execution paradigms.
+ *
+ * By default this alias provides a foundation based on specific wait set type
+ * and a scheduling policy.
+ * The wait set is expected to be dynamic, i.e. items can be added or removed
+ * after creation, and thread-safe, i.e. items can be added or removed while
+ * also waiting concurrently.
+ */
+using Executor = ExecutorTemplate<
+  rclcpp::ThreadSafeWaitSet,
+  rclcpp::executor_policies::TimerFavoringPriorityQueue>;
+
+namespace executor
+{
+
+using Executor [[deprecated("use rclcpp::Executor instead")]] = Executor;
+
 }  // namespace executor
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/executor_options.hpp

@@ -0,0 +1,49 @@
+// 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__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;
+
+}  // namespace executor
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXECUTOR_OPTIONS_HPP_

rclcpp/include/rclcpp/executor_policies/scheduling_result.hpp

@@ -0,0 +1,35 @@
+// 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__EXECUTOR_POLICIES__SCHEDULING_RESULT_HPP_
+#define RCLCPP__EXECUTOR_POLICIES__SCHEDULING_RESULT_HPP_
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace executor_policies
+{
+
+/// Represents the directions a SchedulingPolicy can give to an Executor.
+enum RCLCPP_PUBLIC SchedulingResult
+{
+  ContinueExecuting,  //<! Indicates more work should be done before waiting.
+  WaitForWork,  //<! Indicates that the executor should wait on the wait set again.
+};
+
+}  // namespace executor_policies
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXECUTOR_POLICIES__SCHEDULING_RESULT_HPP_

rclcpp/include/rclcpp/executor_policies/timer_favoring_priority_queue.hpp

@@ -0,0 +1,77 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__EXECUTOR_POLICIES__TIMER_FAVORING_PRIORITY_QUEUE_HPP_
+#define RCLCPP__EXECUTOR_POLICIES__TIMER_FAVORING_PRIORITY_QUEUE_HPP_
+
+#include "rclcpp/any_executable.hpp"
+#include "rclcpp/executor_options.hpp"
+#include "rclcpp/executor_policies/scheduling_result.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/wait_result.hpp"
+
+namespace rclcpp
+{
+namespace executor_policies
+{
+
+/// A naive scheduling policy which selects Timers first, then Subscriptions and other items.
+/**
+ * Items are executed in the order they were added, favoring Timers, then
+ * Subscriptions, Service Servers, Service Clients, and finally Waitables.
+ * All Timers are executed before any Subscriptions, and all Subscriptions
+ * before any Service Servers, and so on.
+ *
+ * User guard conditions are not yet supported by the Executor and so all guard
+ * condition are used by the executor itself and are handled before this policy
+ * is consulted.
+ * Therefore, guard conditions are ignored for the purposes of scheduling.
+ *
+ * This is a naive policy, but is the default until a better one is implemented.
+ */
+class TimerFavoringPriorityQueue
+{
+public:
+  explicit TimerFavoringPriorityQueue(const rclcpp::ExecutorOptions &) {}
+
+  /// Select which item should next be executed, and indicate if waiting should resume.
+  /**
+   * Selects which item to be executed next, assign it to the any_executable, or
+   * assigning nullptr if no work should be done right now.
+   *
+   * This method is called by the executor after waiting on a wait set, in
+   * order to determine what to execute next based on the result.
+   *
+   * Additionally, if returning SchedulingResult::ContinueExecuting then the
+   * executor will call this function again without waiting on the wait set, or
+   * if returning SchedulingResult::WaitForWork then the executor will wait on
+   * the wait set again after executing the selected any_executable, or
+   * immediately if any_executable was assigned nullptr.
+   */
+  template<class WaitSetT>
+  rclcpp::executor_policies::SchedulingResult
+  schedule_next_any_executable(
+    const WaitResult<WaitSetT> & wait_result,
+    rclcpp::AnyExecutable & any_executable)
+  {
+    // Explicitly ignore guard conditions.
+    // Check Timers for being ready.
+
+  }
+};
+
+}  // namespace executor_policies
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXECUTOR_POLICIES__TIMER_FAVORING_PRIORITY_QUEUE_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,53 @@
+// 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;
+
+}  // 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/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/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

@@ -33,13 +33,12 @@
 #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_subscription.hpp"
+#include "rclcpp/detail/resolve_enable_topic_statistics.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
@@ -107,7 +106,7 @@ 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),
@@ -122,7 +121,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 +138,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 +157,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/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

@@ -55,7 +55,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,7 +68,7 @@ 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 *

rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp

@@ -22,7 +22,9 @@
 #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/publisher.hpp"
 #include "rclcpp/publisher_factory.hpp"
 #include "rclcpp/subscription.hpp"
@@ -57,7 +59,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,7 +74,7 @@ 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/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().
   }
 

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

@@ -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,102 @@
+// 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<rcl_serialized_message_t>: std::true_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,107 @@
+// 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;
+
+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

@@ -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"
@@ -118,40 +119,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(),
@@ -175,11 +191,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 +208,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,15 +242,18 @@ public:
     return message_memory_strategy_->borrow_message();
   }
 
-  std::shared_ptr<rcl_serialized_message_t> create_serialized_message() override
+  std::shared_ptr<rcl_serialized_message_t>
+  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;
@@ -229,7 +264,8 @@ public:
 
   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 +276,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<rcl_serialized_message_t> & 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();
   }

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,6 +30,7 @@
 #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/type_support_decl.hpp"
@@ -110,6 +114,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(rcl_serialized_message_t & message_out, rclcpp::MessageInfo & message_info_out);
+
   /// Borrow a new message.
   /** \return Shared pointer to the fresh message. */
   RCLCPP_PUBLIC
@@ -132,12 +175,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. */
@@ -193,6 +236,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 +265,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 +293,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_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
@@ -49,6 +51,15 @@ struct is_serialized_subscription_argument<std::shared_ptr<rcl_serialized_messag
   : std::true_type
 {};
 
+template<>
+struct is_serialized_subscription_argument<SerializedMessage>: std::true_type
+{};
+
+template<>
+struct is_serialized_subscription_argument<std::shared_ptr<SerializedMessage>>
+  : std::true_type
+{};
+
 template<typename T>
 struct is_serialized_subscription : is_serialized_subscription_argument<T>
 {};
@@ -75,6 +86,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 +97,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,204 @@
+// 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_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 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.
+  /**
+   * \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
+  {
+    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.
+  virtual void publish_message()
+  {
+    rclcpp::Time window_end{get_current_nanoseconds_since_epoch()};
+
+    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);
+      publisher_->publish(message);
+    }
+    window_start_ = window_end;
+  }
+
+protected:
+  /// Return a vector of all the currently collected data.
+  /**
+   * \return a vector of all the collected data
+   */
+  std::vector<StatisticData> get_current_collector_data() const
+  {
+    std::vector<StatisticData> data;
+    for (const auto & collector : subscriber_statistics_collectors_) {
+      data.push_back(collector->GetStatisticsResults());
+    }
+    return data;
+  }
+
+private:
+  /// Construct and start all collectors and set window_start_.
+  void bring_up()
+  {
+    auto received_message_period = std::make_unique<ReceivedMessagePeriod>();
+    received_message_period->Start();
+    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.
+  void tear_down()
+  {
+    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();
+  }
+
+  /// 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_algorithms.hpp

@@ -0,0 +1,48 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAIT_SET_ALGORITHMS_HPP_
+#define RCLCPP__WAIT_SET_ALGORITHMS_HPP_
+
+#include <algorithm>
+
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/wait_result.hpp"
+
+namespace rclcpp
+{
+
+/// Find next ready entity out of the given sequence from start to last.
+template<class InputIt>
+InputIt
+find_next_ready_wait_set_entity(InputIt start, InputIt last)
+{
+  return std::find_if(start, last, [](const auto & entity) {return nullptr != entity;});
+}
+
+/// Find next ready guard condition given a WaitResult.
+template<class WaitSetT>
+typename WaitSetT::StoragePolicy::GuardConditionsIterable::const_iterator
+find_next_ready_guard_condition(
+  const typename rclcpp::WaitResult<WaitSetT> & wait_result,
+  typename WaitSetT::StoragePolicy::GuardConditionsIterable::const_iterator start = (
+    wait_result.get_wait_set()))
+{
+  return
+}
+
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_SET_ALGORITHMS_HPP_

rclcpp/include/rclcpp/wait_set_policies/detail/entry_types.hpp

@@ -0,0 +1,123 @@
+// 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__ENTRY_TYPES_HPP_
+#define RCLCPP__WAIT_SET_POLICIES__DETAIL__ENTRY_TYPES_HPP_
+
+#include <limits>
+#include <memory>
+
+#include "rclcpp/client.hpp"
+#include "rclcpp/guard_condition.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/waitable.hpp"
+
+namespace rclcpp
+{
+namespace wait_set_policies
+{
+namespace detail
+{
+
+constexpr const size_t not_in_wait_set = (std::numeric_limits<std::size_t>::max)();
+
+/// Templated ownership entries used for storage.
+template<class PointerT>
+class EntryTemplate : public PointerT
+{
+public:
+  size_t rcl_wait_set_index;
+  constexpr static const size_t not_in_wait_set = not_in_wait_set;
+
+  /// Conversion constructor, which is intentionally not marked explicit.
+  EntryTemplate(
+    const PointerT & entity_in = nullptr,
+    size_t rcl_wait_set_index_in = not_in_wait_set) noexcept
+  : PointerT(entity_in),
+    rcl_wait_set_index(rcl_wait_set_index_in)
+  {}
+};
+
+/// Templated ownership subscription entries used for storage.
+template<class PointerT>
+class SubscriptionEntryTemplate : public EntryTemplate<PointerT>
+{
+public:
+  rclcpp::SubscriptionWaitSetMask mask;
+
+  /// Conversion constructor, which is intentionally not marked explicit.
+  SubscriptionEntryTemplate(
+    const PointerT & entity_in = nullptr,
+    const rclcpp::SubscriptionWaitSetMask & mask_in = {},
+    size_t rcl_wait_set_index_in = not_in_wait_set) noexcept
+  : EntryTemplate<PointerT>(entity_in, rcl_wait_set_index_in),
+    mask(mask_in)
+  {}
+};
+
+/// Templated ownership entries with associated entity used for storage.
+template<class PointerT>
+class EntryWithAssociatedEntityTemplate : public PointerT
+{
+public:
+  std::shared_ptr<void> associated_entity;
+
+  /// Conversion constructor, which is intentionally not marked explicit.
+  EntryWithAssociatedEntityTemplate(
+    const PointerT & entity_in = nullptr,
+    const std::shared_ptr<void> & associated_entity_in = nullptr) noexcept
+  : PointerT(entity_in),
+    associated_entity(associated_entity_in)
+  {}
+};
+
+/// Shared ownership SubscriptionBase entry for storage.
+using SubscriptionEntry = SubscriptionEntryTemplate<std::shared_ptr<rclcpp::SubscriptionBase>>;
+/// Weak ownership SubscriptionBase entry for storage.
+using WeakSubscriptionEntry = SubscriptionEntryTemplate<std::weak_ptr<rclcpp::SubscriptionBase>>;
+
+/// Shared ownership GuardCondition entry for storage.
+using GuardConditionEntry = EntryTemplate<std::shared_ptr<rclcpp::GuardCondition>>;
+/// Weak ownership GuardCondition entry for storage.
+using WeakGuardConditionEntry = EntryTemplate<std::weak_ptr<rclcpp::GuardCondition>>;
+
+/// Shared ownership Timer entry for storage.
+using TimerEntry = EntryTemplate<std::shared_ptr<rclcpp::TimerBase>>;
+/// Weak ownership Timer entry for storage.
+using WeakTimerEntry = EntryTemplate<std::weak_ptr<rclcpp::TimerBase>>;
+
+/// Shared ownership Client entry for storage.
+using ClientEntry = EntryTemplate<std::shared_ptr<rclcpp::ClientBase>>;
+/// Weak ownership Client entry for storage.
+using WeakClientEntry = EntryTemplate<std::weak_ptr<rclcpp::ClientBase>>;
+
+/// Shared ownership Service entry for storage.
+using ServiceEntry = EntryTemplate<std::shared_ptr<rclcpp::ServiceBase>>;
+/// Weak ownership Service entry for storage.
+using WeakServiceEntry = EntryTemplate<std::weak_ptr<rclcpp::ServiceBase>>;
+
+/// Shared ownership Waitable entry for storage.
+using WaitableEntry = EntryWithAssociatedEntityTemplate<std::shared_ptr<rclcpp::Waitable>>;
+/// Weak ownership Waitable entry for storage.
+using WeakWaitableEntry = EntryWithAssociatedEntityTemplate<std::weak_ptr<rclcpp::Waitable>>;
+
+}  // namespace detail
+}  // namespace wait_set_policies
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__ENTRY_TYPES_HPP_

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

@@ -0,0 +1,420 @@
+// 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),
+    previous_size_of_extra_guard_conditions_(extra_guard_conditions.size())
+  {
+    // 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;
+    // Check if the extra_guard_conditions has changed.
+    if (previous_size_of_extra_guard_conditions_ != extra_guard_conditions.size()) {
+      previous_size_of_extra_guard_conditions_ = extra_guard_conditions.size();
+      needs_resize_ = true;
+    }
+    // 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);
+      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(),
+        &subscription_entry.rcl_wait_set_index);
+      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(),
+            &guard_condition.rcl_wait_set_index);
+          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(),
+        &timer.rcl_wait_set_index);
+      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(),
+        &clients.rcl_wait_set_index);
+      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(),
+        &service.rcl_wait_set_index);
+      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);
+      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;
+  size_t previous_size_of_extra_guard_conditions_ = 0;
+};
+
+}  // 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,370 @@
+// 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/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/wait_set_policies/detail/entry_types.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;
+
+  using SubscriptionEntry = detail::SubscriptionEntry;
+  using SequenceOfWeakSubscriptions = std::vector<detail::WeakSubscriptionEntry>;
+  using SubscriptionsIterable = std::vector<detail::SubscriptionEntry>;
+
+  using SequenceOfWeakGuardConditions = std::vector<detail::WeakGuardConditionEntry>;
+  using GuardConditionsIterable = std::vector<detail::GuardConditionEntry>;
+
+  using SequenceOfWeakTimers = std::vector<detail::WeakTimerEntry>;
+  using TimersIterable = std::vector<detail::TimerEntry>;
+
+  using SequenceOfWeakClients = std::vector<detail::WeakClientEntry>;
+  using ClientsIterable = std::vector<detail::ClientEntry>;
+
+  using SequenceOfWeakServices = std::vector<detail::WeakServiceEntry>;
+  using ServicesIterable = std::vector<detail::ServiceEntry>;
+
+  using WaitableEntry = detail::WaitableEntry;
+  using SequenceOfWeakWaitables = std::vector<detail::WeakWaitableEntry>;
+  using WaitablesIterable = std::vector<detail::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(
+      shared_subscriptions_,
+      shared_guard_conditions_,
+      extra_guard_conditions,
+      shared_timers_,
+      shared_clients_,
+      shared_services_,
+      shared_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");
+    }
+    detail::WeakSubscriptionEntry weak_entry{std::move(subscription), {}, detail::not_in_wait_set};
+    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), detail::not_in_wait_set});
+    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), detail::not_in_wait_set});
+    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), detail::not_in_wait_set});
+    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), detail::not_in_wait_set});
+    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");
+    }
+    detail::WeakWaitableEntry weak_entry(
+      std::move(waitable),
+      std::move(associated_entity),
+      detail::not_in_wait_set);
+    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
+    subscriptions_.erase(std::remove_if(subscriptions_.begin(), subscriptions_.end(), p));
+    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(subscriptions_, shared_subscriptions_);
+    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++] = detail::WaitableEntry{
+            weak_ptr.lock(),
+            weak_ptr.associated_entity};
+        }
+      };
+    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_subscriptions_);
+    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,175 @@
+// 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/entry_types.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;
+
+  using ArrayOfSubscriptions = std::array<
+    detail::SubscriptionEntry,
+    NumberOfSubscriptions
+  >;
+  using SubscriptionsIterable = ArrayOfSubscriptions;
+
+  using ArrayOfGuardConditions = std::array<
+    detail::GuardConditionEntry,
+    NumberOfGuardCondtions
+  >;
+  using GuardConditionsIterable = ArrayOfGuardConditions;
+
+  using ArrayOfTimers = std::array<
+    detail::TimerEntry,
+    NumberOfTimers
+  >;
+  using TimersIterable = ArrayOfTimers;
+
+  using ArrayOfClients = std::array<
+    detail::ClientEntry,
+    NumberOfClients
+  >;
+  using ClientsIterable = ArrayOfClients;
+
+  using ArrayOfServices = std::array<
+    detail::ServiceEntry,
+    NumberOfServices
+  >;
+  using ServicesIterable = ArrayOfServices;
+
+  using ArrayOfWaitables = std::array<
+    detail::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 destroyed, 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,29 +9,28 @@
 
   <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>

rclcpp/resource/logging.hpp.em

@@ -17,6 +17,7 @@
 #ifndef RCLCPP__LOGGING_HPP_
 #define RCLCPP__LOGGING_HPP_
 
+#include <sstream>
 #include <type_traits>
 
 #include "rclcpp/logger.hpp"
@@ -128,9 +129,9 @@ def get_rclcpp_suffix_from_features(features):
       typename ::rclcpp::Logger>::value, \
       "First argument to logging macros must be an rclcpp::Logger"); \
 @[ if 'throttle' in feature_combination]@ \
-    auto get_time_point = [&clock](rcutils_time_point_value_t * time_point) -> rcutils_ret_t { \
+    auto get_time_point = [&c=clock](rcutils_time_point_value_t * time_point) -> rcutils_ret_t { \
       try { \
-        *time_point = clock.now().nanoseconds(); \
+        *time_point = c.now().nanoseconds(); \
       } catch (...) { \
         RCUTILS_SAFE_FWRITE_TO_STDERR( \
         "[rclcpp|logging.hpp] RCLCPP_@(severity)@(suffix) could not get current time stamp\n"); \

rclcpp/src/rclcpp/any_executable.cpp

@@ -14,7 +14,7 @@
 
 #include "rclcpp/any_executable.hpp"
 
-using rclcpp::executor::AnyExecutable;
+using rclcpp::AnyExecutable;
 
 AnyExecutable::AnyExecutable()
 : subscription(nullptr),

rclcpp/src/rclcpp/callback_group.cpp

@@ -16,8 +16,8 @@
 
 #include <vector>
 
-using rclcpp::callback_group::CallbackGroup;
-using rclcpp::callback_group::CallbackGroupType;
+using rclcpp::CallbackGroup;
+using rclcpp::CallbackGroupType;
 
 CallbackGroup::CallbackGroup(CallbackGroupType group_type)
 : type_(group_type), can_be_taken_from_(true)

rclcpp/src/rclcpp/client.cpp

@@ -70,6 +70,21 @@ ClientBase::~ClientBase()
   client_handle_.reset();
 }
 
+bool
+ClientBase::take_type_erased_response(void * response_out, rmw_request_id_t & request_header_out)
+{
+  rcl_ret_t ret = rcl_take_response(
+    this->get_client_handle().get(),
+    &request_header_out,
+    response_out);
+  if (RCL_RET_CLIENT_TAKE_FAILED == ret) {
+    return false;
+  } else if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+  return true;
+}
+
 const char *
 ClientBase::get_service_name() const
 {
@@ -177,3 +192,9 @@ ClientBase::get_rcl_node_handle() const
 {
   return node_handle_.get();
 }
+
+bool
+ClientBase::exchange_in_use_by_wait_set_state(bool in_use_state)
+{
+  return in_use_by_wait_set_.exchange(in_use_state);
+}