make it compile on osx

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

README.md

@@ -0,0 +1,17 @@
+# rclcpp
+
+This repository contains the source code for the ROS Client Library for C++ package, included with a standard install of any ROS 2 distro.
+
+rclcpp provides the standard C++ API for interacting with ROS 2.
+
+## Usage
+
+`#include "rclcpp/rclcpp.hpp"` allows use of the most common elements of the ROS 2 system.
+
+Visit the [rclcpp API documentation](http://docs.ros2.org/eloquent/api/rclcpp/) for a complete list of its main components.
+
+### Examples
+
+The ROS 2 tutorials [Writing a simple publisher and subscriber](https://index.ros.org/doc/ros2/Tutorials/Writing-A-Simple-Cpp-Publisher-And-Subscriber/)
+and [Writing a simple service and client](https://index.ros.org/doc/ros2/Tutorials/Writing-A-Simple-Cpp-Service-And-Client/)
+contain some examples of rclcpp APIs in use.

rclcpp/CMakeLists.txt

@@ -8,10 +8,10 @@ find_package(rcl REQUIRED)
 find_package(rcl_interfaces REQUIRED)
 find_package(rcl_yaml_param_parser REQUIRED)
 find_package(rcpputils REQUIRED)
+find_package(rcutils REQUIRED)
 find_package(rmw REQUIRED)
-find_package(rmw_implementation REQUIRED)
 find_package(rosgraph_msgs REQUIRED)
-find_package(rosidl_generator_cpp REQUIRED)
+find_package(rosidl_runtime_cpp REQUIRED)
 find_package(rosidl_typesupport_c REQUIRED)
 find_package(rosidl_typesupport_cpp REQUIRED)
 find_package(tracetools REQUIRED)
@@ -36,20 +36,26 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/detail/rmw_implementation_specific_payload.cpp
   src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp
   src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp
+  src/rclcpp/detail/utilities.cpp
   src/rclcpp/duration.cpp
   src/rclcpp/event.cpp
   src/rclcpp/exceptions.cpp
+  src/rclcpp/executable_list.cpp
   src/rclcpp/executor.cpp
   src/rclcpp/executors.cpp
   src/rclcpp/expand_topic_or_service_name.cpp
   src/rclcpp/executors/multi_threaded_executor.cpp
   src/rclcpp/executors/single_threaded_executor.cpp
+  src/rclcpp/executors/static_executor_entities_collector.cpp
+  src/rclcpp/executors/static_single_threaded_executor.cpp
   src/rclcpp/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
@@ -80,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
 )
 
@@ -112,10 +119,11 @@ ament_target_dependencies(${PROJECT_NAME}
   "rcl"
   "rcl_yaml_param_parser"
   "rcpputils"
+  "rcutils"
   "builtin_interfaces"
   "rosgraph_msgs"
   "rosidl_typesupport_cpp"
-  "rosidl_generator_cpp"
+  "rosidl_runtime_cpp"
   "tracetools"
 )
 
@@ -135,14 +143,14 @@ install(
 ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})
 
-ament_export_dependencies(ament_cmake)
 ament_export_dependencies(rcl)
 ament_export_dependencies(rcpputils)
+ament_export_dependencies(rcutils)
 ament_export_dependencies(builtin_interfaces)
 ament_export_dependencies(rosgraph_msgs)
 ament_export_dependencies(rosidl_typesupport_cpp)
 ament_export_dependencies(rosidl_typesupport_c)
-ament_export_dependencies(rosidl_generator_cpp)
+ament_export_dependencies(rosidl_runtime_cpp)
 ament_export_dependencies(rcl_yaml_param_parser)
 ament_export_dependencies(tracetools)
 
@@ -164,7 +172,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_client
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_client ${PROJECT_NAME})
@@ -175,7 +183,7 @@ if(BUILD_TESTING)
       "rcl_interfaces"
       "rmw"
       "rcl"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_create_timer ${PROJECT_NAME})
@@ -186,7 +194,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_expand_topic_or_service_name
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME})
@@ -196,7 +204,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_function_traits
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
   endif()
@@ -206,7 +214,7 @@ if(BUILD_TESTING)
       "rcl"
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_intra_process_manager ${PROJECT_NAME})
@@ -216,7 +224,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_ring_buffer_implementation
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_ring_buffer_implementation ${PROJECT_NAME})
@@ -226,7 +234,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_intra_process_buffer
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_intra_process_buffer ${PROJECT_NAME})
@@ -237,6 +245,18 @@ if(BUILD_TESTING)
     "test_msgs"
   )
   target_link_libraries(test_loaned_message ${PROJECT_NAME})
+  ament_add_gtest(test_intra_process_communication test/test_intra_process_communication.cpp
+    TIMEOUT 120)
+  if(TARGET test_intra_process_communication)
+    ament_target_dependencies(test_intra_process_communication
+      "rcl"
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_typesupport_cpp"
+      "test_msgs"
+    )
+  endif()
+  target_link_libraries(test_intra_process_communication ${PROJECT_NAME})
 
   ament_add_gtest(test_node test/test_node.cpp TIMEOUT 240)
   if(TARGET test_node)
@@ -244,8 +264,9 @@ if(BUILD_TESTING)
       "rcl_interfaces"
       "rcpputils"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
+      "test_msgs"
     )
     target_link_libraries(test_node ${PROJECT_NAME})
   endif()
@@ -282,7 +303,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_node_global_args
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_node_global_args ${PROJECT_NAME})
@@ -304,7 +325,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_parameter_events_filter
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_parameter_events_filter ${PROJECT_NAME})
@@ -314,7 +335,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_parameter
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_parameter ${PROJECT_NAME})
@@ -326,10 +347,10 @@ if(BUILD_TESTING)
   ament_add_gtest(test_publisher test/test_publisher.cpp)
   if(TARGET test_publisher)
     ament_target_dependencies(test_publisher
-      "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
+      "test_msgs"
     )
     target_link_libraries(test_publisher ${PROJECT_NAME})
   endif()
@@ -338,17 +359,37 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_publisher_subscription_count_api
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
+      "test_msgs"
     )
     target_link_libraries(test_publisher_subscription_count_api ${PROJECT_NAME})
   endif()
+  ament_add_gtest(test_qos test/test_qos.cpp)
+  if(TARGET test_qos)
+    ament_target_dependencies(test_qos
+      "rmw"
+    )
+    target_link_libraries(test_qos
+      ${PROJECT_NAME}
+    )
+  endif()
+  ament_add_gtest(test_qos_event test/test_qos_event.cpp)
+  if(TARGET test_qos_event)
+    ament_target_dependencies(test_qos_event
+      "rmw"
+      "test_msgs"
+    )
+    target_link_libraries(test_qos_event
+      ${PROJECT_NAME}
+    )
+  endif()
   ament_add_gtest(test_rate test/test_rate.cpp)
   if(TARGET test_rate)
     ament_target_dependencies(test_rate
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_rate
@@ -369,7 +410,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_service
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_service ${PROJECT_NAME})
@@ -379,8 +420,9 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_subscription
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
+      "test_msgs"
     )
     target_link_libraries(test_subscription ${PROJECT_NAME})
   endif()
@@ -389,8 +431,9 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_subscription_publisher_count_api
       "rcl_interfaces"
       "rmw"
-      "rosidl_generator_cpp"
+      "rosidl_runtime_cpp"
       "rosidl_typesupport_cpp"
+      "test_msgs"
     )
     target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME})
   endif()
@@ -491,6 +534,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_service_callback.hpp

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

rclcpp/include/rclcpp/any_subscription_callback.hpp

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

rclcpp/include/rclcpp/client.hpp

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

rclcpp/include/rclcpp/clock.hpp

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

rclcpp/include/rclcpp/context.hpp

@@ -159,7 +159,7 @@ public:
    *
    * \param[in] reason the description of why shutdown happened
    * \return true if shutdown was successful, false if context was already shutdown
-   * \throw various exceptions derived from RCLErrorBase, if rcl_shutdown fails
+   * \throw various exceptions derived from rclcpp::exceptions::RCLError, if rcl_shutdown fails
    */
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/create_publisher.hpp

