[rclcpp] add WaitSet class and modify entities to work without executor (#1047)

* add rclcpp::GuardCondition wrapping rcl_guard_condition_t

Signed-off-by: William Woodall <william@osrfoundation.org>

* WIP second wait set refactor, just guard conditions so far

Signed-off-by: William Woodall <william@osrfoundation.org>

* fix typo

Signed-off-by: William Woodall <william@osrfoundation.org>

* removing a question/todo, I think this is fine as is

Signed-off-by: William Woodall <william@osrfoundation.org>

* added subscriptions and waitable to wait sets

Signed-off-by: William Woodall <william@osrfoundation.org>

* improve usability with subscriptions and wait sets

Signed-off-by: William Woodall <william@osrfoundation.org>

* adding take to subscription so it can be used without the executor

Signed-off-by: William Woodall <william@osrfoundation.org>

* add rclcpp::MessageInfo to replace use of rmw_message_info_t

Signed-off-by: William Woodall <william@osrfoundation.org>

* refactor Subscription and Executor so they can be used separately

Signed-off-by: William Woodall <william@osrfoundation.org>

* style and cpplint

Signed-off-by: William Woodall <william@osrfoundation.org>

* fixup take_serialized() and add tests for it

Signed-off-by: William Woodall <william@osrfoundation.org>

* add support for client and service to wait set

Signed-off-by: William Woodall <william@osrfoundation.org>

* fix typo

Signed-off-by: William Woodall <william@osrfoundation.org>

* fix typo

Signed-off-by: William Woodall <william@osrfoundation.org>

* fix review comment

Signed-off-by: William Woodall <william@osrfoundation.org>

* add thread-safe wait set policy

Signed-off-by: William Woodall <william@osrfoundation.org>

* add check for use with multiple wait set

Signed-off-by: William Woodall <william@osrfoundation.org>

* fixup visibility macro usage

Signed-off-by: William Woodall <william@osrfoundation.org>

* remove vestigial test case

Signed-off-by: William Woodall <william@osrfoundation.org>

* move visibility macro fixes

Signed-off-by: William Woodall <william@osrfoundation.org>

* remove vestigial TODO

Signed-off-by: William Woodall <william@osrfoundation.org>

rclcpp/CMakeLists.txt

@@ -49,11 +49,13 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/executors/static_executor_entities_collector.cpp
   src/rclcpp/executors/static_single_threaded_executor.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
@@ -84,6 +86,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
 )
 
@@ -519,6 +522,19 @@ 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()
+
   # Install test resources
   install(
     DIRECTORY test/resources

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_) {

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/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/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::default_context::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/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/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/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(
+  explicit ServiceBase(std::shared_ptr<rcl_node_t> node_handle);
-    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;
+  /// Take the next request from the service as a type erased pointer.
-  virtual std::shared_ptr<rmw_request_id_t> create_request_header() = 0;
+  /**
-  virtual void handle_request(
+   * 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>
@@ -222,36 +265,63 @@ public:
   {
   }
 
-  std::shared_ptr<void> create_request() override
+  /// 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() override
+  std::shared_ptr<void>
+  create_request() override
   {
-    // TODO(wjwwood): This should probably use rmw_request_id's allocator.
+    return std::make_shared<typename ServiceT::Request>();
-    //                (since it is a C type)
-    return std::shared_ptr<rmw_request_id_t>(new rmw_request_id_t);
   }
 
-  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) 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) {
+  void
-      rclcpp::exceptions::throw_from_rcl_error(status, "failed to send response");
+  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/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"
@@ -145,19 +146,17 @@ public:
 
       // 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<
+      using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess<
-        rclcpp::experimental::SubscriptionIntraProcess<
+        CallbackMessageT,
-          CallbackMessageT,
+        AllocatorT,
-          AllocatorT,
+        typename MessageUniquePtr::deleter_type>;
-          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
+        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)
+        resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback));
-        );
       TRACEPOINT(
         rclcpp_subscription_init,
         (const void *)get_subscription_handle().get(),
@@ -187,7 +186,8 @@ public:
   }
 
   /// 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)
@@ -197,19 +197,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.
+   * Data may be taken (written) into the message_out and message_info_out even
-   * \param[in] message_memory_strategy Shared pointer to the memory strategy to set.
+   * 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(
+  bool
-    typename message_memory_strategy::MessageMemoryStrategy<CallbackMessageT,
+  take(CallbackMessageT & message_out, rclcpp::MessageInfo & message_info_out)
-    AllocatorT>::SharedPtr message_memory_strategy)
   {
-    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
@@ -218,15 +231,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(
+  void
-    std::shared_ptr<void> & message, const rmw_message_info_t & message_info) override
+  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;
@@ -237,7 +253,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
@@ -248,18 +265,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,6 +265,7 @@ 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);
   }
 
@@ -229,6 +290,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_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/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/utilities.hpp

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

rclcpp/include/rclcpp/wait_result.hpp

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

rclcpp/include/rclcpp/wait_result_kind.hpp

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

rclcpp/include/rclcpp/wait_set.hpp

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

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

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

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

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

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

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

rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp

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

rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp

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

rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp

@@ -0,0 +1,201 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAIT_SET_POLICIES__STATIC_STORAGE_HPP_
+#define RCLCPP__WAIT_SET_POLICIES__STATIC_STORAGE_HPP_
+
+#include <array>
+#include <memory>
+
+#include "rclcpp/client.hpp"
+#include "rclcpp/guard_condition.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/subscription_wait_set_mask.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/wait_set_policies/detail/storage_policy_common.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace wait_set_policies
+{
+
+/// WaitSet policy that explicitly provides fixed sized storage only.
+/**
+ * Note the underlying rcl_wait_set_t is still dynamically allocated, but only
+ * once during construction, and deallocated once during destruction.
+ */
+template<
+  std::size_t NumberOfSubscriptions,
+  std::size_t NumberOfGuardCondtions,
+  std::size_t NumberOfTimers,
+  std::size_t NumberOfClients,
+  std::size_t NumberOfServices,
+  std::size_t NumberOfWaitables
+>
+class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCommon<true>
+{
+protected:
+  using is_mutable = std::false_type;
+
+  class SubscriptionEntry
+  {
+public:
+    std::shared_ptr<rclcpp::SubscriptionBase> subscription;
+    rclcpp::SubscriptionWaitSetMask mask;
+
+    /// Conversion constructor, which is intentionally not marked explicit.
+    SubscriptionEntry(
+      const std::shared_ptr<rclcpp::SubscriptionBase> & subscription_in = nullptr,
+      const rclcpp::SubscriptionWaitSetMask & mask_in = {})
+    : subscription(subscription_in),
+      mask(mask_in)
+    {}
+  };
+  using ArrayOfSubscriptions = std::array<
+    SubscriptionEntry,
+    NumberOfSubscriptions
+  >;
+  using SubscriptionsIterable = ArrayOfSubscriptions;
+
+  using ArrayOfGuardConditions = std::array<
+    std::shared_ptr<rclcpp::GuardCondition>,
+    NumberOfGuardCondtions
+  >;
+  using GuardConditionsIterable = ArrayOfGuardConditions;
+
+  using ArrayOfTimers = std::array<
+    std::shared_ptr<rclcpp::TimerBase>,
+    NumberOfTimers
+  >;
+  using TimersIterable = ArrayOfTimers;
+
+  using ArrayOfClients = std::array<
+    std::shared_ptr<rclcpp::ClientBase>,
+    NumberOfClients
+  >;
+  using ClientsIterable = ArrayOfClients;
+
+  using ArrayOfServices = std::array<
+    std::shared_ptr<rclcpp::ServiceBase>,
+    NumberOfServices
+  >;
+  using ServicesIterable = ArrayOfServices;
+
+  struct WaitableEntry
+  {
+    /// Conversion constructor, which is intentionally not marked explicit.
+    WaitableEntry(
+      const std::shared_ptr<rclcpp::Waitable> & waitable_in = nullptr,
+      const std::shared_ptr<void> & associated_entity_in = nullptr) noexcept
+    : waitable(waitable_in),
+      associated_entity(associated_entity_in)
+    {}
+
+    std::shared_ptr<rclcpp::Waitable> waitable;
+    std::shared_ptr<void> associated_entity;
+  };
+  using ArrayOfWaitables = std::array<
+    WaitableEntry,
+    NumberOfWaitables
+  >;
+  using WaitablesIterable = ArrayOfWaitables;
+
+  template<class ArrayOfExtraGuardConditions>
+  explicit
+  StaticStorage(
+    const ArrayOfSubscriptions & subscriptions,
+    const ArrayOfGuardConditions & guard_conditions,
+    const ArrayOfExtraGuardConditions & extra_guard_conditions,
+    const ArrayOfTimers & timers,
+    const ArrayOfClients & clients,
+    const ArrayOfServices & services,
+    const ArrayOfWaitables & waitables,
+    rclcpp::Context::SharedPtr context
+  )
+  : StoragePolicyCommon(
+      subscriptions,
+      guard_conditions,
+      extra_guard_conditions,
+      timers,
+      clients,
+      services,
+      waitables,
+      context),
+    subscriptions_(subscriptions),
+    guard_conditions_(guard_conditions),
+    timers_(timers),
+    clients_(clients),
+    services_(services),
+    waitables_(waitables)
+  {}
+
+  ~StaticStorage() = default;
+
+  template<class ArrayOfExtraGuardConditions>
+  void
+  storage_rebuild_rcl_wait_set(const ArrayOfExtraGuardConditions & extra_guard_conditions)
+  {
+    this->storage_rebuild_rcl_wait_set_with_sets(
+      subscriptions_,
+      guard_conditions_,
+      extra_guard_conditions,
+      timers_,
+      clients_,
+      services_,
+      waitables_
+    );
+  }
+
+  // storage_add_subscription() explicitly not declared here
+  // storage_remove_subscription() explicitly not declared here
+  // storage_add_guard_condition() explicitly not declared here
+  // storage_remove_guard_condition() explicitly not declared here
+  // storage_add_timer() explicitly not declared here
+  // storage_remove_timer() explicitly not declared here
+  // storage_add_client() explicitly not declared here
+  // storage_remove_client() explicitly not declared here
+  // storage_add_service() explicitly not declared here
+  // storage_remove_service() explicitly not declared here
+  // storage_add_waitable() explicitly not declared here
+  // storage_remove_waitable() explicitly not declared here
+  // storage_prune_deleted_entities() explicitly not declared here
+
+  void
+  storage_acquire_ownerships()
+  {
+    // Explicitly do nothing.
+  }
+
+  void
+  storage_release_ownerships()
+  {
+    // Explicitly do nothing.
+  }
+
+  const ArrayOfSubscriptions subscriptions_;
+  const ArrayOfGuardConditions guard_conditions_;
+  const ArrayOfTimers timers_;
+  const ArrayOfClients clients_;
+  const ArrayOfServices services_;
+  const ArrayOfWaitables waitables_;
+};
+
+}  // namespace wait_set_policies
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_SET_POLICIES__STATIC_STORAGE_HPP_

rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp

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

rclcpp/include/rclcpp/wait_set_template.hpp

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

rclcpp/include/rclcpp/waitable.hpp

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

rclcpp/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);
+}