@@ -43,6 +43,7 @@ std::shared_ptr<PublisherT>
 create_publisher(
   NodeT & node,
   const std::string & topic_name,
+  const rosidl_message_type_support_t & type_support,
   const rclcpp::QoS & qos,
   const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options = (
     rclcpp::PublisherOptionsWithAllocator<AllocatorT>()
@@ -56,7 +57,7 @@ create_publisher(
   // Create the publisher.
   auto pub = node_topics->create_publisher(
     topic_name,
-    rclcpp::create_publisher_factory<MessageT, AllocatorT, PublisherT>(options),
+    rclcpp::create_publisher_factory<MessageT, AllocatorT, PublisherT>(options, type_support),
     qos
   );
 
@@ -66,6 +67,28 @@ create_publisher(
   return std::dynamic_pointer_cast<PublisherT>(pub);
 }
 
+template<
+  typename MessageT,
+  typename AllocatorT = std::allocator<void>,
+  typename PublisherT = rclcpp::Publisher<MessageT, AllocatorT>,
+  typename NodeT>
+std::shared_ptr<PublisherT>
+create_publisher(
+  NodeT & node,
+  const std::string & topic_name,
+  const rclcpp::QoS & qos,
+  const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options = (
+    rclcpp::PublisherOptionsWithAllocator<AllocatorT>()
+  )
+)
+{
+  const auto type_support = *rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>();
+
+  return create_publisher<MessageT, AllocatorT, PublisherT, NodeT>(
+    node, topic_name, type_support,
+    qos, options);
+}
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__CREATE_PUBLISHER_HPP_

rclcpp/include/rclcpp/create_subscription.hpp

@@ -51,14 +51,13 @@ typename std::shared_ptr<SubscriptionT>
 create_subscription(
   NodeT && node,
   const std::string & topic_name,
+  const rosidl_message_type_support_t & type_support,
   const rclcpp::QoS & qos,
   CallbackT && callback,
-  const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
-    rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
-  ),
-  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = (
+  const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options =
+    rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>(),
+  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat =
     MessageMemoryStrategyT::create_default()
-  )
 )
 {
   using rclcpp::node_interfaces::get_node_topics_interface;
@@ -67,7 +66,8 @@ create_subscription(
   auto factory = rclcpp::create_subscription_factory<MessageT>(
     std::forward<CallbackT>(callback),
     options,
-    msg_mem_strat
+    msg_mem_strat,
+    type_support
   );
 
   auto sub = node_topics->create_subscription(topic_name, factory, qos);
@@ -76,6 +76,39 @@ create_subscription(
   return std::dynamic_pointer_cast<SubscriptionT>(sub);
 }
 
+template<
+  typename MessageT,
+  typename CallbackT,
+  typename AllocatorT = std::allocator<void>,
+  typename CallbackMessageT =
+  typename rclcpp::subscription_traits::has_message_type<CallbackT>::type,
+  typename SubscriptionT = rclcpp::Subscription<CallbackMessageT, AllocatorT>,
+  typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy<
+    CallbackMessageT,
+    AllocatorT
+  >,
+  typename NodeT>
+typename std::shared_ptr<SubscriptionT>
+create_subscription(
+  NodeT && node,
+  const std::string & topic_name,
+  const rclcpp::QoS & qos,
+  CallbackT && callback,
+  const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options =
+    rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>(),
+  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat =
+    MessageMemoryStrategyT::create_default()
+)
+{
+  const auto type_support = *rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>();
+
+  return create_subscription<MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT,
+           MessageMemoryStrategyT>(
+    std::forward<NodeT>(
+      node), topic_name, type_support, qos, std::forward<CallbackT>(
+      callback), options, msg_mem_strat);
+}
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__CREATE_SUBSCRIPTION_HPP_

rclcpp/include/rclcpp/detail/utilities.hpp

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

rclcpp/include/rclcpp/exceptions.hpp

@@ -226,6 +226,25 @@ class InvalidParameterValueException : public std::runtime_error
   using std::runtime_error::runtime_error;
 };
 
+/// Thrown if requested parameter type is invalid.
+/**
+ * Essentially the same as rclcpp::ParameterTypeException, but with parameter
+ * name in the error message.
+ */
+class InvalidParameterTypeException : public std::runtime_error
+{
+public:
+  /// Construct an instance.
+  /**
+   * \param[in] name the name of the parameter.
+   * \param[in] message custom exception message.
+   */
+  RCLCPP_PUBLIC
+  InvalidParameterTypeException(const std::string & name, const std::string message)
+  : std::runtime_error("parameter '" + name + "' has invalid type: " + message)
+  {}
+};
+
 /// Thrown if parameter is already declared.
 class ParameterAlreadyDeclaredException : public std::runtime_error
 {

rclcpp/include/rclcpp/executable_list.hpp

@@ -0,0 +1,92 @@
+// Copyright 2019 Nobleo Technology
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__EXECUTABLE_LIST_HPP_
+#define RCLCPP__EXECUTABLE_LIST_HPP_
+
+#include <memory>
+#include <vector>
+
+#include "rclcpp/client.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace executor
+{
+
+/// This class contains subscriptionbase, timerbase, etc. which can be used to run callbacks.
+class ExecutableList final
+{
+public:
+  RCLCPP_PUBLIC
+  ExecutableList();
+
+  RCLCPP_PUBLIC
+  ~ExecutableList();
+
+  RCLCPP_PUBLIC
+  void
+  clear();
+
+  RCLCPP_PUBLIC
+  void
+  add_subscription(rclcpp::SubscriptionBase::SharedPtr subscription);
+
+  RCLCPP_PUBLIC
+  void
+  add_timer(rclcpp::TimerBase::SharedPtr timer);
+
+  RCLCPP_PUBLIC
+  void
+  add_service(rclcpp::ServiceBase::SharedPtr service);
+
+  RCLCPP_PUBLIC
+  void
+  add_client(rclcpp::ClientBase::SharedPtr client);
+
+  RCLCPP_PUBLIC
+  void
+  add_waitable(rclcpp::Waitable::SharedPtr waitable);
+
+  // Vector containing the SubscriptionBase of all the subscriptions added to the executor.
+  std::vector<rclcpp::SubscriptionBase::SharedPtr> subscription;
+  // Contains the count of added subscriptions
+  size_t number_of_subscriptions;
+  // Vector containing the TimerBase of all the timers added to the executor.
+  std::vector<rclcpp::TimerBase::SharedPtr> timer;
+  // Contains the count of added timers
+  size_t number_of_timers;
+  // Vector containing the ServiceBase of all the services added to the executor.
+  std::vector<rclcpp::ServiceBase::SharedPtr> service;
+  // Contains the count of added services
+  size_t number_of_services;
+  // Vector containing the ClientBase of all the clients added to the executor.
+  std::vector<rclcpp::ClientBase::SharedPtr> client;
+  // Contains the count of added clients
+  size_t number_of_clients;
+  // Vector containing all the waitables added to the executor.
+  std::vector<rclcpp::Waitable::SharedPtr> waitable;
+  // Contains the count of added waitables
+  size_t number_of_waitables;
+};
+
+}  // namespace executor
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXECUTABLE_LIST_HPP_

rclcpp/include/rclcpp/executor.hpp

@@ -223,7 +223,7 @@ public:
   template<typename ResponseT, typename TimeRepT = int64_t, typename TimeT = std::milli>
   FutureReturnCode
   spin_until_future_complete(
-    std::shared_future<ResponseT> & future,
+    const std::shared_future<ResponseT> & future,
     std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
   {
     // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
@@ -357,7 +357,6 @@ protected:
   /// The context associated with this executor.
   std::shared_ptr<rclcpp::Context> context_;
 
-private:
   RCLCPP_DISABLE_COPY(Executor)
 
   std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;

rclcpp/include/rclcpp/executors.hpp

@@ -20,6 +20,7 @@
 
 #include "rclcpp/executors/multi_threaded_executor.hpp"
 #include "rclcpp/executors/single_threaded_executor.hpp"
+#include "rclcpp/executors/static_single_threaded_executor.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -70,7 +71,7 @@ rclcpp::executor::FutureReturnCode
 spin_node_until_future_complete(
   rclcpp::executor::Executor & executor,
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  std::shared_future<ResponseT> & future,
+  const std::shared_future<ResponseT> & future,
   std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
@@ -87,7 +88,7 @@ rclcpp::executor::FutureReturnCode
 spin_node_until_future_complete(
   rclcpp::executor::Executor & executor,
   std::shared_ptr<NodeT> node_ptr,
-  std::shared_future<ResponseT> & future,
+  const std::shared_future<ResponseT> & future,
   std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   return rclcpp::executors::spin_node_until_future_complete(
@@ -103,7 +104,7 @@ template<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::mi
 rclcpp::executor::FutureReturnCode
 spin_until_future_complete(
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  std::shared_future<FutureT> & future,
+  const std::shared_future<FutureT> & future,
   std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   rclcpp::executors::SingleThreadedExecutor executor;
@@ -115,7 +116,7 @@ template<typename NodeT = rclcpp::Node, typename FutureT, typename TimeRepT = in
 rclcpp::executor::FutureReturnCode
 spin_until_future_complete(
   std::shared_ptr<NodeT> node_ptr,
-  std::shared_future<FutureT> & future,
+  const std::shared_future<FutureT> & future,
   std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout);

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -62,7 +62,7 @@ public:
 
   RCLCPP_PUBLIC
   void
-  spin();
+  spin() override;
 
   RCLCPP_PUBLIC
   size_t

rclcpp/include/rclcpp/executors/single_threaded_executor.hpp

@@ -56,7 +56,7 @@ public:
   // It will only be interrupt by a CTRL-C (managed by the global signal handler).
   RCLCPP_PUBLIC
   void
-  spin();
+  spin() override;
 
 private:
   RCLCPP_DISABLE_COPY(SingleThreadedExecutor)

rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp

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

rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp

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

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

@@ -110,11 +110,14 @@ public:
    * In addition this generates a unique intra process id for the subscription.
    *
    * \param subscription the SubscriptionIntraProcess to register.
+   * \param is_serialized true if the buffer expects serialized messages
    * \return an unsigned 64-bit integer which is the subscription's unique id.
    */
   RCLCPP_PUBLIC
   uint64_t
-  add_subscription(rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription);
+  add_subscription(
+    rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription,
+    const bool is_serialized = false);
 
   /// Unregister a subscription using the subscription's unique id.
   /**
@@ -134,11 +137,12 @@ public:
    * In addition this generates a unique intra process id for the publisher.
    *
    * \param publisher publisher to be registered with the manager.
+   * \param is_serialized true if the buffer expects serialized messages
    * \return an unsigned 64-bit integer which is the publisher's unique id.
    */
   RCLCPP_PUBLIC
   uint64_t
-  add_publisher(rclcpp::PublisherBase::SharedPtr publisher);
+  add_publisher(rclcpp::PublisherBase::SharedPtr publisher, const bool is_serialized = false);
 
   /// Unregister a publisher using the publisher's unique id.
   /**
@@ -227,12 +231,10 @@ public:
       // for the buffers that do not require ownership
       auto shared_msg = std::allocate_shared<MessageT, MessageAllocatorT>(*allocator, *message);
 
-      this->template add_shared_msg_to_buffers<MessageT>(shared_msg,
-        sub_ids.take_shared_subscriptions);
+      this->template add_shared_msg_to_buffers<MessageT>(
+        shared_msg, sub_ids.take_shared_subscriptions);
       this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter>(
-        std::move(message),
-        sub_ids.take_ownership_subscriptions,
-        allocator);
+        std::move(message), sub_ids.take_ownership_subscriptions, allocator);
     }
   }
 
@@ -265,8 +267,8 @@ public:
       // If there are no owning, just convert to shared.
       std::shared_ptr<MessageT> shared_msg = std::move(message);
       if (!sub_ids.take_shared_subscriptions.empty()) {
-        this->template add_shared_msg_to_buffers<MessageT>(shared_msg,
-          sub_ids.take_shared_subscriptions);
+        this->template add_shared_msg_to_buffers<MessageT>(
+          shared_msg, sub_ids.take_shared_subscriptions);
       }
       return shared_msg;
     } else {
@@ -313,6 +315,7 @@ private:
     rmw_qos_profile_t qos;
     const char * topic_name;
     bool use_take_shared_method;
+    bool is_serialized;
   };
 
   struct PublisherInfo
@@ -322,6 +325,7 @@ private:
     rclcpp::PublisherBase::WeakPtr publisher;
     rmw_qos_profile_t qos;
     const char * topic_name;
+    bool is_serialized;
   };
 
   struct SplittedSubscriptions

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -28,6 +28,8 @@
 #include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
 #include "rclcpp/experimental/create_intra_process_buffer.hpp"
 #include "rclcpp/experimental/subscription_intra_process_base.hpp"
+#include "rclcpp/serialization.hpp"
+#include "rclcpp/serialized_message.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/waitable.hpp"
 #include "tracetools/tracetools.h"
@@ -37,6 +39,8 @@ namespace rclcpp
 namespace experimental
 {
 
+class SerializedMessage;
+
 template<
   typename MessageT,
   typename Alloc = std::allocator<void>,
@@ -51,6 +55,10 @@ public:
   using MessageAlloc = typename MessageAllocTraits::allocator_type;
   using ConstMessageSharedPtr = std::shared_ptr<const MessageT>;
   using MessageUniquePtr = std::unique_ptr<MessageT, Deleter>;
+  using CallbackMessageAllocTraits = allocator::AllocRebind<CallbackMessageT, Alloc>;
+  using CallbackMessageAlloc = typename CallbackMessageAllocTraits::allocator_type;
+  using CallbackMessageUniquePtr = std::unique_ptr<CallbackMessageT>;
+  using CallbackMessageSharedPtr = std::shared_ptr<CallbackMessageT>;
 
   using BufferUniquePtr = typename rclcpp::experimental::buffers::IntraProcessBuffer<
     MessageT,
@@ -64,11 +72,15 @@ public:
     rclcpp::Context::SharedPtr context,
     const std::string & topic_name,
     rmw_qos_profile_t qos_profile,
-    rclcpp::IntraProcessBufferType buffer_type)
+    rclcpp::IntraProcessBufferType buffer_type,
+    std::shared_ptr<SerializationBase> serializer)
   : SubscriptionIntraProcessBase(topic_name, qos_profile),
-    any_callback_(callback)
+    any_callback_(callback), serializer_(serializer)
   {
-    if (!std::is_same<MessageT, CallbackMessageT>::value) {
+    if (!std::is_same<MessageT, CallbackMessageT>::value &&
+      !std::is_same<MessageT, rclcpp::SerializedMessage>::value &&
+      !std::is_same<CallbackMessageT, rcl_serialized_message_t>::value)
+    {
       throw std::runtime_error("SubscriptionIntraProcess wrong callback type");
     }
 
@@ -97,7 +109,9 @@ public:
     // The callback object gets copied, so if registration is done too early/before this point
     // (e.g. in `AnySubscriptionCallback::set()`), its address won't match any address used later
     // in subsequent tracepoints.
+#ifndef TRACETOOLS_DISABLED
     any_callback_.register_callback_for_tracing();
+#endif
   }
 
   bool
@@ -140,18 +154,48 @@ private:
     (void)ret;
   }
 
+  // convert from ROS2 message to rcl_serialized_message_t (serilizatino needed)
   template<typename T>
-  typename std::enable_if<std::is_same<T, rcl_serialized_message_t>::value, void>::type
+  typename std::enable_if<
+    std::is_same<T, rcl_serialized_message_t>::value &&
+    !std::is_same<MessageT, rclcpp::SerializedMessage>::value,
+    void>::type
   execute_impl()
   {
-    throw std::runtime_error("Subscription intra-process can't handle serialized messages");
+    if (nullptr == serializer_) {
+      throw std::runtime_error("Subscription intra-process can't handle serialized messages");
+    }
+
+    rmw_message_info_t msg_info = {};
+    msg_info.from_intra_process = true;
+
+    ConstMessageSharedPtr msg = buffer_->consume_shared();
+    auto serialized_msg =
+      serializer_->serialize_message(reinterpret_cast<const void *>(msg.get()));
+
+    if (nullptr == serialized_msg) {
+      throw std::runtime_error("Subscription intra-process could not serialize message");
+    }
+
+    if (any_callback_.use_take_shared_method()) {
+      any_callback_.dispatch_intra_process(serialized_msg, msg_info);
+    } else {
+      throw std::runtime_error(
+              "Subscription intra-process for serialized "
+              "messages does not support unique pointers.");
+    }
   }
 
+  // forward from ROS2 message to ROS2 message (same type)
   template<class T>
-  typename std::enable_if<!std::is_same<T, rcl_serialized_message_t>::value, void>::type
+  typename std::enable_if<
+    !std::is_same<T, rcl_serialized_message_t>::value &&
+    !std::is_same<MessageT, rclcpp::SerializedMessage>::value,
+    void>::type
   execute_impl()
   {
-    rmw_message_info_t msg_info;
+    rmw_message_info_t msg_info = {};
+    msg_info.publisher_gid = {0, {0}};
     msg_info.from_intra_process = true;
 
     if (any_callback_.use_take_shared_method()) {
@@ -163,8 +207,80 @@ private:
     }
   }
 
+  // forward from rcl_serialized_message_t to SerializationMessage (no conversion needed)
+  template<typename T>
+  typename std::enable_if<
+    std::is_same<T, rcl_serialized_message_t>::value &&
+    std::is_same<MessageT, rclcpp::SerializedMessage>::value,
+    void>::type
+  execute_impl()
+  {
+    rmw_message_info_t msg_info = {};
+    msg_info.from_intra_process = true;
+
+    if (any_callback_.use_take_shared_method()) {
+      ConstMessageSharedPtr msg = buffer_->consume_shared();
+      if (msg == nullptr) {
+        throw std::runtime_error("Subscription intra-process could not get serialized message");
+      }
+      any_callback_.dispatch_intra_process(msg, msg_info);
+    } else {
+      throw std::runtime_error(
+              "Subscription intra-process for serialized "
+              "messages does not support unique pointers.");
+    }
+  }
+
+  // convert from rcl_serialized_message_t to ROS2 message (deserialization needed)
+  template<class T>
+  typename std::enable_if<
+    !std::is_same<T, rcl_serialized_message_t>::value &&
+    std::is_same<MessageT, rclcpp::SerializedMessage>::value,
+    void>::type
+  execute_impl()
+  {
+    if (nullptr == serializer_) {
+      throw std::runtime_error("Subscription intra-process can't handle unserialized messages");
+    }
+
+    ConstMessageSharedPtr serialized_container = buffer_->consume_shared();
+    if (nullptr == serialized_container) {
+      throw std::runtime_error("Subscription intra-process could not get serialized message");
+    }
+
+    rmw_message_info_t msg_info = {};
+    msg_info.from_intra_process = true;
+
+    if (any_callback_.use_take_shared_method()) {
+      CallbackMessageSharedPtr msg = construct_unique();
+      serializer_->deserialize_message(
+        serialized_container.get(),
+        reinterpret_cast<void *>(msg.get()));
+      any_callback_.dispatch_intra_process(msg, msg_info);
+    } else {
+      CallbackMessageUniquePtr msg = construct_unique();
+      serializer_->deserialize_message(
+        serialized_container.get(),
+        reinterpret_cast<void *>(msg.get()));
+      any_callback_.dispatch_intra_process(std::move(msg), msg_info);
+    }
+  }
+
+  CallbackMessageUniquePtr construct_unique()
+  {
+    CallbackMessageUniquePtr unique_msg;
+    auto ptr = CallbackMessageAllocTraits::allocate(*message_allocator_.get(), 1);
+    CallbackMessageAllocTraits::construct(*message_allocator_.get(), ptr);
+    unique_msg = CallbackMessageUniquePtr(ptr);
+
+    return unique_msg;
+  }
+
   AnySubscriptionCallback<CallbackMessageT, Alloc> any_callback_;
   BufferUniquePtr buffer_;
+  std::shared_ptr<SerializationBase> serializer_;
+  std::shared_ptr<CallbackMessageAlloc> message_allocator_ =
+    std::make_shared<CallbackMessageAlloc>();
 };
 
 }  // namespace experimental

rclcpp/include/rclcpp/function_traits.hpp

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

rclcpp/include/rclcpp/graph_listener.hpp

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

rclcpp/include/rclcpp/guard_condition.hpp

@@ -0,0 +1,100 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__GUARD_CONDITION_HPP_
+#define RCLCPP__GUARD_CONDITION_HPP_
+
+#include <atomic>
+
+#include "rcl/guard_condition.h"
+
+#include "rclcpp/context.hpp"
+#include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// A condition that can be waited on in a single wait set and asynchronously triggered.
+class GuardCondition
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(GuardCondition)
+
+  // TODO(wjwwood): support custom allocator, maybe restrict to polymorphic allocator
+  /// Construct the guard condition, optionally specifying which Context to use.
+  /**
+   * \param[in] context Optional custom context to be used.
+   *   Defaults to using the global default context singleton.
+   *   Shared ownership of the context is held with the guard condition until
+   *   destruction.
+   * \throws std::invalid_argument if the context is nullptr.
+   * \throws rclcpp::exceptions::RCLError based exceptions when underlying
+   *   rcl functions fail.
+   */
+  RCLCPP_PUBLIC
+  explicit GuardCondition(
+    rclcpp::Context::SharedPtr context =
+    rclcpp::contexts::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/memory_strategy.hpp

@@ -56,6 +56,7 @@ public:
   virtual size_t number_of_guard_conditions() const = 0;
   virtual size_t number_of_waitables() const = 0;
 
+  virtual void add_waitable_handle(const rclcpp::Waitable::SharedPtr & waitable) = 0;
   virtual bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) = 0;
   virtual void clear_handles() = 0;
   virtual void remove_null_handles(rcl_wait_set_t * wait_set) = 0;

rclcpp/include/rclcpp/message_info.hpp

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

rclcpp/include/rclcpp/node.hpp

@@ -301,7 +301,7 @@ public:
    *
    * If the type of the default value, and therefore also the type of return
    * value, differs from the initial value provided in the node options, then
-   * a rclcpp::ParameterTypeException may be thrown.
+   * a rclcpp::exceptions::InvalidParameterTypeException may be thrown.
    * To avoid this, use the declare_parameter() method which returns an
    * rclcpp::ParameterValue instead.
    *
@@ -867,6 +867,58 @@ public:
   size_t
   count_subscribers(const std::string & topic_name) const;
 
+  /// Return the topic endpoint information about publishers on a given topic.
+  /**
+   * The returned parameter is a list of topic endpoint information, where each item will contain
+   * the node name, node namespace, topic type, endpoint type, topic endpoint's GID, and its QoS
+   * profile.
+   *
+   * When the `no_mangle` parameter is `true`, the provided `topic_name` should be a valid topic
+   * name for the middleware (useful when combining ROS with native middleware (e.g. DDS) apps).
+   * When the `no_mangle` parameter is `false`, the provided `topic_name` should follow
+   * ROS topic name conventions.
+   *
+   * `topic_name` may be a relative, private, or fully qualified topic name.
+   * A relative or private topic will be expanded using this node's namespace and name.
+   * The queried `topic_name` is not remapped.
+   *
+   * \param[in] topic_name the topic_name on which to find the publishers.
+   * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name,
+   *   otherwise it should be a valid ROS topic name. Defaults to `false`.
+   * \return a list of TopicEndpointInfo representing all the publishers on this topic.
+   * \throws InvalidTopicNameError if the given topic_name is invalid.
+   * \throws std::runtime_error if internal error happens.
+   */
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_publishers_info_by_topic(const std::string & topic_name, bool no_mangle = false) const;
+
+  /// Return the topic endpoint information about subscriptions on a given topic.
+  /**
+   * The returned parameter is a list of topic endpoint information, where each item will contain
+   * the node name, node namespace, topic type, endpoint type, topic endpoint's GID, and its QoS
+   * profile.
+   *
+   * When the `no_mangle` parameter is `true`, the provided `topic_name` should be a valid topic
+   * name for the middleware (useful when combining ROS with native middleware (e.g. DDS) apps).
+   * When the `no_mangle` parameter is `false`, the provided `topic_name` should follow
+   * ROS topic name conventions.
+   *
+   * `topic_name` may be a relative, private, or fully qualified topic name.
+   * A relative or private topic will be expanded using this node's namespace and name.
+   * The queried `topic_name` is not remapped.
+   *
+   * \param[in] topic_name the topic_name on which to find the subscriptions.
+   * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name,
+   *   otherwise it should be a valid ROS topic name. Defaults to `false`.
+   * \return a list of TopicEndpointInfo representing all the subscriptions on this topic.
+   * \throws InvalidTopicNameError if the given topic_name is invalid.
+   * \throws std::runtime_error if internal error happens.
+   */
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_subscriptions_info_by_topic(const std::string & topic_name, bool no_mangle = false) const;
+
   /// Return a graph event, which will be set anytime a graph change occurs.
   /* The graph Event object is a loan which must be returned.
    * The Event object is scoped and therefore to return the loan just let it go
@@ -894,9 +946,13 @@ public:
   rclcpp::Clock::SharedPtr
   get_clock();
 
+  RCLCPP_PUBLIC
+  rclcpp::Clock::ConstSharedPtr
+  get_clock() const;
+
   RCLCPP_PUBLIC
   Time
-  now();
+  now() const;
 
   /// Return the Node's internal NodeBaseInterface implementation.
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/node_impl.hpp

@@ -33,8 +33,6 @@
 #include "rcl/publisher.h"
 #include "rcl/subscription.h"
 
-#include "rcl_interfaces/msg/intra_process_message.hpp"
-
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/create_client.hpp"
 #include "rclcpp/create_publisher.hpp"
@@ -158,12 +156,16 @@ Node::declare_parameter(
   const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor,
   bool ignore_override)
 {
-  return this->declare_parameter(
-    name,
-    rclcpp::ParameterValue(default_value),
-    parameter_descriptor,
-    ignore_override
-  ).get<ParameterT>();
+  try {
+    return this->declare_parameter(
+      name,
+      rclcpp::ParameterValue(default_value),
+      parameter_descriptor,
+      ignore_override
+    ).get<ParameterT>();
+  } catch (const ParameterTypeException & ex) {
+    throw exceptions::InvalidParameterTypeException(name, ex.what());
+  }
 }
 
 template<typename ParameterT>

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -23,7 +23,6 @@
 #include "rclcpp/context.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
-#include "rclcpp/node_options.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -50,89 +49,89 @@ public:
   ~NodeBase();
 
   RCLCPP_PUBLIC
-  virtual
+
   const char *
-  get_name() const;
+  get_name() const override;
 
   RCLCPP_PUBLIC
-  virtual
+
   const char *
-  get_namespace() const;
+  get_namespace() const override;
 
   RCLCPP_PUBLIC
-  virtual
+
   const char *
-  get_fully_qualified_name() const;
+  get_fully_qualified_name() const override;
 
   RCLCPP_PUBLIC
-  virtual
+
   rclcpp::Context::SharedPtr
-  get_context();
+  get_context() override;
 
   RCLCPP_PUBLIC
-  virtual
+
   rcl_node_t *
-  get_rcl_node_handle();
+  get_rcl_node_handle() override;
 
   RCLCPP_PUBLIC
-  virtual
+
   const rcl_node_t *
-  get_rcl_node_handle() const;
+  get_rcl_node_handle() const override;
 
   RCLCPP_PUBLIC
-  virtual
+
   std::shared_ptr<rcl_node_t>
-  get_shared_rcl_node_handle();
+  get_shared_rcl_node_handle() override;
 
   RCLCPP_PUBLIC
-  virtual
+
   std::shared_ptr<const rcl_node_t>
-  get_shared_rcl_node_handle() const;
+  get_shared_rcl_node_handle() const override;
 
   RCLCPP_PUBLIC
-  virtual
+
   bool
-  assert_liveliness() const;
+  assert_liveliness() const override;
 
   RCLCPP_PUBLIC
-  virtual
+
   rclcpp::callback_group::CallbackGroup::SharedPtr
-  create_callback_group(rclcpp::callback_group::CallbackGroupType group_type);
+  create_callback_group(rclcpp::callback_group::CallbackGroupType group_type) override;
 
   RCLCPP_PUBLIC
-  virtual
+
   rclcpp::callback_group::CallbackGroup::SharedPtr
-  get_default_callback_group();
+  get_default_callback_group() override;
 
   RCLCPP_PUBLIC
-  virtual
+
   bool
-  callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group);
+  callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) override;
 
   RCLCPP_PUBLIC
-  virtual
+
   const std::vector<rclcpp::callback_group::CallbackGroup::WeakPtr> &
-  get_callback_groups() const;
+  get_callback_groups() const override;
 
   RCLCPP_PUBLIC
-  virtual
+
   std::atomic_bool &
-  get_associated_with_executor_atomic();
+  get_associated_with_executor_atomic() override;
 
   RCLCPP_PUBLIC
-  virtual
+
   rcl_guard_condition_t *
-  get_notify_guard_condition();
+  get_notify_guard_condition() override;
 
   RCLCPP_PUBLIC
-  virtual
+
   std::unique_lock<std::recursive_mutex>
-  acquire_notify_guard_condition_lock() const;
+  acquire_notify_guard_condition_lock() const override;
 
   RCLCPP_PUBLIC
-  virtual
+
   bool
-  get_use_intra_process_default() const;
+  get_use_intra_process_default() const override;
 
 private:
   RCLCPP_DISABLE_COPY(NodeBase)

rclcpp/include/rclcpp/node_interfaces/node_clock.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_logging.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_services.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_timers.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp

@@ -22,7 +22,9 @@
 #include "rcl/publisher.h"
 #include "rcl/subscription.h"
 
+#include "rclcpp/callback_group.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/publisher.hpp"
 #include "rclcpp/publisher_factory.hpp"
 #include "rclcpp/subscription.hpp"

rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp

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

rclcpp/include/rclcpp/node_options.hpp

@@ -152,6 +152,22 @@ public:
   NodeOptions &
   use_global_arguments(bool use_global_arguments);
 
+  /// Return the enable_rosout flag.
+  RCLCPP_PUBLIC
+  bool
+  enable_rosout() const;
+
+  /// Set the enable_rosout flag, return this for parameter idiom.
+  /**
+   * If false this will cause the node not to use rosout logging.
+   *
+   * Defaults to true for now, as there are still some cases where it is
+   * desirable.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  enable_rosout(bool enable_rosout);
+
   /// Return the use_intra_process_comms flag.
   RCLCPP_PUBLIC
   bool
@@ -312,6 +328,8 @@ private:
 
   bool use_global_arguments_ {true};
 
+  bool enable_rosout_ {true};
+
   bool use_intra_process_comms_ {false};
 
   bool start_parameter_services_ {true};

rclcpp/include/rclcpp/parameter.hpp

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

rclcpp/include/rclcpp/parameter_client.hpp

@@ -271,10 +271,11 @@ public:
   {
     return get_parameter_impl(
       parameter_name,
-      std::function<T()>([&parameter_name]() -> T
-      {
-        throw std::runtime_error("Parameter '" + parameter_name + "' is not set");
-      })
+      std::function<T()>(
+        [&parameter_name]() -> T
+        {
+          throw std::runtime_error("Parameter '" + parameter_name + "' is not set");
+        })
     );
   }
 

rclcpp/include/rclcpp/publisher.hpp

@@ -56,6 +56,12 @@ public:
   using MessageDeleter = allocator::Deleter<MessageAllocator, MessageT>;
   using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
   using MessageSharedPtr = std::shared_ptr<const MessageT>;
+  using SerializedMessageAllocatorTraits =
+    allocator::AllocRebind<rclcpp::SerializedMessage,
+      AllocatorT>;
+  using SerializedMessageAllocator = typename SerializedMessageAllocatorTraits::allocator_type;
+  using SerializedMessageDeleter = allocator::Deleter<SerializedMessageAllocator,
+      rclcpp::SerializedMessage>;
 
   RCLCPP_SMART_PTR_DEFINITIONS(Publisher<MessageT, AllocatorT>)
 
@@ -70,22 +76,28 @@ public:
       *rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>(),
       options.template to_rcl_publisher_options<MessageT>(qos)),
     options_(options),
-    message_allocator_(new MessageAllocator(*options.get_allocator().get()))
+    message_allocator_(new MessageAllocator(*options.get_allocator().get())),
+    message_allocator_serialized_(new SerializedMessageAllocator(*options.get_allocator().get()))
   {
-    allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get());
+    init_setup();
+  }
 
-    if (options_.event_callbacks.deadline_callback) {
-      this->add_event_handler(
-        options_.event_callbacks.deadline_callback,
-        RCL_PUBLISHER_OFFERED_DEADLINE_MISSED);
-    }
-    if (options_.event_callbacks.liveliness_callback) {
-      this->add_event_handler(
-        options_.event_callbacks.liveliness_callback,
-        RCL_PUBLISHER_LIVELINESS_LOST);
-    }
-
-    // Setup continues in the post construction method, post_init_setup().
+  Publisher(
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    const std::string & topic,
+    const rclcpp::QoS & qos,
+    const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options,
+    const rosidl_message_type_support_t & type_support)
+  : PublisherBase(
+      node_base,
+      topic,
+      type_support,
+      options.template to_rcl_publisher_options<MessageT>(qos)),
+    options_(options),
+    message_allocator_(new MessageAllocator(*options.get_allocator().get())),
+    message_allocator_serialized_(new SerializedMessageAllocator(*options.get_allocator().get()))
+  {
+    init_setup();
   }
 
   /// Called post construction, so that construction may continue after shared_from_this() works.
@@ -120,8 +132,11 @@ public:
                 "intraprocess communication allowed only with volatile durability");
       }
       uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this());
+      uint64_t intra_process_publisher_id_serialized = ipm->add_publisher(
+        this->shared_from_this(), true);
       this->setup_intra_process(
         intra_process_publisher_id,
+        intra_process_publisher_id_serialized,
         ipm);
     }
   }
@@ -158,6 +173,11 @@ public:
   virtual void
   publish(std::unique_ptr<MessageT, MessageDeleter> msg)
   {
+    if (std::is_same<MessageT, rcl_serialized_message_t>::value) {
+      this->template publish<MessageDeleter>(std::move(msg));
+      return;
+    }
+
     if (!intra_process_is_enabled_) {
       this->do_inter_process_publish(*msg);
       return;
@@ -172,34 +192,41 @@ public:
       get_subscription_count() > get_intra_process_subscription_count();
 
     if (inter_process_publish_needed) {
-      auto shared_msg = this->do_intra_process_publish_and_return_shared(std::move(msg));
+      auto shared_msg = this->do_intra_process_publish_and_return_shared(
+        std::move(msg), message_allocator_);
       this->do_inter_process_publish(*shared_msg);
     } else {
-      this->do_intra_process_publish(std::move(msg));
+      this->do_intra_process_publish(std::move(msg), message_allocator_);
     }
   }
 
   virtual void
   publish(const MessageT & msg)
   {
-    // Avoid allocating when not using intra process.
-    if (!intra_process_is_enabled_) {
-      // In this case we're not using intra process.
-      return this->do_inter_process_publish(msg);
-    }
-    // Otherwise we have to allocate memory in a unique_ptr and pass it along.
-    // As the message is not const, a copy should be made.
-    // A shared_ptr<const MessageT> could also be constructed here.
-    auto ptr = MessageAllocatorTraits::allocate(*message_allocator_.get(), 1);
-    MessageAllocatorTraits::construct(*message_allocator_.get(), ptr, msg);
-    MessageUniquePtr unique_msg(ptr, message_deleter_);
-    this->publish(std::move(unique_msg));
+    this->do_publish_message(msg);
   }
 
-  void
+  template<class T = MessageT>
+  typename std::enable_if<!std::is_same<T, rcl_serialized_message_t>::value>::type
   publish(const rcl_serialized_message_t & serialized_msg)
   {
-    return this->do_serialized_publish(&serialized_msg);
+    this->do_publish_message<rcl_serialized_message_t>(serialized_msg);
+  }
+
+  /// Publish a serialized message. Non specialized version to prevent compiling errors.
+  template<typename TDeleter, typename T>
+  void publish(std::unique_ptr<T, TDeleter> serialized_msg)
+  {
+    (void)serialized_msg;
+    throw std::runtime_error(
+            "publishing unique_ptr with custom deleter only supported for serialized messages");
+  }
+
+  /// Publish a serialized message.
+  template<typename TDeleter>
+  void publish(std::unique_ptr<rcl_serialized_message_t, TDeleter> serialized_msg)
+  {
+    this->do_serialized_publish(*serialized_msg);
   }
 
   /// Publish an instance of a LoanedMessage.
@@ -244,6 +271,69 @@ public:
   }
 
 protected:
+  void init_setup()
+  {
+    allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get());
+
+    if (options_.event_callbacks.deadline_callback) {
+      this->add_event_handler(
+        options_.event_callbacks.deadline_callback,
+        RCL_PUBLISHER_OFFERED_DEADLINE_MISSED);
+    }
+    if (options_.event_callbacks.liveliness_callback) {
+      this->add_event_handler(
+        options_.event_callbacks.liveliness_callback,
+        RCL_PUBLISHER_LIVELINESS_LOST);
+    }
+    if (options_.event_callbacks.incompatible_qos_callback) {
+      this->add_event_handler(
+        options_.event_callbacks.incompatible_qos_callback,
+        RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
+    } else if (options_.use_default_callbacks) {
+      // Register default callback when not specified
+      try {
+        this->add_event_handler(
+          [this](QOSOfferedIncompatibleQoSInfo & info) {
+            this->default_incompatible_qos_callback(info);
+          },
+          RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
+      } catch (UnsupportedEventTypeException & /*exc*/) {
+        RCLCPP_WARN_ONCE(
+          rclcpp::get_logger(rcl_node_get_logger_name(rcl_node_handle_.get())),
+          "This rmw implementation does not support ON_OFFERED_INCOMPATIBLE_QOS "
+          "events, you will not be notified when Publishers offer an incompatible "
+          "QoS profile to Subscriptions on the same topic.");
+      }
+    }
+    // Setup continues in the post construction method, post_init_setup().
+  }
+
+  template<class T = MessageT>
+  typename std::enable_if<!std::is_same<T, rcl_serialized_message_t>::value>::type
+  do_publish_message(const T & msg)
+  {
+    // Avoid allocating when not using intra process.
+    if (!intra_process_is_enabled_) {
+      // In this case we're not using intra process.
+      return this->do_inter_process_publish(msg);
+    }
+    // Otherwise we have to allocate memory in a unique_ptr and pass it along.
+    // As the message is not const, a copy should be made.
+    // A shared_ptr<const MessageT> could also be constructed here.
+    auto ptr = MessageAllocatorTraits::allocate(*message_allocator_.get(), 1);
+    MessageAllocatorTraits::construct(*message_allocator_.get(), ptr, msg);
+    MessageUniquePtr unique_msg(ptr, message_deleter_);
+    this->publish(std::move(unique_msg));
+  }
+
+  template<class T = MessageT>
+  typename std::enable_if<std::is_same<T, rcl_serialized_message_t>::value>::type
+  do_publish_message(const T & msg)
+  {
+    // Kept for backwards compatibility. Copies compelete memory!
+    this->publish(std::make_unique<rclcpp::SerializedMessage>(msg));
+  }
+
   void
   do_inter_process_publish(const MessageT & msg)
   {
@@ -265,15 +355,24 @@ protected:
   }
 
   void
-  do_serialized_publish(const rcl_serialized_message_t * serialized_msg)
+  do_serialized_publish(rcl_serialized_message_t serialized_msg)
   {
-    if (intra_process_is_enabled_) {
-      // TODO(Karsten1987): support serialized message passed by intraprocess
-      throw std::runtime_error("storing serialized messages in intra process is not supported yet");
+    bool inter_process_publish_needed =
+      get_subscription_count() > get_intra_process_subscription_count();
+
+    if (inter_process_publish_needed) {
+      // declare here to avoid deletion before returning method
+      auto status = rcl_publish_serialized_message(&publisher_handle_, &serialized_msg, nullptr);
+      if (RCL_RET_OK != status) {
+        rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish serialized message");
+      }
     }
-    auto status = rcl_publish_serialized_message(&publisher_handle_, serialized_msg, nullptr);
-    if (RCL_RET_OK != status) {
-      rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish serialized message");
+
+    auto msg = std::make_unique<rclcpp::SerializedMessage>(
+      std::move(serialized_msg));
+
+    if (intra_process_is_enabled_) {
+      do_intra_process_publish(std::move(msg), message_allocator_serialized_);
     }
   }
 
@@ -297,8 +396,11 @@ protected:
     }
   }
 
+  template<typename T, typename Deleter, typename Allocator>
   void
-  do_intra_process_publish(std::unique_ptr<MessageT, MessageDeleter> msg)
+  do_intra_process_publish(
+    std::unique_ptr<T, Deleter> msg,
+    std::shared_ptr<Allocator> & message_allocator)
   {
     auto ipm = weak_ipm_.lock();
     if (!ipm) {
@@ -309,14 +411,21 @@ protected:
       throw std::runtime_error("cannot publish msg which is a null pointer");
     }
 
-    ipm->template do_intra_process_publish<MessageT, AllocatorT>(
-      intra_process_publisher_id_,
+    const uint64_t intra_process_publisher_id = std::is_same<T,
+        rclcpp::SerializedMessage>::value ?
+      intra_process_publisher_id_serialized_ : intra_process_publisher_id_;
+
+    ipm->template do_intra_process_publish<T, AllocatorT>(
+      intra_process_publisher_id,
       std::move(msg),
-      message_allocator_);
+      message_allocator);
   }
 
-  std::shared_ptr<const MessageT>
-  do_intra_process_publish_and_return_shared(std::unique_ptr<MessageT, MessageDeleter> msg)
+  template<typename T, typename Deleter, typename Allocator>
+  std::shared_ptr<const T>
+  do_intra_process_publish_and_return_shared(
+    std::unique_ptr<T, Deleter> msg,
+    std::shared_ptr<Allocator> & message_allocator)
   {
     auto ipm = weak_ipm_.lock();
     if (!ipm) {
@@ -327,10 +436,14 @@ protected:
       throw std::runtime_error("cannot publish msg which is a null pointer");
     }
 
-    return ipm->template do_intra_process_publish_and_return_shared<MessageT, AllocatorT>(
-      intra_process_publisher_id_,
+    const uint64_t intra_process_publisher_id = std::is_same<T,
+        rclcpp::SerializedMessage>::value ?
+      intra_process_publisher_id_serialized_ : intra_process_publisher_id_;
+
+    return ipm->template do_intra_process_publish_and_return_shared<T, AllocatorT>(
+      intra_process_publisher_id,
       std::move(msg),
-      message_allocator_);
+      message_allocator);
   }
 
   /// Copy of original options passed during construction.
@@ -341,6 +454,7 @@ protected:
   const rclcpp::PublisherOptionsWithAllocator<AllocatorT> options_;
 
   std::shared_ptr<MessageAllocator> message_allocator_;
+  std::shared_ptr<SerializedMessageAllocator> message_allocator_serialized_;
 
   MessageDeleter message_deleter_;
 };

rclcpp/include/rclcpp/publisher_base.hpp

@@ -191,6 +191,7 @@ public:
   void
   setup_intra_process(
     uint64_t intra_process_publisher_id,
+    uint64_t intra_process_publisher_id_serialized,
     IntraProcessManagerSharedPtr ipm);
 
 protected:
@@ -208,6 +209,9 @@ protected:
     event_handlers_.emplace_back(handler);
   }
 
+  RCLCPP_PUBLIC
+  void default_incompatible_qos_callback(QOSOfferedIncompatibleQoSInfo & info) const;
+
   std::shared_ptr<rcl_node_t> rcl_node_handle_;
 
   rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher();
@@ -219,6 +223,7 @@ protected:
   bool intra_process_is_enabled_;
   IntraProcessManagerWeakPtr weak_ipm_;
   uint64_t intra_process_publisher_id_;
+  uint64_t intra_process_publisher_id_serialized_;
 
   rmw_gid_t rmw_gid_;
 };

rclcpp/include/rclcpp/publisher_factory.hpp

@@ -63,17 +63,21 @@ struct PublisherFactory
 /// Return a PublisherFactory with functions setup for creating a PublisherT<MessageT, AllocatorT>.
 template<typename MessageT, typename AllocatorT, typename PublisherT>
 PublisherFactory
-create_publisher_factory(const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options)
+create_publisher_factory(
+  const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options,
+  const rosidl_message_type_support_t & type_support)
 {
   PublisherFactory factory {
     // factory function that creates a MessageT specific PublisherT
-    [options](
+    [options, type_support](
       rclcpp::node_interfaces::NodeBaseInterface * node_base,
       const std::string & topic_name,
       const rclcpp::QoS & qos
     ) -> std::shared_ptr<PublisherT>
     {
-      auto publisher = std::make_shared<PublisherT>(node_base, topic_name, qos, options);
+      auto publisher = std::make_shared<PublisherT>(
+        node_base, topic_name, qos, options,
+        type_support);
       // This is used for setting up things like intra process comms which
       // require this->shared_from_this() which cannot be called from
       // the constructor.

rclcpp/include/rclcpp/publisher_options.hpp

@@ -44,6 +44,9 @@ struct PublisherOptionsBase
   /// Callbacks for various events related to publishers.
   PublisherEventCallbacks event_callbacks;
 
+  /// Whether or not to use default callbacks when user doesn't supply any in event_callbacks
+  bool use_default_callbacks = true;
+
   /// Callback group in which the waitable items from the publisher should be placed.
   std::shared_ptr<rclcpp::callback_group::CallbackGroup> callback_group;
 

rclcpp/include/rclcpp/qos.hpp

@@ -15,15 +15,19 @@
 #ifndef RCLCPP__QOS_HPP_
 #define RCLCPP__QOS_HPP_
 
-#include <rmw/qos_profiles.h>
-#include <rmw/types.h>
+#include <string>
 
 #include "rclcpp/duration.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rmw/incompatible_qos_events_statuses.h"
+#include "rmw/qos_profiles.h"
+#include "rmw/types.h"
 
 namespace rclcpp
 {
 
+std::string qos_policy_name_from_kind(rmw_qos_policy_kind_t policy_kind);
+
 /// QoS initialization values, cannot be created directly, use KeepAll or KeepLast instead.
 struct RCLCPP_PUBLIC QoSInitialization
 {
@@ -151,6 +155,12 @@ private:
   rmw_qos_profile_t rmw_qos_profile_;
 };
 
+/// Check if two QoS profiles are exactly equal in all policy values.
+RCLCPP_PUBLIC
+bool operator==(const QoS & left, const QoS & right);
+RCLCPP_PUBLIC
+bool operator!=(const QoS & left, const QoS & right);
+
 class RCLCPP_PUBLIC SensorDataQoS : public QoS
 {
 public:

rclcpp/include/rclcpp/qos_event.hpp

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

rclcpp/include/rclcpp/rclcpp.hpp

@@ -143,6 +143,7 @@
 #include <memory>
 
 #include "rclcpp/executors.hpp"
+#include "rclcpp/guard_condition.hpp"
 #include "rclcpp/logging.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/parameter.hpp"
@@ -152,6 +153,7 @@
 #include "rclcpp/time.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/wait_set.hpp"
 #include "rclcpp/waitable.hpp"
 
 #endif  // RCLCPP__RCLCPP_HPP_

rclcpp/include/rclcpp/serialization.hpp

@@ -0,0 +1,123 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__SERIALIZATION_HPP_
+#define RCLCPP__SERIALIZATION_HPP_
+
+#include <rmw/rmw.h>
+
+#include <memory>
+#include <string>
+
+#include "rcl/error_handling.h"
+
+namespace rclcpp
+{
+
+/// Interface to (de)serialize a message
+class SerializationBase
+{
+public:
+  virtual ~SerializationBase() = default;
+
+  /// Serialize a ROS2 message to a serialized stream
+  /**
+   * \param[in] message The ROS2 message which is read and serialized by rmw.
+   */
+  virtual std::shared_ptr<rcl_serialized_message_t> serialize_message(const void * message) = 0;
+
+  /// Deserialize a serialized stream to a ROS message
+  /**
+   * \param[in] serialized_message The serialized message to be converted to ROS2 by rmw.
+   * \param[out] message The deserialized ROS2 message.
+   */
+  virtual void deserialize_message(
+    const rcl_serialized_message_t * serialized_message,
+    void * msg) = 0;
+};
+
+/// Default implementation to (de)serialize a message by using rmw_(de)serialize
+class Serialization : public SerializationBase
+{
+public:
+  Serialization(
+    const rosidl_message_type_support_t & type_support,
+    const rcutils_allocator_t allocator = rcutils_get_default_allocator())
+  : type_support_(type_support), rcutils_allocator_(allocator)
+  {}
+
+  std::shared_ptr<rcl_serialized_message_t> serialize_message(const void * message) override
+  {
+    auto serialized_message = new rcl_serialized_message_t;
+    *serialized_message = rmw_get_zero_initialized_serialized_message();
+    const auto ret = rmw_serialized_message_init(serialized_message, 0, &rcutils_allocator_);
+    if (ret != RCUTILS_RET_OK) {
+      throw std::runtime_error(
+              "Error allocating resources for serialized message: " +
+              std::string(rcutils_get_error_string().str));
+    }
+
+    if (nullptr == message) {
+      delete serialized_message;
+      throw std::runtime_error("Message is nullpointer while serialization.");
+    }
+
+    const auto error = rmw_serialize(
+      message,
+      &type_support_,
+      serialized_message);
+    if (error != RCL_RET_OK) {
+      delete serialized_message;
+      throw std::runtime_error("Failed to serialize.");
+    }
+
+    auto shared_serialized_msg = std::shared_ptr<rcl_serialized_message_t>(
+      serialized_message,
+      [](rcl_serialized_message_t * msg) {
+        auto fini_ret = rmw_serialized_message_fini(msg);
+        delete msg;
+        if (fini_ret != RCL_RET_OK) {
+          RCUTILS_LOG_ERROR_NAMED(
+            "rclcpp",
+            "Failed to destroy serialized message: %s", rcl_get_error_string().str);
+        }
+      });
+
+    return shared_serialized_msg;
+  }
+
+  void deserialize_message(const rcl_serialized_message_t * serialized_message, void * msg) override
+  {
+    if (nullptr == serialized_message ||
+      serialized_message->buffer_capacity == 0 ||
+      serialized_message->buffer_length == 0 ||
+      !serialized_message->buffer)
+    {
+      throw std::runtime_error("Failed to deserialize nullptr serialized message.");
+    }
+
+    const auto ret = rmw_deserialize(serialized_message, &type_support_, msg);
+    if (ret != RMW_RET_OK) {
+      throw std::runtime_error("Failed to deserialize serialized message.");
+    }
+  }
+
+private:
+  rosidl_message_type_support_t type_support_;
+  rcutils_allocator_t rcutils_allocator_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__SERIALIZATION_HPP_

rclcpp/include/rclcpp/serialized_message.hpp

@@ -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.
+
+#ifndef RCLCPP__SERIALIZED_MESSAGE_HPP_
+#define RCLCPP__SERIALIZED_MESSAGE_HPP_
+
+#include <rclcpp/exceptions.hpp>
+
+#include <cstring>
+
+#include "rcutils/logging_macros.h"
+
+#include "rmw/serialized_message.h"
+
+namespace rclcpp
+{
+
+/// Object oriented version of rcl_serialized_message_t with destructor to avoid memory leaks
+class SerializedMessage : public rcl_serialized_message_t
+{
+public:
+  SerializedMessage()
+  : rcl_serialized_message_t(rmw_get_zero_initialized_serialized_message())
+  {}
+
+  explicit SerializedMessage(const SerializedMessage & serialized_message)
+  : SerializedMessage(static_cast<const rcl_serialized_message_t>(serialized_message))
+  {}
+
+  explicit SerializedMessage(const rcl_serialized_message_t & serialized_message)
+  : rcl_serialized_message_t(rmw_get_zero_initialized_serialized_message())
+  {
+    const auto ret = rmw_serialized_message_init(
+      this, serialized_message.buffer_length,
+      &serialized_message.allocator);
+    if (ret != RCL_RET_OK) {
+      rclcpp::exceptions::throw_from_rcl_error(ret);
+    }
+
+    // do not call memcpy if the pointer is "static"
+    if (buffer != serialized_message.buffer) {
+      std::memcpy(buffer, serialized_message.buffer, serialized_message.buffer_length);
+    }
+    buffer_length = serialized_message.buffer_length;
+  }
+
+  explicit SerializedMessage(rcl_serialized_message_t && msg)
+  : rcl_serialized_message_t(msg)
+  {
+    // reset buffer to prevent double free
+    msg = rmw_get_zero_initialized_serialized_message();
+  }
+
+  ~SerializedMessage()
+  {
+    if (nullptr != buffer) {
+      const auto fini_ret = rmw_serialized_message_fini(this);
+      if (fini_ret != RCL_RET_OK) {
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Failed to destroy serialized message: %s", rcl_get_error_string().str);
+      }
+    }
+  }
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__SERIALIZED_MESSAGE_HPP_

rclcpp/include/rclcpp/service.hpp

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

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

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

rclcpp/include/rclcpp/subscription.hpp

@@ -38,8 +38,10 @@
 #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/serialized_message.hpp"
 #include "rclcpp/subscription_base.hpp"
 #include "rclcpp/subscription_options.hpp"
 #include "rclcpp/subscription_traits.hpp"
@@ -118,50 +120,120 @@ public:
         options.event_callbacks.liveliness_callback,
         RCL_SUBSCRIPTION_LIVELINESS_CHANGED);
     }
+    if (options.event_callbacks.incompatible_qos_callback) {
+      this->add_event_handler(
+        options.event_callbacks.incompatible_qos_callback,
+        RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
+    } else if (options_.use_default_callbacks) {
+      // Register default callback when not specified
+      try {
+        this->add_event_handler(
+          [this](QOSRequestedIncompatibleQoSInfo & info) {
+            this->default_incompatible_qos_callback(info);
+          },
+          RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
+      } catch (UnsupportedEventTypeException & /*exc*/) {
+        // pass
+      }
+    }
 
     // Setup intra process publishing if requested.
     if (rclcpp::detail::resolve_use_intra_process(options, *node_base)) {
       using rclcpp::detail::resolve_intra_process_buffer_type;
 
       // Check if the QoS is compatible with intra-process.
-      if (qos.get_rmw_qos_profile().history == RMW_QOS_POLICY_HISTORY_KEEP_ALL) {
+      rmw_qos_profile_t qos_profile = get_actual_qos().get_rmw_qos_profile();
+      if (qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_ALL) {
         throw std::invalid_argument(
                 "intraprocess communication is not allowed with keep all history qos policy");
       }
-      if (qos.get_rmw_qos_profile().depth == 0) {
+      if (qos_profile.depth == 0) {
         throw std::invalid_argument(
                 "intraprocess communication is not allowed with 0 depth qos policy");
       }
-      if (qos.get_rmw_qos_profile().durability != RMW_QOS_POLICY_DURABILITY_VOLATILE) {
+      if (qos_profile.durability != RMW_QOS_POLICY_DURABILITY_VOLATILE) {
         throw std::invalid_argument(
                 "intraprocess communication allowed only with volatile durability");
       }
 
-      // First create a SubscriptionIntraProcess which will be given to the intra-process manager.
-      auto context = node_base->get_context();
-      auto subscription_intra_process = std::make_shared<
-        rclcpp::experimental::SubscriptionIntraProcess<
-          CallbackMessageT,
-          AllocatorT,
-          typename MessageUniquePtr::deleter_type
-        >>(
-        callback,
-        options.get_allocator(),
-        context,
-        this->get_topic_name(),    // important to get like this, as it has the fully-qualified name
-        qos.get_rmw_qos_profile(),
-        resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback)
-        );
-      TRACEPOINT(
-        rclcpp_subscription_init,
-        (const void *)get_subscription_handle().get(),
-        (const void *)subscription_intra_process.get());
-
-      // Add it to the intra process manager.
       using rclcpp::experimental::IntraProcessManager;
+      uint64_t intra_process_subscription_id;
+      uint64_t intra_process_subscription_id_serialized;
+
+      auto context = node_base->get_context();
       auto ipm = context->get_sub_context<IntraProcessManager>();
-      uint64_t intra_process_subscription_id = ipm->add_subscription(subscription_intra_process);
-      this->setup_intra_process(intra_process_subscription_id, ipm);
+
+      {
+        // First create a SubscriptionIntraProcess which will be given to the intra-process manager.
+        auto subscription_intra_process = std::make_shared<
+          rclcpp::experimental::SubscriptionIntraProcess<
+            CallbackMessageT,
+            AllocatorT,
+            typename MessageUniquePtr::deleter_type
+          >>(
+          callback,
+          options.get_allocator(),
+          context,
+          this->get_topic_name(),  // important to get it by the fully-qualified name
+          qos.get_rmw_qos_profile(),
+          resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback),
+          std::make_shared<rclcpp::Serialization>(
+            type_support_handle,
+            options.template to_rcl_subscription_options<CallbackMessageT>(qos).allocator)
+          );
+        TRACEPOINT(
+          rclcpp_subscription_init,
+          (const void *)get_subscription_handle().get(),
+          (const void *)subscription_intra_process.get());
+
+        // Add it to the intra process manager.
+        intra_process_subscription_id = ipm->add_subscription(subscription_intra_process);
+      }
+
+      {
+        using SerializedMessageAllocatorTraits =
+          allocator::AllocRebind<rclcpp::SerializedMessage,
+            AllocatorT>;
+        using SerializedMessageAllocator =
+          typename SerializedMessageAllocatorTraits::allocator_type;
+        using SerializedMessageDeleter = allocator::Deleter<SerializedMessageAllocator,
+            rclcpp::SerializedMessage>;
+        using SerializedMessageUniquePtr =
+          std::unique_ptr<rclcpp::SerializedMessage,
+            SerializedMessageDeleter>;
+
+        // First create a SubscriptionIntraProcess which will be given to the intra-process manager.
+        auto subscription_intra_process = std::make_shared<
+          rclcpp::experimental::SubscriptionIntraProcess<
+            rclcpp::SerializedMessage,
+            AllocatorT,
+            typename SerializedMessageUniquePtr::deleter_type,
+            CallbackMessageT
+          >>(
+          callback,
+          options.get_allocator(),
+          context,
+          this->get_topic_name(),    // important to get it by the fully-qualified name
+          qos.get_rmw_qos_profile(),
+          resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback),
+          std::make_shared<rclcpp::Serialization>(
+            type_support_handle,
+            options.template to_rcl_subscription_options<CallbackMessageT>(qos).allocator)
+          );
+        TRACEPOINT(
+          rclcpp_subscription_init,
+          (const void *)get_subscription_handle().get(),
+          (const void *)subscription_intra_process.get());
+
+        // Add it to the intra process manager.
+        intra_process_subscription_id_serialized = ipm->add_subscription(
+          subscription_intra_process,
+          true);
+      }
+
+      this->setup_intra_process(
+        {intra_process_subscription_id,
+          intra_process_subscription_id_serialized}, ipm);
     }
 
     TRACEPOINT(
@@ -175,11 +247,14 @@ public:
     // The callback object gets copied, so if registration is done too early/before this point
     // (e.g. in `AnySubscriptionCallback::set()`), its address won't match any address used later
     // in subsequent tracepoints.
+#ifndef TRACETOOLS_DISABLED
     any_callback_.register_callback_for_tracing();
+#endif
   }
 
   /// Called after construction to continue setup that requires shared_from_this().
-  void post_init_setup(
+  void
+  post_init_setup(
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
     const rclcpp::QoS & qos,
     const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options)
@@ -189,19 +264,32 @@ public:
     (void)options;
   }
 
-  /// Support dynamically setting the message memory strategy.
+  /// Take the next message from the inter-process subscription.
   /**
-   * Behavior may be undefined if called while the subscription could be executing.
-   * \param[in] message_memory_strategy Shared pointer to the memory strategy to set.
+   * Data may be taken (written) into the message_out and message_info_out even
+   * if false is returned.
+   * Specifically in the case of dropping redundant intra-process data, where
+   * data is received via both intra-process and inter-process (due to the
+   * underlying middleware being unabled to avoid this duplicate delivery) and
+   * so inter-process data from those intra-process publishers is ignored, but
+   * it has to be taken to know if it came from an intra-process publisher or
+   * not, and therefore could be dropped.
+   *
+   * \sa SubscriptionBase::take_type_erased()
+   *
+   * \param[out] message_out The message into which take will copy the data.
+   * \param[out] message_info_out The message info for the taken message.
+   * \returns true if data was taken and is valid, otherwise false
+   * \throws any rcl errors from rcl_take, \sa rclcpp::exceptions::throw_from_rcl_error()
    */
-  void set_message_memory_strategy(
-    typename message_memory_strategy::MessageMemoryStrategy<CallbackMessageT,
-    AllocatorT>::SharedPtr message_memory_strategy)
+  bool
+  take(CallbackMessageT & message_out, rclcpp::MessageInfo & message_info_out)
   {
-    message_memory_strategy_ = message_memory_strategy;
+    return this->take_type_erased(static_cast<void *>(&message_out), message_info_out);
   }
 
-  std::shared_ptr<void> create_message() override
+  std::shared_ptr<void>
+  create_message() override
   {
     /* The default message memory strategy provides a dynamically allocated message on each call to
      * create_message, though alternative memory strategies that re-use a preallocated message may be
@@ -210,15 +298,18 @@ public:
     return message_memory_strategy_->borrow_message();
   }
 
-  std::shared_ptr<rcl_serialized_message_t> create_serialized_message() override
+  std::shared_ptr<rcl_serialized_message_t>
+  create_serialized_message() override
   {
     return message_memory_strategy_->borrow_serialized_message();
   }
 
-  void handle_message(
-    std::shared_ptr<void> & message, const rmw_message_info_t & message_info) override
+  void
+  handle_message(
+    std::shared_ptr<void> & message,
+    const rclcpp::MessageInfo & message_info) override
   {
-    if (matches_any_intra_process_publishers(&message_info.publisher_gid)) {
+    if (matches_any_intra_process_publishers(&message_info.get_rmw_message_info().publisher_gid)) {
       // In this case, the message will be delivered via intra process and
       // we should ignore this copy of the message.
       return;
@@ -229,7 +320,8 @@ public:
 
   void
   handle_loaned_message(
-    void * loaned_message, const rmw_message_info_t & message_info) override
+    void * loaned_message,
+    const rclcpp::MessageInfo & message_info) override
   {
     auto typed_message = static_cast<CallbackMessageT *>(loaned_message);
     // message is loaned, so we have to make sure that the deleter does not deallocate the message
@@ -240,18 +332,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. */
@@ -185,13 +228,30 @@ public:
   RCLCPP_PUBLIC
   void
   setup_intra_process(
-    uint64_t intra_process_subscription_id,
+    const std::vector<uint64_t> & intra_process_subscription_ids,
     IntraProcessManagerWeakPtr weak_ipm);
 
   /// Return the waitable for intra-process, or nullptr if intra-process is not setup.
   RCLCPP_PUBLIC
-  rclcpp::Waitable::SharedPtr
-  get_intra_process_waitable() const;
+  std::vector<rclcpp::Waitable::SharedPtr>
+  get_intra_process_waitables() 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>
@@ -205,9 +265,13 @@ protected:
       rcl_subscription_event_init,
       get_subscription_handle().get(),
       event_type);
+    qos_events_in_use_by_wait_set_.insert(std::make_pair(handler.get(), false));
     event_handlers_.emplace_back(handler);
   }
 
+  RCLCPP_PUBLIC
+  void default_incompatible_qos_callback(QOSRequestedIncompatibleQoSInfo & info) const;
+
   RCLCPP_PUBLIC
   bool
   matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const;
@@ -222,13 +286,18 @@ protected:
 
   bool use_intra_process_;
   IntraProcessManagerWeakPtr weak_ipm_;
-  uint64_t intra_process_subscription_id_;
+  std::vector<uint64_t> intra_process_subscription_ids_;
 
 private:
   RCLCPP_DISABLE_COPY(SubscriptionBase)
 
   rosidl_message_type_support_t type_support_;
   bool is_serialized_;
+
+  std::atomic<bool> subscription_in_use_by_wait_set_{false};
+  std::atomic<bool> intra_process_subscription_waitable_in_use_by_wait_set_{false};
+  std::unordered_map<rclcpp::QOSEventHandlerBase *,
+    std::atomic<bool>> qos_events_in_use_by_wait_set_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/subscription_factory.hpp

@@ -78,7 +78,8 @@ SubscriptionFactory
 create_subscription_factory(
   CallbackT && callback,
   const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
-  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat)
+  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat,
+  const rosidl_message_type_support_t & type_support)
 {
   auto allocator = options.get_allocator();
 
@@ -88,7 +89,7 @@ create_subscription_factory(
 
   SubscriptionFactory factory {
     // factory function that creates a MessageT specific SubscriptionT
-    [options, msg_mem_strat, any_subscription_callback](
+    [options, msg_mem_strat, any_subscription_callback, type_support](
       rclcpp::node_interfaces::NodeBaseInterface * node_base,
       const std::string & topic_name,
       const rclcpp::QoS & qos
@@ -99,7 +100,7 @@ create_subscription_factory(
 
       auto sub = Subscription<CallbackMessageT, AllocatorT>::make_shared(
         node_base,
-        *rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>(),
+        type_support,
         topic_name,
         qos,
         any_subscription_callback,

rclcpp/include/rclcpp/subscription_options.hpp

@@ -36,6 +36,9 @@ struct SubscriptionOptionsBase
   /// Callbacks for events related to this subscription.
   SubscriptionEventCallbacks event_callbacks;
 
+  /// Whether or not to use default callbacks when user doesn't supply any in event_callbacks
+  bool use_default_callbacks = true;
+
   /// True to ignore local publications.
   bool ignore_local_publications = false;
 

rclcpp/include/rclcpp/subscription_traits.hpp

@@ -18,6 +18,7 @@
 #include <memory>
 
 #include "rclcpp/function_traits.hpp"
+#include "rclcpp/subscription_options.hpp"
 #include "rcl/types.h"
 
 namespace rclcpp
@@ -75,6 +76,7 @@ struct extract_message_type<std::unique_ptr<MessageT, Deleter>>: extract_message
 
 template<
   typename CallbackT,
+  typename AllocatorT = std::allocator<void>,
   // Do not attempt if CallbackT is an integer (mistaken for depth)
   typename = std::enable_if_t<!std::is_integral<
     std::remove_cv_t<std::remove_reference_t<CallbackT>>>::value>,
@@ -85,6 +87,10 @@ template<
   // Do not attempt if CallbackT is a rmw_qos_profile_t (mistaken for qos profile)
   typename = std::enable_if_t<!std::is_same<
     rmw_qos_profile_t,
+    std::remove_cv_t<std::remove_reference_t<CallbackT>>>::value>,
+  // Do not attempt if CallbackT is a rclcpp::SubscriptionOptionsWithAllocator
+  typename = std::enable_if_t<!std::is_same<
+    rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>,
     std::remove_cv_t<std::remove_reference_t<CallbackT>>>::value>
 >
 struct has_message_type : extract_message_type<

rclcpp/include/rclcpp/subscription_wait_set_mask.hpp

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

rclcpp/include/rclcpp/time.hpp

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

rclcpp/include/rclcpp/timer.hpp

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

rclcpp/include/rclcpp/type_support_decl.hpp

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

rclcpp/include/rclcpp/utilities.hpp

@@ -121,7 +121,7 @@ init_and_remove_ros_arguments(
  * \param[in] argv Argument vector.
  * \returns Members of the argument vector that are not ROS arguments.
  * \throws anything throw_from_rcl_error can throw
- * \throws rclcpp::exceptions::RCLErrorBase if the parsing fails
+ * \throws rclcpp::exceptions::RCLError if the parsing fails
  */
 RCLCPP_PUBLIC
 std::vector<std::string>
@@ -153,6 +153,7 @@ ok(rclcpp::Context::SharedPtr context = nullptr);
  * \param[in] context Check for initialization of this Context.
  * \return true if the context is initialized, and false otherwise
  */
+[[deprecated("use the function ok() instead, which has the same usage.")]]
 RCLCPP_PUBLIC
 bool
 is_initialized(rclcpp::Context::SharedPtr context = nullptr);

rclcpp/include/rclcpp/wait_result.hpp

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

rclcpp/include/rclcpp/wait_result_kind.hpp

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

rclcpp/include/rclcpp/wait_set.hpp

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

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

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

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

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

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

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

rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp

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

rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp

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

rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp

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

rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp

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

rclcpp/include/rclcpp/wait_set_template.hpp

@@ -0,0 +1,745 @@
+// 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;
+          for (auto waitable : inner_subscription->get_intra_process_waitables()) {
+            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(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) {
+          for (auto local_waitable : inner_subscription->get_intra_process_waitables()) {
+            inner_subscription->exchange_in_use_by_wait_set_state(local_waitable.get(), false);
+            if (nullptr != local_waitable) {
+              // This is the case when intra process is disabled for the subscription.
+              this->storage_remove_waitable(std::move(local_waitable));
+            }
+          }
+        }
+      });
+  }
+
+  /// Add a guard condition to this wait set.
+  /**
+   * Guard condition is added to the wait set, and shared ownership is held
+   * while waiting.
+   * However, if between calls to wait() the guard condition's reference count
+   * goes to zero, it will be implicitly removed on the next call to wait().
+   *
+   * Except in the case of a fixed sized storage, where changes to the wait set
+   * cannot occur after construction, in which case it holds shared ownership
+   * at all times until the wait set is destroy, but this method also does not
+   * exist on a fixed sized wait set.
+   *
+   * This function may be thread-safe depending on the SynchronizationPolicy
+   * used with this class.
+   * Using the ThreadSafeWaitSetPolicy will ensure that wait() is interrupted
+   * and returns before this function adds the guard condition.
+   * Otherwise, it is not safe to call this function concurrently with wait().
+   *
+   * This function will not be enabled (will not be available) if the
+   * StoragePolicy does not allow editing of the wait set after initialization.
+   *
+   * \param[in] guard_condition Guard condition to be added.
+   * \throws std::invalid_argument if guard_condition is nullptr.
+   * \throws std::runtime_error if guard_condition has already been added or is
+   *   associated with another wait set.
+   * \throws exceptions based on the policies used.
+   */
+  void
+  add_guard_condition(std::shared_ptr<rclcpp::GuardCondition> guard_condition)
+  {
+    if (nullptr == guard_condition) {
+      throw std::invalid_argument("guard_condition is nullptr");
+    }
+    // this method comes from the SynchronizationPolicy
+    this->sync_add_guard_condition(
+      std::move(guard_condition),
+      [this](std::shared_ptr<rclcpp::GuardCondition> && inner_guard_condition) {
+        bool already_in_use = inner_guard_condition->exchange_in_use_by_wait_set_state(true);
+        if (already_in_use) {
+          throw std::runtime_error("guard condition already in use by another wait set");
+        }
+        // This method comes from the StoragePolicy, and it may not exist for
+        // fixed sized storage policies.
+        // It will throw if the guard condition has already been added.
+        this->storage_add_guard_condition(std::move(inner_guard_condition));
+      });
+  }
+
+  /// Remove a guard condition from this wait set.
+  /**
+   * Guard condition is removed from the wait set, and if needed the shared
+   * ownership is released.
+   *
+   * This function may be thread-safe depending on the SynchronizationPolicy
+   * used with this class.
+   * Using the ThreadSafeWaitSetPolicy will ensure that wait() is interrupted
+   * and returns before this function removes the guard condition.
+   * Otherwise, it is not safe to call this function concurrently with wait().
+   *
+   * This function will not be enabled (will not be available) if the
+   * StoragePolicy does not allow editing of the wait set after initialization.
+   *
+   * \param[in] guard_condition Guard condition to be removed.
+   * \throws std::invalid_argument if guard_condition is nullptr.
+   * \throws std::runtime_error if guard_condition is not part of the wait set.
+   * \throws exceptions based on the policies used.
+   */
+  void
+  remove_guard_condition(std::shared_ptr<rclcpp::GuardCondition> guard_condition)
+  {
+    if (nullptr == guard_condition) {
+      throw std::invalid_argument("guard_condition is nullptr");
+    }
+    // this method comes from the SynchronizationPolicy
+    this->sync_remove_guard_condition(
+      std::move(guard_condition),
+      [this](std::shared_ptr<rclcpp::GuardCondition> && inner_guard_condition) {
+        inner_guard_condition->exchange_in_use_by_wait_set_state(false);
+        // This method comes from the StoragePolicy, and it may not exist for
+        // fixed sized storage policies.
+        // It will throw if the guard condition is not in the wait set.
+        this->storage_remove_guard_condition(std::move(inner_guard_condition));
+      });
+  }
+
+  /// Add a timer to this wait set.
+  /**
+   * \sa add_guard_condition() for details of how this method works.
+   *
+   * \param[in] timer Timer to be added.
+   * \throws std::invalid_argument if timer is nullptr.
+   * \throws std::runtime_error if timer has already been added or is
+   *   associated with another wait set.
+   * \throws exceptions based on the policies used.
+   */
+  void
+  add_timer(std::shared_ptr<rclcpp::TimerBase> timer)
+  {
+    if (nullptr == timer) {
+      throw std::invalid_argument("timer is nullptr");
+    }
+    // this method comes from the SynchronizationPolicy
+    this->sync_add_timer(
+      std::move(timer),
+      [this](std::shared_ptr<rclcpp::TimerBase> && inner_timer) {
+        bool already_in_use = inner_timer->exchange_in_use_by_wait_set_state(true);
+        if (already_in_use) {
+          throw std::runtime_error("timer already in use by another wait set");
+        }
+        // This method comes from the StoragePolicy, and it may not exist for
+        // fixed sized storage policies.
+        // It will throw if the timer has already been added.
+        this->storage_add_timer(std::move(inner_timer));
+      });
+  }
+
+  /// Remove a timer from this wait set.
+  /**
+   * \sa remove_guard_condition() for details of how this method works.
+   *
+   * \param[in] timer Timer to be removed.
+   * \throws std::invalid_argument if timer is nullptr.
+   * \throws std::runtime_error if timer is not part of the wait set.
+   * \throws exceptions based on the policies used.
+   */
+  void
+  remove_timer(std::shared_ptr<rclcpp::TimerBase> timer)
+  {
+    if (nullptr == timer) {
+      throw std::invalid_argument("timer is nullptr");
+    }
+    // this method comes from the SynchronizationPolicy
+    this->sync_remove_timer(
+      std::move(timer),
+      [this](std::shared_ptr<rclcpp::TimerBase> && inner_timer) {
+        inner_timer->exchange_in_use_by_wait_set_state(false);
+        // This method comes from the StoragePolicy, and it may not exist for
+        // fixed sized storage policies.
+        // It will throw if the timer is not in the wait set.
+        this->storage_remove_timer(std::move(inner_timer));
+      });
+  }
+
+  /// Add a client to this wait set.
+  /**
+   * \sa add_guard_condition() for details of how this method works.
+   *
+   * \param[in] client Client to be added.
+   * \throws std::invalid_argument if client is nullptr.
+   * \throws std::runtime_error if client has already been added or is
+   *   associated with another wait set.
+   * \throws exceptions based on the policies used.
+   */
+  void
+  add_client(std::shared_ptr<rclcpp::ClientBase> client)
+  {
+    if (nullptr == client) {
+      throw std::invalid_argument("client is nullptr");
+    }
+    // this method comes from the SynchronizationPolicy
+    this->sync_add_client(
+      std::move(client),
+      [this](std::shared_ptr<rclcpp::ClientBase> && inner_client) {
+        bool already_in_use = inner_client->exchange_in_use_by_wait_set_state(true);
+        if (already_in_use) {
+          throw std::runtime_error("client already in use by another wait set");
+        }
+        // This method comes from the StoragePolicy, and it may not exist for
+        // fixed sized storage policies.
+        // It will throw if the client has already been added.
+        this->storage_add_client(std::move(inner_client));
+      });
+  }
+
+  /// Remove a client from this wait set.
+  /**
+   * \sa remove_guard_condition() for details of how this method works.
+   *
+   * \param[in] client Client to be removed.
+   * \throws std::invalid_argument if client is nullptr.
+   * \throws std::runtime_error if client is not part of the wait set.
+   * \throws exceptions based on the policies used.
+   */
+  void
+  remove_client(std::shared_ptr<rclcpp::ClientBase> client)
+  {
+    if (nullptr == client) {
+      throw std::invalid_argument("client is nullptr");
+    }
+    // this method comes from the SynchronizationPolicy
+    this->sync_remove_client(
+      std::move(client),
+      [this](std::shared_ptr<rclcpp::ClientBase> && inner_client) {
+        inner_client->exchange_in_use_by_wait_set_state(false);
+        // This method comes from the StoragePolicy, and it may not exist for
+        // fixed sized storage policies.
+        // It will throw if the client is not in the wait set.
+        this->storage_remove_client(std::move(inner_client));
+      });
+  }
+
+  /// Add a service to this wait set.
+  /**
+   * \sa add_guard_condition() for details of how this method works.
+   *
+   * \param[in] service Service to be added.
+   * \throws std::invalid_argument if service is nullptr.
+   * \throws std::runtime_error if service has already been added or is
+   *   associated with another wait set.
+   * \throws exceptions based on the policies used.
+   */
+  void
+  add_service(std::shared_ptr<rclcpp::ServiceBase> service)
+  {
+    if (nullptr == service) {
+      throw std::invalid_argument("service is nullptr");
+    }
+    // this method comes from the SynchronizationPolicy
+    this->sync_add_service(
+      std::move(service),
+      [this](std::shared_ptr<rclcpp::ServiceBase> && inner_service) {
+        bool already_in_use = inner_service->exchange_in_use_by_wait_set_state(true);
+        if (already_in_use) {
+          throw std::runtime_error("service already in use by another wait set");
+        }
+        // This method comes from the StoragePolicy, and it may not exist for
+        // fixed sized storage policies.
+        // It will throw if the service has already been added.
+        this->storage_add_service(std::move(inner_service));
+      });
+  }
+
+  /// Remove a service from this wait set.
+  /**
+   * \sa remove_guard_condition() for details of how this method works.
+   *
+   * \param[in] service Service to be removed.
+   * \throws std::invalid_argument if service is nullptr.
+   * \throws std::runtime_error if service is not part of the wait set.
+   * \throws exceptions based on the policies used.
+   */
+  void
+  remove_service(std::shared_ptr<rclcpp::ServiceBase> service)
+  {
+    if (nullptr == service) {
+      throw std::invalid_argument("service is nullptr");
+    }
+    // this method comes from the SynchronizationPolicy
+    this->sync_remove_service(
+      std::move(service),
+      [this](std::shared_ptr<rclcpp::ServiceBase> && inner_service) {
+        inner_service->exchange_in_use_by_wait_set_state(false);
+        // This method comes from the StoragePolicy, and it may not exist for
+        // fixed sized storage policies.
+        // It will throw if the service is not in the wait set.
+        this->storage_remove_service(std::move(inner_service));
+      });
+  }
+
+  /// Add a waitable to this wait set.
+  /**
+   * \sa add_guard_condition() for details of how this method works.
+   *
+   * Additionally, this function has an optional parameter which can be used to
+   * more quickly associate this waitable with an entity when it is ready, and
+   * so that the ownership maybe held in order to keep the waitable's parent in
+   * scope while waiting.
+   * If it is set to nullptr it will be ignored.
+   * The destruction of the associated entity's shared pointer will not cause
+   * the waitable to be removed, but it will cause the associated entity pointer
+   * to be nullptr when introspecting this waitable after waiting.
+   *
+   * Note that rclcpp::QOSEventHandlerBase are just a special case of
+   * rclcpp::Waitable and can be added with this function.
+   *
+   * \param[in] waitable Waitable to be added.
+   * \param[in] associated_entity Type erased shared pointer associated with the waitable.
+   *   This may be nullptr.
+   * \throws std::invalid_argument if waitable is nullptr.
+   * \throws std::runtime_error if waitable has already been added or is
+   *   associated with another wait set.
+   * \throws exceptions based on the policies used.
+   */
+  void
+  add_waitable(
+    std::shared_ptr<rclcpp::Waitable> waitable,
+    std::shared_ptr<void> associated_entity = nullptr)
+  {
+    if (nullptr == waitable) {
+      throw std::invalid_argument("waitable is nullptr");
+    }
+    // this method comes from the SynchronizationPolicy
+    this->sync_add_waitable(
+      std::move(waitable),
+      std::move(associated_entity),
+      [this](
+        std::shared_ptr<rclcpp::Waitable> && inner_waitable,
+        std::shared_ptr<void> && associated_entity)
+      {
+        bool already_in_use = inner_waitable->exchange_in_use_by_wait_set_state(true);
+        if (already_in_use) {
+          throw std::runtime_error("waitable already in use by another wait set");
+        }
+        // This method comes from the StoragePolicy, and it may not exist for
+        // fixed sized storage policies.
+        // It will throw if the waitable has already been added.
+        this->storage_add_waitable(std::move(inner_waitable), std::move(associated_entity));
+      });
+  }
+
+  /// Remove a waitable from this wait set.
+  /**
+   * \sa remove_guard_condition() for details of how this method works.
+   *
+   * \param[in] waitable Waitable to be removed.
+   * \throws std::invalid_argument if waitable is nullptr.
+   * \throws std::runtime_error if waitable is not part of the wait set.
+   * \throws exceptions based on the policies used.
+   */
+  void
+  remove_waitable(std::shared_ptr<rclcpp::Waitable> waitable)
+  {
+    if (nullptr == waitable) {
+      throw std::invalid_argument("waitable is nullptr");
+    }
+    // this method comes from the SynchronizationPolicy
+    this->sync_remove_waitable(
+      std::move(waitable),
+      [this](std::shared_ptr<rclcpp::Waitable> && inner_waitable) {
+        inner_waitable->exchange_in_use_by_wait_set_state(false);
+        // This method comes from the StoragePolicy, and it may not exist for
+        // fixed sized storage policies.
+        // It will throw if the waitable is not in the wait set.
+        this->storage_remove_waitable(std::move(inner_waitable));
+      });
+  }
+
+  /// Remove any destroyed entities from the wait set.
+  /**
+   * When the storage policy does not maintain shared ownership for the life
+   * of the wait set, e.g. the DynamicStorage policy, it is possible for an
+   * entity to go out of scope and be deleted without this wait set noticing.
+   * Therefore there are weak references in this wait set which need to be
+   * periodically cleared.
+   * This function performs that clean up.
+   *
+   * Since this involves removing entities from the wait set, and is only
+   * needed if the wait set does not keep ownership of the added entities, the
+   * storage policies which are static will not need this function and therefore
+   * do not provide this function.
+   *
+   * \throws exceptions based on the policies used.
+   */
+  void
+  prune_deleted_entities()
+  {
+    // this method comes from the SynchronizationPolicy
+    this->sync_prune_deleted_entities(
+      [this]() {
+        // This method comes from the StoragePolicy, and it may not exist for
+        // fixed sized storage policies.
+        this->storage_prune_deleted_entities();
+      });
+  }
+
+  /// Wait for any of the entities in the wait set to be ready, or a period of time to pass.
+  /**
+   * This function will return when either one of the entities within this wait
+   * set is ready, or a period of time has passed, which ever is first.
+   * The term "ready" means different things for different entities, but
+   * generally it means some condition is met asynchronously for which this
+   * function waits.
+   *
+   * This function can either wait for a period of time, do no waiting
+   * (non-blocking), or wait indefinitely, all based on the value of the
+   * time_to_wait parameter.
+   * Waiting is always measured against the std::chrono::steady_clock.
+   * If waiting indefinitely, the Timeout result is not possible.
+   * There is no "cancel wait" function on this class, but if you want to wait
+   * indefinitely but have a way to asynchronously interrupt this method, then
+   * you can use a dedicated rclcpp::GuardCondition for that purpose.
+   *
+   * This function will modify the internal rcl_wait_set_t, so introspecting
+   * the wait set during a call to wait is never safe.
+   * You should always wait, then introspect, and then, only when done
+   * introspecting, wait again.
+   *
+   * It may be thread-safe to add and remove entities to the wait set
+   * concurrently with this function, depending on the SynchronizationPolicy
+   * that is used.
+   * With the rclcpp::wait_set_policies::ThreadSafeSynchronization policy this
+   * function will stop waiting to allow add or remove of an entity, and then
+   * resume waiting, so long as the timeout has not been reached.
+   *
+   * \param[in] time_to_wait If > 0, time to wait for entities to be ready,
+   *   if == 0, check if anything is ready without blocking, or
+   *   if < 0, wait indefinitely until one of the items is ready.
+   *   Default is -1, so wait indefinitely.
+   * \returns Ready when one of the entities is ready, or
+   * \returns Timeout when the given time to wait is exceeded, not possible
+   *   when time_to_wait is < 0, or
+   * \returns Empty if the wait set is empty, avoiding the possibility of
+   *   waiting indefinitely on an empty wait set.
+   * \throws rclcpp::exceptions::RCLError on unhandled rcl errors
+   */
+  template<class Rep = int64_t, class Period = std::milli>
+  RCUTILS_WARN_UNUSED
+  WaitResult<WaitSetTemplate>
+  wait(std::chrono::duration<Rep, Period> time_to_wait = std::chrono::duration<Rep, Period>(-1))
+  {
+    auto time_to_wait_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(time_to_wait);
+
+    // ensure the ownership of the entities in the wait set is shared for the duration of wait
+    this->storage_acquire_ownerships();
+    RCLCPP_SCOPE_EXIT({this->storage_release_ownerships();});
+
+    // this method comes from the SynchronizationPolicy
+    return this->template sync_wait<WaitResult<WaitSetTemplate>>(
+      // pass along the time_to_wait duration as nanoseconds
+      time_to_wait_ns,
+      // this method provides the ability to rebuild the wait set, if needed
+      [this]() {
+        // This method comes from the StoragePolicy
+        this->storage_rebuild_rcl_wait_set(
+          // This method comes from the SynchronizationPolicy
+          this->get_extra_guard_conditions()
+        );
+      },
+      // this method provides access to the rcl wait set
+      [this]() -> rcl_wait_set_t & {
+        // This method comes from the StoragePolicy
+        return this->storage_get_rcl_wait_set();
+      },
+      // this method provides a way to create the WaitResult
+      [this](WaitResultKind wait_result_kind) -> WaitResult<WaitSetTemplate> {
+        // convert the result into a WaitResult
+        switch (wait_result_kind) {
+          case WaitResultKind::Ready:
+            return WaitResult<WaitSetTemplate>::from_ready_wait_result_kind(*this);
+          case WaitResultKind::Timeout:
+            return WaitResult<WaitSetTemplate>::from_timeout_wait_result_kind();
+          case WaitResultKind::Empty:
+            return WaitResult<WaitSetTemplate>::from_empty_wait_result_kind();
+          default:
+            auto msg = "unknown WaitResultKind with value: " + std::to_string(wait_result_kind);
+            throw std::runtime_error(msg);
+        }
+      }
+    );
+  }
+
+private:
+  // Add WaitResult type as a friend so it can call private methods for
+  // acquiring and releasing resources as the WaitResult is initialized and
+  // destructed, respectively.
+  friend WaitResult<WaitSetTemplate>;
+
+  /// Called by the WaitResult's constructor to place a hold on ownership and thread-safety.
+  /**
+   * Should only be called in pairs with wait_result_release().
+   *
+   * \throws std::runtime_error If called twice before wait_result_release().
+   */
+  void
+  wait_result_acquire()
+  {
+    if (wait_result_holding_) {
+      throw std::runtime_error("wait_result_acquire() called while already holding");
+    }
+    wait_result_holding_ = true;
+    // this method comes from the SynchronizationPolicy
+    this->sync_wait_result_acquire();
+    // this method comes from the StoragePolicy
+    this->storage_acquire_ownerships();
+  }
+
+  /// Called by the WaitResult's destructor to release resources.
+  /**
+   * Should only be called if wait_result_acquire() has been called.
+   *
+   * \throws std::runtime_error If called before wait_result_acquire().
+   */
+  void
+  wait_result_release()
+  {
+    if (!wait_result_holding_) {
+      throw std::runtime_error("wait_result_release() called while not holding");
+    }
+    wait_result_holding_ = false;
+    // this method comes from the StoragePolicy
+    this->storage_release_ownerships();
+    // this method comes from the SynchronizationPolicy
+    this->sync_wait_result_release();
+  }
+
+  bool wait_result_holding_ = false;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_SET_TEMPLATE_HPP_

rclcpp/include/rclcpp/waitable.hpp

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

rclcpp/package.xml

@@ -9,29 +9,26 @@
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
-  <build_export_depend>rmw</build_export_depend>
-
   <build_depend>builtin_interfaces</build_depend>
   <build_depend>rcl_interfaces</build_depend>
   <build_depend>rosgraph_msgs</build_depend>
-  <build_depend>rosidl_generator_cpp</build_depend>
+  <build_depend>rosidl_runtime_cpp</build_depend>
   <build_depend>rosidl_typesupport_c</build_depend>
   <build_depend>rosidl_typesupport_cpp</build_depend>
   <build_export_depend>builtin_interfaces</build_export_depend>
   <build_export_depend>rcl_interfaces</build_export_depend>
   <build_export_depend>rosgraph_msgs</build_export_depend>
-  <build_export_depend>rosidl_generator_cpp</build_export_depend>
+  <build_export_depend>rosidl_runtime_cpp</build_export_depend>
   <build_export_depend>rosidl_typesupport_c</build_export_depend>
   <build_export_depend>rosidl_typesupport_cpp</build_export_depend>
 
   <depend>rcl</depend>
   <depend>rcl_yaml_param_parser</depend>
   <depend>rcpputils</depend>
-  <depend>rmw_implementation</depend>
+  <depend>rcutils</depend>
+  <depend>rmw</depend>
   <depend>tracetools</depend>
 
-  <exec_depend>ament_cmake</exec_depend>
-
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>

rclcpp/resource/logging.hpp.em

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

rclcpp/src/rclcpp/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/clock.cpp

@@ -14,6 +14,9 @@
 
 #include "rclcpp/clock.hpp"
 
+#include <memory>
+#include <thread>
+
 #include "rclcpp/exceptions.hpp"
 
 #include "rcutils/logging_macros.h"
@@ -21,6 +24,31 @@
 namespace rclcpp
 {
 
+class Clock::Impl
+{
+public:
+  explicit Impl(rcl_clock_type_t clock_type)
+  : allocator_{rcl_get_default_allocator()}
+  {
+    rcl_ret_t ret = rcl_clock_init(clock_type, &rcl_clock_, &allocator_);
+    if (ret != RCL_RET_OK) {
+      exceptions::throw_from_rcl_error(ret, "could not get current time stamp");
+    }
+  }
+
+  ~Impl()
+  {
+    rcl_ret_t ret = rcl_clock_fini(&rcl_clock_);
+    if (ret != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR("Failed to fini rcl clock.");
+    }
+  }
+
+  rcl_clock_t rcl_clock_;
+  rcl_allocator_t allocator_;
+  std::mutex clock_mutex_;
+};
+
 JumpHandler::JumpHandler(
   pre_callback_t pre_callback,
   post_callback_t post_callback,
@@ -31,28 +59,16 @@ JumpHandler::JumpHandler(
 {}
 
 Clock::Clock(rcl_clock_type_t clock_type)
-{
-  allocator_ = rcl_get_default_allocator();
-  auto ret = rcl_clock_init(clock_type, &rcl_clock_, &allocator_);
-  if (ret != RCL_RET_OK) {
-    exceptions::throw_from_rcl_error(ret, "could not get current time stamp");
-  }
-}
+: impl_(new Clock::Impl(clock_type)) {}
 
-Clock::~Clock()
-{
-  auto ret = rcl_clock_fini(&rcl_clock_);
-  if (ret != RCL_RET_OK) {
-    RCUTILS_LOG_ERROR("Failed to fini rcl clock.");
-  }
-}
+Clock::~Clock() {}
 
 Time
 Clock::now()
 {
-  Time now(0, 0, rcl_clock_.type);
+  Time now(0, 0, impl_->rcl_clock_.type);
 
-  auto ret = rcl_clock_get_now(&rcl_clock_, &now.rcl_time_.nanoseconds);
+  auto ret = rcl_clock_get_now(&impl_->rcl_clock_, &now.rcl_time_.nanoseconds);
   if (ret != RCL_RET_OK) {
     exceptions::throw_from_rcl_error(ret, "could not get current time stamp");
   }
@@ -63,13 +79,13 @@ Clock::now()
 bool
 Clock::ros_time_is_active()
 {
-  if (!rcl_clock_valid(&rcl_clock_)) {
+  if (!rcl_clock_valid(&impl_->rcl_clock_)) {
     RCUTILS_LOG_ERROR("ROS time not valid!");
     return false;
   }
 
   bool is_enabled = false;
-  auto ret = rcl_is_enabled_ros_time_override(&rcl_clock_, &is_enabled);
+  auto ret = rcl_is_enabled_ros_time_override(&impl_->rcl_clock_, &is_enabled);
   if (ret != RCL_RET_OK) {
     exceptions::throw_from_rcl_error(
       ret, "Failed to check ros_time_override_status");
@@ -80,13 +96,19 @@ Clock::ros_time_is_active()
 rcl_clock_t *
 Clock::get_clock_handle() noexcept
 {
-  return &rcl_clock_;
+  return &impl_->rcl_clock_;
 }
 
 rcl_clock_type_t
 Clock::get_clock_type() const noexcept
 {
-  return rcl_clock_.type;
+  return impl_->rcl_clock_.type;
+}
+
+std::mutex &
+Clock::get_clock_mutex() noexcept
+{
+  return impl_->clock_mutex_;
 }
 
 void
@@ -118,25 +140,31 @@ Clock::create_jump_callback(
     throw std::bad_alloc{};
   }
 
-  // Try to add the jump callback to the clock
-  rcl_ret_t ret = rcl_clock_add_jump_callback(
-    &rcl_clock_, threshold, Clock::on_time_jump,
-    handler.get());
-  if (RCL_RET_OK != ret) {
-    exceptions::throw_from_rcl_error(ret, "Failed to add time jump callback");
+  {
+    std::lock_guard<std::mutex> clock_guard(impl_->clock_mutex_);
+    // Try to add the jump callback to the clock
+    rcl_ret_t ret = rcl_clock_add_jump_callback(
+      &impl_->rcl_clock_, threshold, Clock::on_time_jump,
+      handler.get());
+    if (RCL_RET_OK != ret) {
+      exceptions::throw_from_rcl_error(ret, "Failed to add time jump callback");
+    }
   }
 
+  std::weak_ptr<Clock::Impl> weak_impl = impl_;
   // *INDENT-OFF*
   // create shared_ptr that removes the callback automatically when all copies are destructed
-  // TODO(dorezyuk) UB, if the clock leaves scope before the JumpHandler
-  return JumpHandler::SharedPtr(handler.release(), [this](JumpHandler * handler) noexcept {
-    rcl_ret_t ret = rcl_clock_remove_jump_callback(&rcl_clock_, Clock::on_time_jump,
-        handler);
-    delete handler;
-    handler = NULL;
-    if (RCL_RET_OK != ret) {
-      RCUTILS_LOG_ERROR("Failed to remove time jump callback");
+  return JumpHandler::SharedPtr(handler.release(), [weak_impl](JumpHandler * handler) noexcept {
+    auto shared_impl = weak_impl.lock();
+    if (shared_impl) {
+      std::lock_guard<std::mutex> clock_guard(shared_impl->clock_mutex_);
+      rcl_ret_t ret = rcl_clock_remove_jump_callback(&shared_impl->rcl_clock_,
+          Clock::on_time_jump, handler);
+      if (RCL_RET_OK != ret) {
+        RCUTILS_LOG_ERROR("Failed to remove time jump callback");
+      }
     }
+    delete handler;
   });
   // *INDENT-ON*
 }

rclcpp/src/rclcpp/context.cpp

@@ -16,10 +16,13 @@
 
 #include <memory>
 #include <mutex>
+#include <sstream>
 #include <string>
 #include <vector>
+#include <utility>
 
 #include "rcl/init.h"
+#include "rclcpp/detail/utilities.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/logging.hpp"
 #include "rmw/impl/cpp/demangle.hpp"
@@ -91,10 +94,28 @@ Context::init(
     rcl_context_.reset();
     rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl");
   }
-  init_options_ = init_options;
+  try {
+    std::vector<std::string> unparsed_ros_arguments = detail::get_unparsed_ros_arguments(
+      argc, argv, &(rcl_context_->global_arguments), rcl_get_default_allocator());
+    if (!unparsed_ros_arguments.empty()) {
+      throw exceptions::UnknownROSArgsError(std::move(unparsed_ros_arguments));
+    }
 
-  std::lock_guard<std::mutex> lock(g_contexts_mutex);
-  g_contexts.push_back(this->shared_from_this());
+    init_options_ = init_options;
+
+    std::lock_guard<std::mutex> lock(g_contexts_mutex);
+    g_contexts.push_back(this->shared_from_this());
+  } catch (const std::exception & e) {
+    ret = rcl_shutdown(rcl_context_.get());
+    rcl_context_.reset();
+    if (RCL_RET_OK != ret) {
+      std::ostringstream oss;
+      oss << "While handling: " << e.what() << std::endl <<
+        "    another exception was thrown";
+      rclcpp::exceptions::throw_from_rcl_error(ret, oss.str());
+    }
+    throw;
+  }
 }
 
 bool
@@ -288,6 +309,7 @@ Context::clean_up()
 {
   shutdown_reason_ = "";
   rcl_context_.reset();
+  sub_contexts_.clear();
 }
 
 std::vector<Context::SharedPtr>

rclcpp/src/rclcpp/detail/utilities.cpp

@@ -0,0 +1,64 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/detail/utilities.hpp"
+
+#include <cassert>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "rclcpp/exceptions.hpp"
+
+#include "rcl/allocator.h"
+#include "rcl/arguments.h"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+std::vector<std::string>
+get_unparsed_ros_arguments(
+  int argc, char const * const argv[],
+  rcl_arguments_t * arguments,
+  rcl_allocator_t allocator)
+{
+  (void)argc;
+  std::vector<std::string> unparsed_ros_arguments;
+  int unparsed_ros_args_count = rcl_arguments_get_count_unparsed_ros(arguments);
+  if (unparsed_ros_args_count > 0) {
+    int * unparsed_ros_args_indices = nullptr;
+    rcl_ret_t ret =
+      rcl_arguments_get_unparsed_ros(arguments, allocator, &unparsed_ros_args_indices);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get unparsed ROS arguments");
+    }
+    try {
+      for (int i = 0; i < unparsed_ros_args_count; ++i) {
+        assert(unparsed_ros_args_indices[i] >= 0);
+        assert(unparsed_ros_args_indices[i] < argc);
+        unparsed_ros_arguments.push_back(argv[unparsed_ros_args_indices[i]]);
+      }
+      allocator.deallocate(unparsed_ros_args_indices, allocator.state);
+    } catch (...) {
+      allocator.deallocate(unparsed_ros_args_indices, allocator.state);
+      throw;
+    }
+  }
+  return unparsed_ros_arguments;
+}
+
+}  // namespace detail
+}  // namespace rclcpp

rclcpp/src/rclcpp/duration.cpp

@@ -55,7 +55,8 @@ Duration::Duration(const Duration & rhs)
 Duration::Duration(
   const builtin_interfaces::msg::Duration & duration_msg)
 {
-  rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast<uint64_t>(duration_msg.sec));
+  rcl_duration_.nanoseconds =
+    static_cast<rcl_duration_value_t>(RCL_S_TO_NS(duration_msg.sec));
   rcl_duration_.nanoseconds += duration_msg.nanosec;
 }
 
@@ -122,15 +123,15 @@ Duration::operator>(const rclcpp::Duration & rhs) const
 void
 bounds_check_duration_sum(int64_t lhsns, int64_t rhsns, uint64_t max)
 {
-  auto abs_lhs = (uint64_t)std::abs(lhsns);
-  auto abs_rhs = (uint64_t)std::abs(rhsns);
+  auto abs_lhs = static_cast<uint64_t>(std::abs(lhsns));
+  auto abs_rhs = static_cast<uint64_t>(std::abs(rhsns));
 
   if (lhsns > 0 && rhsns > 0) {
-    if (abs_lhs + abs_rhs > (uint64_t) max) {
+    if (abs_lhs + abs_rhs > max) {
       throw std::overflow_error("addition leads to int64_t overflow");
     }
   } else if (lhsns < 0 && rhsns < 0) {
-    if (abs_lhs + abs_rhs > (uint64_t) max) {
+    if (abs_lhs + abs_rhs > max) {
       throw std::underflow_error("addition leads to int64_t underflow");
     }
   }
@@ -150,15 +151,15 @@ Duration::operator+(const rclcpp::Duration & rhs) const
 void
 bounds_check_duration_difference(int64_t lhsns, int64_t rhsns, uint64_t max)
 {
-  auto abs_lhs = (uint64_t)std::abs(lhsns);
-  auto abs_rhs = (uint64_t)std::abs(rhsns);
+  auto abs_lhs = static_cast<uint64_t>(std::abs(lhsns));
+  auto abs_rhs = static_cast<uint64_t>(std::abs(rhsns));
 
   if (lhsns > 0 && rhsns < 0) {
-    if (abs_lhs + abs_rhs > (uint64_t) max) {
+    if (abs_lhs + abs_rhs > max) {
       throw std::overflow_error("duration subtraction leads to int64_t overflow");
     }
   } else if (lhsns < 0 && rhsns > 0) {
-    if (abs_lhs + abs_rhs > (uint64_t) max) {
+    if (abs_lhs + abs_rhs > max) {
       throw std::underflow_error("duration subtraction leads to int64_t underflow");
     }
   }
@@ -181,8 +182,9 @@ bounds_check_duration_scale(int64_t dns, double scale, uint64_t max)
 {
   auto abs_dns = static_cast<uint64_t>(std::abs(dns));
   auto abs_scale = std::abs(scale);
-
-  if (abs_scale > 1.0 && abs_dns > static_cast<uint64_t>(max / abs_scale)) {
+  if (abs_scale > 1.0 && abs_dns >
+    static_cast<uint64_t>(static_cast<long double>(max) / static_cast<long double>(abs_scale)))
+  {
     if ((dns > 0 && scale > 0) || (dns < 0 && scale < 0)) {
       throw std::overflow_error("duration scaling leads to int64_t overflow");
     } else {
@@ -201,7 +203,10 @@ Duration::operator*(double scale) const
     this->rcl_duration_.nanoseconds,
     scale,
     std::numeric_limits<rcl_duration_value_t>::max());
-  return Duration(static_cast<rcl_duration_value_t>(rcl_duration_.nanoseconds * scale));
+  long double scale_ld = static_cast<long double>(scale);
+  return Duration(
+    static_cast<rcl_duration_value_t>(
+      static_cast<long double>(rcl_duration_.nanoseconds) * scale_ld));
 }
 
 rcl_duration_value_t
@@ -228,8 +233,8 @@ Duration::to_rmw_time() const
   // reuse conversion logic from msg creation
   builtin_interfaces::msg::Duration msg = *this;
   rmw_time_t result;
-  result.sec = msg.sec;
-  result.nsec = msg.nanosec;
+  result.sec = static_cast<uint64_t>(msg.sec);
+  result.nsec = static_cast<uint64_t>(msg.nanosec);
   return result;
 }
 

rclcpp/src/rclcpp/executable_list.cpp

@@ -0,0 +1,84 @@
+// Copyright 2019 Nobleo Technology
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <utility>
+
+#include "rclcpp/executable_list.hpp"
+
+using rclcpp::executor::ExecutableList;
+
+ExecutableList::ExecutableList()
+: number_of_subscriptions(0),
+  number_of_timers(0),
+  number_of_services(0),
+  number_of_clients(0),
+  number_of_waitables(0)
+{}
+
+ExecutableList::~ExecutableList()
+{}
+
+void
+ExecutableList::clear()
+{
+  this->timer.clear();
+  this->number_of_timers = 0;
+
+  this->subscription.clear();
+  this->number_of_subscriptions = 0;
+
+  this->service.clear();
+  this->number_of_services = 0;
+
+  this->client.clear();
+  this->number_of_clients = 0;
+
+  this->waitable.clear();
+  this->number_of_waitables = 0;
+}
+
+void
+ExecutableList::add_subscription(rclcpp::SubscriptionBase::SharedPtr subscription)
+{
+  this->subscription.push_back(std::move(subscription));
+  this->number_of_subscriptions++;
+}
+
+void
+ExecutableList::add_timer(rclcpp::TimerBase::SharedPtr timer)
+{
+  this->timer.push_back(std::move(timer));
+  this->number_of_timers++;
+}
+
+void
+ExecutableList::add_service(rclcpp::ServiceBase::SharedPtr service)
+{
+  this->service.push_back(std::move(service));
+  this->number_of_services++;
+}
+
+void
+ExecutableList::add_client(rclcpp::ClientBase::SharedPtr client)
+{
+  this->client.push_back(std::move(client));
+  this->number_of_clients++;
+}
+
+void
+ExecutableList::add_waitable(rclcpp::Waitable::SharedPtr waitable)
+{
+  this->waitable.push_back(std::move(waitable));
+  this->number_of_waitables++;
+}

rclcpp/src/rclcpp/executor.cpp

@@ -26,8 +26,6 @@
 #include "rclcpp/scope_exit.hpp"
 #include "rclcpp/utilities.hpp"
 
-#include "rcl_interfaces/msg/intra_process_message.hpp"
-
 #include "rcutils/logging_macros.h"
 
 using rclcpp::exceptions::throw_from_rcl_error;
@@ -234,9 +232,11 @@ Executor::spin_some(std::chrono::nanoseconds max_duration)
     throw std::runtime_error("spin_some() called while already spinning");
   }
   RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
+  // non-blocking call to pre-load all available work
+  wait_for_work(std::chrono::milliseconds::zero());
   while (spinning.load() && max_duration_not_elapsed()) {
     AnyExecutable any_exec;
-    if (get_next_executable(any_exec, std::chrono::milliseconds::zero())) {
+    if (get_next_ready_executable(any_exec)) {
       execute_any_executable(any_exec);
     } else {
       break;
@@ -305,99 +305,127 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
   }
 }
 
+static
 void
-Executor::execute_subscription(
-  rclcpp::SubscriptionBase::SharedPtr subscription)
+take_and_do_error_handling(
+  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;
-  message_info.from_intra_process = false;
+  bool taken = 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();
-    auto ret = rcl_take_serialized_message(
-      subscription->get_subscription_handle().get(),
-      serialized_msg.get(), &message_info, nullptr);
-    if (RCL_RET_OK == ret) {
-      auto void_serialized_msg = std::static_pointer_cast<void>(serialized_msg);
-      subscription->handle_message(void_serialized_msg, message_info);
-    } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) {
-      RCUTILS_LOG_ERROR_NAMED(
-        "rclcpp",
-        "take_serialized failed for subscription on topic '%s': %s",
-        subscription->get_topic_name(), rcl_get_error_string().str);
-      rcl_reset_error();
-    }
+    // This is the case where a copy of the serialized message is taken from
+    // the middleware via inter-process communication.
+    std::shared_ptr<rcl_serialized_message_t> serialized_msg =
+      subscription->create_serialized_message();
+    take_and_do_error_handling(
+      "taking a serialized message from topic",
+      subscription->get_topic_name(),
+      [&]() {return subscription->take_serialized(*serialized_msg.get(), message_info);},
+      [&]()
+      {
+        auto void_serialized_msg = std::static_pointer_cast<void>(serialized_msg);
+        subscription->handle_message(void_serialized_msg, message_info);
+      });
     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(
-      subscription->get_subscription_handle().get(),
-      &loaned_msg,
-      &message_info,
-      nullptr);
-    if (RCL_RET_OK == ret) {
-      subscription->handle_loaned_message(loaned_msg, message_info);
-    } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) {
-      RCUTILS_LOG_ERROR_NAMED(
-        "rclcpp",
-        "take_loaned failed for subscription on topic '%s': %s",
-        subscription->get_topic_name(), rcl_get_error_string().str);
-      rcl_reset_error();
-    }
-    ret = rcl_return_loaned_message_from_subscription(
+    // TODO(wjwwood): refactor this into methods on subscription when LoanedMessage
+    //   is extened to support subscriptions as well.
+    take_and_do_error_handling(
+      "taking a loaned message from topic",
+      subscription->get_topic_name(),
+      [&]()
+      {
+        rcl_ret_t ret = rcl_take_loaned_message(
+          subscription->get_subscription_handle().get(),
+          &loaned_msg,
+          &message_info.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;
+      },
+      [&]() {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",
-        "return_loaned_message failed for subscription on topic '%s': %s",
+      RCLCPP_ERROR(
+        rclcpp::get_logger("rclcpp"),
+        "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(
-      subscription->get_subscription_handle().get(),
-      message.get(), &message_info, nullptr);
-    if (RCL_RET_OK == ret) {
-      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();
-    }
+    take_and_do_error_handling(
+      "taking a message from topic",
+      subscription->get_topic_name(),
+      [&]() {return subscription->take_type_erased(message.get(), message_info);},
+      [&]() {subscription->handle_message(message, message_info);});
     subscription->return_message(message);
   }
 }
 
 void
-Executor::execute_timer(
-  rclcpp::TimerBase::SharedPtr timer)
+Executor::execute_timer(rclcpp::TimerBase::SharedPtr timer)
 {
   timer->execute_callback();
 }
 
 void
-Executor::execute_service(
-  rclcpp::ServiceBase::SharedPtr service)
+Executor::execute_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(
-    service->get_service_handle().get(),
-    request_header.get(),
-    request.get());
-  if (status == RCL_RET_OK) {
-    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();
-  }
+  take_and_do_error_handling(
+    "taking a service server request from service",
+    service->get_service_name(),
+    [&]() {return service->take_type_erased_request(request.get(), *request_header);},
+    [&]() {service->handle_request(request_header, request);});
 }
 
 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(
-    client->get_client_handle().get(),
-    request_header.get(),
-    response.get());
-  if (status == RCL_RET_OK) {
-    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();
-  }
+  take_and_do_error_handling(
+    "taking a service client response from service",
+    client->get_service_name(),
+    [&]() {return client->take_type_erased_response(response.get(), *request_header);},
+    [&]() {client->handle_response(request_header, response);});
 }
 
 void
@@ -531,51 +551,39 @@ Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer)
 bool
 Executor::get_next_ready_executable(AnyExecutable & any_executable)
 {
+  bool success = false;
   // Check the timers to see if there are any that are ready
   memory_strategy_->get_next_timer(any_executable, weak_nodes_);
   if (any_executable.timer) {
-    return true;
+    success = true;
   }
-  // Check the subscriptions to see if there are any that are ready
-  memory_strategy_->get_next_subscription(any_executable, weak_nodes_);
-  if (any_executable.subscription) {
-    return true;
-  }
-  // Check the services to see if there are any that are ready
-  memory_strategy_->get_next_service(any_executable, weak_nodes_);
-  if (any_executable.service) {
-    return true;
-  }
-  // Check the clients to see if there are any that are ready
-  memory_strategy_->get_next_client(any_executable, weak_nodes_);
-  if (any_executable.client) {
-    return true;
-  }
-  // Check the waitables to see if there are any that are ready
-  memory_strategy_->get_next_waitable(any_executable, weak_nodes_);
-  if (any_executable.waitable) {
-    return true;
-  }
-  // If there is no ready executable, return a null ptr
-  return false;
-}
-
-bool
-Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanoseconds timeout)
-{
-  bool success = false;
-  // Check to see if there are any subscriptions or timers needing service
-  // TODO(wjwwood): improve run to run efficiency of this function
-  success = get_next_ready_executable(any_executable);
-  // If there are none
   if (!success) {
-    // Wait for subscriptions or timers to work on
-    wait_for_work(timeout);
-    if (!spinning.load()) {
-      return false;
+    // Check the subscriptions to see if there are any that are ready
+    memory_strategy_->get_next_subscription(any_executable, weak_nodes_);
+    if (any_executable.subscription) {
+      success = true;
+    }
+  }
+  if (!success) {
+    // Check the services to see if there are any that are ready
+    memory_strategy_->get_next_service(any_executable, weak_nodes_);
+    if (any_executable.service) {
+      success = true;
+    }
+  }
+  if (!success) {
+    // Check the clients to see if there are any that are ready
+    memory_strategy_->get_next_client(any_executable, weak_nodes_);
+    if (any_executable.client) {
+      success = true;
+    }
+  }
+  if (!success) {
+    // Check the waitables to see if there are any that are ready
+    memory_strategy_->get_next_waitable(any_executable, weak_nodes_);
+    if (any_executable.waitable) {
+      success = true;
     }
-    // Try again
-    success = get_next_ready_executable(any_executable);
   }
   // At this point any_exec should be valid with either a valid subscription
   // or a valid timer, or it should be a null shared_ptr
@@ -595,6 +603,27 @@ Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanos
       any_executable.callback_group->can_be_taken_from().store(false);
     }
   }
+  // If there is no ready executable, return a null ptr
+  return success;
+}
+
+bool
+Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanoseconds timeout)
+{
+  bool success = false;
+  // Check to see if there are any subscriptions or timers needing service
+  // TODO(wjwwood): improve run to run efficiency of this function
+  success = get_next_ready_executable(any_executable);
+  // If there are none
+  if (!success) {
+    // Wait for subscriptions or timers to work on
+    wait_for_work(timeout);
+    if (!spinning.load()) {
+      return false;
+    }
+    // Try again
+    success = get_next_ready_executable(any_executable);
+  }
   return success;
 }
 

rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp

@@ -0,0 +1,289 @@
+// 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/executors/static_executor_entities_collector.hpp"
+
+#include <algorithm>
+#include <memory>
+#include <stdexcept>
+#include <string>
+
+#include "rclcpp/memory_strategy.hpp"
+#include "rclcpp/executors/static_single_threaded_executor.hpp"
+
+using rclcpp::executors::StaticExecutorEntitiesCollector;
+
+StaticExecutorEntitiesCollector::~StaticExecutorEntitiesCollector()
+{
+  // Disassociate all nodes
+  for (auto & weak_node : weak_nodes_) {
+    auto node = weak_node.lock();
+    if (node) {
+      std::atomic_bool & has_executor = node->get_associated_with_executor_atomic();
+      has_executor.store(false);
+    }
+  }
+  exec_list_.clear();
+  weak_nodes_.clear();
+  guard_conditions_.clear();
+}
+
+void
+StaticExecutorEntitiesCollector::init(
+  rcl_wait_set_t * p_wait_set,
+  rclcpp::memory_strategy::MemoryStrategy::SharedPtr & memory_strategy,
+  rcl_guard_condition_t * executor_guard_condition)
+{
+  // Empty initialize executable list
+  exec_list_ = executor::ExecutableList();
+  // Get executor's wait_set_ pointer
+  p_wait_set_ = p_wait_set;
+  // Get executor's memory strategy ptr
+  if (memory_strategy == nullptr) {
+    throw std::runtime_error("Received NULL memory strategy in executor waitable.");
+  }
+  memory_strategy_ = memory_strategy;
+
+  // Add executor's guard condition
+  guard_conditions_.push_back(executor_guard_condition);
+
+  // Get memory strategy and executable list. Prepare wait_set_
+  execute();
+}
+
+void
+StaticExecutorEntitiesCollector::execute()
+{
+  // Fill memory strategy with entities coming from weak_nodes_
+  fill_memory_strategy();
+  // Fill exec_list_ with entities coming from weak_nodes_ (same as memory strategy)
+  fill_executable_list();
+  // Resize the wait_set_ based on memory_strategy handles (rcl_wait_set_resize)
+  prepare_wait_set();
+}
+
+void
+StaticExecutorEntitiesCollector::fill_memory_strategy()
+{
+  memory_strategy_->clear_handles();
+  bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_);
+
+  // Clean up any invalid nodes, if they were detected
+  if (has_invalid_weak_nodes) {
+    auto node_it = weak_nodes_.begin();
+    while (node_it != weak_nodes_.end()) {
+      if (node_it->expired()) {
+        node_it = weak_nodes_.erase(node_it);
+      } else {
+        ++node_it;
+      }
+    }
+  }
+
+  // Add the static executor waitable to the memory strategy
+  memory_strategy_->add_waitable_handle(this->shared_from_this());
+}
+
+void
+StaticExecutorEntitiesCollector::fill_executable_list()
+{
+  exec_list_.clear();
+
+  for (auto & weak_node : weak_nodes_) {
+    auto node = weak_node.lock();
+    if (!node) {
+      continue;
+    }
+    // Check in all the callback groups
+    for (auto & weak_group : node->get_callback_groups()) {
+      auto group = weak_group.lock();
+      if (!group || !group->can_be_taken_from().load()) {
+        continue;
+      }
+
+      group->find_timer_ptrs_if(
+        [this](const rclcpp::TimerBase::SharedPtr & timer) {
+          if (timer) {
+            exec_list_.add_timer(timer);
+          }
+          return false;
+        });
+      group->find_subscription_ptrs_if(
+        [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
+          if (subscription) {
+            exec_list_.add_subscription(subscription);
+          }
+          return false;
+        });
+      group->find_service_ptrs_if(
+        [this](const rclcpp::ServiceBase::SharedPtr & service) {
+          if (service) {
+            exec_list_.add_service(service);
+          }
+          return false;
+        });
+      group->find_client_ptrs_if(
+        [this](const rclcpp::ClientBase::SharedPtr & client) {
+          if (client) {
+            exec_list_.add_client(client);
+          }
+          return false;
+        });
+      group->find_waitable_ptrs_if(
+        [this](const rclcpp::Waitable::SharedPtr & waitable) {
+          if (waitable) {
+            exec_list_.add_waitable(waitable);
+          }
+          return false;
+        });
+    }
+  }
+
+  // Add the executor's waitable to the executable list
+  exec_list_.add_waitable(shared_from_this());
+}
+
+void
+StaticExecutorEntitiesCollector::prepare_wait_set()
+{
+  // clear wait set
+  if (rcl_wait_set_clear(p_wait_set_) != RCL_RET_OK) {
+    throw std::runtime_error("Couldn't clear wait set");
+  }
+
+  // The size of waitables are accounted for in size of the other entities
+  rcl_ret_t ret = rcl_wait_set_resize(
+    p_wait_set_, memory_strategy_->number_of_ready_subscriptions(),
+    memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(),
+    memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(),
+    memory_strategy_->number_of_ready_events());
+
+  if (RCL_RET_OK != ret) {
+    throw std::runtime_error(
+            std::string("Couldn't resize the wait set : ") + rcl_get_error_string().str);
+  }
+}
+
+void
+StaticExecutorEntitiesCollector::refresh_wait_set(std::chrono::nanoseconds timeout)
+{
+  // clear wait set (memeset to '0' all wait_set_ entities
+  // but keeps the wait_set_ number of entities)
+  if (rcl_wait_set_clear(p_wait_set_) != RCL_RET_OK) {
+    throw std::runtime_error("Couldn't clear wait set");
+  }
+
+  if (!memory_strategy_->add_handles_to_wait_set(p_wait_set_)) {
+    throw std::runtime_error("Couldn't fill wait set");
+  }
+
+  rcl_ret_t status =
+    rcl_wait(p_wait_set_, std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count());
+
+  if (status == RCL_RET_WAIT_SET_EMPTY) {
+    RCUTILS_LOG_WARN_NAMED(
+      "rclcpp",
+      "empty wait set received in rcl_wait(). This should never happen.");
+  } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) {
+    using rclcpp::exceptions::throw_from_rcl_error;
+    throw_from_rcl_error(status, "rcl_wait() failed");
+  }
+}
+
+bool
+StaticExecutorEntitiesCollector::add_to_wait_set(rcl_wait_set_t * wait_set)
+{
+  // Add waitable guard conditions (one for each registered node) into the wait set.
+  for (const auto & gc : guard_conditions_) {
+    rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, gc, NULL);
+    if (ret != RCL_RET_OK) {
+      throw std::runtime_error("Executor waitable: couldn't add guard condition to wait set");
+    }
+  }
+  return true;
+}
+
+size_t StaticExecutorEntitiesCollector::get_number_of_ready_guard_conditions()
+{
+  return guard_conditions_.size();
+}
+
+void
+StaticExecutorEntitiesCollector::add_node(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
+{
+  // Check to ensure node not already added
+  for (auto & weak_node : weak_nodes_) {
+    auto node = weak_node.lock();
+    if (node == node_ptr) {
+      // TODO(jacquelinekay): Use a different error here?
+      throw std::runtime_error("Cannot add node to executor, node already added.");
+    }
+  }
+
+  weak_nodes_.push_back(node_ptr);
+  guard_conditions_.push_back(node_ptr->get_notify_guard_condition());
+}
+
+bool
+StaticExecutorEntitiesCollector::remove_node(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
+{
+  auto node_it = weak_nodes_.begin();
+
+  while (node_it != weak_nodes_.end()) {
+    bool matched = (node_it->lock() == node_ptr);
+    if (matched) {
+      // Find and remove node and its guard condition
+      auto gc_it = std::find(
+        guard_conditions_.begin(),
+        guard_conditions_.end(),
+        node_ptr->get_notify_guard_condition());
+
+      if (gc_it != guard_conditions_.end()) {
+        guard_conditions_.erase(gc_it);
+        weak_nodes_.erase(node_it);
+        return true;
+      }
+
+      throw std::runtime_error("Didn't find guard condition associated with node.");
+
+    } else {
+      ++node_it;
+    }
+  }
+
+  return false;
+}
+
+bool
+StaticExecutorEntitiesCollector::is_ready(rcl_wait_set_t * p_wait_set)
+{
+  // Check wait_set guard_conditions for added/removed entities to/from a node
+  for (size_t i = 0; i < p_wait_set->size_of_guard_conditions; ++i) {
+    if (p_wait_set->guard_conditions[i] != NULL) {
+      // Check if the guard condition triggered belongs to a node
+      auto it = std::find(
+        guard_conditions_.begin(), guard_conditions_.end(),
+        p_wait_set->guard_conditions[i]);
+
+      // If it does, we are ready to re-collect entities
+      if (it != guard_conditions_.end()) {
+        return true;
+      }
+    }
+  }
+  // None of the guard conditions triggered belong to a registered node
+  return false;
+}

rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp

@@ -0,0 +1,144 @@
+// Copyright 2019 Nobleo Technology
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/executors/static_single_threaded_executor.hpp"
+
+#include <memory>
+
+#include "rclcpp/scope_exit.hpp"
+
+using rclcpp::executors::StaticSingleThreadedExecutor;
+using rclcpp::executor::ExecutableList;
+
+StaticSingleThreadedExecutor::StaticSingleThreadedExecutor(
+  const rclcpp::executor::ExecutorArgs & args)
+: executor::Executor(args)
+{
+  entities_collector_ = std::make_shared<StaticExecutorEntitiesCollector>();
+}
+
+StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor() {}
+
+void
+StaticSingleThreadedExecutor::spin()
+{
+  if (spinning.exchange(true)) {
+    throw std::runtime_error("spin() called while already spinning");
+  }
+  RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
+
+  // Set memory_strategy_ and exec_list_ based on weak_nodes_
+  // Prepare wait_set_ based on memory_strategy_
+  entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_);
+
+  while (rclcpp::ok(this->context_) && spinning.load()) {
+    // Refresh wait set and wait for work
+    entities_collector_->refresh_wait_set();
+    execute_ready_executables();
+  }
+}
+
+void
+StaticSingleThreadedExecutor::add_node(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
+{
+  // If the node already has an executor
+  std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
+  if (has_executor.exchange(true)) {
+    throw std::runtime_error("Node has already been added to an executor.");
+  }
+
+  if (notify) {
+    // Interrupt waiting to handle new node
+    if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+      throw std::runtime_error(rcl_get_error_string().str);
+    }
+  }
+
+  entities_collector_->add_node(node_ptr);
+}
+
+void
+StaticSingleThreadedExecutor::add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify)
+{
+  this->add_node(node_ptr->get_node_base_interface(), notify);
+}
+
+void
+StaticSingleThreadedExecutor::remove_node(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
+{
+  bool node_removed = entities_collector_->remove_node(node_ptr);
+
+  if (notify) {
+    // If the node was matched and removed, interrupt waiting
+    if (node_removed) {
+      if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+        throw std::runtime_error(rcl_get_error_string().str);
+      }
+    }
+  }
+
+  std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
+  has_executor.store(false);
+}
+
+void
+StaticSingleThreadedExecutor::remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify)
+{
+  this->remove_node(node_ptr->get_node_base_interface(), notify);
+}
+
+void
+StaticSingleThreadedExecutor::execute_ready_executables()
+{
+  // Execute all the ready subscriptions
+  for (size_t i = 0; i < wait_set_.size_of_subscriptions; ++i) {
+    if (i < entities_collector_->get_number_of_subscriptions()) {
+      if (wait_set_.subscriptions[i]) {
+        execute_subscription(entities_collector_->get_subscription(i));
+      }
+    }
+  }
+  // Execute all the ready timers
+  for (size_t i = 0; i < wait_set_.size_of_timers; ++i) {
+    if (i < entities_collector_->get_number_of_timers()) {
+      if (wait_set_.timers[i] && entities_collector_->get_timer(i)->is_ready()) {
+        execute_timer(entities_collector_->get_timer(i));
+      }
+    }
+  }
+  // Execute all the ready services
+  for (size_t i = 0; i < wait_set_.size_of_services; ++i) {
+    if (i < entities_collector_->get_number_of_services()) {
+      if (wait_set_.services[i]) {
+        execute_service(entities_collector_->get_service(i));
+      }
+    }
+  }
+  // Execute all the ready clients
+  for (size_t i = 0; i < wait_set_.size_of_clients; ++i) {
+    if (i < entities_collector_->get_number_of_clients()) {
+      if (wait_set_.clients[i]) {
+        execute_client(entities_collector_->get_client(i));
+      }
+    }
+  }
+  // Execute all the ready waitables
+  for (size_t i = 0; i < entities_collector_->get_number_of_waitables(); ++i) {
+    if (entities_collector_->get_waitable(i)->is_ready(&wait_set_)) {
+      entities_collector_->get_waitable(i)->execute();
+    }
+  }
+}

rclcpp/src/rclcpp/graph_listener.cpp

@@ -23,6 +23,7 @@
 #include "rcl/error_handling.h"
 #include "rcl/types.h"
 #include "rclcpp/exceptions.hpp"
+#include "rclcpp/logging.hpp"
 #include "rclcpp/node.hpp"
 #include "rmw/impl/cpp/demangle.hpp"
 
@@ -39,17 +40,15 @@ GraphListener::GraphListener(std::shared_ptr<rclcpp::Context> parent_context)
 : parent_context_(parent_context),
   is_started_(false),
   is_shutdown_(false),
-  interrupt_guard_condition_context_(nullptr),
   shutdown_guard_condition_(nullptr)
 {
   // TODO(wjwwood): make a guard condition class in rclcpp so this can be tracked
   //   automatically with the rcl guard condition
   // hold on to this context to prevent it from going out of scope while this
   // guard condition is using it.
-  interrupt_guard_condition_context_ = parent_context->get_rcl_context();
   rcl_ret_t ret = rcl_guard_condition_init(
     &interrupt_guard_condition_,
-    interrupt_guard_condition_context_.get(),
+    parent_context->get_rcl_context().get(),
     rcl_guard_condition_get_default_options());
   if (RCL_RET_OK != ret) {
     throw_from_rcl_error(ret, "failed to create interrupt guard condition");
@@ -72,6 +71,10 @@ GraphListener::start_if_not_started()
   }
   if (!is_started_) {
     // Initialize the wait set before starting.
+    auto parent_context = parent_context_.lock();
+    if (!parent_context) {
+      throw std::runtime_error("parent context was destroyed");
+    }
     rcl_ret_t ret = rcl_wait_set_init(
       &wait_set_,
       0,  // number_of_subscriptions
@@ -80,7 +83,7 @@ GraphListener::start_if_not_started()
       0,  // number_of_clients
       0,  // number_of_services
       0,  // number_of_events
-      this->parent_context_->get_rcl_context().get(),
+      parent_context->get_rcl_context().get(),
       rcl_get_default_allocator());
     if (RCL_RET_OK != ret) {
       throw_from_rcl_error(ret, "failed to initialize wait set");
@@ -141,6 +144,13 @@ GraphListener::run_loop()
     }
     // This lock is released when the loop continues or exits.
     std::lock_guard<std::mutex> nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock);
+    // Ensure that the context doesn't go out of scope.
+    auto parent_context = parent_context_.lock();
+    if (!parent_context) {
+      // The parent context may be destroyed before this loop is stopped.
+      // In that case, the loop is broken and the function just returns silently.
+      return;
+    }
 
     // Resize the wait set if necessary.
     const size_t node_graph_interfaces_size = node_graph_interfaces_.size();
@@ -350,15 +360,28 @@ GraphListener::__shutdown(bool should_throw)
       listener_thread_.join();
     }
     rcl_ret_t ret = rcl_guard_condition_fini(&interrupt_guard_condition_);
-    interrupt_guard_condition_context_.reset();  // release context guard condition was using
     if (RCL_RET_OK != ret) {
       throw_from_rcl_error(ret, "failed to finalize interrupt guard condition");
     }
     if (shutdown_guard_condition_) {
-      if (should_throw) {
-        parent_context_->release_interrupt_guard_condition(&wait_set_);
+      auto parent_context_ptr = parent_context_.lock();
+      if (parent_context_ptr) {
+        if (should_throw) {
+          parent_context_ptr->release_interrupt_guard_condition(&wait_set_);
+        } else {
+          parent_context_ptr->release_interrupt_guard_condition(&wait_set_, std::nothrow);
+        }
       } else {
-        parent_context_->release_interrupt_guard_condition(&wait_set_, std::nothrow);
+        ret = rcl_guard_condition_fini(shutdown_guard_condition_);
+        if (RCL_RET_OK != ret) {
+          if (should_throw) {
+            throw_from_rcl_error(ret, "failed to finalize shutdown guard condition");
+          } else {
+            RCLCPP_ERROR(
+              rclcpp::get_logger("rclcpp"),
+              "failed to finalize shutdown guard condition");
+          }
+        }
       }
       shutdown_guard_condition_ = nullptr;
     }

rclcpp/src/rclcpp/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/intra_process_manager.cpp

@@ -32,7 +32,9 @@ IntraProcessManager::~IntraProcessManager()
 {}
 
 uint64_t
-IntraProcessManager::add_publisher(rclcpp::PublisherBase::SharedPtr publisher)
+IntraProcessManager::add_publisher(
+  rclcpp::PublisherBase::SharedPtr publisher,
+  const bool is_serialized)
 {
   std::unique_lock<std::shared_timed_mutex> lock(mutex_);
 
@@ -41,6 +43,7 @@ IntraProcessManager::add_publisher(rclcpp::PublisherBase::SharedPtr publisher)
   publishers_[id].publisher = publisher;
   publishers_[id].topic_name = publisher->get_topic_name();
   publishers_[id].qos = publisher->get_actual_qos().get_rmw_qos_profile();
+  publishers_[id].is_serialized = is_serialized;
 
   // Initialize the subscriptions storage for this publisher.
   pub_to_subs_[id] = SplittedSubscriptions();
@@ -56,7 +59,9 @@ IntraProcessManager::add_publisher(rclcpp::PublisherBase::SharedPtr publisher)
 }
 
 uint64_t
-IntraProcessManager::add_subscription(SubscriptionIntraProcessBase::SharedPtr subscription)
+IntraProcessManager::add_subscription(
+  SubscriptionIntraProcessBase::SharedPtr subscription,
+  const bool is_serialized)
 {
   std::unique_lock<std::shared_timed_mutex> lock(mutex_);
 
@@ -66,6 +71,7 @@ IntraProcessManager::add_subscription(SubscriptionIntraProcessBase::SharedPtr su
   subscriptions_[id].topic_name = subscription->get_topic_name();
   subscriptions_[id].qos = subscription->get_actual_qos();
   subscriptions_[id].use_take_shared_method = subscription->use_take_shared_method();
+  subscriptions_[id].is_serialized = is_serialized;
 
   // adds the subscription id to all the matchable publishers
   for (auto & pair : publishers_) {
@@ -220,6 +226,13 @@ IntraProcessManager::can_communicate(
     return false;
   }
 
+  // a publisher and a subscription with different content type can't communicate
+  // if is_serialized is true, the expected message typ is rcl_serialized_message_t
+  // otherwise the templated ROS2 message type
+  if (sub_info.is_serialized != pub_info.is_serialized) {
+    return false;
+  }
+
   return true;
 }
 

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/node.cpp

@@ -364,6 +364,18 @@ Node::count_subscribers(const std::string & topic_name) const
   return node_graph_->count_subscribers(topic_name);
 }
 
+std::vector<rclcpp::TopicEndpointInfo>
+Node::get_publishers_info_by_topic(const std::string & topic_name, bool no_mangle) const
+{
+  return node_graph_->get_publishers_info_by_topic(topic_name, no_mangle);
+}
+
+std::vector<rclcpp::TopicEndpointInfo>
+Node::get_subscriptions_info_by_topic(const std::string & topic_name, bool no_mangle) const
+{
+  return node_graph_->get_subscriptions_info_by_topic(topic_name, no_mangle);
+}
+
 const std::vector<rclcpp::callback_group::CallbackGroup::WeakPtr> &
 Node::get_callback_groups() const
 {
@@ -390,8 +402,14 @@ Node::get_clock()
   return node_clock_->get_clock();
 }
 
+rclcpp::Clock::ConstSharedPtr
+Node::get_clock() const
+{
+  return node_clock_->get_clock();
+}
+
 rclcpp::Time
-Node::now()
+Node::now() const
 {
   return node_clock_->get_clock()->now();
 }

rclcpp/src/rclcpp/node_interfaces/node_clock.cpp

@@ -36,8 +36,14 @@ NodeClock::NodeClock(
 NodeClock::~NodeClock()
 {}
 
-std::shared_ptr<rclcpp::Clock>
+rclcpp::Clock::SharedPtr
 NodeClock::get_clock()
 {
   return ros_clock_;
 }
+
+rclcpp::Clock::ConstSharedPtr
+NodeClock::get_clock() const
+{
+  return ros_clock_;
+}

rclcpp/src/rclcpp/node_interfaces/node_graph.cpp

@@ -21,9 +21,12 @@
 #include <vector>
 
 #include "rcl/graph.h"
-#include "rclcpp/exceptions.hpp"
+#include "rcl/remap.h"
 #include "rclcpp/event.hpp"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/graph_listener.hpp"
+#include "rclcpp/node_interfaces/node_graph_interface.hpp"
 
 using rclcpp::node_interfaces::NodeGraph;
 using rclcpp::exceptions::throw_from_rcl_error;
@@ -369,3 +372,189 @@ NodeGraph::count_graph_users()
 {
   return graph_users_count_.load();
 }
+
+static
+std::vector<rclcpp::TopicEndpointInfo>
+convert_to_topic_info_list(const rcl_topic_endpoint_info_array_t & info_array)
+{
+  std::vector<rclcpp::TopicEndpointInfo> topic_info_list;
+  for (size_t i = 0; i < info_array.size; ++i) {
+    topic_info_list.push_back(rclcpp::TopicEndpointInfo(info_array.info_array[i]));
+  }
+  return topic_info_list;
+}
+
+template<const char * EndpointType, typename FunctionT>
+static std::vector<rclcpp::TopicEndpointInfo>
+get_info_by_topic(
+  rclcpp::node_interfaces::NodeBaseInterface * node_base,
+  const std::string & topic_name,
+  bool no_mangle,
+  FunctionT rcl_get_info_by_topic)
+{
+  std::string fqdn;
+  auto rcl_node_handle = node_base->get_rcl_node_handle();
+
+  if (no_mangle) {
+    fqdn = topic_name;
+  } else {
+    fqdn = rclcpp::expand_topic_or_service_name(
+      topic_name,
+      rcl_node_get_name(rcl_node_handle),
+      rcl_node_get_namespace(rcl_node_handle),
+      false);    // false = not a service
+
+    // Get the node options
+    const rcl_node_options_t * node_options = rcl_node_get_options(rcl_node_handle);
+    if (nullptr == node_options) {
+      throw std::runtime_error("Need valid node options in get_info_by_topic()");
+    }
+    const rcl_arguments_t * global_args = nullptr;
+    if (node_options->use_global_arguments) {
+      global_args = &(rcl_node_handle->context->global_arguments);
+    }
+
+    char * remapped_topic_name = nullptr;
+    rcl_ret_t ret = rcl_remap_topic_name(
+      &(node_options->arguments),
+      global_args,
+      fqdn.c_str(),
+      rcl_node_get_name(rcl_node_handle),
+      rcl_node_get_namespace(rcl_node_handle),
+      node_options->allocator,
+      &remapped_topic_name);
+    if (RCL_RET_OK != ret) {
+      throw_from_rcl_error(ret, std::string("Failed to remap topic name ") + fqdn);
+    } else if (nullptr != remapped_topic_name) {
+      fqdn = remapped_topic_name;
+      node_options->allocator.deallocate(remapped_topic_name, node_options->allocator.state);
+    }
+  }
+
+  rcutils_allocator_t allocator = rcutils_get_default_allocator();
+  rcl_topic_endpoint_info_array_t info_array = rcl_get_zero_initialized_topic_endpoint_info_array();
+  rcl_ret_t ret =
+    rcl_get_info_by_topic(rcl_node_handle, &allocator, fqdn.c_str(), no_mangle, &info_array);
+  if (RCL_RET_OK != ret) {
+    auto error_msg =
+      std::string("Failed to get information by topic for ") + EndpointType + std::string(":");
+    if (RCL_RET_UNSUPPORTED == ret) {
+      error_msg += std::string("function not supported by RMW_IMPLEMENTATION");
+    } else {
+      error_msg += rcl_get_error_string().str;
+    }
+    rcl_reset_error();
+    if (RCL_RET_OK != rcl_topic_endpoint_info_array_fini(&info_array, &allocator)) {
+      error_msg += std::string(", failed also to cleanup topic info array, leaking memory: ") +
+        rcl_get_error_string().str;
+      rcl_reset_error();
+    }
+    throw_from_rcl_error(ret, error_msg);
+  }
+
+  std::vector<rclcpp::TopicEndpointInfo> topic_info_list = convert_to_topic_info_list(info_array);
+  ret = rcl_topic_endpoint_info_array_fini(&info_array, &allocator);
+  if (RCL_RET_OK != ret) {
+    throw_from_rcl_error(ret, "rcl_topic_info_array_fini failed.");
+  }
+
+  return topic_info_list;
+}
+
+static constexpr char kPublisherEndpointTypeName[] = "publishers";
+std::vector<rclcpp::TopicEndpointInfo>
+NodeGraph::get_publishers_info_by_topic(
+  const std::string & topic_name,
+  bool no_mangle) const
+{
+  return get_info_by_topic<kPublisherEndpointTypeName>(
+    node_base_,
+    topic_name,
+    no_mangle,
+    rcl_get_publishers_info_by_topic);
+}
+
+static constexpr char kSubscriptionEndpointTypeName[] = "subscriptions";
+std::vector<rclcpp::TopicEndpointInfo>
+NodeGraph::get_subscriptions_info_by_topic(
+  const std::string & topic_name,
+  bool no_mangle) const
+{
+  return get_info_by_topic<kSubscriptionEndpointTypeName>(
+    node_base_,
+    topic_name,
+    no_mangle,
+    rcl_get_subscriptions_info_by_topic);
+}
+
+std::string &
+rclcpp::TopicEndpointInfo::node_name()
+{
+  return node_name_;
+}
+
+const std::string &
+rclcpp::TopicEndpointInfo::node_name() const
+{
+  return node_name_;
+}
+
+std::string &
+rclcpp::TopicEndpointInfo::node_namespace()
+{
+  return node_namespace_;
+}
+
+const std::string &
+rclcpp::TopicEndpointInfo::node_namespace() const
+{
+  return node_namespace_;
+}
+
+std::string &
+rclcpp::TopicEndpointInfo::topic_type()
+{
+  return topic_type_;
+}
+
+const std::string &
+rclcpp::TopicEndpointInfo::topic_type() const
+{
+  return topic_type_;
+}
+
+rclcpp::EndpointType &
+rclcpp::TopicEndpointInfo::endpoint_type()
+{
+  return endpoint_type_;
+}
+
+const rclcpp::EndpointType &
+rclcpp::TopicEndpointInfo::endpoint_type() const
+{
+  return endpoint_type_;
+}
+
+std::array<uint8_t, RMW_GID_STORAGE_SIZE> &
+rclcpp::TopicEndpointInfo::endpoint_gid()
+{
+  return endpoint_gid_;
+}
+
+const std::array<uint8_t, RMW_GID_STORAGE_SIZE> &
+rclcpp::TopicEndpointInfo::endpoint_gid() const
+{
+  return endpoint_gid_;
+}
+
+rclcpp::QoS &
+rclcpp::TopicEndpointInfo::qos_profile()
+{
+  return qos_profile_;
+}
+
+const rclcpp::QoS &
+rclcpp::TopicEndpointInfo::qos_profile() const
+{
+  return qos_profile_;
+}

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -69,7 +69,7 @@ NodeParameters::NodeParameters(
   if (start_parameter_event_publisher) {
     events_publisher_ = rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
       node_topics,
-      "parameter_events",
+      "/parameter_events",
       parameter_event_qos,
       publisher_options);
   }
@@ -402,6 +402,8 @@ NodeParameters::declare_parameter(
 
   // Publish if events_publisher_ is not nullptr, which may be if disabled in the constructor.
   if (nullptr != events_publisher_) {
+    parameter_event.node = combined_name_;
+    parameter_event.stamp = node_clock_->get_clock()->now();
     events_publisher_->publish(parameter_event);
   }
 

rclcpp/src/rclcpp/node_interfaces/node_topics.cpp

@@ -99,10 +99,12 @@ NodeTopics::add_subscription(
     callback_group->add_waitable(subscription_event);
   }
 
-  auto intra_process_waitable = subscription->get_intra_process_waitable();
-  if (nullptr != intra_process_waitable) {
-    // Add to the callback group to be notified about intra-process msgs.
-    callback_group->add_waitable(intra_process_waitable);
+  const auto intra_process_waitables = subscription->get_intra_process_waitables();
+  for (auto & intra_process_waitable : intra_process_waitables) {
+    if (nullptr != intra_process_waitable) {
+      // Add to the callback group to be notified about intra-process msgs.
+      callback_group->add_waitable(intra_process_waitable);
+    }
   }
 
   // Notify the executor that a new subscription was created using the parent Node.

rclcpp/src/rclcpp/node_options.cpp

@@ -20,6 +20,7 @@
 #include <vector>
 #include <utility>
 
+#include "rclcpp/detail/utilities.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/logging.hpp"
 #include "rclcpp/publisher_options.hpp"
@@ -70,6 +71,7 @@ NodeOptions::operator=(const NodeOptions & other)
     this->arguments_ = other.arguments_;
     this->parameter_overrides_ = other.parameter_overrides_;
     this->use_global_arguments_ = other.use_global_arguments_;
+    this->enable_rosout_ = other.enable_rosout_;
     this->use_intra_process_comms_ = other.use_intra_process_comms_;
     this->start_parameter_services_ = other.start_parameter_services_;
     this->allocator_ = other.allocator_;
@@ -90,6 +92,7 @@ NodeOptions::get_rcl_node_options() const
     node_options_->allocator = this->allocator_;
     node_options_->use_global_arguments = this->use_global_arguments_;
     node_options_->domain_id = this->get_domain_id_from_env();
+    node_options_->enable_rosout = this->enable_rosout_;
 
     int c_argc = 0;
     std::unique_ptr<const char *[]> c_argv;
@@ -113,25 +116,10 @@ NodeOptions::get_rcl_node_options() const
       throw_from_rcl_error(ret, "failed to parse arguments");
     }
 
-    int unparsed_ros_args_count =
-      rcl_arguments_get_count_unparsed_ros(&(node_options_->arguments));
-    if (unparsed_ros_args_count > 0) {
-      int * unparsed_ros_args_indices = nullptr;
-      ret = rcl_arguments_get_unparsed_ros(
-        &(node_options_->arguments), this->allocator_, &unparsed_ros_args_indices);
-      if (RCL_RET_OK != ret) {
-        throw_from_rcl_error(ret, "failed to get unparsed ROS arguments");
-      }
-      try {
-        std::vector<std::string> unparsed_ros_args;
-        for (int i = 0; i < unparsed_ros_args_count; ++i) {
-          unparsed_ros_args.push_back(c_argv[unparsed_ros_args_indices[i]]);
-        }
-        throw exceptions::UnknownROSArgsError(std::move(unparsed_ros_args));
-      } catch (...) {
-        this->allocator_.deallocate(unparsed_ros_args_indices, this->allocator_.state);
-        throw;
-      }
+    std::vector<std::string> unparsed_ros_arguments = detail::get_unparsed_ros_arguments(
+      c_argc, c_argv.get(), &(node_options_->arguments), this->allocator_);
+    if (!unparsed_ros_arguments.empty()) {
+      throw exceptions::UnknownROSArgsError(std::move(unparsed_ros_arguments));
     }
   }
 
@@ -198,6 +186,20 @@ NodeOptions::use_global_arguments(bool use_global_arguments)
   return *this;
 }
 
+bool
+NodeOptions::enable_rosout() const
+{
+  return this->enable_rosout_;
+}
+
+NodeOptions &
+NodeOptions::enable_rosout(bool enable_rosout)
+{
+  this->node_options_.reset();  // reset node options to make it be recreated on next access.
+  this->enable_rosout_ = enable_rosout;
+  return *this;
+}
+
 bool
 NodeOptions::use_intra_process_comms() const
 {

rclcpp/src/rclcpp/parameter_client.cpp

@@ -152,7 +152,7 @@ AsyncParametersClient::get_parameters(
       auto & pvalues = cb_f.get()->values;
 
       for (auto & pvalue : pvalues) {
-        auto i = &pvalue - &pvalues[0];
+        auto i = static_cast<size_t>(&pvalue - &pvalues[0]);
         rcl_interfaces::msg::Parameter parameter;
         parameter.name = request->names[i];
         parameter.value = pvalue;
@@ -191,7 +191,7 @@ AsyncParametersClient::get_parameter_types(
       std::vector<rclcpp::ParameterType> types;
       auto & pts = cb_f.get()->types;
       for (auto & pt : pts) {
-        pts.push_back(static_cast<rclcpp::ParameterType>(pt));
+        types.push_back(static_cast<rclcpp::ParameterType>(pt));
       }
       promise_result->set_value(types);
       if (callback != nullptr) {

rclcpp/src/rclcpp/parameter_service.cpp

@@ -19,6 +19,8 @@
 #include <string>
 #include <vector>
 
+#include "rclcpp/logging.hpp"
+
 #include "./parameter_service_names.hpp"
 
 using rclcpp::ParameterService;