rclcpp/src/rclcpp/executor.cpp

@@ -305,99 +305,127 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
   }
 }
 
+static
 void
-Executor::execute_subscription(
+take_and_do_error_handling(
-  rclcpp::SubscriptionBase::SharedPtr subscription)
+  const char * action_description,
+  const char * topic_or_service_name,
+  std::function<bool()> take_action,
+  std::function<void()> handle_action)
 {
-  rmw_message_info_t message_info;
+  bool taken = false;
-  message_info.from_intra_process = false;
+  try {
+    taken = take_action();
+  } catch (const rclcpp::exceptions::RCLError & rcl_error) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("rclcpp"),
+      "executor %s '%s' unexpectedly failed: %s",
+      action_description,
+      topic_or_service_name,
+      rcl_error.what());
+  }
+  if (taken) {
+    handle_action();
+  } else {
+    // Message or Service was not taken for some reason.
+    // Note that this can be normal, if the underlying middleware needs to
+    // interrupt wait spuriously it is allowed.
+    // So in that case the executor cannot tell the difference in a
+    // spurious wake up and an entity actually having data until trying
+    // to take the data.
+    RCLCPP_DEBUG(
+      rclcpp::get_logger("rclcpp"),
+      "executor %s '%s' failed to take anything",
+      action_description,
+      topic_or_service_name);
+  }
+}
+
+void
+Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription)
+{
+  rclcpp::MessageInfo message_info;
+  message_info.get_rmw_message_info().from_intra_process = false;
 
   if (subscription->is_serialized()) {
-    auto serialized_msg = subscription->create_serialized_message();
+    // This is the case where a copy of the serialized message is taken from
-    auto ret = rcl_take_serialized_message(
+    // the middleware via inter-process communication.
-      subscription->get_subscription_handle().get(),
+    std::shared_ptr<rcl_serialized_message_t> serialized_msg =
-      serialized_msg.get(), &message_info, nullptr);
+      subscription->create_serialized_message();
-    if (RCL_RET_OK == ret) {
+    take_and_do_error_handling(
-      auto void_serialized_msg = std::static_pointer_cast<void>(serialized_msg);
+      "taking a serialized message from topic",
-      subscription->handle_message(void_serialized_msg, message_info);
+      subscription->get_topic_name(),
-    } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) {
+      [&]() {return subscription->take_serialized(*serialized_msg.get(), message_info);},
-      RCUTILS_LOG_ERROR_NAMED(
+      [&]()
-        "rclcpp",
+      {
-        "take_serialized failed for subscription on topic '%s': %s",
+        auto void_serialized_msg = std::static_pointer_cast<void>(serialized_msg);
-        subscription->get_topic_name(), rcl_get_error_string().str);
+        subscription->handle_message(void_serialized_msg, message_info);
-      rcl_reset_error();
+      });
-    }
     subscription->return_serialized_message(serialized_msg);
   } else if (subscription->can_loan_messages()) {
+    // This is the case where a loaned message is taken from the middleware via
+    // inter-process communication, given to the user for their callback,
+    // and then returned.
     void * loaned_msg = nullptr;
-    auto ret = rcl_take_loaned_message(
+    // TODO(wjwwood): refactor this into methods on subscription when LoanedMessage
-      subscription->get_subscription_handle().get(),
+    //   is extened to support subscriptions as well.
-      &loaned_msg,
+    take_and_do_error_handling(
-      &message_info,
+      "taking a loaned message from topic",
-      nullptr);
+      subscription->get_topic_name(),
-    if (RCL_RET_OK == ret) {
+      [&]()
-      subscription->handle_loaned_message(loaned_msg, message_info);
+      {
-    } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) {
+        rcl_ret_t ret = rcl_take_loaned_message(
-      RCUTILS_LOG_ERROR_NAMED(
+          subscription->get_subscription_handle().get(),
-        "rclcpp",
+          &loaned_msg,
-        "take_loaned failed for subscription on topic '%s': %s",
+          &message_info.get_rmw_message_info(),
-        subscription->get_topic_name(), rcl_get_error_string().str);
+          nullptr);
-      rcl_reset_error();
+        if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) {
-    }
+          return false;
-    ret = rcl_return_loaned_message_from_subscription(
+        } else if (RCL_RET_OK != ret) {
+          rclcpp::exceptions::throw_from_rcl_error(ret);
+        }
+        return true;
+      },
+      [&]() {subscription->handle_loaned_message(loaned_msg, message_info);});
+    rcl_ret_t ret = rcl_return_loaned_message_from_subscription(
       subscription->get_subscription_handle().get(),
       loaned_msg);
     if (RCL_RET_OK != ret) {
-      RCUTILS_LOG_ERROR_NAMED(
+      RCLCPP_ERROR(
-        "rclcpp",
+        rclcpp::get_logger("rclcpp"),
-        "return_loaned_message failed for subscription on topic '%s': %s",
+        "rcl_return_loaned_message_from_subscription() failed for subscription on topic '%s': %s",
         subscription->get_topic_name(), rcl_get_error_string().str);
     }
     loaned_msg = nullptr;
   } else {
+    // This case is taking a copy of the message data from the middleware via
+    // inter-process communication.
     std::shared_ptr<void> message = subscription->create_message();
-    auto ret = rcl_take(
+    take_and_do_error_handling(
-      subscription->get_subscription_handle().get(),
+      "taking a message from topic",
-      message.get(), &message_info, nullptr);
+      subscription->get_topic_name(),
-    if (RCL_RET_OK == ret) {
+      [&]() {return subscription->take_type_erased(message.get(), message_info);},
-      subscription->handle_message(message, message_info);
+      [&]() {subscription->handle_message(message, message_info);});
-    } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) {
-      RCUTILS_LOG_ERROR_NAMED(
-        "rclcpp",
-        "could not deserialize serialized message on topic '%s': %s",
-        subscription->get_topic_name(), rcl_get_error_string().str);
-      rcl_reset_error();
-    }
     subscription->return_message(message);
   }
 }
 
 void
-Executor::execute_timer(
+Executor::execute_timer(rclcpp::TimerBase::SharedPtr timer)
-  rclcpp::TimerBase::SharedPtr timer)
 {
   timer->execute_callback();
 }
 
 void
-Executor::execute_service(
+Executor::execute_service(rclcpp::ServiceBase::SharedPtr service)
-  rclcpp::ServiceBase::SharedPtr service)
 {
   auto request_header = service->create_request_header();
   std::shared_ptr<void> request = service->create_request();
-  rcl_ret_t status = rcl_take_request(
+  take_and_do_error_handling(
-    service->get_service_handle().get(),
+    "taking a service server request from service",
-    request_header.get(),
+    service->get_service_name(),
-    request.get());
+    [&]() {return service->take_type_erased_request(request.get(), *request_header);},
-  if (status == RCL_RET_OK) {
+    [&]() {service->handle_request(request_header, request);});
-    service->handle_request(request_header, request);
-  } else if (status != RCL_RET_SERVICE_TAKE_FAILED) {
-    RCUTILS_LOG_ERROR_NAMED(
-      "rclcpp",
-      "take request failed for server of service '%s': %s",
-      service->get_service_name(), rcl_get_error_string().str);
-    rcl_reset_error();
-  }
 }
 
 void
@@ -406,19 +434,11 @@ Executor::execute_client(
 {
   auto request_header = client->create_request_header();
   std::shared_ptr<void> response = client->create_response();
-  rcl_ret_t status = rcl_take_response(
+  take_and_do_error_handling(
-    client->get_client_handle().get(),
+    "taking a service client response from service",
-    request_header.get(),
+    client->get_service_name(),
-    response.get());
+    [&]() {return client->take_type_erased_response(response.get(), *request_header);},
-  if (status == RCL_RET_OK) {
+    [&]() {client->handle_response(request_header, response);});
-    client->handle_response(request_header, response);
-  } else if (status != RCL_RET_CLIENT_TAKE_FAILED) {
-    RCUTILS_LOG_ERROR_NAMED(
-      "rclcpp",
-      "take response failed for client of service '%s': %s",
-      client->get_service_name(), rcl_get_error_string().str);
-    rcl_reset_error();
-  }
 }
 
 void

rclcpp/src/rclcpp/guard_condition.cpp

@@ -0,0 +1,80 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/guard_condition.hpp"
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/logging.hpp"
+
+namespace rclcpp
+{
+
+GuardCondition::GuardCondition(rclcpp::Context::SharedPtr context)
+: context_(context), rcl_guard_condition_{rcl_get_zero_initialized_guard_condition()}
+{
+  if (!context_) {
+    throw std::invalid_argument("context argument unexpectedly nullptr");
+  }
+  rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options();
+  rcl_ret_t ret = rcl_guard_condition_init(
+    &this->rcl_guard_condition_,
+    context_->get_rcl_context().get(),
+    guard_condition_options);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+GuardCondition::~GuardCondition()
+{
+  rcl_ret_t ret = rcl_guard_condition_fini(&this->rcl_guard_condition_);
+  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 guard condition: %s", exception.what());
+    }
+  }
+}
+
+rclcpp::Context::SharedPtr
+GuardCondition::get_context() const
+{
+  return context_;
+}
+
+const rcl_guard_condition_t &
+GuardCondition::get_rcl_guard_condition() const
+{
+  return rcl_guard_condition_;
+}
+
+void
+GuardCondition::trigger()
+{
+  rcl_ret_t ret = rcl_trigger_guard_condition(&rcl_guard_condition_);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+bool
+GuardCondition::exchange_in_use_by_wait_set_state(bool in_use_state)
+{
+  return in_use_by_wait_set_.exchange(in_use_state);
+}
+
+}  // namespace rclcpp

rclcpp/src/rclcpp/message_info.cpp

@@ -0,0 +1,39 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/message_info.hpp"
+
+namespace rclcpp
+{
+
+MessageInfo::MessageInfo(const rmw_message_info_t & rmw_message_info)
+: rmw_message_info_(rmw_message_info)
+{}
+
+MessageInfo::~MessageInfo()
+{}
+
+const rmw_message_info_t &
+MessageInfo::get_rmw_message_info() const
+{
+  return rmw_message_info_;
+}
+
+rmw_message_info_t &
+MessageInfo::get_rmw_message_info()
+{
+  return rmw_message_info_;
+}
+
+}  // namespace rclcpp

rclcpp/src/rclcpp/service.cpp

@@ -34,6 +34,21 @@ ServiceBase::ServiceBase(std::shared_ptr<rcl_node_t> node_handle)
 ServiceBase::~ServiceBase()
 {}
 
+bool
+ServiceBase::take_type_erased_request(void * request_out, rmw_request_id_t & request_id_out)
+{
+  rcl_ret_t ret = rcl_take_request(
+    this->get_service_handle().get(),
+    &request_id_out,
+    request_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 *
 ServiceBase::get_service_name()
 {
@@ -63,3 +78,9 @@ ServiceBase::get_rcl_node_handle() const
 {
   return node_handle_.get();
 }
+
+bool
+ServiceBase::exchange_in_use_by_wait_set_state(bool in_use_state)
+{
+  return in_use_by_wait_set_.exchange(in_use_state);
+}

rclcpp/src/rclcpp/subscription_base.cpp

@@ -133,6 +133,48 @@ SubscriptionBase::get_actual_qos() const
   return rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos);
 }
 
+bool
+SubscriptionBase::take_type_erased(void * message_out, rclcpp::MessageInfo & message_info_out)
+{
+  rcl_ret_t ret = rcl_take(
+    this->get_subscription_handle().get(),
+    message_out,
+    &message_info_out.get_rmw_message_info(),
+    nullptr  // rmw_subscription_allocation_t is unused here
+  );
+  if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) {
+    return false;
+  } else if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+  if (
+    matches_any_intra_process_publishers(&message_info_out.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 false;
+  }
+  return true;
+}
+
+bool
+SubscriptionBase::take_serialized(
+  rcl_serialized_message_t & message_out,
+  rclcpp::MessageInfo & message_info_out)
+{
+  rcl_ret_t ret = rcl_take_serialized_message(
+    this->get_subscription_handle().get(),
+    &message_out,
+    &message_info_out.get_rmw_message_info(),
+    nullptr);
+  if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) {
+    return false;
+  } else if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+  return true;
+}
+
 const rosidl_message_type_support_t &
 SubscriptionBase::get_message_type_support_handle() const
 {
@@ -209,3 +251,25 @@ SubscriptionBase::matches_any_intra_process_publishers(const rmw_gid_t * sender_
   }
   return ipm->matches_any_publishers(sender_gid);
 }
+
+bool
+SubscriptionBase::exchange_in_use_by_wait_set_state(
+  void * pointer_to_subscription_part,
+  bool in_use_state)
+{
+  if (nullptr == pointer_to_subscription_part) {
+    throw std::invalid_argument("pointer_to_subscription_part is unexpectedly nullptr");
+  }
+  if (this == pointer_to_subscription_part) {
+    return subscription_in_use_by_wait_set_.exchange(in_use_state);
+  }
+  if (get_intra_process_waitable().get() == pointer_to_subscription_part) {
+    return intra_process_subscription_waitable_in_use_by_wait_set_.exchange(in_use_state);
+  }
+  for (const auto & qos_event : event_handlers_) {
+    if (qos_event.get() == pointer_to_subscription_part) {
+      return qos_events_in_use_by_wait_set_[qos_event.get()].exchange(in_use_state);
+    }
+  }
+  throw std::runtime_error("given pointer_to_subscription_part does not match any part");
+}

rclcpp/src/rclcpp/timer.cpp

@@ -135,3 +135,9 @@ TimerBase::get_timer_handle()
 {
   return timer_handle_;
 }
+
+bool
+TimerBase::exchange_in_use_by_wait_set_state(bool in_use_state)
+{
+  return in_use_by_wait_set_.exchange(in_use_state);
+}

rclcpp/src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp

@@ -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.
+
+#include "rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp"
+
+namespace rclcpp
+{
+namespace wait_set_policies
+{
+namespace detail
+{
+
+WritePreferringReadWriteLock::WritePreferringReadWriteLock(
+  std::function<void()> enter_waiting_function)
+: read_mutex_(*this), write_mutex_(*this), enter_waiting_function_(enter_waiting_function)
+{}
+
+WritePreferringReadWriteLock::ReadMutex &
+WritePreferringReadWriteLock::get_read_mutex()
+{
+  return read_mutex_;
+}
+
+WritePreferringReadWriteLock::WriteMutex &
+WritePreferringReadWriteLock::get_write_mutex()
+{
+  return write_mutex_;
+}
+
+WritePreferringReadWriteLock::ReadMutex::ReadMutex(WritePreferringReadWriteLock & parent_lock)
+: parent_lock_(parent_lock)
+{}
+
+void
+WritePreferringReadWriteLock::ReadMutex::lock()
+{
+  std::unique_lock<std::mutex> lock(parent_lock_.mutex_);
+  while (
+    parent_lock_.number_of_writers_waiting_ > 0 ||
+    parent_lock_.writer_active_ ||
+    parent_lock_.reader_active_)
+  {
+    parent_lock_.condition_variable_.wait(lock);
+  }
+  parent_lock_.reader_active_ = true;
+  // implicit unlock of parent_lock_.mutex_
+}
+
+void
+WritePreferringReadWriteLock::ReadMutex::unlock()
+{
+  std::unique_lock<std::mutex> lock(parent_lock_.mutex_);
+  parent_lock_.reader_active_ = false;
+  parent_lock_.condition_variable_.notify_all();
+  // implicit unlock of parent_lock_.mutex_
+}
+
+WritePreferringReadWriteLock::WriteMutex::WriteMutex(WritePreferringReadWriteLock & parent_lock)
+: parent_lock_(parent_lock)
+{}
+
+void
+WritePreferringReadWriteLock::WriteMutex::lock()
+{
+  std::unique_lock<std::mutex> lock(parent_lock_.mutex_);
+  parent_lock_.number_of_writers_waiting_ += 1;
+  if (nullptr != parent_lock_.enter_waiting_function_) {
+    parent_lock_.enter_waiting_function_();
+  }
+  while (parent_lock_.reader_active_ || parent_lock_.writer_active_) {
+    parent_lock_.condition_variable_.wait(lock);
+  }
+  parent_lock_.number_of_writers_waiting_ -= 1;
+  parent_lock_.writer_active_ = true;
+  // implicit unlock of parent_lock_.mutex_
+}
+
+void
+WritePreferringReadWriteLock::WriteMutex::unlock()
+{
+  std::unique_lock<std::mutex> lock(parent_lock_.mutex_);
+  parent_lock_.writer_active_ = false;
+  parent_lock_.condition_variable_.notify_all();
+  // implicit unlock of parent_lock_.mutex_
+}
+
+}  // namespace detail
+}  // namespace wait_set_policies
+}  // namespace rclcpp

rclcpp/src/rclcpp/waitable.cpp

@@ -51,3 +51,9 @@ Waitable::get_number_of_ready_guard_conditions()
 {
   return 0u;
 }
+
+bool
+Waitable::exchange_in_use_by_wait_set_state(bool in_use_state)
+{
+  return in_use_by_wait_set_.exchange(in_use_state);
+}

rclcpp/test/test_guard_condition.cpp

@@ -0,0 +1,87 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+
+class TestGuardCondition : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+};
+
+/*
+ * Testing normal construction and destruction.
+ */
+TEST_F(TestGuardCondition, construction_and_destruction) {
+  {
+    auto gc = std::make_shared<rclcpp::GuardCondition>();
+    (void)gc;
+  }
+
+  {
+    // invalid context (nullptr)
+    ASSERT_THROW(
+    {
+      auto gc = std::make_shared<rclcpp::GuardCondition>(nullptr);
+      (void)gc;
+    }, std::invalid_argument);
+  }
+
+  {
+    // invalid context (uninitialized)
+    auto context = std::make_shared<rclcpp::Context>();
+    ASSERT_THROW(
+    {
+      auto gc = std::make_shared<rclcpp::GuardCondition>(context);
+      (void)gc;
+    }, rclcpp::exceptions::RCLInvalidArgument);
+  }
+}
+
+/*
+ * Testing context accessor.
+ */
+TEST_F(TestGuardCondition, get_context) {
+  {
+    auto gc = std::make_shared<rclcpp::GuardCondition>();
+    gc->get_context();
+  }
+}
+
+/*
+ * Testing rcl guard condition accessor.
+ */
+TEST_F(TestGuardCondition, get_rcl_guard_condition) {
+  {
+    auto gc = std::make_shared<rclcpp::GuardCondition>();
+    gc->get_rcl_guard_condition();
+  }
+}
+
+/*
+ * Testing tigger method.
+ */
+TEST_F(TestGuardCondition, trigger) {
+  {
+    auto gc = std::make_shared<rclcpp::GuardCondition>();
+    gc->trigger();
+  }
+}

rclcpp/test/test_subscription.cpp

@@ -14,8 +14,10 @@
 
 #include <gtest/gtest.h>
 
-#include <string>
+#include <chrono>
 #include <memory>
+#include <string>
+#include <thread>
 #include <vector>
 
 #include "rclcpp/exceptions.hpp"
@@ -23,6 +25,8 @@
 
 #include "test_msgs/msg/empty.hpp"
 
+using namespace std::chrono_literals;
+
 class TestSubscription : public ::testing::Test
 {
 public:
@@ -235,13 +239,13 @@ TEST_F(TestSubscription, callback_bind) {
   using test_msgs::msg::Empty;
   {
     // Member callback for plain class
-    SubscriptionClass subscriptionObject;
+    SubscriptionClass subscription_object;
-    subscriptionObject.CreateSubscription();
+    subscription_object.CreateSubscription();
   }
   {
     // Member callback for class inheriting from rclcpp::Node
-    SubscriptionClassNodeInheritance subscriptionObject;
+    SubscriptionClassNodeInheritance subscription_object;
-    subscriptionObject.CreateSubscription();
+    subscription_object.CreateSubscription();
   }
   {
     // Member callback for class inheriting from testing::Test
@@ -252,6 +256,79 @@ TEST_F(TestSubscription, callback_bind) {
   }
 }
 
+/*
+   Testing take.
+ */
+TEST_F(TestSubscription, take) {
+  initialize();
+  using test_msgs::msg::Empty;
+  auto do_nothing = [](std::shared_ptr<const test_msgs::msg::Empty>) {FAIL();};
+  {
+    auto sub = node->create_subscription<test_msgs::msg::Empty>("~/test_take", 1, do_nothing);
+    test_msgs::msg::Empty msg;
+    rclcpp::MessageInfo msg_info;
+    EXPECT_FALSE(sub->take(msg, msg_info));
+  }
+  {
+    rclcpp::SubscriptionOptions so;
+    so.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable;
+    auto sub = node->create_subscription<test_msgs::msg::Empty>("~/test_take", 1, do_nothing, so);
+    rclcpp::PublisherOptions po;
+    po.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable;
+    auto pub = node->create_publisher<test_msgs::msg::Empty>("~/test_take", 1, po);
+    {
+      test_msgs::msg::Empty msg;
+      pub->publish(msg);
+    }
+    test_msgs::msg::Empty msg;
+    rclcpp::MessageInfo msg_info;
+    bool message_recieved = false;
+    auto start = std::chrono::steady_clock::now();
+    do {
+      message_recieved = sub->take(msg, msg_info);
+      std::this_thread::sleep_for(100ms);
+    } while (!message_recieved && std::chrono::steady_clock::now() - start < 10s);
+    EXPECT_TRUE(message_recieved);
+  }
+  // TODO(wjwwood): figure out a good way to test the intra-process exclusion behavior.
+}
+
+/*
+   Testing take_serialized.
+ */
+TEST_F(TestSubscription, take_serialized) {
+  initialize();
+  using test_msgs::msg::Empty;
+  auto do_nothing = [](std::shared_ptr<const rcl_serialized_message_t>) {FAIL();};
+  {
+    auto sub = node->create_subscription<test_msgs::msg::Empty>("~/test_take", 1, do_nothing);
+    std::shared_ptr<rcl_serialized_message_t> msg = sub->create_serialized_message();
+    rclcpp::MessageInfo msg_info;
+    EXPECT_FALSE(sub->take_serialized(*msg, msg_info));
+  }
+  {
+    rclcpp::SubscriptionOptions so;
+    so.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable;
+    auto sub = node->create_subscription<test_msgs::msg::Empty>("~/test_take", 1, do_nothing, so);
+    rclcpp::PublisherOptions po;
+    po.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable;
+    auto pub = node->create_publisher<test_msgs::msg::Empty>("~/test_take", 1, po);
+    {
+      test_msgs::msg::Empty msg;
+      pub->publish(msg);
+    }
+    std::shared_ptr<rcl_serialized_message_t> msg = sub->create_serialized_message();
+    rclcpp::MessageInfo msg_info;
+    bool message_recieved = false;
+    auto start = std::chrono::steady_clock::now();
+    do {
+      message_recieved = sub->take_serialized(*msg, msg_info);
+      std::this_thread::sleep_for(100ms);
+    } while (!message_recieved && std::chrono::steady_clock::now() - start < 10s);
+    EXPECT_TRUE(message_recieved);
+  }
+}
+
 /*
    Testing subscription with intraprocess enabled and invalid QoS
  */

rclcpp/test/test_wait_set.cpp

@@ -0,0 +1,264 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <vector>
+
+#include "rcl_interfaces/srv/list_parameters.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "test_msgs/msg/basic_types.hpp"
+
+class TestWaitSet : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+};
+
+/*
+ * Testing normal construction and destruction.
+ */
+TEST_F(TestWaitSet, construction_and_destruction) {
+  {
+    rclcpp::WaitSet wait_set;
+    (void)wait_set;
+  }
+
+  {
+    rclcpp::WaitSet wait_set(
+      std::vector<rclcpp::WaitSet::SubscriptionEntry>{},
+      std::vector<rclcpp::GuardCondition::SharedPtr>{},
+      std::vector<rclcpp::TimerBase::SharedPtr>{},
+      std::vector<rclcpp::ClientBase::SharedPtr>{},
+      std::vector<rclcpp::ServiceBase::SharedPtr>{},
+      std::vector<rclcpp::WaitSet::WaitableEntry>{});
+    (void)wait_set;
+  }
+
+  {
+    auto gc = std::make_shared<rclcpp::GuardCondition>();
+    rclcpp::WaitSet wait_set({}, {gc});
+    (void)wait_set;
+  }
+
+  {
+    auto context = std::make_shared<rclcpp::Context>();
+    context->init(0, nullptr);
+    auto gc = std::make_shared<rclcpp::GuardCondition>(context);
+    rclcpp::WaitSet wait_set({}, {gc}, {}, {}, {}, {}, context);
+    (void)wait_set;
+  }
+
+  {
+    // invalid context (nullptr)
+    ASSERT_THROW(
+    {
+      rclcpp::WaitSet wait_set(
+        std::vector<rclcpp::WaitSet::SubscriptionEntry>{},
+        std::vector<rclcpp::GuardCondition::SharedPtr>{},
+        std::vector<rclcpp::TimerBase::SharedPtr>{},
+        std::vector<rclcpp::ClientBase::SharedPtr>{},
+        std::vector<rclcpp::ServiceBase::SharedPtr>{},
+        std::vector<rclcpp::WaitSet::WaitableEntry>{},
+        nullptr);
+      (void)wait_set;
+    }, std::invalid_argument);
+  }
+
+  {
+    // invalid context (uninitialized)
+    auto context = std::make_shared<rclcpp::Context>();
+    ASSERT_THROW(
+    {
+      rclcpp::WaitSet wait_set(
+        std::vector<rclcpp::WaitSet::SubscriptionEntry>{},
+        std::vector<rclcpp::GuardCondition::SharedPtr>{},
+        std::vector<rclcpp::TimerBase::SharedPtr>{},
+        std::vector<rclcpp::ClientBase::SharedPtr>{},
+        std::vector<rclcpp::ServiceBase::SharedPtr>{},
+        std::vector<rclcpp::WaitSet::WaitableEntry>{},
+        context);
+      (void)wait_set;
+    }, rclcpp::exceptions::RCLInvalidArgument);
+  }
+}
+
+/*
+ * Testing rcl wait set accessor.
+ */
+TEST_F(TestWaitSet, get_rcl_wait_set) {
+  {
+    rclcpp::WaitSet wait_set;
+    wait_set.get_rcl_wait_set();
+  }
+}
+
+/*
+ * Testing add/remove for guard condition methods.
+ */
+TEST_F(TestWaitSet, add_remove_guard_condition) {
+  // normal, mixed initialization
+  {
+    auto gc = std::make_shared<rclcpp::GuardCondition>();
+    auto gc2 = std::make_shared<rclcpp::GuardCondition>();
+    rclcpp::WaitSet wait_set({}, {gc});
+    wait_set.add_guard_condition(gc2);
+    wait_set.remove_guard_condition(gc2);
+    wait_set.remove_guard_condition(gc);
+  }
+
+  // out of order removal
+  {
+    auto gc = std::make_shared<rclcpp::GuardCondition>();
+    auto gc2 = std::make_shared<rclcpp::GuardCondition>();
+    rclcpp::WaitSet wait_set({}, {gc});
+    wait_set.add_guard_condition(gc2);
+    wait_set.remove_guard_condition(gc);
+    wait_set.remove_guard_condition(gc2);
+  }
+
+  // start empty, normal
+  {
+    auto gc = std::make_shared<rclcpp::GuardCondition>();
+    rclcpp::WaitSet wait_set;
+    wait_set.add_guard_condition(gc);
+    wait_set.remove_guard_condition(gc);
+  }
+
+  // add invalid (nullptr)
+  {
+    rclcpp::WaitSet wait_set;
+    ASSERT_THROW(
+    {
+      wait_set.add_guard_condition(nullptr);
+    }, std::invalid_argument);
+  }
+
+  // double add
+  {
+    auto gc = std::make_shared<rclcpp::GuardCondition>();
+    rclcpp::WaitSet wait_set;
+    wait_set.add_guard_condition(gc);
+    ASSERT_THROW(
+    {
+      wait_set.add_guard_condition(gc);
+    }, std::runtime_error);
+  }
+
+  // remove invalid (nullptr)
+  {
+    rclcpp::WaitSet wait_set;
+    ASSERT_THROW(
+    {
+      wait_set.remove_guard_condition(nullptr);
+    }, std::invalid_argument);
+  }
+
+  // remove unrelated
+  {
+    auto gc = std::make_shared<rclcpp::GuardCondition>();
+    auto gc2 = std::make_shared<rclcpp::GuardCondition>();
+    rclcpp::WaitSet wait_set({}, {gc});
+    ASSERT_THROW(
+    {
+      wait_set.remove_guard_condition(gc2);
+    }, std::runtime_error);
+  }
+
+  // double remove
+  {
+    auto gc = std::make_shared<rclcpp::GuardCondition>();
+    rclcpp::WaitSet wait_set({}, {gc});
+    wait_set.remove_guard_condition(gc);
+    ASSERT_THROW(
+    {
+      wait_set.remove_guard_condition(gc);
+    }, std::runtime_error);
+  }
+
+  // remove from empty
+  {
+    auto gc = std::make_shared<rclcpp::GuardCondition>();
+    rclcpp::WaitSet wait_set;
+    ASSERT_THROW(
+    {
+      wait_set.remove_guard_condition(gc);
+    }, std::runtime_error);
+  }
+}
+
+/*
+ * Testing adding each entity to two separate wait sets.
+ */
+TEST_F(TestWaitSet, add_guard_condition_to_two_different_wait_set) {
+  {
+    rclcpp::WaitSet wait_set1;
+    rclcpp::WaitSet wait_set2;
+    auto node = std::make_shared<rclcpp::Node>("add_guard_condition_to_two_different_wait_set");
+
+    auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+    wait_set1.add_guard_condition(guard_condition);
+    ASSERT_THROW(
+    {
+      wait_set2.add_guard_condition(guard_condition);
+    }, std::runtime_error);
+
+    auto do_nothing = [](const std::shared_ptr<test_msgs::msg::BasicTypes>) {};
+    auto sub = node->create_subscription<test_msgs::msg::BasicTypes>("~/test", 1, do_nothing);
+    wait_set1.add_subscription(sub);
+    ASSERT_THROW(
+    {
+      wait_set2.add_subscription(sub);
+    }, std::runtime_error);
+
+    auto timer = node->create_wall_timer(std::chrono::seconds(1), []() {});
+    wait_set1.add_timer(timer);
+    ASSERT_THROW(
+    {
+      wait_set2.add_timer(timer);
+    }, std::runtime_error);
+
+    auto client = node->create_client<rcl_interfaces::srv::ListParameters>("~/test");
+    wait_set1.add_client(client);
+    ASSERT_THROW(
+    {
+      wait_set2.add_client(client);
+    }, std::runtime_error);
+
+    auto srv_do_nothing = [](
+      const std::shared_ptr<rcl_interfaces::srv::ListParameters::Request>,
+      std::shared_ptr<rcl_interfaces::srv::ListParameters::Response>) {};
+    auto service =
+      node->create_service<rcl_interfaces::srv::ListParameters>("~/test", srv_do_nothing);
+    wait_set1.add_service(service);
+    ASSERT_THROW(
+    {
+      wait_set2.add_service(service);
+    }, std::runtime_error);
+
+    rclcpp::PublisherOptions po;
+    po.event_callbacks.deadline_callback = [](rclcpp::QOSDeadlineOfferedInfo &) {};
+    auto pub = node->create_publisher<test_msgs::msg::BasicTypes>("~/test", 1, po);
+    auto qos_event = pub->get_event_handlers()[0];
+    wait_set1.add_waitable(qos_event, pub);
+    ASSERT_THROW(
+    {
+      wait_set2.add_waitable(qos_event, pub);
+    }, std::runtime_error);
+  }
+}