add parameter helpers (redo of #233 ) (#237 )

* add parameter helpers * respond to comments * remove unnecessary indent comments * replace temp variable assignment with explicit constructor invocation
use the param profile for param services (#236 )
2016-07-08 15:45:05 -07:00 · 2016-07-08 13:39:18 -07:00 · 2016-06-28 20:02:25 -07:00 · 2016-06-28 16:20:59 -07:00 · 2016-06-26 17:15:05 -07:00 · 2016-06-22 20:18:46 -07:00
41 changed files with 2420 additions and 678 deletions
--- a/rclcpp/CMakeLists.txt
+++ b/rclcpp/CMakeLists.txt
@@ -1,8 +1,9 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)

 project(rclcpp)

 find_package(ament_cmake REQUIRED)
+find_package(rcl REQUIRED)
 find_package(rcl_interfaces REQUIRED)
 find_package(rmw REQUIRED)
 find_package(rmw_implementation REQUIRED)
@@ -21,10 +22,13 @@ set(${PROJECT_NAME}_SRCS
  src/rclcpp/client.cpp
  src/rclcpp/context.cpp
  src/rclcpp/contexts/default_context.cpp
+  src/rclcpp/event.cpp
+  src/rclcpp/exceptions.cpp
  src/rclcpp/executor.cpp
  src/rclcpp/executors.cpp
  src/rclcpp/executors/multi_threaded_executor.cpp
  src/rclcpp/executors/single_threaded_executor.cpp
+  src/rclcpp/graph_listener.cpp
  src/rclcpp/intra_process_manager.cpp
  src/rclcpp/intra_process_manager_impl.cpp
  src/rclcpp/memory_strategies.cpp
@@ -42,13 +46,14 @@ set(${PROJECT_NAME}_SRCS
 )

 macro(target)
+  if(NOT target_suffix STREQUAL "")
+    get_rcl_information("${rmw_implementation}" "rcl${target_suffix}")
+  endif()
  add_library(${PROJECT_NAME}${target_suffix} SHARED
    ${${PROJECT_NAME}_SRCS})
  ament_target_dependencies(${PROJECT_NAME}${target_suffix}
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_generator_cpp"
-    "${rmw_implementation}")
+    "rcl${target_suffix}"
+    "rosidl_generator_cpp")

  # Causes the visibility macros to use dllexport rather than dllimport,
  # which is appropriate when building the dll but not consuming it.
@@ -66,16 +71,14 @@ endmacro()
 call_for_each_rmw_implementation(target GENERATE_DEFAULT)

 ament_export_dependencies(ament_cmake)
-ament_export_dependencies(rcl_interfaces)
-ament_export_dependencies(rmw)
-ament_export_dependencies(rmw_implementation)
+ament_export_dependencies(rcl)
 ament_export_dependencies(rosidl_generator_cpp)

 ament_export_include_directories(include)

 ament_export_libraries(${PROJECT_NAME})

-if(AMENT_ENABLE_TESTING)
+if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()

@@ -90,6 +93,7 @@ if(AMENT_ENABLE_TESTING)
  ament_add_gtest(test_mapped_ring_buffer test/test_mapped_ring_buffer.cpp)
  if(TARGET test_mapped_ring_buffer)
    target_include_directories(test_mapped_ring_buffer PUBLIC
+      ${rcl_INCLUDE_DIRS}
      ${rcl_interfaces_INCLUDE_DIRS}
      ${rmw_INCLUDE_DIRS}
      ${rosidl_generator_cpp_INCLUDE_DIRS}
@@ -98,12 +102,14 @@ if(AMENT_ENABLE_TESTING)
  ament_add_gtest(test_intra_process_manager test/test_intra_process_manager.cpp)
  if(TARGET test_intra_process_manager)
    target_include_directories(test_intra_process_manager PUBLIC
+      ${rcl_INCLUDE_DIRS}
      ${rcl_interfaces_INCLUDE_DIRS}
      ${rmw_INCLUDE_DIRS}
      ${rosidl_generator_cpp_INCLUDE_DIRS}
    )
  endif()
-  ament_add_gtest(test_rate test/test_rate.cpp)
+  ament_add_gtest(test_rate test/test_rate.cpp
+    ENV RCL_ASSERT_RMW_ID_MATCHES=${rmw_implementation})
  if(TARGET test_rate)
    target_include_directories(test_rate PUBLIC
      ${rcl_interfaces_INCLUDE_DIRS}
@@ -111,7 +117,7 @@ if(AMENT_ENABLE_TESTING)
      ${rosidl_generator_cpp_INCLUDE_DIRS}
    )
    target_link_libraries(test_rate
-      ${PROJECT_NAME}${target_suffix}
+      ${PROJECT_NAME}
    )
  endif()
 endif()
--- a/rclcpp/cmake/get_rclcpp_information.cmake
+++ b/rclcpp/cmake/get_rclcpp_information.cmake
@@ -28,6 +28,11 @@ macro(get_rclcpp_information rmw_implementation var_prefix)
  # so that the variables can be used by various functions / macros
  set(${var_prefix}_FOUND TRUE)

+  # Get rcl using the existing macro
+  if(NOT target_suffix STREQUAL "")
+    get_rcl_information("${rmw_implementation}" "rcl${target_suffix}")
+  endif()
+
  # include directories
  normalize_path(${var_prefix}_INCLUDE_DIRS
    "${rclcpp_DIR}/../../../include")
@@ -63,8 +68,7 @@ macro(get_rclcpp_information rmw_implementation var_prefix)
  # dependencies
  set(_exported_dependencies
    "rcl_interfaces"
-    "rmw"
-    "${rmw_implementation}"
+    "rcl${target_suffix}"
    "rosidl_generator_cpp")
  set(${var_prefix}_DEFINITIONS)
  foreach(_dep ${_exported_dependencies})
--- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp
+++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp
@@ -17,6 +17,8 @@

 #include <memory>

+#include "rcl/allocator.h"
+
 #include "rclcpp/allocator/allocator_deleter.hpp"

 namespace rclcpp
@@ -27,6 +29,64 @@ namespace allocator
 template<typename T, typename Alloc>
 using AllocRebind = typename std::allocator_traits<Alloc>::template rebind_traits<T>;

+template<typename Alloc>
+void * retyped_allocate(size_t size, void * untyped_allocator)
+{
+  auto typed_allocator = static_cast<Alloc *>(untyped_allocator);
+  if (!typed_allocator) {
+    throw std::runtime_error("Received incorrect allocator type");
+  }
+  return std::allocator_traits<Alloc>::allocate(*typed_allocator, size);
+}
+
+template<typename T, typename Alloc>
+void retyped_deallocate(void * untyped_pointer, void * untyped_allocator)
+{
+  auto typed_allocator = static_cast<Alloc *>(untyped_allocator);
+  if (!typed_allocator) {
+    throw std::runtime_error("Received incorrect allocator type");
+  }
+  auto typed_ptr = static_cast<T *>(untyped_pointer);
+  std::allocator_traits<Alloc>::deallocate(*typed_allocator, typed_ptr, 1);
+}
+
+template<typename T, typename Alloc>
+void * retyped_reallocate(void * untyped_pointer, size_t size, void * untyped_allocator)
+{
+  auto typed_allocator = static_cast<Alloc *>(untyped_allocator);
+  if (!typed_allocator) {
+    throw std::runtime_error("Received incorrect allocator type");
+  }
+  auto typed_ptr = static_cast<T *>(untyped_pointer);
+  std::allocator_traits<Alloc>::deallocate(*typed_allocator, typed_ptr, 1);
+  return std::allocator_traits<Alloc>::allocate(*typed_allocator, size);
+}
+
+
+// Convert a std::allocator_traits-formatted Allocator into an rcl allocator
+template<typename T, typename Alloc,
+typename std::enable_if<!std::is_same<Alloc, std::allocator<void>>::value>::type * = nullptr>
+rcl_allocator_t get_rcl_allocator(Alloc & allocator)
+{
+  rcl_allocator_t rcl_allocator = rcl_get_default_allocator();
+#ifndef _WIN32
+  rcl_allocator.allocate = &retyped_allocate<Alloc>;
+  rcl_allocator.deallocate = &retyped_deallocate<T, Alloc>;
+  rcl_allocator.reallocate = &retyped_reallocate<T, Alloc>;
+  rcl_allocator.state = &allocator;
+#endif
+  return rcl_allocator;
+}
+
+// TODO(jacquelinekay) Workaround for an incomplete implementation of std::allocator<void>
+template<typename T, typename Alloc,
+typename std::enable_if<std::is_same<Alloc, std::allocator<void>>::value>::type * = nullptr>
+rcl_allocator_t get_rcl_allocator(Alloc & allocator)
+{
+  (void)allocator;
+  return rcl_get_default_allocator();
+}
+
 }  // namespace allocator
 }  // namespace rclcpp

--- a/rclcpp/include/rclcpp/callback_group.hpp
+++ b/rclcpp/include/rclcpp/callback_group.hpp
@@ -16,6 +16,7 @@
 #define RCLCPP__CALLBACK_GROUP_HPP_

 #include <atomic>
+#include <mutex>
 #include <string>
 #include <vector>

@@ -97,6 +98,8 @@ private:
  add_client(const rclcpp::client::ClientBase::SharedPtr client_ptr);

  CallbackGroupType type_;
+  // Mutex to protect the subsequent vectors of pointers.
+  mutable std::mutex mutex_;
  std::vector<rclcpp::subscription::SubscriptionBase::WeakPtr> subscription_ptrs_;
  std::vector<rclcpp::timer::TimerBase::WeakPtr> timer_ptrs_;
  std::vector<rclcpp::service::ServiceBase::SharedPtr> service_ptrs_;
--- a/rclcpp/include/rclcpp/client.hpp
+++ b/rclcpp/include/rclcpp/client.hpp
@@ -23,15 +23,27 @@
 #include <tuple>
 #include <utility>

+#include "rcl/client.h"
+#include "rcl/error_handling.h"
+#include "rcl/wait.h"
+
 #include "rclcpp/function_traits.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
+
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"

 namespace rclcpp
 {
+
+namespace node
+{
+class Node;
+}  // namespace node
+
 namespace client
 {

@@ -42,8 +54,7 @@ public:

  RCLCPP_PUBLIC
  ClientBase(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_client_t * client_handle,
+    std::shared_ptr<rclcpp::node::Node> parent_node,
    const std::string & service_name);

  RCLCPP_PUBLIC
@@ -54,20 +65,43 @@ public:
  get_service_name() const;

  RCLCPP_PUBLIC
-  const rmw_client_t *
+  const rcl_client_t *
  get_client_handle() const;

-  virtual std::shared_ptr<void> create_response() = 0;
-  virtual std::shared_ptr<void> create_request_header() = 0;
-  virtual void handle_response(
-    std::shared_ptr<void> request_header, std::shared_ptr<void> response) = 0;
+  RCLCPP_PUBLIC
+  bool
+  service_is_ready() const;

-private:
+  template<typename RatioT = std::milli>
+  bool
+  wait_for_service(
+    std::chrono::duration<int64_t, RatioT> timeout = std::chrono::duration<int64_t, RatioT>(-1))
+  {
+    return wait_for_service_nanoseconds(
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
+  }
+
+  virtual std::shared_ptr<void> create_response() = 0;
+  virtual std::shared_ptr<rmw_request_id_t> create_request_header() = 0;
+  virtual void handle_response(
+    std::shared_ptr<rmw_request_id_t> request_header, std::shared_ptr<void> response) = 0;
+
+protected:
  RCLCPP_DISABLE_COPY(ClientBase);

-  std::shared_ptr<rmw_node_t> node_handle_;
+  RCLCPP_PUBLIC
+  bool
+  wait_for_service_nanoseconds(std::chrono::nanoseconds timeout);

-  rmw_client_t * client_handle_;
+  RCLCPP_PUBLIC
+  rcl_node_t *
+  get_rcl_node_handle() const;
+
+  std::weak_ptr<rclcpp::node::Node> node_;
+  std::shared_ptr<rcl_node_t> node_handle_;
+
+  rcl_client_t client_handle_ = rcl_get_zero_initialized_client();
  std::string service_name_;
 };

@@ -93,30 +127,54 @@ public:
  RCLCPP_SMART_PTR_DEFINITIONS(Client);

  Client(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_client_t * client_handle,
-    const std::string & service_name)
-  : ClientBase(node_handle, client_handle, service_name)
-  {}
+    std::shared_ptr<rclcpp::node::Node> parent_node,
+    const std::string & service_name,
+    rcl_client_options_t & client_options)
+  : ClientBase(parent_node, service_name)
+  {
+    using rosidl_generator_cpp::get_service_type_support_handle;
+    auto service_type_support_handle =
+      get_service_type_support_handle<ServiceT>();
+    if (rcl_client_init(&client_handle_, this->get_rcl_node_handle(),
+      service_type_support_handle, service_name.c_str(), &client_options) != RCL_RET_OK)
+    {
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
+      throw std::runtime_error(
+        std::string("could not create client: ") +
+        rcl_get_error_string_safe());
+      // *INDENT-ON*
+    }
+  }

-  std::shared_ptr<void> create_response()
+  virtual ~Client()
+  {
+    if (rcl_client_fini(&client_handle_, this->get_rcl_node_handle()) != RCL_RET_OK) {
+      fprintf(stderr,
+        "Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe());
+    }
+  }
+
+  std::shared_ptr<void>
+  create_response()
  {
    return std::shared_ptr<void>(new typename ServiceT::Response());
  }

-  std::shared_ptr<void> create_request_header()
+  std::shared_ptr<rmw_request_id_t>
+  create_request_header()
  {
    // TODO(wjwwood): This should probably use rmw_request_id's allocator.
    //                (since it is a C type)
-    return std::shared_ptr<void>(new rmw_request_id_t);
+    return std::shared_ptr<rmw_request_id_t>(new rmw_request_id_t);
  }

-  void handle_response(std::shared_ptr<void> request_header, std::shared_ptr<void> response)
+  void
+  handle_response(std::shared_ptr<rmw_request_id_t> request_header,
+    std::shared_ptr<void> response)
  {
    std::lock_guard<std::mutex> lock(pending_requests_mutex_);
-    auto typed_request_header = std::static_pointer_cast<rmw_request_id_t>(request_header);
    auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(response);
-    int64_t sequence_number = typed_request_header->sequence_number;
+    int64_t sequence_number = request_header->sequence_number;
    // TODO(esteve) this should throw instead since it is not expected to happen in the first place
    if (this->pending_requests_.count(sequence_number) == 0) {
      fprintf(stderr, "Received invalid sequence number. Ignoring...\n");
@@ -131,7 +189,8 @@ public:
    callback(future);
  }

-  SharedFuture async_send_request(SharedRequest request)
+  SharedFuture
+  async_send_request(SharedRequest request)
  {
    return async_send_request(request, [](SharedFuture) {});
  }
@@ -145,14 +204,15 @@ public:
      >::value
    >::type * = nullptr
  >
-  SharedFuture async_send_request(SharedRequest request, CallbackT && cb)
+  SharedFuture
+  async_send_request(SharedRequest request, CallbackT && cb)
  {
    std::lock_guard<std::mutex> lock(pending_requests_mutex_);
    int64_t sequence_number;
-    if (RMW_RET_OK != rmw_send_request(get_client_handle(), request.get(), &sequence_number)) {
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+    if (RCL_RET_OK != rcl_send_request(get_client_handle(), request.get(), &sequence_number)) {
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
      throw std::runtime_error(
-        std::string("failed to send request: ") + rmw_get_error_string_safe());
+        std::string("failed to send request: ") + rcl_get_error_string_safe());
      // *INDENT-ON*
    }

@@ -172,7 +232,8 @@ public:
      >::value
    >::type * = nullptr
  >
-  SharedFutureWithRequest async_send_request(SharedRequest request, CallbackT && cb)
+  SharedFutureWithRequest
+  async_send_request(SharedRequest request, CallbackT && cb)
  {
    SharedPromiseWithRequest promise = std::make_shared<PromiseWithRequest>();
    SharedFutureWithRequest future_with_request(promise->get_future());
--- a/rclcpp/include/rclcpp/context.hpp
+++ b/rclcpp/include/rclcpp/context.hpp
@@ -51,7 +51,7 @@ public:
    auto it = sub_contexts_.find(type_i);
    if (it == sub_contexts_.end()) {
      // It doesn't exist yet, make it
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
      sub_context = std::shared_ptr<SubContext>(
        new SubContext(std::forward<Args>(args) ...),
        [] (SubContext * sub_context_ptr) {
--- a/rclcpp/include/rclcpp/event.hpp
+++ b/rclcpp/include/rclcpp/event.hpp
@@ -0,0 +1,58 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__EVENT_HPP_
+#define RCLCPP__EVENT_HPP_
+
+#include <atomic>
+#include <memory>
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace event
+{
+
+class Event
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Event);
+
+  RCLCPP_PUBLIC
+  Event();
+
+  RCLCPP_PUBLIC
+  bool
+  set();
+
+  RCLCPP_PUBLIC
+  bool
+  check();
+
+  RCLCPP_PUBLIC
+  bool
+  check_and_clear();
+
+private:
+  RCLCPP_DISABLE_COPY(Event);
+
+  std::atomic_bool state_;
+};
+
+}  // namespace event
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EVENT_HPP_
--- a/rclcpp/include/rclcpp/exceptions.hpp
+++ b/rclcpp/include/rclcpp/exceptions.hpp
@@ -0,0 +1,101 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__EXCEPTIONS_HPP_
+#define RCLCPP__EXCEPTIONS_HPP_
+
+#include <stdexcept>
+#include <string>
+
+#include "rcl/error_handling.h"
+#include "rcl/types.h"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace exceptions
+{
+
+/// Thrown when a method is trying to use a node, but it is invalid.
+class InvalidNodeError : public std::runtime_error
+{
+public:
+  InvalidNodeError()
+  : std::runtime_error("node is invalid") {}
+};
+
+/// Throw a C++ std::exception which was created based on an rcl error.
+/*
+ * \param ret the return code for the current error state
+ * \param prefix string to prefix to the error if applicable (not all errors have custom messages)
+ * \param reset_error if true rcl_reset_error() is called before returning
+ * \throws std::invalid_argument if ret is RCL_RET_OK
+ * \throws std::runtime_error if the rcl_get_error_state returns 0
+ * \throws RCLErrorBase some child class exception based on ret
+ */
+RCLCPP_PUBLIC
+void
+throw_from_rcl_error(rcl_ret_t ret, const std::string & prefix = "", bool reset_error = true);
+
+class RCLErrorBase
+{
+public:
+  RCLCPP_PUBLIC
+  RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state);
+  virtual ~RCLErrorBase() {}
+
+  rcl_ret_t ret;
+  std::string message;
+  std::string file;
+  size_t line;
+  std::string formatted_message;
+};
+
+/// Created when the return code does not match one of the other specialized exceptions.
+class RCLError : public RCLErrorBase, public std::runtime_error
+{
+public:
+  RCLCPP_PUBLIC
+  RCLError(rcl_ret_t ret, const rcl_error_state_t * error_state, const std::string & prefix);
+  RCLCPP_PUBLIC
+  RCLError(const RCLErrorBase & base_exc, const std::string & prefix);
+};
+
+/// Created when the ret is RCL_RET_BAD_ALLOC.
+class RCLBadAlloc : public RCLErrorBase, public std::bad_alloc
+{
+public:
+  RCLCPP_PUBLIC
+  RCLBadAlloc(rcl_ret_t ret, const rcl_error_state_t * error_state);
+  RCLCPP_PUBLIC
+  explicit RCLBadAlloc(const RCLErrorBase & base_exc);
+};
+
+/// Created when the ret is RCL_RET_INVALID_ARGUMENT.
+class RCLInvalidArgument : public RCLErrorBase, public std::invalid_argument
+{
+public:
+  RCLCPP_PUBLIC
+  RCLInvalidArgument(
+    rcl_ret_t ret,
+    const rcl_error_state_t * error_state,
+    const std::string & prefix);
+  RCLCPP_PUBLIC
+  RCLInvalidArgument(const RCLErrorBase & base_exc, const std::string & prefix);
+};
+
+}  // namespace exceptions
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXCEPTIONS_HPP_
--- a/rclcpp/include/rclcpp/executor.hpp
+++ b/rclcpp/include/rclcpp/executor.hpp
@@ -17,12 +17,17 @@

 #include <algorithm>
 #include <cassert>
+#include <chrono>
 #include <cstdlib>
 #include <iostream>
 #include <list>
 #include <memory>
+#include <string>
 #include <vector>

+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
+
 #include "rclcpp/any_executable.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/memory_strategies.hpp"
@@ -42,7 +47,15 @@ namespace executor
 * INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error.
 * TIMEOUT: Spinning timed out.
 */
-enum FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT};
+enum class FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT};
+
+RCLCPP_PUBLIC
+std::ostream &
+operator<<(std::ostream & os, const FutureReturnCode & future_return_code);
+
+RCLCPP_PUBLIC
+std::string
+to_string(const FutureReturnCode & future_return_code);

 ///
 /**
@@ -159,7 +172,8 @@ public:
  /**
   * \param[in] executor The executor which will spin the node.
   * \param[in] node_ptr The node to spin.
-   * \param[in] future The future to wait on. If SUCCESS, the future is safe to access after this function
+   * \param[in] future The future to wait on. If SUCCESS, the future is safe to access after this
+     function.
   * \param[in] timeout Optional timeout parameter, which gets passed to Executor::spin_node_once.
     -1 is block forever, 0 is non-blocking.
     If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return code.
@@ -177,23 +191,40 @@ public:
    // Check the future before entering the while loop.
    // If the future is already complete, don't try to spin.
    std::future_status status = future.wait_for(std::chrono::seconds(0));
-
-    auto start_time = std::chrono::system_clock::now();
-
-    while (status != std::future_status::ready && rclcpp::utilities::ok()) {
-      spin_once(timeout);
-      if (timeout.count() >= 0) {
-        if (start_time + timeout < std::chrono::system_clock::now()) {
-          return TIMEOUT;
-        }
-      }
-      status = future.wait_for(std::chrono::seconds(0));
-    }
-
-    // If the future completed, and we weren't interrupted by ctrl-C, return the response
    if (status == std::future_status::ready) {
      return 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;
+
+    while (rclcpp::utilities::ok()) {
+      // Do one item of work.
+      spin_once(timeout_left);
+      // 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 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 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 FutureReturnCode::INTERRUPTED;
  }

@@ -264,10 +295,6 @@ protected:
  void
  get_next_timer(AnyExecutable::SharedPtr any_exec);

-  RCLCPP_PUBLIC
-  std::chrono::nanoseconds
-  get_earliest_timer();
-
  RCLCPP_PUBLIC
  AnyExecutable::SharedPtr
  get_next_ready_executable();
@@ -279,13 +306,11 @@ protected:
  /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins.
  std::atomic_bool spinning;

-  rmw_guard_conditions_t fixed_guard_conditions_;
-
  /// Guard condition for signaling the rmw layer to wake up for special events.
-  rmw_guard_condition_t * interrupt_guard_condition_;
+  rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();

  /// Waitset for managing entities that the rmw layer waits on.
-  rmw_waitset_t * waitset_;
+  rcl_wait_set_t waitset_ = rcl_get_zero_initialized_wait_set();

  /// The memory strategy: an interface for handling user-defined memory allocation strategies.
  memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;
@@ -294,7 +319,6 @@ private:
  RCLCPP_DISABLE_COPY(Executor);

  std::vector<std::weak_ptr<rclcpp::node::Node>> weak_nodes_;
-  std::array<void *, 2> guard_cond_handles_;
 };

 }  // namespace executor
--- a/rclcpp/include/rclcpp/graph_listener.hpp
+++ b/rclcpp/include/rclcpp/graph_listener.hpp
@@ -0,0 +1,174 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__GRAPH_LISTENER_HPP_
+#define RCLCPP__GRAPH_LISTENER_HPP_
+
+#include <atomic>
+#include <memory>
+#include <mutex>
+#include <thread>
+#include <vector>
+
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+namespace node
+{
+class Node;
+}  // namespace node
+
+namespace graph_listener
+{
+
+/// Thrown when a function is called on a GraphListener that is already shutdown.
+class GraphListenerShutdownError : public std::runtime_error
+{
+public:
+  GraphListenerShutdownError()
+  : std::runtime_error("GraphListener already shutdown") {}
+};
+
+/// Thrown when a node has already been added to the GraphListener.
+class NodeAlreadyAddedError : public std::runtime_error
+{
+public:
+  NodeAlreadyAddedError()
+  : std::runtime_error("node already added") {}
+};
+
+/// Thrown when the given node is not in the GraphListener.
+class NodeNotFoundError : public std::runtime_error
+{
+public:
+  NodeNotFoundError()
+  : std::runtime_error("node not found") {}
+};
+
+/// Notifies many nodes of graph changes by listening in a thread.
+class GraphListener : public std::enable_shared_from_this<GraphListener>
+{
+public:
+  RCLCPP_PUBLIC
+  GraphListener();
+
+  RCLCPP_PUBLIC
+  virtual ~GraphListener();
+
+  /// Start the graph listener's listen thread if it hasn't been started.
+  /* This function is thread-safe.
+   *
+   * \throws GraphListenerShutdownError if the GraphListener is shutdown
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  start_if_not_started();
+
+  /// Add a node to the graph listener's list of nodes.
+  /*
+   * \throws GraphListenerShutdownError if the GraphListener is shutdown
+   * \throws NodeAlreadyAddedError if the given node is already in the list
+   * \throws std::invalid_argument if node is nullptr
+   * \throws std::system_error anything std::mutex::lock() throws
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_node(rclcpp::node::Node * node);
+
+  /// Return true if the given node is in the graph listener's list of nodes.
+  /* Also return false if given nullptr.
+   *
+   * \throws std::system_error anything std::mutex::lock() throws
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  has_node(rclcpp::node::Node * node);
+
+  /// Remove a node from the graph listener's list of nodes.
+  /*
+   * \throws NodeNotFoundError if the given node is not in the list
+   * \throws std::invalid_argument if node is nullptr
+   * \throws std::system_error anything std::mutex::lock() throws
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  remove_node(rclcpp::node::Node * node);
+
+  /// Stop the listening thread.
+  /* The thread cannot be restarted, and the class is defunct after calling.
+   * This function is called by the ~GraphListener() and does nothing if
+   * shutdown() was already called.
+   * This function exists separately from the ~GraphListener() so that it can
+   * be called before and exceptions can be caught.
+   *
+   * If start_if_not_started() was never called, this function still succeeds,
+   * but start_if_not_started() still cannot be called after this function.
+   *
+   * \throws rclcpp::execptions::RCLError from rcl_guard_condition_fini()
+   * \throws rclcpp::execptions::RCLError from rcl_wait_set_fini()
+   * \throws std::system_error anything std::mutex::lock() throws
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  shutdown();
+
+  /// Return true if shutdown() has been called, else false.
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  is_shutdown();
+
+protected:
+  /// Main function for the listening thread.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  run();
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  run_loop();
+
+private:
+  RCLCPP_DISABLE_COPY(GraphListener);
+
+  std::thread listener_thread_;
+  bool is_started_;
+  std::atomic_bool is_shutdown_;
+  mutable std::mutex shutdown_mutex_;
+
+  mutable std::mutex nodes_barrier_mutex_;
+  mutable std::mutex nodes_mutex_;
+  std::vector<rclcpp::node::Node *> nodes_;
+
+  rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
+  rcl_guard_condition_t * shutdown_guard_condition_;
+  rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
+};
+
+}  // namespace graph_listener
+}  // namespace rclcpp
+
+#endif  // RCLCPP__GRAPH_LISTENER_HPP_
--- a/rclcpp/include/rclcpp/mapped_ring_buffer.hpp
+++ b/rclcpp/include/rclcpp/mapped_ring_buffer.hpp
@@ -220,7 +220,7 @@ private:
  typename std::vector<element, VectorAlloc>::iterator
  get_iterator_of_key(uint64_t key)
  {
-    // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+    // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
    auto it = std::find_if(elements_.begin(), elements_.end(), [key](element & e) -> bool {
      return e.key == key && e.in_use;
    });
--- a/rclcpp/include/rclcpp/memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/memory_strategy.hpp
@@ -18,6 +18,9 @@
 #include <memory>
 #include <vector>

+#include "rcl/allocator.h"
+#include "rcl/wait.h"
+
 #include "rclcpp/any_executable.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node.hpp"
@@ -40,25 +43,26 @@ public:
  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy);
  using WeakNodeVector = std::vector<std::weak_ptr<rclcpp::node::Node>>;

-  // return the new number of subscribers
-  virtual size_t fill_subscriber_handles(void ** & ptr) = 0;
-
-  // return the new number of services
-  virtual size_t fill_service_handles(void ** & ptr) = 0;
-
-  // return the new number of clients
-  virtual size_t fill_client_handles(void ** & ptr) = 0;
-
-  virtual void clear_active_entities() = 0;
-
-  virtual void clear_handles() = 0;
-  virtual void remove_null_handles() = 0;
  virtual bool collect_entities(const WeakNodeVector & weak_nodes) = 0;

+  virtual size_t number_of_ready_subscriptions() const = 0;
+  virtual size_t number_of_ready_services() const = 0;
+  virtual size_t number_of_ready_clients() const = 0;
+  virtual size_t number_of_ready_timers() const = 0;
+  virtual size_t number_of_guard_conditions() const = 0;
+
+  virtual bool add_handles_to_waitset(rcl_wait_set_t * wait_set) = 0;
+  virtual void clear_handles() = 0;
+  virtual void remove_null_handles(rcl_wait_set_t * wait_set) = 0;
+
  /// Provide a newly initialized AnyExecutable object.
  // \return Shared pointer to the fresh executable.
  virtual rclcpp::executor::AnyExecutable::SharedPtr instantiate_next_executable() = 0;

+  virtual void add_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
+
+  virtual void remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
+
  virtual void
  get_next_subscription(rclcpp::executor::AnyExecutable::SharedPtr any_exec,
    const WeakNodeVector & weak_nodes) = 0;
@@ -71,15 +75,18 @@ public:
  get_next_client(rclcpp::executor::AnyExecutable::SharedPtr any_exec,
    const WeakNodeVector & weak_nodes) = 0;

+  virtual rcl_allocator_t
+  get_allocator() = 0;
+
  static rclcpp::subscription::SubscriptionBase::SharedPtr
-  get_subscription_by_handle(void * subscriber_handle,
+  get_subscription_by_handle(const rcl_subscription_t * subscriber_handle,
    const WeakNodeVector & weak_nodes);

  static rclcpp::service::ServiceBase::SharedPtr
-  get_service_by_handle(void * service_handle, const WeakNodeVector & weak_nodes);
+  get_service_by_handle(const rcl_service_t * service_handle, const WeakNodeVector & weak_nodes);

  static rclcpp::client::ClientBase::SharedPtr
-  get_client_by_handle(void * client_handle, const WeakNodeVector & weak_nodes);
+  get_client_by_handle(const rcl_client_t * client_handle, const WeakNodeVector & weak_nodes);

  static rclcpp::node::Node::SharedPtr
  get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group,
--- a/rclcpp/include/rclcpp/node.hpp
+++ b/rclcpp/include/rclcpp/node.hpp
@@ -15,13 +15,19 @@
 #ifndef RCLCPP__NODE_HPP_
 #define RCLCPP__NODE_HPP_

+#include <atomic>
+#include <condition_variable>
 #include <list>
 #include <map>
 #include <memory>
+#include <mutex>
 #include <string>
 #include <tuple>
 #include <vector>

+#include "rcl/error_handling.h"
+#include "rcl/node.h"
+
 #include "rcl_interfaces/msg/list_parameters_result.hpp"
 #include "rcl_interfaces/msg/parameter_descriptor.hpp"
 #include "rcl_interfaces/msg/parameter_event.hpp"
@@ -30,6 +36,7 @@
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/client.hpp"
 #include "rclcpp/context.hpp"
+#include "rclcpp/event.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/message_memory_strategy.hpp"
 #include "rclcpp/parameter.hpp"
@@ -39,19 +46,38 @@
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"

-// Forward declaration of ROS middleware class
-namespace rmw
+namespace rcl
 {
-struct rmw_node_t;
-}  // namespace rmw
+struct rcl_node_t;
+}  // namespace rcl

 namespace rclcpp
 {

+namespace graph_listener
+{
+class GraphListener;
+}  // namespace graph_listener
+
 namespace node
 {
+
+class InvalidEventError : public std::runtime_error
+{
+public:
+  InvalidEventError()
+  : std::runtime_error("event is invalid") {}
+};
+
+class EventNotRegisteredError : public std::runtime_error
+{
+public:
+  EventNotRegisteredError()
+  : std::runtime_error("event already registered") {}
+};
+
 /// Node is the single point of entry for creating publishers and subscribers.
-class Node
+class Node : public std::enable_shared_from_this<Node>
 {
 public:
  RCLCPP_SMART_PTR_DEFINITIONS(Node);
@@ -77,6 +103,9 @@ public:
    const std::string & node_name, rclcpp::context::Context::SharedPtr context,
    bool use_intra_process_comms = false);

+  RCLCPP_PUBLIC
+  virtual ~Node();
+
  /// Get the name of the node.
  // \return The name of the node.
  RCLCPP_PUBLIC
@@ -252,6 +281,78 @@ public:
  const CallbackGroupWeakPtrList &
  get_callback_groups() const;

+  RCLCPP_PUBLIC
+  const rcl_guard_condition_t *
+  get_notify_guard_condition() const;
+
+  RCLCPP_PUBLIC
+  const rcl_guard_condition_t *
+  get_graph_guard_condition() const;
+
+  RCLCPP_PUBLIC
+  const rcl_node_t *
+  get_rcl_node_handle() const;
+
+  /// Return the rcl_node_t node handle (non-const version).
+  RCLCPP_PUBLIC
+  rcl_node_t *
+  get_rcl_node_handle();
+
+  /// Return the rcl_node_t node handle in a std::shared_ptr.
+  /* This handle remains valid after the Node is destroyed.
+   * The actual rcl node is not finalized until it is out of scope everywhere.
+   */
+  RCLCPP_PUBLIC
+  std::shared_ptr<rcl_node_t>
+  get_shared_node_handle();
+
+  /// Notify threads waiting on graph changes.
+  /* Affects threads waiting on the notify guard condition, see:
+   * get_notify_guard_condition(), as well as the threads waiting on graph
+   * changes using a graph Event, see: wait_for_graph_change().
+   *
+   * This is typically only used by the rclcpp::graph_listener::GraphListener.
+   *
+   * \throws RCLBaseError (a child of that exception) when an rcl error occurs
+   */
+  RCLCPP_PUBLIC
+  void
+  notify_graph_change();
+
+  /// Notify any and all blocking node actions that shutdown has occurred.
+  RCLCPP_PUBLIC
+  void
+  notify_shutdown();
+
+  /// 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 load just let it go
+   * out of scope.
+   */
+  RCLCPP_PUBLIC
+  rclcpp::event::Event::SharedPtr
+  get_graph_event();
+
+  /// Wait for a graph event to occur by waiting on an Event to become set.
+  /* The given Event must be acquire through the get_graph_event() method.
+   *
+   * \throws InvalidEventError if the given event is nullptr
+   * \throws EventNotRegisteredError if the given event was not acquired with
+   *   get_graph_event().
+   */
+  RCLCPP_PUBLIC
+  void
+  wait_for_graph_change(
+    rclcpp::event::Event::SharedPtr event,
+    std::chrono::nanoseconds timeout);
+
+  /// Return the number of on loan graph events, see get_graph_event().
+  /* This is typically only used by the rclcpp::graph_listener::GraphListener.
+   */
+  RCLCPP_PUBLIC
+  size_t
+  count_graph_users();
+
  std::atomic_bool has_executor;

 private:
@@ -263,7 +364,7 @@ private:

  std::string name_;

-  std::shared_ptr<rmw_node_t> node_handle_;
+  std::shared_ptr<rcl_node_t> node_handle_;

  rclcpp::context::Context::SharedPtr context_;

@@ -279,6 +380,26 @@ private:

  mutable std::mutex mutex_;

+  /// Guard condition for notifying the Executor of changes to this node.
+  mutable std::mutex notify_guard_condition_mutex_;
+  rcl_guard_condition_t notify_guard_condition_ = rcl_get_zero_initialized_guard_condition();
+  bool notify_guard_condition_is_valid_;
+
+  /// Graph Listener which waits on graph changes for the node and is shared across nodes.
+  std::shared_ptr<rclcpp::graph_listener::GraphListener> graph_listener_;
+  /// Whether or not this node needs to be added to the graph listener.
+  std::atomic_bool should_add_to_graph_listener_;
+
+  /// Mutex to guard the graph event related data structures.
+  std::mutex graph_mutex_;
+  /// For notifying waiting threads (wait_for_graph_change()) on changes (notify_graph_change()).
+  std::condition_variable graph_cv_;
+  /// Weak references to graph events out on loan.
+  std::vector<rclcpp::event::Event::WeakPtr> graph_events_;
+  /// Number of graph events out on loan, used to determine if the graph should be monitored.
+  /* graph_users_count_ is atomic so that it can be accessed without acquiring the graph_mutex_ */
+  std::atomic_size_t graph_users_count_;
+
  std::map<std::string, rclcpp::parameter::ParameterVariant> parameters_;

  publisher::Publisher<rcl_interfaces::msg::ParameterEvent>::SharedPtr events_publisher_;
--- a/rclcpp/include/rclcpp/node_impl.hpp
+++ b/rclcpp/include/rclcpp/node_impl.hpp
@@ -30,6 +30,9 @@
 #include <utility>
 #include <vector>

+#include "rcl/publisher.h"
+#include "rcl/subscription.h"
+
 #include "rcl_interfaces/msg/intra_process_message.hpp"

 #include "rclcpp/contexts/default_context.hpp"
@@ -70,33 +73,20 @@ Node::create_publisher(
  if (!allocator) {
    allocator = std::make_shared<Alloc>();
  }
-  using rosidl_generator_cpp::get_message_type_support_handle;
-  auto type_support_handle = get_message_type_support_handle<MessageT>();
-  rmw_publisher_t * publisher_handle = rmw_create_publisher(
-    node_handle_.get(), type_support_handle, topic_name.c_str(), &qos_profile);
-  if (!publisher_handle) {
-    // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-    throw std::runtime_error(
-      std::string("could not create publisher: ") +
-      rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
+
+  auto publisher_options = rcl_publisher_get_default_options();
+  publisher_options.qos = qos_profile;
+
+  auto message_alloc =
+    std::make_shared<typename publisher::Publisher<MessageT, Alloc>::MessageAlloc>(
+    *allocator.get());
+  publisher_options.allocator = allocator::get_rcl_allocator<MessageT>(
+    *message_alloc.get());

  auto publisher = publisher::Publisher<MessageT, Alloc>::make_shared(
-    node_handle_, publisher_handle, topic_name, qos_profile.depth, allocator);
+    node_handle_, topic_name, publisher_options, message_alloc);

  if (use_intra_process_comms_) {
-    rmw_publisher_t * intra_process_publisher_handle = rmw_create_publisher(
-      node_handle_.get(), rclcpp::type_support::get_intra_process_message_msg_type_support(),
-      (topic_name + "__intra").c_str(), &qos_profile);
-    if (!intra_process_publisher_handle) {
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-      throw std::runtime_error(
-        std::string("could not create intra process publisher: ") +
-        rmw_get_error_string_safe());
-      // *INDENT-ON*
-    }
-
    auto intra_process_manager =
      context_->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
    uint64_t intra_process_publisher_id =
@@ -132,7 +122,12 @@ Node::create_publisher(
    publisher->setup_intra_process(
      intra_process_publisher_id,
      shared_publish_callback,
-      intra_process_publisher_handle);
+      publisher_options);
+  }
+  if (rcl_trigger_guard_condition(&notify_guard_condition_) != RCL_RET_OK) {
+    throw std::runtime_error(
+            std::string(
+              "Failed to notify waitset on publisher creation: ") + rmw_get_error_string());
  }
  return publisher;
 }
@@ -157,46 +152,37 @@ Node::create_subscription(
  Alloc> any_subscription_callback(allocator);
  any_subscription_callback.set(std::forward<CallbackT>(callback));

-  using rosidl_generator_cpp::get_message_type_support_handle;
-
  if (!msg_mem_strat) {
    msg_mem_strat =
      rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::create_default();
  }
+  auto message_alloc =
+    std::make_shared<typename subscription::Subscription<MessageT, Alloc>::MessageAlloc>();

-  auto type_support_handle = get_message_type_support_handle<MessageT>();
-  rmw_subscription_t * subscriber_handle = rmw_create_subscription(
-    node_handle_.get(), type_support_handle,
-    topic_name.c_str(), &qos_profile, ignore_local_publications);
-  if (!subscriber_handle) {
-    // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-    throw std::runtime_error(
-      std::string("could not create subscription: ") + rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
+  auto subscription_options = rcl_subscription_get_default_options();
+  subscription_options.qos = qos_profile;
+  subscription_options.allocator = rclcpp::allocator::get_rcl_allocator<MessageT>(
+    *message_alloc.get());
+  subscription_options.ignore_local_publications = ignore_local_publications;

  using rclcpp::subscription::Subscription;
  using rclcpp::subscription::SubscriptionBase;

  auto sub = Subscription<MessageT, Alloc>::make_shared(
    node_handle_,
-    subscriber_handle,
    topic_name,
-    ignore_local_publications,
+    subscription_options,
    any_subscription_callback,
    msg_mem_strat);
  auto sub_base_ptr = std::dynamic_pointer_cast<SubscriptionBase>(sub);
  // Setup intra process.
  if (use_intra_process_comms_) {
-    rmw_subscription_t * intra_process_subscriber_handle = rmw_create_subscription(
-      node_handle_.get(), rclcpp::type_support::get_intra_process_message_msg_type_support(),
-      (topic_name + "__intra").c_str(), &qos_profile, false);
-    if (!subscriber_handle) {
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-      throw std::runtime_error(
-        std::string("could not create intra process subscription: ") + rmw_get_error_string_safe());
-      // *INDENT-ON*
-    }
+    auto intra_process_options = rcl_subscription_get_default_options();
+    intra_process_options.allocator = rclcpp::allocator::get_rcl_allocator<MessageT>(
+      *message_alloc.get());
+    intra_process_options.qos = qos_profile;
+    intra_process_options.ignore_local_publications = false;
+
    auto intra_process_manager =
      context_->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
    rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = intra_process_manager;
@@ -205,7 +191,6 @@ Node::create_subscription(
    // *INDENT-OFF*
    sub->setup_intra_process(
      intra_process_subscription_id,
-      intra_process_subscriber_handle,
      [weak_ipm](
        uint64_t publisher_id,
        uint64_t message_sequence,
@@ -228,7 +213,8 @@ Node::create_subscription(
            "intra process publisher check called after destruction of intra process manager");
        }
        return ipm->matches_any_publishers(sender_gid);
-      }
+      },
+      intra_process_options
    );
    // *INDENT-ON*
  }
@@ -243,6 +229,11 @@ Node::create_subscription(
    default_callback_group_->add_subscription(sub_base_ptr);
  }
  number_of_subscriptions_++;
+  if (rcl_trigger_guard_condition(&notify_guard_condition_) != RCL_RET_OK) {
+    throw std::runtime_error(
+            std::string(
+              "Failed to notify waitset on subscription creation: ") + rmw_get_error_string());
+  }
  return sub;
 }

@@ -289,6 +280,11 @@ Node::create_wall_timer(
    default_callback_group_->add_timer(timer);
  }
  number_of_timers_++;
+  if (rcl_trigger_guard_condition(&notify_guard_condition_) != RCL_RET_OK) {
+    throw std::runtime_error(
+            std::string(
+              "Failed to notify waitset on timer creation: ") + rmw_get_error_string());
+  }
  return timer;
 }

@@ -299,27 +295,16 @@ Node::create_client(
  const rmw_qos_profile_t & qos_profile,
  rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
-  using rosidl_generator_cpp::get_service_type_support_handle;
-  auto service_type_support_handle =
-    get_service_type_support_handle<ServiceT>();
-
-  rmw_client_t * client_handle = rmw_create_client(
-    this->node_handle_.get(), service_type_support_handle, service_name.c_str(), &qos_profile);
-  if (!client_handle) {
-    // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-    throw std::runtime_error(
-      std::string("could not create client: ") +
-      rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
+  rcl_client_options_t options = rcl_client_get_default_options();
+  options.qos = qos_profile;

  using rclcpp::client::Client;
  using rclcpp::client::ClientBase;

  auto cli = Client<ServiceT>::make_shared(
-    node_handle_,
-    client_handle,
-    service_name);
+    shared_from_this(),
+    service_name,
+    options);

  auto cli_base_ptr = std::dynamic_pointer_cast<ClientBase>(cli);
  if (group) {
@@ -333,6 +318,11 @@ Node::create_client(
  }
  number_of_clients_++;

+  if (rcl_trigger_guard_condition(&notify_guard_condition_) != RCL_RET_OK) {
+    throw std::runtime_error(
+            std::string(
+              "Failed to notify waitset on client creation: ") + rmw_get_error_string());
+  }
  return cli;
 }

@@ -344,25 +334,15 @@ Node::create_service(
  const rmw_qos_profile_t & qos_profile,
  rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
-  using rosidl_generator_cpp::get_service_type_support_handle;
-  auto service_type_support_handle =
-    get_service_type_support_handle<ServiceT>();
-
  rclcpp::service::AnyServiceCallback<ServiceT> any_service_callback;
  any_service_callback.set(std::forward<CallbackT>(callback));

-  rmw_service_t * service_handle = rmw_create_service(
-    node_handle_.get(), service_type_support_handle, service_name.c_str(), &qos_profile);
-  if (!service_handle) {
-    // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-    throw std::runtime_error(
-      std::string("could not create service: ") +
-      rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
+  rcl_service_options_t service_options = rcl_service_get_default_options();
+  service_options.qos = qos_profile;

  auto serv = service::Service<ServiceT>::make_shared(
-    node_handle_, service_handle, service_name, any_service_callback);
+    node_handle_,
+    service_name, any_service_callback, service_options);
  auto serv_base_ptr = std::dynamic_pointer_cast<service::ServiceBase>(serv);
  if (group) {
    if (!group_in_node(group)) {
@@ -374,6 +354,11 @@ Node::create_service(
    default_callback_group_->add_service(serv_base_ptr);
  }
  number_of_services_++;
+  if (rcl_trigger_guard_condition(&notify_guard_condition_) != RCL_RET_OK) {
+    throw std::runtime_error(
+            std::string(
+              "Failed to notify waitset on service creation: ") + rmw_get_error_string());
+  }
  return serv;
 }

--- a/rclcpp/include/rclcpp/parameter.hpp
+++ b/rclcpp/include/rclcpp/parameter.hpp
@@ -80,6 +80,8 @@ public:
  rcl_interfaces::msg::ParameterValue
  get_parameter_value() const;

+  // The following get_value() variants require the use of ParameterType
+
  template<ParameterType type>
  typename std::enable_if<type == ParameterType::PARAMETER_INTEGER, int64_t>::type
  get_value() const
@@ -125,8 +127,8 @@ public:
  }

  template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_BYTES,
-  const std::vector<uint8_t> &>::type
+  typename std::enable_if<
+    type == ParameterType::PARAMETER_BYTES, const std::vector<uint8_t> &>::type
  get_value() const
  {
    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BYTES) {
@@ -136,6 +138,46 @@ public:
    return value_.bytes_value;
  }

+  // The following get_value() variants allow the use of primitive types
+
+  template<typename type>
+  typename std::enable_if<
+    std::is_integral<type>::value && !std::is_same<type, bool>::value, int64_t>::type
+  get_value() const
+  {
+    return get_value<ParameterType::PARAMETER_INTEGER>();
+  }
+
+  template<typename type>
+  typename std::enable_if<std::is_floating_point<type>::value, double>::type
+  get_value() const
+  {
+    return get_value<ParameterType::PARAMETER_DOUBLE>();
+  }
+
+  template<typename type>
+  typename std::enable_if<std::is_convertible<type, std::string>::value, const std::string &>::type
+  get_value() const
+  {
+    return get_value<ParameterType::PARAMETER_STRING>();
+  }
+
+  template<typename type>
+  typename std::enable_if<std::is_same<type, bool>::value, bool>::type
+  get_value() const
+  {
+    return get_value<ParameterType::PARAMETER_BOOL>();
+  }
+
+  template<typename type>
+  typename std::enable_if<
+    std::is_convertible<
+      type, const std::vector<uint8_t> &>::value, const std::vector<uint8_t> &>::type
+  get_value() const
+  {
+    return get_value<ParameterType::PARAMETER_BYTES>();
+  }
+
  RCLCPP_PUBLIC
  int64_t
  as_int() const;
--- a/rclcpp/include/rclcpp/parameter_client.hpp
+++ b/rclcpp/include/rclcpp/parameter_client.hpp
@@ -48,7 +48,8 @@ public:
  RCLCPP_PUBLIC
  AsyncParametersClient(
    const rclcpp::node::Node::SharedPtr node,
-    const std::string & remote_node_name = "");
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);

  RCLCPP_PUBLIC
  std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>
@@ -119,18 +120,59 @@ public:
  RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient);

  RCLCPP_PUBLIC
-  SyncParametersClient(
-    rclcpp::node::Node::SharedPtr node);
+  explicit SyncParametersClient(
+    rclcpp::node::Node::SharedPtr node,
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);

  RCLCPP_PUBLIC
  SyncParametersClient(
    rclcpp::executor::Executor::SharedPtr executor,
-    rclcpp::node::Node::SharedPtr node);
+    rclcpp::node::Node::SharedPtr node,
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);

  RCLCPP_PUBLIC
  std::vector<rclcpp::parameter::ParameterVariant>
  get_parameters(const std::vector<std::string> & parameter_names);

+  RCLCPP_PUBLIC
+  bool
+  has_parameter(const std::string & parameter_name);
+
+  template<typename T>
+  T
+  get_parameter_impl(
+    const std::string & parameter_name, std::function<T()> parameter_not_found_handler)
+  {
+    std::vector<std::string> names;
+    names.push_back(parameter_name);
+    auto vars = get_parameters(names);
+    if ((vars.size() != 1) || (vars[0].get_type() == rclcpp::parameter::PARAMETER_NOT_SET)) {
+      return parameter_not_found_handler();
+    } else {
+      return static_cast<T>(vars[0].get_value<T>());
+    }
+  }
+
+  template<typename T>
+  T
+  get_parameter(const std::string & parameter_name, const T & default_value)
+  {
+    // *INDENT-OFF*
+    return get_parameter_impl(parameter_name,
+      std::function<T()>([&default_value]() -> T {return default_value; }));
+    // *INDENT-ON*
+  }
+
+  template<typename T>
+  T
+  get_parameter(const std::string & parameter_name)
+  {
+    // *INDENT-OFF*
+    return get_parameter_impl(parameter_name,
+      std::function<T()>([]() -> T {throw std::runtime_error("Parameter not set"); }));
+    // *INDENT-ON*
+  }
+
  RCLCPP_PUBLIC
  std::vector<rclcpp::parameter::ParameterType>
  get_parameter_types(const std::vector<std::string> & parameter_names);
--- a/rclcpp/include/rclcpp/parameter_service.hpp
+++ b/rclcpp/include/rclcpp/parameter_service.hpp
@@ -41,7 +41,9 @@ public:
  RCLCPP_SMART_PTR_DEFINITIONS(ParameterService);

  RCLCPP_PUBLIC
-  explicit ParameterService(const rclcpp::node::Node::SharedPtr node);
+  explicit ParameterService(
+    const rclcpp::node::Node::SharedPtr node,
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);

 private:
  const rclcpp::node::Node::SharedPtr node_;
--- a/rclcpp/include/rclcpp/publisher.hpp
+++ b/rclcpp/include/rclcpp/publisher.hpp
@@ -24,12 +24,16 @@
 #include <sstream>
 #include <string>

+#include "rcl/error_handling.h"
+#include "rcl/publisher.h"
+
 #include "rcl_interfaces/msg/intra_process_message.hpp"
 #include "rmw/impl/cpp/demangle.hpp"

 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/allocator/allocator_deleter.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"

 namespace rclcpp
@@ -52,19 +56,16 @@ public:
  /**
   * Typically, a publisher is not created through this method, but instead is created through a
   * call to `Node::create_publisher`.
-   * \param[in] node_handle The corresponding rmw representation of the owner node.
-   * \param[in] publisher_handle The rmw publisher handle corresponding to this publisher.
+   * \param[in] node_handle The corresponding rcl representation of the owner node.
   * \param[in] topic The topic that this publisher publishes on.
   * \param[in] queue_size The maximum number of unpublished messages to queue.
   */
  RCLCPP_PUBLIC
  PublisherBase(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_publisher_t * publisher_handle,
+    std::shared_ptr<rcl_node_t> node_handle,
    std::string topic,
    size_t queue_size);

-  /// Default destructor.
  RCLCPP_PUBLIC
  virtual ~PublisherBase();

@@ -120,12 +121,13 @@ protected:
  setup_intra_process(
    uint64_t intra_process_publisher_id,
    StoreMessageCallbackT callback,
-    rmw_publisher_t * intra_process_publisher_handle);
+    const rcl_publisher_options_t & intra_process_options);

-  std::shared_ptr<rmw_node_t> node_handle_;
+  std::shared_ptr<rcl_node_t> node_handle_;

-  rmw_publisher_t * publisher_handle_;
-  rmw_publisher_t * intra_process_publisher_handle_;
+  rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher();
+  rcl_publisher_t intra_process_publisher_handle_ = rcl_get_zero_initialized_publisher();
+  rcl_allocator_t rcl_allocator_ = rcl_get_default_allocator();

  std::string topic_;
  size_t queue_size_;
@@ -152,15 +154,54 @@ public:
  RCLCPP_SMART_PTR_DEFINITIONS(Publisher<MessageT, Alloc>);

  Publisher(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_publisher_t * publisher_handle,
+    std::shared_ptr<rcl_node_t> node_handle,
    std::string topic,
-    size_t queue_size,
-    std::shared_ptr<Alloc> allocator)
-  : PublisherBase(node_handle, publisher_handle, topic, queue_size)
+    const rcl_publisher_options_t & publisher_options,
+    std::shared_ptr<MessageAlloc> allocator)
+  : PublisherBase(node_handle, topic, publisher_options.qos.depth), message_allocator_(allocator)
  {
-    message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
+    using rosidl_generator_cpp::get_message_type_support_handle;
    allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get());
+
+    rcl_allocator_ = publisher_options.allocator;
+    auto type_support_handle = get_message_type_support_handle<MessageT>();
+    if (rcl_publisher_init(
+        &publisher_handle_, node_handle_.get(), type_support_handle,
+        topic.c_str(), &publisher_options) != RCL_RET_OK)
+    {
+      throw std::runtime_error(
+              std::string("could not create publisher: ") +
+              rcl_get_error_string_safe());
+    }
+    // Life time of this object is tied to the publisher handle.
+    rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(&publisher_handle_);
+    if (!publisher_rmw_handle) {
+      throw std::runtime_error(
+              std::string("failed to get rmw handle: ") + rcl_get_error_string_safe());
+    }
+    if (rmw_get_gid_for_publisher(publisher_rmw_handle, &rmw_gid_) != RMW_RET_OK) {
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
+      throw std::runtime_error(
+        std::string("failed to get publisher gid: ") + rmw_get_error_string_safe());
+      // *INDENT-ON*
+    }
+  }
+
+  virtual ~Publisher()
+  {
+    if (rcl_publisher_fini(&intra_process_publisher_handle_, node_handle_.get()) != RCL_RET_OK) {
+      fprintf(
+        stderr,
+        "Error in destruction of intra process rcl publisher handle: %s\n",
+        rcl_get_error_string_safe());
+    }
+
+    if (rcl_publisher_fini(&publisher_handle_, node_handle_.get()) != RCL_RET_OK) {
+      fprintf(
+        stderr,
+        "Error in destruction of rcl publisher handle: %s\n",
+        rcl_get_error_string_safe());
+    }
  }


@@ -188,11 +229,11 @@ public:
      rcl_interfaces::msg::IntraProcessMessage ipm;
      ipm.publisher_id = intra_process_publisher_id_;
      ipm.message_sequence = message_seq;
-      auto status = rmw_publish(intra_process_publisher_handle_, &ipm);
-      if (status != RMW_RET_OK) {
-        // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+      auto status = rcl_publish(&intra_process_publisher_handle_, &ipm);
+      if (status != RCL_RET_OK) {
+        // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
        throw std::runtime_error(
-          std::string("failed to publish intra process message: ") + rmw_get_error_string_safe());
+          std::string("failed to publish intra process message: ") + rcl_get_error_string_safe());
        // *INDENT-ON*
      }
    } else {
@@ -263,11 +304,11 @@ protected:
  void
  do_inter_process_publish(const MessageT * msg)
  {
-    auto status = rmw_publish(publisher_handle_, msg);
-    if (status != RMW_RET_OK) {
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+    auto status = rcl_publish(&publisher_handle_, msg);
+    if (status != RCL_RET_OK) {
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
      throw std::runtime_error(
-        std::string("failed to publish message: ") + rmw_get_error_string_safe());
+        std::string("failed to publish message: ") + rcl_get_error_string_safe());
      // *INDENT-ON*
    }
  }
--- a/rclcpp/include/rclcpp/service.hpp
+++ b/rclcpp/include/rclcpp/service.hpp
@@ -21,8 +21,12 @@
 #include <sstream>
 #include <string>

+#include "rcl/error_handling.h"
+#include "rcl/service.h"
+
 #include "rclcpp/any_service_callback.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
@@ -39,8 +43,7 @@ public:

  RCLCPP_PUBLIC
  ServiceBase(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_service_t * service_handle,
+    std::shared_ptr<rcl_node_t> node_handle,
    const std::string service_name);

  RCLCPP_PUBLIC
@@ -51,21 +54,21 @@ public:
  get_service_name();

  RCLCPP_PUBLIC
-  const rmw_service_t *
+  const rcl_service_t *
  get_service_handle();

  virtual std::shared_ptr<void> create_request() = 0;
-  virtual std::shared_ptr<void> create_request_header() = 0;
+  virtual std::shared_ptr<rmw_request_id_t> create_request_header() = 0;
  virtual void handle_request(
-    std::shared_ptr<void> request_header,
+    std::shared_ptr<rmw_request_id_t> request_header,
    std::shared_ptr<void> request) = 0;

-private:
+protected:
  RCLCPP_DISABLE_COPY(ServiceBase);

-  std::shared_ptr<rmw_node_t> node_handle_;
+  std::shared_ptr<rcl_node_t> node_handle_;

-  rmw_service_t * service_handle_;
+  rcl_service_t service_handle_ = rcl_get_zero_initialized_service();
  std::string service_name_;
 };

@@ -88,45 +91,68 @@ public:
  RCLCPP_SMART_PTR_DEFINITIONS(Service);

  Service(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_service_t * service_handle,
+    std::shared_ptr<rcl_node_t> node_handle,
    const std::string & service_name,
-    AnyServiceCallback<ServiceT> any_callback)
-  : ServiceBase(node_handle, service_handle, service_name), any_callback_(any_callback)
-  {}
+    AnyServiceCallback<ServiceT> any_callback,
+    rcl_service_options_t & service_options)
+  : ServiceBase(node_handle, service_name), any_callback_(any_callback)
+  {
+    using rosidl_generator_cpp::get_service_type_support_handle;
+    auto service_type_support_handle = get_service_type_support_handle<ServiceT>();
+
+    if (rcl_service_init(
+        &service_handle_, node_handle.get(), service_type_support_handle, service_name.c_str(),
+        &service_options) != RCL_RET_OK)
+    {
+      throw std::runtime_error(
+              std::string("could not create service: ") +
+              rcl_get_error_string_safe());
+    }
+  }

  Service() = delete;

+  virtual ~Service()
+  {
+    if (rcl_service_fini(&service_handle_, node_handle_.get()) != RCL_RET_OK) {
+      std::stringstream ss;
+      ss << "Error in destruction of rcl service_handle_ handle: " <<
+        rcl_get_error_string_safe() << '\n';
+      (std::cerr << ss.str()).flush();
+    }
+  }
+
  std::shared_ptr<void> create_request()
  {
    return std::shared_ptr<void>(new typename ServiceT::Request());
  }

-  std::shared_ptr<void> create_request_header()
+  std::shared_ptr<rmw_request_id_t> create_request_header()
  {
    // TODO(wjwwood): This should probably use rmw_request_id's allocator.
    //                (since it is a C type)
-    return std::shared_ptr<void>(new rmw_request_id_t);
+    return std::shared_ptr<rmw_request_id_t>(new rmw_request_id_t);
  }

-  void handle_request(std::shared_ptr<void> request_header, std::shared_ptr<void> request)
+  void handle_request(std::shared_ptr<rmw_request_id_t> request_header,
+    std::shared_ptr<void> request)
  {
    auto typed_request = std::static_pointer_cast<typename ServiceT::Request>(request);
-    auto typed_request_header = std::static_pointer_cast<rmw_request_id_t>(request_header);
    auto response = std::shared_ptr<typename ServiceT::Response>(new typename ServiceT::Response);
-    any_callback_.dispatch(typed_request_header, typed_request, response);
-    send_response(typed_request_header, response);
+    any_callback_.dispatch(request_header, typed_request, response);
+    send_response(request_header, response);
  }

  void send_response(
    std::shared_ptr<rmw_request_id_t> req_id,
    std::shared_ptr<typename ServiceT::Response> response)
  {
-    rmw_ret_t status = rmw_send_response(get_service_handle(), req_id.get(), response.get());
-    if (status != RMW_RET_OK) {
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+    rcl_ret_t status = rcl_send_response(get_service_handle(), req_id.get(), response.get());
+
+    if (status != RCL_RET_OK) {
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
      throw std::runtime_error(
-        std::string("failed to send response: ") + rmw_get_error_string_safe());
+        std::string("failed to send response: ") + rcl_get_error_string_safe());
      // *INDENT-ON*
    }
  }
--- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
@@ -18,11 +18,15 @@
 #include <memory>
 #include <vector>

+#include "rcl/allocator.h"
+
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/memory_strategy.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/visibility_control.hpp"

+#include "rmw/types.h"
+
 namespace rclcpp
 {
 namespace memory_strategies
@@ -62,57 +66,60 @@ public:
    allocator_ = std::make_shared<VoidAlloc>();
  }

-  size_t fill_subscriber_handles(void ** & ptr)
+  void add_guard_condition(const rcl_guard_condition_t * guard_condition)
  {
-    for (auto & subscription : subscriptions_) {
-      subscriber_handles_.push_back(subscription->get_subscription_handle()->data);
-      if (subscription->get_intra_process_subscription_handle()) {
-        subscriber_handles_.push_back(subscription->get_intra_process_subscription_handle()->data);
+    for (const auto & existing_guard_condition : guard_conditions_) {
+      if (existing_guard_condition == guard_condition) {
+        return;
      }
    }
-    ptr = subscriber_handles_.data();
-    return subscriber_handles_.size();
+    guard_conditions_.push_back(guard_condition);
  }

-  // return the new number of services
-  size_t fill_service_handles(void ** & ptr)
+  void remove_guard_condition(const rcl_guard_condition_t * guard_condition)
  {
-    for (auto & service : services_) {
-      service_handles_.push_back(service->get_service_handle()->data);
+    for (auto it = guard_conditions_.begin(); it != guard_conditions_.end(); ++it) {
+      if (*it == guard_condition) {
+        guard_conditions_.erase(it);
+        break;
+      }
    }
-    ptr = service_handles_.data();
-    return service_handles_.size();
-  }
-
-  // return the new number of clients
-  size_t fill_client_handles(void ** & ptr)
-  {
-    for (auto & client : clients_) {
-      client_handles_.push_back(client->get_client_handle()->data);
-    }
-    ptr = client_handles_.data();
-    return client_handles_.size();
-  }
-
-  void clear_active_entities()
-  {
-    subscriptions_.clear();
-    services_.clear();
-    clients_.clear();
  }

  void clear_handles()
  {
-    subscriber_handles_.clear();
+    subscription_handles_.clear();
    service_handles_.clear();
    client_handles_.clear();
+    timer_handles_.clear();
  }

-  void remove_null_handles()
+  virtual void remove_null_handles(rcl_wait_set_t * wait_set)
  {
-    subscriber_handles_.erase(
-      std::remove(subscriber_handles_.begin(), subscriber_handles_.end(), nullptr),
-      subscriber_handles_.end()
+    for (size_t i = 0; i < wait_set->size_of_subscriptions; ++i) {
+      if (!wait_set->subscriptions[i]) {
+        subscription_handles_[i] = nullptr;
+      }
+    }
+    for (size_t i = 0; i < wait_set->size_of_services; ++i) {
+      if (!wait_set->services[i]) {
+        service_handles_[i] = nullptr;
+      }
+    }
+    for (size_t i = 0; i < wait_set->size_of_clients; ++i) {
+      if (!wait_set->clients[i]) {
+        client_handles_[i] = nullptr;
+      }
+    }
+    for (size_t i = 0; i < wait_set->size_of_timers; ++i) {
+      if (!wait_set->timers[i]) {
+        timer_handles_[i] = nullptr;
+      }
+    }
+
+    subscription_handles_.erase(
+      std::remove(subscription_handles_.begin(), subscription_handles_.end(), nullptr),
+      subscription_handles_.end()
    );

    service_handles_.erase(
@@ -124,6 +131,11 @@ public:
      std::remove(client_handles_.begin(), client_handles_.end(), nullptr),
      client_handles_.end()
    );
+
+    timer_handles_.erase(
+      std::remove(timer_handles_.begin(), timer_handles_.end(), nullptr),
+      timer_handles_.end()
+    );
  }

  bool collect_entities(const WeakNodeVector & weak_nodes)
@@ -143,18 +155,28 @@ public:
        for (auto & weak_subscription : group->get_subscription_ptrs()) {
          auto subscription = weak_subscription.lock();
          if (subscription) {
-            subscriptions_.push_back(subscription);
+            subscription_handles_.push_back(subscription->get_subscription_handle());
+            if (subscription->get_intra_process_subscription_handle()) {
+              subscription_handles_.push_back(
+                subscription->get_intra_process_subscription_handle());
+            }
          }
        }
        for (auto & service : group->get_service_ptrs()) {
          if (service) {
-            services_.push_back(service);
+            service_handles_.push_back(service->get_service_handle());
          }
        }
        for (auto & weak_client : group->get_client_ptrs()) {
          auto client = weak_client.lock();
          if (client) {
-            clients_.push_back(client);
+            client_handles_.push_back(client->get_client_handle());
+          }
+        }
+        for (auto & weak_timer : group->get_timer_ptrs()) {
+          auto timer = weak_timer.lock();
+          if (timer) {
+            timer_handles_.push_back(timer->get_timer_handle());
          }
        }
      }
@@ -162,6 +184,45 @@ public:
    return has_invalid_weak_nodes;
  }

+  bool add_handles_to_waitset(rcl_wait_set_t * wait_set)
+  {
+    for (auto subscription : subscription_handles_) {
+      if (rcl_wait_set_add_subscription(wait_set, subscription) != RCL_RET_OK) {
+        fprintf(stderr, "Couldn't add subscription to waitset: %s\n", rcl_get_error_string_safe());
+        return false;
+      }
+    }
+
+    for (auto client : client_handles_) {
+      if (rcl_wait_set_add_client(wait_set, client) != RCL_RET_OK) {
+        fprintf(stderr, "Couldn't add client to waitset: %s\n", rcl_get_error_string_safe());
+        return false;
+      }
+    }
+
+    for (auto service : service_handles_) {
+      if (rcl_wait_set_add_service(wait_set, service) != RCL_RET_OK) {
+        fprintf(stderr, "Couldn't add service to waitset: %s\n", rcl_get_error_string_safe());
+        return false;
+      }
+    }
+
+    for (auto timer : timer_handles_) {
+      if (rcl_wait_set_add_timer(wait_set, timer) != RCL_RET_OK) {
+        fprintf(stderr, "Couldn't add timer to waitset: %s\n", rcl_get_error_string_safe());
+        return false;
+      }
+    }
+
+    for (auto guard_condition : guard_conditions_) {
+      if (rcl_wait_set_add_guard_condition(wait_set, guard_condition) != RCL_RET_OK) {
+        fprintf(stderr, "Couldn't add guard_condition to waitset: %s\n",
+          rcl_get_error_string_safe());
+        return false;
+      }
+    }
+    return true;
+  }

  /// Provide a newly initialized AnyExecutable object.
  // \return Shared pointer to the fresh executable.
@@ -174,21 +235,21 @@ public:
  get_next_subscription(executor::AnyExecutable::SharedPtr any_exec,
    const WeakNodeVector & weak_nodes)
  {
-    auto it = subscriber_handles_.begin();
-    while (it != subscriber_handles_.end()) {
+    auto it = subscription_handles_.begin();
+    while (it != subscription_handles_.end()) {
      auto subscription = get_subscription_by_handle(*it, weak_nodes);
      if (subscription) {
        // Figure out if this is for intra-process or not.
        bool is_intra_process = false;
        if (subscription->get_intra_process_subscription_handle()) {
-          is_intra_process = subscription->get_intra_process_subscription_handle()->data == *it;
+          is_intra_process = subscription->get_intra_process_subscription_handle() == *it;
        }
        // Find the group for this handle and see if it can be serviced
        auto group = get_group_by_subscription(subscription, weak_nodes);
        if (!group) {
          // Group was not found, meaning the subscription is not valid...
          // Remove it from the ready list and continue looking
-          subscriber_handles_.erase(it);
+          subscription_handles_.erase(it);
          continue;
        }
        if (!group->can_be_taken_from().load()) {
@@ -205,11 +266,11 @@ public:
        }
        any_exec->callback_group = group;
        any_exec->node = get_node_by_group(group, weak_nodes);
-        subscriber_handles_.erase(it);
+        subscription_handles_.erase(it);
        return;
      }
      // Else, the subscription is no longer valid, remove it and continue
-      subscriber_handles_.erase(it);
+      subscription_handles_.erase(it);
    }
  }

@@ -280,18 +341,47 @@ public:
    }
  }

+  virtual rcl_allocator_t get_allocator()
+  {
+    return rclcpp::allocator::get_rcl_allocator<void *, VoidAlloc>(*allocator_.get());
+  }
+
+  size_t number_of_ready_subscriptions() const
+  {
+    return subscription_handles_.size();
+  }
+
+  size_t number_of_ready_services() const
+  {
+    return service_handles_.size();
+  }
+
+  size_t number_of_ready_clients() const
+  {
+    return client_handles_.size();
+  }
+
+  size_t number_of_guard_conditions() const
+  {
+    return guard_conditions_.size();
+  }
+
+  size_t number_of_ready_timers() const
+  {
+    return timer_handles_.size();
+  }
+
 private:
  template<typename T>
  using VectorRebind =
      std::vector<T, typename std::allocator_traits<Alloc>::template rebind_alloc<T>>;

-  VectorRebind<rclcpp::subscription::SubscriptionBase::SharedPtr> subscriptions_;
-  VectorRebind<rclcpp::service::ServiceBase::SharedPtr> services_;
-  VectorRebind<rclcpp::client::ClientBase::SharedPtr> clients_;
+  VectorRebind<const rcl_guard_condition_t *> guard_conditions_;

-  VectorRebind<void *> subscriber_handles_;
-  VectorRebind<void *> service_handles_;
-  VectorRebind<void *> client_handles_;
+  VectorRebind<const rcl_subscription_t *> subscription_handles_;
+  VectorRebind<const rcl_service_t *> service_handles_;
+  VectorRebind<const rcl_client_t *> client_handles_;
+  VectorRebind<const rcl_timer_t *> timer_handles_;

  std::shared_ptr<ExecAlloc> executable_allocator_;
  std::shared_ptr<VoidAlloc> allocator_;
--- a/rclcpp/include/rclcpp/subscription.hpp
+++ b/rclcpp/include/rclcpp/subscription.hpp
@@ -24,11 +24,15 @@
 #include <sstream>
 #include <string>

+#include "rcl/error_handling.h"
+#include "rcl/subscription.h"
+
 #include "rcl_interfaces/msg/intra_process_message.hpp"

 #include "rclcpp/macros.hpp"
 #include "rclcpp/message_memory_strategy.hpp"
 #include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"

 namespace rclcpp
@@ -51,14 +55,13 @@ public:

  /// Default constructor.
  /**
-   * \param[in] node_handle The rmw representation of the node that owns this subscription.
+   * \param[in] node_handle The rcl representation of the node that owns this subscription.
   * \param[in] topic_name Name of the topic to subscribe to.
   * \param[in] ignore_local_publications True to ignore local publications (unused).
   */
  RCLCPP_PUBLIC
  SubscriptionBase(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_subscription_t * subscription_handle,
+    std::shared_ptr<rcl_node_t> node_handle,
    const std::string & topic_name,
    bool ignore_local_publications);

@@ -72,11 +75,11 @@ public:
  get_topic_name() const;

  RCLCPP_PUBLIC
-  const rmw_subscription_t *
+  const rcl_subscription_t *
  get_subscription_handle() const;

  RCLCPP_PUBLIC
-  const rmw_subscription_t *
+  virtual const rcl_subscription_t *
  get_intra_process_subscription_handle() const;

  /// Borrow a new message.
@@ -101,15 +104,12 @@ public:
    const rmw_message_info_t & message_info) = 0;

 protected:
-  rmw_subscription_t * intra_process_subscription_handle_;
+  rcl_subscription_t intra_process_subscription_handle_ = rcl_get_zero_initialized_subscription();
+  rcl_subscription_t subscription_handle_ = rcl_get_zero_initialized_subscription();
+  std::shared_ptr<rcl_node_t> node_handle_;

 private:
  RCLCPP_DISABLE_COPY(SubscriptionBase);
-
-  std::shared_ptr<rmw_node_t> node_handle_;
-
-  rmw_subscription_t * subscription_handle_;
-
  std::string topic_name_;
  bool ignore_local_publications_;
 };
@@ -134,27 +134,37 @@ public:
  /**
   * The constructor for a subscription is almost never called directly. Instead, subscriptions
   * should be instantiated through Node::create_subscription.
-   * \param[in] node_handle rmw representation of the node that owns this subscription.
+   * \param[in] node_handle rcl representation of the node that owns this subscription.
   * \param[in] topic_name Name of the topic to subscribe to.
   * \param[in] ignore_local_publications True to ignore local publications (unused).
   * \param[in] callback User-defined callback to call when a message is received.
   * \param[in] memory_strategy The memory strategy to be used for managing message memory.
   */
  Subscription(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_subscription_t * subscription_handle,
+    std::shared_ptr<rcl_node_t> node_handle,
    const std::string & topic_name,
-    bool ignore_local_publications,
+    const rcl_subscription_options_t & subscription_options,
    AnySubscriptionCallback<MessageT, Alloc> callback,
    typename message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
    memory_strategy = message_memory_strategy::MessageMemoryStrategy<MessageT,
    Alloc>::create_default())
-  : SubscriptionBase(node_handle, subscription_handle, topic_name, ignore_local_publications),
+  : SubscriptionBase(
+      node_handle, topic_name, subscription_options.ignore_local_publications),
    any_callback_(callback),
    message_memory_strategy_(memory_strategy),
    get_intra_process_message_callback_(nullptr),
    matches_any_intra_process_publishers_(nullptr)
  {
+    using rosidl_generator_cpp::get_message_type_support_handle;
+
+    auto type_support_handle = get_message_type_support_handle<MessageT>();
+    if (rcl_subscription_init(
+        &subscription_handle_, node_handle_.get(), type_support_handle, topic_name.c_str(),
+        &subscription_options) != RCL_RET_OK)
+    {
+      throw std::runtime_error(
+              std::string("could not create subscription: ") + rcl_get_error_string_safe());
+    }
  }

  /// Support dynamically setting the message memory strategy.
@@ -231,16 +241,36 @@ private:

  void setup_intra_process(
    uint64_t intra_process_subscription_id,
-    rmw_subscription_t * intra_process_subscription,
    GetMessageCallbackType get_message_callback,
-    MatchesAnyPublishersCallbackType matches_any_publisher_callback)
+    MatchesAnyPublishersCallbackType matches_any_publisher_callback,
+    const rcl_subscription_options_t & intra_process_options)
  {
+    if (rcl_subscription_init(
+        &intra_process_subscription_handle_, node_handle_.get(),
+        rclcpp::type_support::get_intra_process_message_msg_type_support(),
+        (get_topic_name() + "__intra").c_str(),
+        &intra_process_options) != RCL_RET_OK)
+    {
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
+      throw std::runtime_error(
+        std::string("could not create intra process subscription: ") + rcl_get_error_string_safe());
+      // *INDENT-ON*
+    }
+
    intra_process_subscription_id_ = intra_process_subscription_id;
-    intra_process_subscription_handle_ = intra_process_subscription;
    get_intra_process_message_callback_ = get_message_callback;
    matches_any_intra_process_publishers_ = matches_any_publisher_callback;
  }

+  const rcl_subscription_t *
+  get_intra_process_subscription_handle() const
+  {
+    if (!get_intra_process_message_callback_) {
+      return nullptr;
+    }
+    return &intra_process_subscription_handle_;
+  }
+
  RCLCPP_DISABLE_COPY(Subscription);

  AnySubscriptionCallback<MessageT, Alloc> any_callback_;
--- a/rclcpp/include/rclcpp/timer.hpp
+++ b/rclcpp/include/rclcpp/timer.hpp
@@ -27,6 +27,10 @@
 #include "rclcpp/rate.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
+
+#include "rcl/error_handling.h"
+#include "rcl/timer.h"
+
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"

@@ -44,7 +48,7 @@ public:
  explicit TimerBase(std::chrono::nanoseconds period);

  RCLCPP_PUBLIC
-  virtual ~TimerBase();
+  ~TimerBase();

  RCLCPP_PUBLIC
  void
@@ -54,27 +58,31 @@ public:
  virtual void
  execute_callback() = 0;

+  RCLCPP_PUBLIC
+  const rcl_timer_t *
+  get_timer_handle();
+
  /// Check how long the timer has until its next scheduled callback.
  // \return A std::chrono::duration representing the relative time until the next callback.
-  virtual std::chrono::nanoseconds
-  time_until_trigger() = 0;
+  RCLCPP_PUBLIC
+  std::chrono::nanoseconds
+  time_until_trigger();

  /// Is the clock steady (i.e. is the time between ticks constant?)
  // \return True if the clock used by this timer is steady.
  virtual bool is_steady() = 0;

-  /// Check if the timer needs to trigger the callback.
+  /// Check if the timer is ready to trigger the callback.
  /**
   * This function expects its caller to immediately trigger the callback after this function,
   * since it maintains the last time the callback was triggered.
   * \return True if the timer needs to trigger.
   */
-  virtual bool check_and_trigger() = 0;
+  RCLCPP_PUBLIC
+  bool is_ready();

 protected:
-  std::chrono::nanoseconds period_;
-
-  bool canceled_;
+  rcl_timer_t timer_handle_ = rcl_get_zero_initialized_timer();
 };


@@ -84,10 +92,11 @@ using TimerCallbackType = std::function<void(TimerBase &)>;
 /// Generic timer templated on the clock type. Periodically executes a user-specified callback.
 template<
  typename FunctorT,
-  class Clock = std::chrono::high_resolution_clock,
+  class Clock,
  typename std::enable_if<
-    rclcpp::function_traits::same_arguments<FunctorT, VoidCallbackType>::value ||
-    rclcpp::function_traits::same_arguments<FunctorT, TimerCallbackType>::value
+    (rclcpp::function_traits::same_arguments<FunctorT, VoidCallbackType>::value ||
+    rclcpp::function_traits::same_arguments<FunctorT, TimerCallbackType>::value) &&
+    Clock::is_steady
  >::type * = nullptr
 >
 class GenericTimer : public TimerBase
@@ -101,10 +110,8 @@ public:
   * \param[in] callback User-specified callback function.
   */
  GenericTimer(std::chrono::nanoseconds period, FunctorT && callback)
-  : TimerBase(period), callback_(std::forward<FunctorT>(callback)), loop_rate_(period)
+  : TimerBase(period), callback_(std::forward<FunctorT>(callback))
  {
-    /* Set last_triggered_time_ so that the timer fires at least one period after being created. */
-    last_triggered_time_ = Clock::now();
  }

  /// Default destructor.
@@ -112,11 +119,21 @@ public:
  {
    // Stop the timer from running.
    cancel();
+    if (rcl_timer_fini(&timer_handle_) != RCL_RET_OK) {
+      fprintf(stderr, "Failed to clean up rcl timer handle: %s\n", rcl_get_error_string_safe());
+    }
  }

  void
  execute_callback()
  {
+    rcl_ret_t ret = rcl_timer_call(&timer_handle_);
+    if (ret == RCL_RET_TIMER_CANCELED) {
+      return;
+    }
+    if (ret != RCL_RET_OK) {
+      throw std::runtime_error("Failed to notify timer that callback occurred");
+    }
    execute_callback_delegate<>();
  }

@@ -145,39 +162,6 @@ public:
    callback_(*this);
  }

-  bool
-  check_and_trigger()
-  {
-    if (canceled_) {
-      return false;
-    }
-    if (Clock::now() < last_triggered_time_) {
-      return false;
-    }
-    if (std::chrono::duration_cast<std::chrono::nanoseconds>(Clock::now() - last_triggered_time_) >=
-      loop_rate_.period())
-    {
-      last_triggered_time_ = Clock::now();
-      return true;
-    }
-    return false;
-  }
-
-  std::chrono::nanoseconds
-  time_until_trigger()
-  {
-    std::chrono::nanoseconds time_until_trigger;
-    // Calculate the time between the next trigger and the current time
-    if (last_triggered_time_ + loop_rate_.period() < Clock::now()) {
-      // time is overdue, need to trigger immediately
-      time_until_trigger = std::chrono::nanoseconds::zero();
-    } else {
-      time_until_trigger = std::chrono::duration_cast<std::chrono::nanoseconds>(
-        last_triggered_time_ - Clock::now()) + loop_rate_.period();
-    }
-    return time_until_trigger;
-  }
-
  virtual bool
  is_steady()
  {
@@ -188,8 +172,6 @@ protected:
  RCLCPP_DISABLE_COPY(GenericTimer);

  FunctorT callback_;
-  rclcpp::rate::GenericRate<Clock> loop_rate_;
-  std::chrono::time_point<Clock> last_triggered_time_;
 };

 template<typename CallbackType>
--- a/rclcpp/include/rclcpp/utilities.hpp
+++ b/rclcpp/include/rclcpp/utilities.hpp
@@ -16,11 +16,32 @@
 #define RCLCPP__UTILITIES_HPP_

 #include <chrono>
+#include <functional>

 #include "rclcpp/visibility_control.hpp"
+
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
+
 #include "rmw/macros.h"
 #include "rmw/rmw.h"

+#ifdef ANDROID
+#include <string>
+#include <sstream>
+
+namespace std
+{
+template<typename T>
+std::string to_string(T value)
+{
+  std::ostringstream os;
+  os << value;
+  return os.str();
+}
+}
+#endif
+
 namespace rclcpp
 {
 namespace utilities
@@ -46,10 +67,40 @@ RCLCPP_PUBLIC
 void
 shutdown();

-/// Get a handle to the rmw guard condition that manages the signal handler.
+/// Register a function to be called when shutdown is called.
+/* Calling the callbacks is the last thing shutdown() does. */
 RCLCPP_PUBLIC
-rmw_guard_condition_t *
-get_global_sigint_guard_condition();
+void
+on_shutdown(std::function<void(void)> callback);
+
+/// Get a handle to the rmw guard condition that manages the signal handler.
+/**
+ * The first time that this function is called for a given waitset a new guard
+ * condition will be created and returned; thereafter the same guard condition
+ * will be returned for the same waitset. This mechanism is designed to ensure
+ * that the same guard condition is not reused across waitsets (e.g., when
+ * using multiple executors in the same process). Will throw an exception if
+ * initialization of the guard condition fails.
+ * \param[waitset] waitset Pointer to the rcl_wait_set_t that will be using the
+ * resulting guard condition.
+ * \return Pointer to the guard condition.
+ */
+RCLCPP_PUBLIC
+rcl_guard_condition_t *
+get_sigint_guard_condition(rcl_wait_set_t * waitset);
+
+/// Release the previously allocated guard condition that manages the signal handler.
+/**
+ * If you previously called get_sigint_guard_condition() for a given waitset
+ * to get a sigint guard condition, then you should call release_sigint_guard_condition()
+ * when you're done, to free that condition.  Will throw an exception if
+ * get_sigint_guard_condition() wasn't previously called for the given waitset.
+ * \param[waitset] waitset Pointer to the rcl_wait_set_t that was using the
+ * resulting guard condition.
+ */
+RCLCPP_PUBLIC
+void
+release_sigint_guard_condition(rcl_wait_set_t * waitset);

 /// Use the global condition variable to block for the specified amount of time.
 /**
--- a/rclcpp/package.xml
+++ b/rclcpp/package.xml
@@ -1,4 +1,5 @@
 <?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
  <name>rclcpp</name>
  <version>0.0.0</version>
@@ -16,6 +17,7 @@
  <build_export_depend>rcl_interfaces</build_export_depend>
  <build_export_depend>rosidl_generator_cpp</build_export_depend>

+  <depend>rcl</depend>
  <depend>rmw_implementation</depend>

  <exec_depend>ament_cmake</exec_depend>
--- a/rclcpp/src/rclcpp/callback_group.cpp
+++ b/rclcpp/src/rclcpp/callback_group.cpp
@@ -26,24 +26,28 @@ CallbackGroup::CallbackGroup(CallbackGroupType group_type)
 const std::vector<rclcpp::subscription::SubscriptionBase::WeakPtr> &
 CallbackGroup::get_subscription_ptrs() const
 {
+  std::lock_guard<std::mutex> lock(mutex_);
  return subscription_ptrs_;
 }

 const std::vector<rclcpp::timer::TimerBase::WeakPtr> &
 CallbackGroup::get_timer_ptrs() const
 {
+  std::lock_guard<std::mutex> lock(mutex_);
  return timer_ptrs_;
 }

 const std::vector<rclcpp::service::ServiceBase::SharedPtr> &
 CallbackGroup::get_service_ptrs() const
 {
+  std::lock_guard<std::mutex> lock(mutex_);
  return service_ptrs_;
 }

 const std::vector<rclcpp::client::ClientBase::WeakPtr> &
 CallbackGroup::get_client_ptrs() const
 {
+  std::lock_guard<std::mutex> lock(mutex_);
  return client_ptrs_;
 }

@@ -63,23 +67,27 @@ void
 CallbackGroup::add_subscription(
  const rclcpp::subscription::SubscriptionBase::SharedPtr subscription_ptr)
 {
+  std::lock_guard<std::mutex> lock(mutex_);
  subscription_ptrs_.push_back(subscription_ptr);
 }

 void
 CallbackGroup::add_timer(const rclcpp::timer::TimerBase::SharedPtr timer_ptr)
 {
+  std::lock_guard<std::mutex> lock(mutex_);
  timer_ptrs_.push_back(timer_ptr);
 }

 void
 CallbackGroup::add_service(const rclcpp::service::ServiceBase::SharedPtr service_ptr)
 {
+  std::lock_guard<std::mutex> lock(mutex_);
  service_ptrs_.push_back(service_ptr);
 }

 void
 CallbackGroup::add_client(const rclcpp::client::ClientBase::SharedPtr client_ptr)
 {
+  std::lock_guard<std::mutex> lock(mutex_);
  client_ptrs_.push_back(client_ptr);
 }
--- a/rclcpp/src/rclcpp/client.cpp
+++ b/rclcpp/src/rclcpp/client.cpp
@@ -14,29 +14,29 @@

 #include "rclcpp/client.hpp"

+#include <chrono>
 #include <cstdio>
 #include <string>

-#include "rmw/rmw.h"
+#include "rcl/graph.h"
+#include "rcl/node.h"
+#include "rcl/wait.h"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/utilities.hpp"

 using rclcpp::client::ClientBase;
+using rclcpp::exceptions::InvalidNodeError;
+using rclcpp::exceptions::throw_from_rcl_error;

 ClientBase::ClientBase(
-  std::shared_ptr<rmw_node_t> node_handle,
-  rmw_client_t * client_handle,
+  std::shared_ptr<rclcpp::node::Node> parent_node,
  const std::string & service_name)
-: node_handle_(node_handle), client_handle_(client_handle), service_name_(service_name)
+: node_(parent_node), node_handle_(parent_node->get_shared_node_handle()),
+  service_name_(service_name)
 {}

-ClientBase::~ClientBase()
-{
-  if (client_handle_) {
-    if (rmw_destroy_client(client_handle_) != RMW_RET_OK) {
-      fprintf(stderr,
-        "Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe());
-    }
-  }
-}
+ClientBase::~ClientBase() {}

 const std::string &
 ClientBase::get_service_name() const
@@ -44,8 +44,70 @@ ClientBase::get_service_name() const
  return this->service_name_;
 }

-const rmw_client_t *
+const rcl_client_t *
 ClientBase::get_client_handle() const
 {
-  return this->client_handle_;
+  return &client_handle_;
+}
+
+bool
+ClientBase::service_is_ready() const
+{
+  bool is_ready;
+  rcl_ret_t ret =
+    rcl_service_server_is_available(this->get_rcl_node_handle(), &client_handle_, &is_ready);
+  if (ret != RCL_RET_OK) {
+    throw_from_rcl_error(ret, "rcl_service_server_is_available failed");
+  }
+  return is_ready;
+}
+
+bool
+ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
+{
+  auto start = std::chrono::steady_clock::now();
+  // check to see if the server is ready immediately
+  if (this->service_is_ready()) {
+    return true;
+  }
+  if (timeout == std::chrono::nanoseconds(0)) {
+    // check was non-blocking, return immediately
+    return false;
+  }
+  // make an event to reuse, rather than create a new one each time
+  auto node_ptr = node_.lock();
+  if (!node_ptr) {
+    throw InvalidNodeError();
+  }
+  auto event = node_ptr->get_graph_event();
+  // update the time even on the first loop to account for time in first server_is_read()
+  std::chrono::nanoseconds time_to_wait = timeout - (std::chrono::steady_clock::now() - start);
+  if (timeout > std::chrono::nanoseconds(0) && time_to_wait < std::chrono::nanoseconds(0)) {
+    // Do not allow the time_to_wait to become negative when timeout was originally positive.
+    // Setting time_to_wait to 0 will allow one non-blocking wait because of the do-while.
+    time_to_wait = std::chrono::nanoseconds(0);
+  }
+  // continue forever if timeout is negative, otherwise continue until out of time_to_wait
+  // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
+  do {
+    if (!rclcpp::utilities::ok()) {
+      return false;
+    }
+    node_ptr->wait_for_graph_change(event, time_to_wait);
+    if (event->check_and_clear()) {
+      if (this->service_is_ready()) {
+        return true;
+      }
+    }
+    // server is not ready, loop if there is time left
+    time_to_wait = timeout - (std::chrono::steady_clock::now() - start);
+  } while (timeout < std::chrono::nanoseconds(0) || time_to_wait > std::chrono::nanoseconds(0));
+  // *INDENT-ON*
+  return false;  // timeout exceeded while waiting for the server to be ready
+}
+
+rcl_node_t *
+ClientBase::get_rcl_node_handle() const
+{
+  return node_handle_.get();
 }
--- a/rclcpp/src/rclcpp/event.cpp
+++ b/rclcpp/src/rclcpp/event.cpp
@@ -0,0 +1,44 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/event.hpp"
+
+namespace rclcpp
+{
+namespace event
+{
+
+Event::Event()
+: state_(false) {}
+
+bool
+Event::set()
+{
+  return state_.exchange(true);
+}
+
+bool
+Event::check()
+{
+  return state_.load();
+}
+
+bool
+Event::check_and_clear()
+{
+  return state_.exchange(false);
+}
+
+}  // namespace event
+}  // namespace rclcpp
--- a/rclcpp/src/rclcpp/exceptions.cpp
+++ b/rclcpp/src/rclcpp/exceptions.cpp
@@ -0,0 +1,94 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/exceptions.hpp"
+
+#include <cstdio>
+#include <functional>
+#include <string>
+
+namespace rclcpp
+{
+namespace exceptions
+{
+
+void
+throw_from_rcl_error(rcl_ret_t ret, const std::string & prefix, bool reset_error)
+{
+  if (RCL_RET_OK == ret) {
+    throw std::invalid_argument("ret is RCL_RET_OK");
+  }
+  const rcl_error_state_t * error_state = rcl_get_error_state();
+  if (!error_state) {
+    throw std::runtime_error("rcl error state is not set");
+  }
+  std::string formated_prefix = prefix;
+  if (!prefix.empty()) {
+    formated_prefix += ": ";
+  }
+  RCLErrorBase base_exc(ret, error_state);
+  if (reset_error) {
+    rcl_reset_error();
+  }
+  switch (ret) {
+    case RCL_RET_BAD_ALLOC:
+      throw RCLBadAlloc(base_exc);
+    case RCL_RET_INVALID_ARGUMENT:
+      throw RCLInvalidArgument(base_exc, formated_prefix);
+    default:
+      throw RCLError(base_exc, formated_prefix);
+  }
+}
+
+RCLErrorBase::RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state)
+: ret(ret), message(error_state->message), file(error_state->file), line(error_state->line_number),
+  formatted_message(rcl_get_error_string_safe())
+{}
+
+RCLError::RCLError(
+  rcl_ret_t ret,
+  const rcl_error_state_t * error_state,
+  const std::string & prefix)
+: RCLError(RCLErrorBase(ret, error_state), prefix)
+{}
+
+RCLError::RCLError(
+  const RCLErrorBase & base_exc,
+  const std::string & prefix)
+: RCLErrorBase(base_exc), std::runtime_error(prefix + base_exc.formatted_message)
+{}
+
+RCLBadAlloc::RCLBadAlloc(rcl_ret_t ret, const rcl_error_state_t * error_state)
+: RCLBadAlloc(RCLErrorBase(ret, error_state))
+{}
+
+RCLBadAlloc::RCLBadAlloc(const RCLErrorBase & base_exc)
+: RCLErrorBase(base_exc), std::bad_alloc()
+{}
+
+RCLInvalidArgument::RCLInvalidArgument(
+  rcl_ret_t ret,
+  const rcl_error_state_t * error_state,
+  const std::string & prefix)
+: RCLInvalidArgument(RCLErrorBase(ret, error_state), prefix)
+{}
+
+RCLInvalidArgument::RCLInvalidArgument(
+  const RCLErrorBase & base_exc,
+  const std::string & prefix)
+: RCLErrorBase(base_exc), std::invalid_argument(prefix + base_exc.formatted_message)
+{}
+
+}  // namespace exceptions
+}  // namespace rclcpp
--- a/rclcpp/src/rclcpp/executor.cpp
+++ b/rclcpp/src/rclcpp/executor.cpp
@@ -12,49 +12,55 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.

+#include <algorithm>
+#include <string>
+#include <type_traits>
+
+#include "rcl/allocator.h"
+#include "rcl/error_handling.h"
+
 #include "rclcpp/executor.hpp"
 #include "rclcpp/scope_exit.hpp"
+#include "rclcpp/utilities.hpp"

 #include "rcl_interfaces/msg/intra_process_message.hpp"

 using rclcpp::executor::AnyExecutable;
 using rclcpp::executor::Executor;
 using rclcpp::executor::ExecutorArgs;
+using rclcpp::executor::FutureReturnCode;

 Executor::Executor(const ExecutorArgs & args)
 : spinning(false),
  memory_strategy_(args.memory_strategy)
 {
-  interrupt_guard_condition_ = rmw_create_guard_condition();
-  if (!interrupt_guard_condition_) {
-    throw std::runtime_error("Failed to create interrupt guard condition in Executor constructor");
+  rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options();
+  if (rcl_guard_condition_init(
+      &interrupt_guard_condition_, guard_condition_options) != RCL_RET_OK)
+  {
+    throw std::runtime_error(
+            std::string("Failed to create interrupt guard condition in Executor constructor: ") +
+            rcl_get_error_string_safe());
  }

-  // The number of guard conditions is fixed at 2: 1 for the ctrl-c guard cond,
+  // The number of guard conditions is always at least 2: 1 for the ctrl-c guard cond,
  // and one for the executor's guard cond (interrupt_guard_condition_)
-  // These guard conditions are permanently attached to the waitset.
-  const size_t number_of_guard_conds = 2;
-  fixed_guard_conditions_.guard_condition_count = number_of_guard_conds;
-  fixed_guard_conditions_.guard_conditions = static_cast<void **>(guard_cond_handles_.data());

  // Put the global ctrl-c guard condition in
-  assert(fixed_guard_conditions_.guard_condition_count > 1);
-  fixed_guard_conditions_.guard_conditions[0] = \
-    rclcpp::utilities::get_global_sigint_guard_condition()->data;
+  memory_strategy_->add_guard_condition(rclcpp::utilities::get_sigint_guard_condition(&waitset_));
+
  // Put the executor's guard condition in
-  fixed_guard_conditions_.guard_conditions[1] = interrupt_guard_condition_->data;
+  memory_strategy_->add_guard_condition(&interrupt_guard_condition_);
+  rcl_allocator_t allocator = memory_strategy_->get_allocator();

-  // The waitset adds the fixed guard conditions to the middleware waitset on initialization,
-  // and removes the guard conditions in rmw_destroy_waitset.
-  waitset_ = rmw_create_waitset(&fixed_guard_conditions_, args.max_conditions);
-
-  if (!waitset_) {
+  if (rcl_wait_set_init(
+      &waitset_, 0, 2, 0, 0, 0, allocator) != RCL_RET_OK)
+  {
    fprintf(stderr,
-      "[rclcpp::error] failed to create waitset: %s\n", rmw_get_error_string_safe());
-    rmw_ret_t status = rmw_destroy_guard_condition(interrupt_guard_condition_);
-    if (status != RMW_RET_OK) {
+      "[rclcpp::error] failed to create waitset: %s\n", rcl_get_error_string_safe());
+    if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
      fprintf(stderr,
-        "[rclcpp::error] failed to destroy guard condition: %s\n", rmw_get_error_string_safe());
+        "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
    }
    throw std::runtime_error("Failed to create waitset in Executor constructor");
  }
@@ -62,22 +68,20 @@ Executor::Executor(const ExecutorArgs & args)

 Executor::~Executor()
 {
-  // Try to deallocate the waitset.
-  if (waitset_) {
-    rmw_ret_t status = rmw_destroy_waitset(waitset_);
-    if (status != RMW_RET_OK) {
-      fprintf(stderr,
-        "[rclcpp::error] failed to destroy waitset: %s\n", rmw_get_error_string_safe());
-    }
+  // Finalize the waitset.
+  if (rcl_wait_set_fini(&waitset_) != RCL_RET_OK) {
+    fprintf(stderr,
+      "[rclcpp::error] failed to destroy waitset: %s\n", rcl_get_error_string_safe());
  }
-  // Try to deallocate the interrupt guard condition.
-  if (interrupt_guard_condition_ != nullptr) {
-    rmw_ret_t status = rmw_destroy_guard_condition(interrupt_guard_condition_);
-    if (status != RMW_RET_OK) {
-      fprintf(stderr,
-        "[rclcpp::error] failed to destroy guard condition: %s\n", rmw_get_error_string_safe());
-    }
+  // Finalize the interrupt guard condition.
+  if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
+    fprintf(stderr,
+      "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
  }
+  // Remove and release the sigint guard condition
+  memory_strategy_->remove_guard_condition(
+    rclcpp::utilities::get_sigint_guard_condition(&waitset_));
+  rclcpp::utilities::release_sigint_guard_condition(&waitset_);
 }

 void
@@ -98,11 +102,12 @@ Executor::add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify)
  weak_nodes_.push_back(node_ptr);
  if (notify) {
    // Interrupt waiting to handle new node
-    rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
-    if (status != RMW_RET_OK) {
-      throw std::runtime_error(rmw_get_error_string_safe());
+    if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+      throw std::runtime_error(rcl_get_error_string_safe());
    }
  }
+  // Add the node's notify condition to the guard condition handles
+  memory_strategy_->add_guard_condition(node_ptr->get_notify_guard_condition());
 }

 void
@@ -112,7 +117,7 @@ Executor::remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify)
  weak_nodes_.erase(
    std::remove_if(
      weak_nodes_.begin(), weak_nodes_.end(),
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
      [&](std::weak_ptr<rclcpp::node::Node> & i)
      {
        bool matched = (i.lock() == node_ptr);
@@ -126,12 +131,12 @@ Executor::remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify)
  if (notify) {
    // If the node was matched and removed, interrupt waiting
    if (node_removed) {
-      rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
-      if (status != RMW_RET_OK) {
-        throw std::runtime_error(rmw_get_error_string_safe());
+      if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+        throw std::runtime_error(rcl_get_error_string_safe());
      }
    }
  }
+  memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition());
 }

 void
@@ -183,9 +188,8 @@ void
 Executor::cancel()
 {
  spinning.store(false);
-  rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
-  if (status != RMW_RET_OK) {
-    throw std::runtime_error(rmw_get_error_string_safe());
+  if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+    throw std::runtime_error(rcl_get_error_string_safe());
  }
 }

@@ -223,9 +227,8 @@ Executor::execute_any_executable(AnyExecutable::SharedPtr any_exec)
  any_exec->callback_group->can_be_taken_from().store(true);
  // Wake the wait, because it may need to be recalculated or work that
  // was previously blocked is now available.
-  rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
-  if (status != RMW_RET_OK) {
-    throw std::runtime_error(rmw_get_error_string_safe());
+  if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+    throw std::runtime_error(rcl_get_error_string_safe());
  }
 }

@@ -234,20 +237,17 @@ Executor::execute_subscription(
  rclcpp::subscription::SubscriptionBase::SharedPtr subscription)
 {
  std::shared_ptr<void> message = subscription->create_message();
-  bool taken = false;
  rmw_message_info_t message_info;
-  auto ret =
-    rmw_take_with_info(subscription->get_subscription_handle(),
-      message.get(), &taken, &message_info);
-  if (ret == RMW_RET_OK) {
-    if (taken) {
-      message_info.from_intra_process = false;
-      subscription->handle_message(message, message_info);
-    }
-  } else {
+
+  auto ret = rcl_take(subscription->get_subscription_handle(),
+      message.get(), &message_info);
+  if (ret == RCL_RET_OK) {
+    message_info.from_intra_process = false;
+    subscription->handle_message(message, message_info);
+  } else if (ret != RCL_RET_SUBSCRIPTION_TAKE_FAILED) {
    fprintf(stderr,
      "[rclcpp::error] take failed for subscription on topic '%s': %s\n",
-      subscription->get_topic_name().c_str(), rmw_get_error_string_safe());
+      subscription->get_topic_name().c_str(), rcl_get_error_string_safe());
  }
  subscription->return_message(message);
 }
@@ -257,22 +257,19 @@ Executor::execute_intra_process_subscription(
  rclcpp::subscription::SubscriptionBase::SharedPtr subscription)
 {
  rcl_interfaces::msg::IntraProcessMessage ipm;
-  bool taken = false;
  rmw_message_info_t message_info;
-  rmw_ret_t status = rmw_take_with_info(
+  rcl_ret_t status = rcl_take(
    subscription->get_intra_process_subscription_handle(),
    &ipm,
-    &taken,
    &message_info);
-  if (status == RMW_RET_OK) {
-    if (taken) {
-      message_info.from_intra_process = true;
-      subscription->handle_intra_process_message(ipm, message_info);
-    }
-  } else {
+
+  if (status == RCL_RET_OK) {
+    message_info.from_intra_process = true;
+    subscription->handle_intra_process_message(ipm, message_info);
+  } else if (status != RCL_RET_SUBSCRIPTION_TAKE_FAILED) {
    fprintf(stderr,
      "[rclcpp::error] take failed for intra process subscription on topic '%s': %s\n",
-      subscription->get_topic_name().c_str(), rmw_get_error_string_safe());
+      subscription->get_topic_name().c_str(), rcl_get_error_string_safe());
  }
 }

@@ -287,22 +284,20 @@ void
 Executor::execute_service(
  rclcpp::service::ServiceBase::SharedPtr service)
 {
-  std::shared_ptr<void> request_header = service->create_request_header();
+  auto request_header = service->create_request_header();
  std::shared_ptr<void> request = service->create_request();
-  bool taken = false;
-  rmw_ret_t status = rmw_take_request(
+  rcl_ret_t status = rcl_take_request(
    service->get_service_handle(),
    request_header.get(),
-    request.get(),
-    &taken);
-  if (status == RMW_RET_OK) {
-    if (taken) {
+    request.get());
+  if (status != RCL_RET_SERVICE_TAKE_FAILED) {
+    if (status == RCL_RET_OK) {
      service->handle_request(request_header, request);
    }
  } else {
    fprintf(stderr,
      "[rclcpp::error] take request failed for server of service '%s': %s\n",
-      service->get_service_name().c_str(), rmw_get_error_string_safe());
+      service->get_service_name().c_str(), rcl_get_error_string_safe());
  }
 }

@@ -310,31 +305,28 @@ void
 Executor::execute_client(
  rclcpp::client::ClientBase::SharedPtr client)
 {
-  std::shared_ptr<void> request_header = client->create_request_header();
+  auto request_header = client->create_request_header();
  std::shared_ptr<void> response = client->create_response();
-  bool taken = false;
-  rmw_ret_t status = rmw_take_response(
+  rcl_ret_t status = rcl_take_response(
    client->get_client_handle(),
    request_header.get(),
-    response.get(),
-    &taken);
-  if (status == RMW_RET_OK) {
-    if (taken) {
+    response.get());
+  if (status != RCL_RET_SERVICE_TAKE_FAILED) {
+    if (status == RCL_RET_OK) {
      client->handle_response(request_header, response);
    }
  } else {
    fprintf(stderr,
      "[rclcpp::error] take response failed for client of service '%s': %s\n",
-      client->get_service_name().c_str(), rmw_get_error_string_safe());
+      client->get_service_name().c_str(), rcl_get_error_string_safe());
  }
 }

 void
 Executor::wait_for_work(std::chrono::nanoseconds timeout)
 {
-  memory_strategy_->clear_active_entities();
-
  // Collect the subscriptions and timers to be waited on
+  memory_strategy_->clear_handles();
  bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_);

  // Clean up any invalid nodes, if they were detected
@@ -342,7 +334,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
    weak_nodes_.erase(
      remove_if(
        weak_nodes_.begin(), weak_nodes_.end(),
-        // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+        // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
        [](std::weak_ptr<rclcpp::node::Node> i)
        {
          return i.expired();
@@ -352,57 +344,75 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
    );
  }

-  // Use the number of subscriptions to allocate memory in the handles
-  rmw_subscriptions_t subscriber_handles;
-  subscriber_handles.subscriber_count =
-    memory_strategy_->fill_subscriber_handles(subscriber_handles.subscribers);
-
-  rmw_services_t service_handles;
-  service_handles.service_count =
-    memory_strategy_->fill_service_handles(service_handles.services);
-
-  rmw_clients_t client_handles;
-  client_handles.client_count =
-    memory_strategy_->fill_client_handles(client_handles.clients);
-
-  // Don't pass guard conditions to rmw_wait; they are permanent fixtures of the waitset
-
-  rmw_time_t * wait_timeout = NULL;
-  rmw_time_t rmw_timeout;
-
-  auto next_timer_duration = get_earliest_timer();
-  // If the next timer timeout must preempt the requested timeout
-  // or if the requested timeout blocks forever, and there exists a valid timer,
-  // replace the requested timeout with the next timeout.
-  bool has_valid_timer = next_timer_duration >= std::chrono::nanoseconds::zero();
-  if ((next_timer_duration < timeout ||
-    timeout < std::chrono::nanoseconds::zero()) && has_valid_timer)
+  if (rcl_wait_set_resize_subscriptions(
+      &waitset_, memory_strategy_->number_of_ready_subscriptions()) != RCL_RET_OK)
  {
-    rmw_timeout.sec =
-      std::chrono::duration_cast<std::chrono::seconds>(next_timer_duration).count();
-    rmw_timeout.nsec = next_timer_duration.count() % (1000 * 1000 * 1000);
-    wait_timeout = &rmw_timeout;
-  } else if (timeout >= std::chrono::nanoseconds::zero()) {
-    // Convert timeout representation to rmw_time
-    rmw_timeout.sec = std::chrono::duration_cast<std::chrono::seconds>(timeout).count();
-    rmw_timeout.nsec = std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count() %
-      (1000 * 1000 * 1000);
-    wait_timeout = &rmw_timeout;
+    throw std::runtime_error(
+            std::string("Couldn't resize the number of subscriptions in waitset : ") +
+            rcl_get_error_string_safe());
  }

-  // Now wait on the waitable subscriptions and timers
-  rmw_ret_t status = rmw_wait(
-    &subscriber_handles,
-    nullptr,
-    &service_handles,
-    &client_handles,
-    waitset_,
-    wait_timeout);
-  if (status != RMW_RET_OK && status != RMW_RET_TIMEOUT) {
-    throw std::runtime_error(rmw_get_error_string_safe());
+  if (rcl_wait_set_resize_services(
+      &waitset_, memory_strategy_->number_of_ready_services()) != RCL_RET_OK)
+  {
+    throw std::runtime_error(
+            std::string("Couldn't resize the number of services in waitset : ") +
+            rcl_get_error_string_safe());
  }

-  memory_strategy_->remove_null_handles();
+  if (rcl_wait_set_resize_clients(
+      &waitset_, memory_strategy_->number_of_ready_clients()) != RCL_RET_OK)
+  {
+    throw std::runtime_error(
+            std::string("Couldn't resize the number of clients in waitset : ") +
+            rcl_get_error_string_safe());
+  }
+
+  if (rcl_wait_set_resize_guard_conditions(
+      &waitset_, memory_strategy_->number_of_guard_conditions()) != RCL_RET_OK)
+  {
+    throw std::runtime_error(
+            std::string("Couldn't resize the number of guard_conditions in waitset : ") +
+            rcl_get_error_string_safe());
+  }
+
+  if (rcl_wait_set_resize_timers(
+      &waitset_, memory_strategy_->number_of_ready_timers()) != RCL_RET_OK)
+  {
+    throw std::runtime_error(
+            std::string("Couldn't resize the number of timers in waitset : ") +
+            rcl_get_error_string_safe());
+  }
+
+  if (!memory_strategy_->add_handles_to_waitset(&waitset_)) {
+    throw std::runtime_error("Couldn't fill waitset");
+  }
+  rcl_ret_t status =
+    rcl_wait(&waitset_, std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count());
+  if (status == RCL_RET_WAIT_SET_EMPTY) {
+    fprintf(stderr, "Warning: empty waitset received in rcl_wait(). This should never happen.\n");
+  } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) {
+    throw std::runtime_error(std::string("rcl_wait() failed: ") + rcl_get_error_string_safe());
+  }
+
+  // check the null handles in the waitset and remove them from the handles in memory strategy
+  // for callback-based entities
+  memory_strategy_->remove_null_handles(&waitset_);
+  if (rcl_wait_set_clear_subscriptions(&waitset_) != RCL_RET_OK) {
+    throw std::runtime_error("Couldn't clear subscriptions from waitset");
+  }
+  if (rcl_wait_set_clear_services(&waitset_) != RCL_RET_OK) {
+    throw std::runtime_error("Couldn't clear servicess from waitset");
+  }
+  if (rcl_wait_set_clear_clients(&waitset_) != RCL_RET_OK) {
+    throw std::runtime_error("Couldn't clear clients from waitset");
+  }
+  if (rcl_wait_set_clear_guard_conditions(&waitset_) != RCL_RET_OK) {
+    throw std::runtime_error("Couldn't clear guard conditions from waitset");
+  }
+  if (rcl_wait_set_clear_timers(&waitset_) != RCL_RET_OK) {
+    throw std::runtime_error("Couldn't clear timers from waitset");
+  }
 }

 rclcpp::node::Node::SharedPtr
@@ -465,7 +475,7 @@ Executor::get_next_timer(AnyExecutable::SharedPtr any_exec)
      }
      for (auto & timer_ref : group->get_timer_ptrs()) {
        auto timer = timer_ref.lock();
-        if (timer && timer->check_and_trigger()) {
+        if (timer && timer->is_ready()) {
          any_exec->timer = timer;
          any_exec->callback_group = group;
          node = get_node_by_group(group);
@@ -476,37 +486,6 @@ Executor::get_next_timer(AnyExecutable::SharedPtr any_exec)
  }
 }

-std::chrono::nanoseconds
-Executor::get_earliest_timer()
-{
-  std::chrono::nanoseconds latest = std::chrono::nanoseconds::max();
-  bool timers_empty = true;
-  for (auto & weak_node : weak_nodes_) {
-    auto node = weak_node.lock();
-    if (!node) {
-      continue;
-    }
-    for (auto & weak_group : node->get_callback_groups()) {
-      auto group = weak_group.lock();
-      if (!group || !group->can_be_taken_from().load()) {
-        continue;
-      }
-      for (auto & timer_ref : group->get_timer_ptrs()) {
-        timers_empty = false;
-        // Check the expected trigger time
-        auto timer = timer_ref.lock();
-        if (timer && timer->time_until_trigger() < latest) {
-          latest = timer->time_until_trigger();
-        }
-      }
-    }
-  }
-  if (timers_empty) {
-    return std::chrono::nanoseconds(-1);
-  }
-  return latest;
-}
-
 AnyExecutable::SharedPtr
 Executor::get_next_ready_executable()
 {
@@ -569,3 +548,29 @@ Executor::get_next_executable(std::chrono::nanoseconds timeout)
  }
  return any_exec;
 }
+
+std::ostream &
+rclcpp::executor::operator<<(std::ostream & os, const FutureReturnCode & future_return_code)
+{
+  return os << to_string(future_return_code);
+}
+
+std::string
+rclcpp::executor::to_string(const FutureReturnCode & future_return_code)
+{
+  using enum_type = std::underlying_type<FutureReturnCode>::type;
+  std::string prefix = "Unknown enum value (";
+  std::string ret_as_string = std::to_string(static_cast<enum_type>(future_return_code));
+  switch (future_return_code) {
+    case FutureReturnCode::SUCCESS:
+      prefix = "SUCCESS (";
+      break;
+    case FutureReturnCode::INTERRUPTED:
+      prefix = "INTERRUPTED (";
+      break;
+    case FutureReturnCode::TIMEOUT:
+      prefix = "TIMEOUT (";
+      break;
+  }
+  return prefix + ret_as_string + ")";
+}
--- a/rclcpp/src/rclcpp/graph_listener.cpp
+++ b/rclcpp/src/rclcpp/graph_listener.cpp
@@ -0,0 +1,342 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/graph_listener.hpp"
+
+#include <cstdio>
+#include <exception>
+#include <string>
+#include <vector>
+
+#include "rcl/error_handling.h"
+#include "rcl/types.h"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/node.hpp"
+#include "rmw/impl/cpp/demangle.hpp"
+
+using rclcpp::exceptions::throw_from_rcl_error;
+
+namespace rclcpp
+{
+namespace graph_listener
+{
+
+GraphListener::GraphListener()
+: is_started_(false), is_shutdown_(false), shutdown_guard_condition_(nullptr)
+{
+  rcl_ret_t ret = rcl_guard_condition_init(
+    &interrupt_guard_condition_,
+    rcl_guard_condition_get_default_options());
+  if (RCL_RET_OK != ret) {
+    throw_from_rcl_error(ret, "failed to create interrupt guard condition");
+  }
+
+  shutdown_guard_condition_ = rclcpp::utilities::get_sigint_guard_condition(&wait_set_);
+}
+
+GraphListener::~GraphListener()
+{
+  this->shutdown();
+}
+
+void
+GraphListener::start_if_not_started()
+{
+  std::lock_guard<std::mutex> shutdown_lock(shutdown_mutex_);
+  if (is_shutdown_.load()) {
+    throw GraphListenerShutdownError();
+  }
+  if (!is_started_) {
+    // Initialize the wait set before starting.
+    rcl_ret_t ret = rcl_wait_set_init(
+      &wait_set_,
+      0,  // number_of_subscriptions
+      2,  // number_of_guard_conditions
+      0,  // number_of_timers
+      0,  // number_of_clients
+      0,  // number_of_services
+      rcl_get_default_allocator());
+    if (RCL_RET_OK != ret) {
+      throw_from_rcl_error(ret, "failed to initialize wait set");
+    }
+    // Register an on_shutdown hook to shtudown the graph listener.
+    // This is important to ensure that the wait set is finalized before
+    // destruction of static objects occurs.
+    std::weak_ptr<GraphListener> weak_this = shared_from_this();
+    rclcpp::utilities::on_shutdown([weak_this]() {
+          auto shared_this = weak_this.lock();
+          if (shared_this) {
+            shared_this->shutdown();
+          }
+        });
+    // Start the listener thread.
+    listener_thread_ = std::thread(&GraphListener::run, this);
+    is_started_ = true;
+  }
+}
+
+void
+GraphListener::run()
+{
+  try {
+    run_loop();
+  } catch (const std::exception & exc) {
+    fprintf(stderr,
+      "[rclcpp] caught %s exception in GraphListener thread: %s\n",
+      rmw::impl::cpp::demangle(exc).c_str(),
+      exc.what());
+    std::rethrow_exception(std::current_exception());
+  } catch (...) {
+    fprintf(stderr, "[rclcpp] unknown error in GraphListener thread\n");
+    std::rethrow_exception(std::current_exception());
+  }
+}
+
+void
+GraphListener::run_loop()
+{
+  while (true) {
+    // If shutdown() was called, exit.
+    if (is_shutdown_.load()) {
+      return;
+    }
+    rcl_ret_t ret;
+    {
+      // This "barrier" lock ensures that other functions can acquire the
+      // nodes_mutex_ after waking up rcl_wait.
+      std::lock_guard<std::mutex> nodes_barrier_lock(nodes_barrier_mutex_);
+      // This is ownership is passed to nodes_lock in the next line.
+      nodes_mutex_.lock();
+    }
+    // This lock is released when the loop continues or exits.
+    std::lock_guard<std::mutex> nodes_lock(nodes_mutex_, std::adopt_lock);
+
+    // Resize the wait set if necessary.
+    if (wait_set_.size_of_guard_conditions < (nodes_.size() + 2)) {
+      ret = rcl_wait_set_resize_guard_conditions(&wait_set_, nodes_.size() + 2);
+      if (RCL_RET_OK != ret) {
+        throw_from_rcl_error(ret, "failed to resize wait set");
+      }
+    }
+    // Clear the wait set's guard conditions.
+    ret = rcl_wait_set_clear_guard_conditions(&wait_set_);
+    if (RCL_RET_OK != ret) {
+      throw_from_rcl_error(ret, "failed to clear wait set");
+    }
+    // Put the interrupt guard condition in the wait set.
+    ret = rcl_wait_set_add_guard_condition(&wait_set_, &interrupt_guard_condition_);
+    if (RCL_RET_OK != ret) {
+      throw_from_rcl_error(ret, "failed to add interrupt guard condition to wait set");
+    }
+    // Put the shutdown guard condition in the wait set.
+    ret = rcl_wait_set_add_guard_condition(&wait_set_, shutdown_guard_condition_);
+    if (RCL_RET_OK != ret) {
+      throw_from_rcl_error(ret, "failed to add shutdown guard condition to wait set");
+    }
+    // Put graph guard conditions for each node into the wait set.
+    for (const auto node_ptr : nodes_) {
+      // Only wait on graph changes if some user of the node is watching.
+      if (node_ptr->count_graph_users() == 0) {
+        continue;
+      }
+      // Add the graph guard condition for the node to the wait set.
+      auto graph_gc = rcl_node_get_graph_guard_condition(node_ptr->get_rcl_node_handle());
+      if (!graph_gc) {
+        throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition");
+      }
+      ret = rcl_wait_set_add_guard_condition(&wait_set_, graph_gc);
+      if (RCL_RET_OK != ret) {
+        throw_from_rcl_error(ret, "failed to add graph guard condition to wait set");
+      }
+    }
+
+    // Wait for: graph changes, interrupt, or shutdown/SIGINT
+    ret = rcl_wait(&wait_set_, -1);  // block for ever until a guard condition is triggered
+    if (RCL_RET_TIMEOUT == ret) {
+      throw std::runtime_error("rcl_wait unexpectedly timed out");
+    }
+    if (RCL_RET_OK != ret) {
+      throw_from_rcl_error(ret, "failed to wait on wait set");
+    }
+
+    bool shutdown_guard_condition_triggered = false;
+    // Check to see if the shutdown guard condition has been triggered.
+    for (size_t i = 0; i < wait_set_.size_of_guard_conditions; ++i) {
+      if (shutdown_guard_condition_ == wait_set_.guard_conditions[i]) {
+        shutdown_guard_condition_triggered = true;
+      }
+    }
+    // Notify nodes who's guard conditions are set (triggered).
+    for (const auto node_ptr : nodes_) {
+      auto graph_gc = rcl_node_get_graph_guard_condition(node_ptr->get_rcl_node_handle());
+      if (!graph_gc) {
+        throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition");
+      }
+      for (size_t i = 0; i < wait_set_.size_of_guard_conditions; ++i) {
+        if (graph_gc == wait_set_.guard_conditions[i]) {
+          node_ptr->notify_graph_change();
+        }
+      }
+      if (shutdown_guard_condition_triggered) {
+        // If shutdown, then notify the node of this as well.
+        node_ptr->notify_shutdown();
+      }
+    }
+  }  // while (true)
+}
+
+static void
+interrupt_(rcl_guard_condition_t * interrupt_guard_condition)
+{
+  rcl_ret_t ret = rcl_trigger_guard_condition(interrupt_guard_condition);
+  if (RCL_RET_OK != ret) {
+    throw_from_rcl_error(ret, "failed to trigger the interrupt guard condition");
+  }
+}
+
+static void
+acquire_nodes_lock_(
+  std::mutex * nodes_barrier_mutex,
+  std::mutex * nodes_mutex,
+  rcl_guard_condition_t * interrupt_guard_condition)
+{
+  {
+    // Acquire this lock to prevent the run loop from re-locking the
+    // nodes_mutext_ after being woken up.
+    std::lock_guard<std::mutex> nodes_barrier_lock(*nodes_barrier_mutex);
+    // Trigger the interrupt guard condition to wake up rcl_wait.
+    interrupt_(interrupt_guard_condition);
+    nodes_mutex->lock();
+  }
+}
+
+static bool
+has_node_(std::vector<rclcpp::node::Node *> * nodes, rclcpp::node::Node * node)
+{
+  for (const auto node_ptr : (*nodes)) {
+    if (node == node_ptr) {
+      return true;
+    }
+  }
+  return false;
+}
+
+bool
+GraphListener::has_node(rclcpp::node::Node * node)
+{
+  if (!node) {
+    return false;
+  }
+  // Acquire the nodes mutex using the barrier to prevent the run loop from
+  // re-locking the nodes mutex after being interrupted.
+  acquire_nodes_lock_(&nodes_barrier_mutex_, &nodes_mutex_, &interrupt_guard_condition_);
+  // Store the now acquired nodes_mutex_ in the scoped lock using adopt_lock.
+  std::lock_guard<std::mutex> nodes_lock(nodes_mutex_, std::adopt_lock);
+  return has_node_(&nodes_, node);
+}
+
+void
+GraphListener::add_node(rclcpp::node::Node * node)
+{
+  if (!node) {
+    throw std::invalid_argument("node is nullptr");
+  }
+  std::lock_guard<std::mutex> shutdown_lock(shutdown_mutex_);
+  if (is_shutdown_.load()) {
+    throw GraphListenerShutdownError();
+  }
+
+  // Acquire the nodes mutex using the barrier to prevent the run loop from
+  // re-locking the nodes mutex after being interrupted.
+  acquire_nodes_lock_(&nodes_barrier_mutex_, &nodes_mutex_, &interrupt_guard_condition_);
+  // Store the now acquired nodes_mutex_ in the scoped lock using adopt_lock.
+  std::lock_guard<std::mutex> nodes_lock(nodes_mutex_, std::adopt_lock);
+  if (has_node_(&nodes_, node)) {
+    throw NodeAlreadyAddedError();
+  }
+  nodes_.push_back(node);
+  // The run loop has already been interrupted by acquire_nodes_lock_() and
+  // will evaluate the new node when nodes_lock releases the nodes_mutex_.
+}
+
+static void
+remove_node_(std::vector<rclcpp::node::Node *> * nodes, rclcpp::node::Node * node)
+{
+  // Remove the node if it is found.
+  for (auto it = nodes->begin(); it != nodes->end(); ++it) {
+    if (node == *it) {
+      // Found the node, remove it.
+      nodes->erase(it);
+      // Now trigger the interrupt guard condition to make sure
+      return;
+    }
+  }
+  // Not found in the loop.
+  throw NodeNotFoundError();
+}
+
+void
+GraphListener::remove_node(rclcpp::node::Node * node)
+{
+  if (!node) {
+    throw std::invalid_argument("node is nullptr");
+  }
+  std::lock_guard<std::mutex> shutdown_lock(shutdown_mutex_);
+  if (is_shutdown()) {
+    // If shutdown, then the run loop has been joined, so we can remove them directly.
+    return remove_node_(&nodes_, node);
+  }
+  // Otherwise, first interrupt and lock against the run loop to safely remove the node.
+  // Acquire the nodes mutex using the barrier to prevent the run loop from
+  // re-locking the nodes mutex after being interrupted.
+  acquire_nodes_lock_(&nodes_barrier_mutex_, &nodes_mutex_, &interrupt_guard_condition_);
+  // Store the now acquired nodes_mutex_ in the scoped lock using adopt_lock.
+  std::lock_guard<std::mutex> nodes_lock(nodes_mutex_, std::adopt_lock);
+  remove_node_(&nodes_, node);
+}
+
+void
+GraphListener::shutdown()
+{
+  std::lock_guard<std::mutex> shutdown_lock(shutdown_mutex_);
+  if (!is_shutdown_.exchange(true)) {
+    if (is_started_) {
+      interrupt_(&interrupt_guard_condition_);
+      listener_thread_.join();
+    }
+    rcl_ret_t ret = rcl_guard_condition_fini(&interrupt_guard_condition_);
+    if (RCL_RET_OK != ret) {
+      throw_from_rcl_error(ret, "failed to finalize interrupt guard condition");
+    }
+    if (shutdown_guard_condition_) {
+      rclcpp::utilities::release_sigint_guard_condition(&wait_set_);
+      shutdown_guard_condition_ = nullptr;
+    }
+    if (is_started_) {
+      ret = rcl_wait_set_fini(&wait_set_);
+      if (RCL_RET_OK != ret) {
+        throw_from_rcl_error(ret, "failed to finalize wait set");
+      }
+    }
+  }
+}
+
+bool
+GraphListener::is_shutdown()
+{
+  return is_shutdown_.load();
+}
+
+}  // namespace graph_listener
+}  // namespace rclcpp
--- a/rclcpp/src/rclcpp/intra_process_manager.cpp
+++ b/rclcpp/src/rclcpp/intra_process_manager.cpp
@@ -69,7 +69,7 @@ IntraProcessManager::get_next_unique_id()
    // So around 585 million years. Even at 1 GHz, it would take 585 years.
    // I think it's safe to avoid trying to handle overflow.
    // If we roll over then it's most likely a bug.
-    // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+    // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
    throw std::overflow_error(
      "exhausted the unique id's for publishers and subscribers in this process "
      "(congratulations your computer is either extremely fast or extremely old)");
--- a/rclcpp/src/rclcpp/memory_strategy.cpp
+++ b/rclcpp/src/rclcpp/memory_strategy.cpp
@@ -18,7 +18,7 @@ using rclcpp::memory_strategy::MemoryStrategy;

 rclcpp::subscription::SubscriptionBase::SharedPtr
 MemoryStrategy::get_subscription_by_handle(
-  void * subscriber_handle, const WeakNodeVector & weak_nodes)
+  const rcl_subscription_t * subscriber_handle, const WeakNodeVector & weak_nodes)
 {
  for (auto & weak_node : weak_nodes) {
    auto node = weak_node.lock();
@@ -33,12 +33,10 @@ MemoryStrategy::get_subscription_by_handle(
      for (auto & weak_subscription : group->get_subscription_ptrs()) {
        auto subscription = weak_subscription.lock();
        if (subscription) {
-          if (subscription->get_subscription_handle()->data == subscriber_handle) {
+          if (subscription->get_subscription_handle() == subscriber_handle) {
            return subscription;
          }
-          if (subscription->get_intra_process_subscription_handle() &&
-            subscription->get_intra_process_subscription_handle()->data == subscriber_handle)
-          {
+          if (subscription->get_intra_process_subscription_handle() == subscriber_handle) {
            return subscription;
          }
        }
@@ -49,7 +47,8 @@ MemoryStrategy::get_subscription_by_handle(
 }

 rclcpp::service::ServiceBase::SharedPtr
-MemoryStrategy::get_service_by_handle(void * service_handle, const WeakNodeVector & weak_nodes)
+MemoryStrategy::get_service_by_handle(const rcl_service_t * service_handle,
+  const WeakNodeVector & weak_nodes)
 {
  for (auto & weak_node : weak_nodes) {
    auto node = weak_node.lock();
@@ -62,7 +61,7 @@ MemoryStrategy::get_service_by_handle(void * service_handle, const WeakNodeVecto
        continue;
      }
      for (auto & service : group->get_service_ptrs()) {
-        if (service->get_service_handle()->data == service_handle) {
+        if (service->get_service_handle() == service_handle) {
          return service;
        }
      }
@@ -72,7 +71,8 @@ MemoryStrategy::get_service_by_handle(void * service_handle, const WeakNodeVecto
 }

 rclcpp::client::ClientBase::SharedPtr
-MemoryStrategy::get_client_by_handle(void * client_handle, const WeakNodeVector & weak_nodes)
+MemoryStrategy::get_client_by_handle(const rcl_client_t * client_handle,
+  const WeakNodeVector & weak_nodes)
 {
  for (auto & weak_node : weak_nodes) {
    auto node = weak_node.lock();
@@ -86,7 +86,7 @@ MemoryStrategy::get_client_by_handle(void * client_handle, const WeakNodeVector
      }
      for (auto & weak_client : group->get_client_ptrs()) {
        auto client = weak_client.lock();
-        if (client && client->get_client_handle()->data == client_handle) {
+        if (client && client->get_client_handle() == client_handle) {
          return client;
        }
      }
--- a/rclcpp/src/rclcpp/node.cpp
+++ b/rclcpp/src/rclcpp/node.cpp
@@ -19,9 +19,13 @@
 #include <utility>
 #include <vector>

+#include "rcl_interfaces/srv/list_parameters.hpp"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/graph_listener.hpp"
 #include "rclcpp/node.hpp"

 using rclcpp::node::Node;
+using rclcpp::exceptions::throw_from_rcl_error;

 Node::Node(const std::string & node_name, bool use_intra_process_comms)
 : Node(
@@ -36,8 +40,16 @@ Node::Node(
  bool use_intra_process_comms)
 : name_(node_name), context_(context),
  number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0),
-  use_intra_process_comms_(use_intra_process_comms)
+  use_intra_process_comms_(use_intra_process_comms), notify_guard_condition_is_valid_(false),
+  graph_listener_(context->get_sub_context<rclcpp::graph_listener::GraphListener>()),
+  should_add_to_graph_listener_(true), graph_users_count_(0)
 {
+  rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options();
+  rcl_ret_t ret = rcl_guard_condition_init(&notify_guard_condition_, guard_condition_options);
+  if (ret != RCL_RET_OK) {
+    throw_from_rcl_error(ret, "failed to create interrupt guard condition");
+  }
+
  has_executor.store(false);
  size_t domain_id = 0;
  char * ros_domain_id = nullptr;
@@ -51,6 +63,11 @@ Node::Node(
  if (ros_domain_id) {
    uint32_t number = strtoul(ros_domain_id, NULL, 0);
    if (number == (std::numeric_limits<uint32_t>::max)()) {
+      // Finalize the interrupt guard condition.
+      if (rcl_guard_condition_fini(&notify_guard_condition_) != RCL_RET_OK) {
+        fprintf(stderr,
+          "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
+      }
      throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number");
    }
    domain_id = static_cast<size_t>(number);
@@ -59,30 +76,54 @@ Node::Node(
 #endif
  }

-  auto node = rmw_create_node(name_.c_str(), domain_id);
-  if (!node) {
-    // *INDENT-OFF*
-    throw std::runtime_error(
-      std::string("could not create node: ") +
-      rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
-  // Initialize node handle shared_ptr with custom deleter.
-  // *INDENT-OFF*
-  node_handle_.reset(node, [](rmw_node_t * node) {
-    auto ret = rmw_destroy_node(node);
-    if (ret != RMW_RET_OK) {
+  rcl_node_t * rcl_node = new rcl_node_t(rcl_get_zero_initialized_node());
+  node_handle_.reset(rcl_node, [](rcl_node_t * node) {
+    if (rcl_node_fini(node) != RCL_RET_OK) {
      fprintf(
-        stderr, "Error in destruction of rmw node handle: %s\n", rmw_get_error_string_safe());
+        stderr, "Error in destruction of rmw node handle: %s\n", rcl_get_error_string_safe());
    }
+    delete node;
  });
-  // *INDENT-ON*
+  rcl_node_options_t options = rcl_node_get_default_options();
+  // TODO(jacquelinekay): Allocator options
+  options.domain_id = domain_id;
+  ret = rcl_node_init(node_handle_.get(), name_.c_str(), &options);
+  if (ret != RCL_RET_OK) {
+    // Finalize the interrupt guard condition.
+    if (rcl_guard_condition_fini(&notify_guard_condition_) != RCL_RET_OK) {
+      fprintf(stderr,
+        "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
+    }
+
+    throw_from_rcl_error(ret, "failed to initialize rcl node");
+  }

  using rclcpp::callback_group::CallbackGroupType;
  default_callback_group_ = create_callback_group(
    CallbackGroupType::MutuallyExclusive);
  events_publisher_ = create_publisher<rcl_interfaces::msg::ParameterEvent>(
    "parameter_events", rmw_qos_profile_parameter_events);
+  notify_guard_condition_is_valid_ = true;
+}
+
+Node::~Node()
+{
+  // Remove self from graph listener.
+  // Exchange with false to prevent others from trying to add this node to the
+  // graph listener after checking that it was not here.
+  if (!should_add_to_graph_listener_.exchange(false)) {
+    // If it was already false, then it needs to now be removed.
+    graph_listener_->remove_node(this);
+  }
+  // Finalize the interrupt guard condition after removing self from graph listener.
+  {
+    std::lock_guard<std::mutex> notify_guard_condition_lock(notify_guard_condition_mutex_);
+    notify_guard_condition_is_valid_ = false;
+    if (rcl_guard_condition_fini(&notify_guard_condition_) != RCL_RET_OK) {
+      fprintf(stderr,
+        "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
+    }
+  }
 }

 const std::string &
@@ -241,17 +282,21 @@ Node::list_parameters(

  // TODO(esteve): define parameter separator, use "." for now
  for (auto & kv : parameters_) {
-    if (std::any_of(prefixes.cbegin(), prefixes.cend(), [&kv, &depth](const std::string & prefix) {
+    if (((prefixes.size() == 0) &&
+      ((depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) ||
+      (static_cast<uint64_t>(std::count(kv.first.begin(), kv.first.end(), '.')) < depth))) ||
+      (std::any_of(prefixes.cbegin(), prefixes.cend(), [&kv, &depth](const std::string & prefix) {
      if (kv.first == prefix) {
        return true;
      } else if (kv.first.find(prefix + ".") == 0) {
        size_t length = prefix.length();
        std::string substr = kv.first.substr(length);
        // Cast as unsigned integer to avoid warning
-        return static_cast<uint64_t>(std::count(substr.begin(), substr.end(), '.')) < depth;
+        return (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) ||
+        (static_cast<uint64_t>(std::count(substr.begin(), substr.end(), '.')) < depth);
      }
      return false;
-    }))
+    })))
    {
      result.names.push_back(kv.first);
      size_t last_separator = kv.first.find_last_of('.');
@@ -276,7 +321,8 @@ Node::get_topic_names_and_types() const
  topic_names_and_types.topic_names = nullptr;
  topic_names_and_types.type_names = nullptr;

-  auto ret = rmw_get_topic_names_and_types(node_handle_.get(), &topic_names_and_types);
+  auto ret = rmw_get_topic_names_and_types(rcl_node_get_rmw_handle(node_handle_.get()),
+      &topic_names_and_types);
  if (ret != RMW_RET_OK) {
    // *INDENT-OFF*
    throw std::runtime_error(
@@ -304,7 +350,8 @@ size_t
 Node::count_publishers(const std::string & topic_name) const
 {
  size_t count;
-  auto ret = rmw_count_publishers(node_handle_.get(), topic_name.c_str(), &count);
+  auto ret = rmw_count_publishers(rcl_node_get_rmw_handle(node_handle_.get()),
+      topic_name.c_str(), &count);
  if (ret != RMW_RET_OK) {
    // *INDENT-OFF*
    throw std::runtime_error(
@@ -318,7 +365,8 @@ size_t
 Node::count_subscribers(const std::string & topic_name) const
 {
  size_t count;
-  auto ret = rmw_count_subscribers(node_handle_.get(), topic_name.c_str(), &count);
+  auto ret = rmw_count_subscribers(rcl_node_get_rmw_handle(node_handle_.get()),
+      topic_name.c_str(), &count);
  if (ret != RMW_RET_OK) {
    // *INDENT-OFF*
    throw std::runtime_error(
@@ -334,3 +382,130 @@ Node::get_callback_groups() const
 {
  return callback_groups_;
 }
+
+const rcl_guard_condition_t *
+Node::get_notify_guard_condition() const
+{
+  std::lock_guard<std::mutex> notify_guard_condition_lock(notify_guard_condition_mutex_);
+  if (!notify_guard_condition_is_valid_) {
+    return nullptr;
+  }
+  return &notify_guard_condition_;
+}
+
+const rcl_guard_condition_t *
+Node::get_graph_guard_condition() const
+{
+  return rcl_node_get_graph_guard_condition(node_handle_.get());
+}
+
+const rcl_node_t *
+Node::get_rcl_node_handle() const
+{
+  return node_handle_.get();
+}
+
+rcl_node_t *
+Node::get_rcl_node_handle()
+{
+  return node_handle_.get();
+}
+
+std::shared_ptr<rcl_node_t>
+Node::get_shared_node_handle()
+{
+  return node_handle_;
+}
+
+void
+Node::notify_graph_change()
+{
+  {
+    std::lock_guard<std::mutex> graph_changed_lock(graph_mutex_);
+    bool bad_ptr_encountered = false;
+    for (auto & event_wptr : graph_events_) {
+      auto event_ptr = event_wptr.lock();
+      if (event_ptr) {
+        event_ptr->set();
+      } else {
+        bad_ptr_encountered = true;
+      }
+    }
+    if (bad_ptr_encountered) {
+      // remove invalid pointers with the erase-remove idiom
+      graph_events_.erase(
+        std::remove_if(
+          graph_events_.begin(),
+          graph_events_.end(),
+          [](const rclcpp::event::Event::WeakPtr & wptr) {
+        return wptr.expired();
+      }),
+        graph_events_.end());
+      // update graph_users_count_
+      graph_users_count_.store(graph_events_.size());
+    }
+  }
+  graph_cv_.notify_all();
+  {
+    std::lock_guard<std::mutex> notify_guard_condition_lock(notify_guard_condition_mutex_);
+    rcl_ret_t ret = rcl_trigger_guard_condition(&notify_guard_condition_);
+    if (RCL_RET_OK != ret) {
+      throw_from_rcl_error(ret, "failed to trigger notify guard condition");
+    }
+  }
+}
+
+void
+Node::notify_shutdown()
+{
+  // notify here anything that will not be woken up by ctrl-c or rclcpp::shutdown().
+  graph_cv_.notify_all();
+}
+
+rclcpp::event::Event::SharedPtr
+Node::get_graph_event()
+{
+  auto event = rclcpp::event::Event::make_shared();
+  std::lock_guard<std::mutex> graph_changed_lock(graph_mutex_);
+  // on first call, add node to graph_listener_
+  if (should_add_to_graph_listener_.exchange(false)) {
+    graph_listener_->add_node(this);
+    graph_listener_->start_if_not_started();
+  }
+  graph_events_.push_back(event);
+  graph_users_count_++;
+  return event;
+}
+
+void
+Node::wait_for_graph_change(
+  rclcpp::event::Event::SharedPtr event,
+  std::chrono::nanoseconds timeout)
+{
+  if (!event) {
+    throw InvalidEventError();
+  }
+  {
+    std::lock_guard<std::mutex> graph_changed_lock(graph_mutex_);
+    bool event_in_graph_events = false;
+    for (const auto & event_wptr : graph_events_) {
+      if (event == event_wptr.lock()) {
+        event_in_graph_events = true;
+        break;
+      }
+    }
+    if (!event_in_graph_events) {
+      throw EventNotRegisteredError();
+    }
+  }
+  std::unique_lock<std::mutex> graph_lock(graph_mutex_);
+  graph_cv_.wait_for(graph_lock, timeout, [&event]() {
+    return event->check() || !rclcpp::utilities::ok();
+  });
+}
+
+size_t
+Node::count_graph_users()
+{
+  return graph_users_count_.load();
+}
--- a/rclcpp/src/rclcpp/parameter.cpp
+++ b/rclcpp/src/rclcpp/parameter.cpp
@@ -12,13 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.

-#include "rclcpp/parameter.hpp"
-
 #include <ostream>
 #include <sstream>
 #include <string>
 #include <vector>

+#include "rclcpp/parameter.hpp"
+#include "rclcpp/utilities.hpp"
+
 using rclcpp::parameter::ParameterType;
 using rclcpp::parameter::ParameterVariant;

@@ -105,7 +106,7 @@ ParameterVariant::get_type_name() const
    case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET:
      return "not set";
    default:
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
      throw std::runtime_error(
        "Unexpected type from ParameterVariant: " + std::to_string(get_type()));
      // *INDENT-ON*
@@ -172,7 +173,7 @@ ParameterVariant::from_parameter(const rcl_interfaces::msg::Parameter & paramete
      throw std::runtime_error("Type from ParameterValue is not set");
    default:
      // TODO(wjwwood): use custom exception
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
      throw std::runtime_error(
        "Unexpected type from ParameterVariant: " + std::to_string(parameter.value.type));
      // *INDENT-ON*
@@ -218,7 +219,7 @@ ParameterVariant::value_to_string() const
    case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET:
      return "not set";
    default:
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
      throw std::runtime_error(
        "Unexpected type from ParameterVariant: " + std::to_string(get_type()));
      // *INDENT-ON*
--- a/rclcpp/src/rclcpp/parameter_client.cpp
+++ b/rclcpp/src/rclcpp/parameter_client.cpp
@@ -23,7 +23,8 @@ using rclcpp::parameter_client::SyncParametersClient;

 AsyncParametersClient::AsyncParametersClient(
  const rclcpp::node::Node::SharedPtr node,
-  const std::string & remote_node_name)
+  const std::string & remote_node_name,
+  const rmw_qos_profile_t & qos_profile)
 : node_(node)
 {
  if (remote_node_name != "") {
@@ -32,15 +33,15 @@ AsyncParametersClient::AsyncParametersClient(
    remote_node_name_ = node_->get_name();
  }
  get_parameters_client_ = node_->create_client<rcl_interfaces::srv::GetParameters>(
-    remote_node_name_ + "__get_parameters");
+    remote_node_name_ + "__get_parameters", qos_profile);
  get_parameter_types_client_ = node_->create_client<rcl_interfaces::srv::GetParameterTypes>(
-    remote_node_name_ + "__get_parameter_types");
+    remote_node_name_ + "__get_parameter_types", qos_profile);
  set_parameters_client_ = node_->create_client<rcl_interfaces::srv::SetParameters>(
-    remote_node_name_ + "__set_parameters");
+    remote_node_name_ + "__set_parameters", qos_profile);
  list_parameters_client_ = node_->create_client<rcl_interfaces::srv::ListParameters>(
-    remote_node_name_ + "__list_parameters");
+    remote_node_name_ + "__list_parameters", qos_profile);
  describe_parameters_client_ = node_->create_client<rcl_interfaces::srv::DescribeParameters>(
-    remote_node_name_ + "__describe_parameters");
+    remote_node_name_ + "__describe_parameters", qos_profile);
 }

 std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>
@@ -57,7 +58,7 @@ AsyncParametersClient::get_parameters(
  auto request = std::make_shared<rcl_interfaces::srv::GetParameters::Request>();
  request->names = names;

-  // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+  // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
  get_parameters_client_->async_send_request(
    request,
    [request, promise_result, future_result, &callback](
@@ -100,7 +101,7 @@ AsyncParametersClient::get_parameter_types(
  auto request = std::make_shared<rcl_interfaces::srv::GetParameterTypes::Request>();
  request->names = names;

-  // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+  // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
  get_parameter_types_client_->async_send_request(
    request,
    [promise_result, future_result, &callback](
@@ -135,7 +136,7 @@ AsyncParametersClient::set_parameters(

  auto request = std::make_shared<rcl_interfaces::srv::SetParameters::Request>();

-  // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+  // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
  std::transform(parameters.begin(), parameters.end(), std::back_inserter(request->parameters),
    [](rclcpp::parameter::ParameterVariant p) {
      return p.to_parameter();
@@ -171,7 +172,7 @@ AsyncParametersClient::set_parameters_atomically(

  auto request = std::make_shared<rcl_interfaces::srv::SetParametersAtomically::Request>();

-  // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+  // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
  std::transform(parameters.begin(), parameters.end(), std::back_inserter(request->parameters),
    [](rclcpp::parameter::ParameterVariant p) {
      return p.to_parameter();
@@ -210,7 +211,7 @@ AsyncParametersClient::list_parameters(
  request->prefixes = prefixes;
  request->depth = depth;

-  // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+  // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
  list_parameters_client_->async_send_request(
    request,
    [promise_result, future_result, &callback](
@@ -228,19 +229,21 @@ AsyncParametersClient::list_parameters(
 }

 SyncParametersClient::SyncParametersClient(
-  rclcpp::node::Node::SharedPtr node)
+  rclcpp::node::Node::SharedPtr node,
+  const rmw_qos_profile_t & qos_profile)
 : node_(node)
 {
  executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
-  async_parameters_client_ = std::make_shared<AsyncParametersClient>(node);
+  async_parameters_client_ = std::make_shared<AsyncParametersClient>(node, "", qos_profile);
 }

 SyncParametersClient::SyncParametersClient(
  rclcpp::executor::Executor::SharedPtr executor,
-  rclcpp::node::Node::SharedPtr node)
+  rclcpp::node::Node::SharedPtr node,
+  const rmw_qos_profile_t & qos_profile)
 : executor_(executor), node_(node)
 {
-  async_parameters_client_ = std::make_shared<AsyncParametersClient>(node);
+  async_parameters_client_ = std::make_shared<AsyncParametersClient>(node, "", qos_profile);
 }

 std::vector<rclcpp::parameter::ParameterVariant>
@@ -256,6 +259,15 @@ SyncParametersClient::get_parameters(const std::vector<std::string> & parameter_
  return std::vector<rclcpp::parameter::ParameterVariant>();
 }

+bool
+SyncParametersClient::has_parameter(const std::string & parameter_name)
+{
+  std::vector<std::string> names;
+  names.push_back(parameter_name);
+  auto vars = list_parameters(names, 1);
+  return vars.names.size() > 0;
+}
+
 std::vector<rclcpp::parameter::ParameterType>
 SyncParametersClient::get_parameter_types(const std::vector<std::string> & parameter_names)
 {
--- a/rclcpp/src/rclcpp/parameter_service.cpp
+++ b/rclcpp/src/rclcpp/parameter_service.cpp
@@ -19,11 +19,13 @@

 using rclcpp::parameter_service::ParameterService;

-ParameterService::ParameterService(const rclcpp::node::Node::SharedPtr node)
+ParameterService::ParameterService(
+  const rclcpp::node::Node::SharedPtr node,
+  const rmw_qos_profile_t & qos_profile)
 : node_(node)
 {
  std::weak_ptr<rclcpp::node::Node> captured_node = node_;
-  // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+  // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
  get_parameters_service_ = node_->create_service<rcl_interfaces::srv::GetParameters>(
    node_->get_name() + "__get_parameters",
    [captured_node](
@@ -39,7 +41,8 @@ ParameterService::ParameterService(const rclcpp::node::Node::SharedPtr node)
      for (auto & pvariant : values) {
        response->values.push_back(pvariant.get_parameter_value());
      }
-    }
+    },
+    qos_profile
  );

  get_parameter_types_service_ = node_->create_service<rcl_interfaces::srv::GetParameterTypes>(
@@ -58,7 +61,8 @@ ParameterService::ParameterService(const rclcpp::node::Node::SharedPtr node)
      std::back_inserter(response->types), [](const uint8_t & type) {
        return static_cast<rclcpp::parameter::ParameterType>(type);
      });
-    }
+    },
+    qos_profile
  );

  set_parameters_service_ = node_->create_service<rcl_interfaces::srv::SetParameters>(
@@ -78,7 +82,8 @@ ParameterService::ParameterService(const rclcpp::node::Node::SharedPtr node)
      }
      auto results = node->set_parameters(pvariants);
      response->results = results;
-    }
+    },
+    qos_profile
  );

  set_parameters_atomically_service_ =
@@ -102,7 +107,8 @@ ParameterService::ParameterService(const rclcpp::node::Node::SharedPtr node)
      });
      auto result = node->set_parameters_atomically(pvariants);
      response->result = result;
-    }
+    },
+    qos_profile
  );

  describe_parameters_service_ = node_->create_service<rcl_interfaces::srv::DescribeParameters>(
@@ -118,7 +124,8 @@ ParameterService::ParameterService(const rclcpp::node::Node::SharedPtr node)
      }
      auto descriptors = node->describe_parameters(request->names);
      response->descriptors = descriptors;
-    }
+    },
+    qos_profile
  );

  list_parameters_service_ = node_->create_service<rcl_interfaces::srv::ListParameters>(
@@ -134,7 +141,8 @@ ParameterService::ParameterService(const rclcpp::node::Node::SharedPtr node)
      }
      auto result = node->list_parameters(request->prefixes, request->depth);
      response->result = result;
-    }
+    },
+    qos_profile
  );
  // *INDENT-ON*
 }
--- a/rclcpp/src/rclcpp/publisher.cpp
+++ b/rclcpp/src/rclcpp/publisher.cpp
@@ -33,42 +33,17 @@
 using rclcpp::publisher::PublisherBase;

 PublisherBase::PublisherBase(
-  std::shared_ptr<rmw_node_t> node_handle,
-  rmw_publisher_t * publisher_handle,
+  std::shared_ptr<rcl_node_t> node_handle,
  std::string topic,
  size_t queue_size)
-: node_handle_(node_handle), publisher_handle_(publisher_handle),
-  intra_process_publisher_handle_(nullptr),
+: node_handle_(node_handle),
  topic_(topic), queue_size_(queue_size),
  intra_process_publisher_id_(0), store_intra_process_message_(nullptr)
 {
-  // Life time of this object is tied to the publisher handle.
-  if (rmw_get_gid_for_publisher(publisher_handle_, &rmw_gid_) != RMW_RET_OK) {
-    // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-    throw std::runtime_error(
-      std::string("failed to get publisher gid: ") + rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
 }

 PublisherBase::~PublisherBase()
 {
-  if (intra_process_publisher_handle_) {
-    if (rmw_destroy_publisher(node_handle_.get(), intra_process_publisher_handle_)) {
-      fprintf(
-        stderr,
-        "Error in destruction of intra process rmw publisher handle: %s\n",
-        rmw_get_error_string_safe());
-    }
-  }
-  if (publisher_handle_) {
-    if (rmw_destroy_publisher(node_handle_.get(), publisher_handle_) != RMW_RET_OK) {
-      fprintf(
-        stderr,
-        "Error in destruction of rmw publisher handle: %s\n",
-        rmw_get_error_string_safe());
-    }
-  }
 }

 const std::string &
@@ -124,15 +99,31 @@ void
 PublisherBase::setup_intra_process(
  uint64_t intra_process_publisher_id,
  StoreMessageCallbackT callback,
-  rmw_publisher_t * intra_process_publisher_handle)
+  const rcl_publisher_options_t & intra_process_options)
 {
+  if (rcl_publisher_init(
+      &intra_process_publisher_handle_, node_handle_.get(),
+      rclcpp::type_support::get_intra_process_message_msg_type_support(),
+      (topic_ + "__intra").c_str(), &intra_process_options) != RCL_RET_OK)
+  {
+    throw std::runtime_error(
+            std::string("could not create intra process publisher: ") +
+            rcl_get_error_string_safe());
+  }
+
  intra_process_publisher_id_ = intra_process_publisher_id;
  store_intra_process_message_ = callback;
-  intra_process_publisher_handle_ = intra_process_publisher_handle;
  // Life time of this object is tied to the publisher handle.
-  auto ret = rmw_get_gid_for_publisher(intra_process_publisher_handle_, &intra_process_rmw_gid_);
+  rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(
+    &intra_process_publisher_handle_);
+  if (publisher_rmw_handle == nullptr) {
+    throw std::runtime_error(std::string(
+              "Failed to get rmw publisher handle") + rcl_get_error_string_safe());
+  }
+  auto ret = rmw_get_gid_for_publisher(
+    publisher_rmw_handle, &intra_process_rmw_gid_);
  if (ret != RMW_RET_OK) {
-    // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+    // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
    throw std::runtime_error(
      std::string("failed to create intra process publisher gid: ") +
      rmw_get_error_string_safe());
--- a/rclcpp/src/rclcpp/service.cpp
+++ b/rclcpp/src/rclcpp/service.cpp
@@ -28,23 +28,13 @@
 using rclcpp::service::ServiceBase;

 ServiceBase::ServiceBase(
-  std::shared_ptr<rmw_node_t> node_handle,
-  rmw_service_t * service_handle,
+  std::shared_ptr<rcl_node_t> node_handle,
  const std::string service_name)
-: node_handle_(node_handle), service_handle_(service_handle), service_name_(service_name)
+: node_handle_(node_handle), service_name_(service_name)
 {}

 ServiceBase::~ServiceBase()
-{
-  if (service_handle_) {
-    if (rmw_destroy_service(service_handle_) != RMW_RET_OK) {
-      std::stringstream ss;
-      ss << "Error in destruction of rmw service_handle_ handle: " <<
-        rmw_get_error_string_safe() << '\n';
-      (std::cerr << ss.str()).flush();
-    }
-  }
-}
+{}

 std::string
 ServiceBase::get_service_name()
@@ -52,8 +42,8 @@ ServiceBase::get_service_name()
  return this->service_name_;
 }

-const rmw_service_t *
+const rcl_service_t *
 ServiceBase::get_service_handle()
 {
-  return this->service_handle_;
+  return &service_handle_;
 }
--- a/rclcpp/src/rclcpp/subscription.cpp
+++ b/rclcpp/src/rclcpp/subscription.cpp
@@ -24,13 +24,10 @@
 using rclcpp::subscription::SubscriptionBase;

 SubscriptionBase::SubscriptionBase(
-  std::shared_ptr<rmw_node_t> node_handle,
-  rmw_subscription_t * subscription_handle,
+  std::shared_ptr<rcl_node_t> node_handle,
  const std::string & topic_name,
  bool ignore_local_publications)
-: intra_process_subscription_handle_(nullptr),
-  node_handle_(node_handle),
-  subscription_handle_(subscription_handle),
+:   node_handle_(node_handle),
  topic_name_(topic_name),
  ignore_local_publications_(ignore_local_publications)
 {
@@ -40,22 +37,19 @@ SubscriptionBase::SubscriptionBase(

 SubscriptionBase::~SubscriptionBase()
 {
-  if (subscription_handle_) {
-    if (rmw_destroy_subscription(node_handle_.get(), subscription_handle_) != RMW_RET_OK) {
-      std::stringstream ss;
-      ss << "Error in destruction of rmw subscription handle: " <<
-        rmw_get_error_string_safe() << '\n';
-      (std::cerr << ss.str()).flush();
-    }
+  if (rcl_subscription_fini(&subscription_handle_, node_handle_.get()) != RCL_RET_OK) {
+    std::stringstream ss;
+    ss << "Error in destruction of rcl subscription handle: " <<
+      rcl_get_error_string_safe() << '\n';
+    (std::cerr << ss.str()).flush();
  }
-  if (intra_process_subscription_handle_) {
-    auto ret = rmw_destroy_subscription(node_handle_.get(), intra_process_subscription_handle_);
-    if (ret != RMW_RET_OK) {
-      std::stringstream ss;
-      ss << "Error in destruction of rmw intra process subscription handle: " <<
-        rmw_get_error_string_safe() << '\n';
-      (std::cerr << ss.str()).flush();
-    }
+  if (rcl_subscription_fini(
+      &intra_process_subscription_handle_, node_handle_.get()) != RCL_RET_OK)
+  {
+    std::stringstream ss;
+    ss << "Error in destruction of rmw intra process subscription handle: " <<
+      rcl_get_error_string_safe() << '\n';
+    (std::cerr << ss.str()).flush();
  }
 }

@@ -65,14 +59,14 @@ SubscriptionBase::get_topic_name() const
  return this->topic_name_;
 }

-const rmw_subscription_t *
+const rcl_subscription_t *
 SubscriptionBase::get_subscription_handle() const
 {
-  return subscription_handle_;
+  return &subscription_handle_;
 }

-const rmw_subscription_t *
+const rcl_subscription_t *
 SubscriptionBase::get_intra_process_subscription_handle() const
 {
-  return intra_process_subscription_handle_;
+  return &intra_process_subscription_handle_;
 }
--- a/rclcpp/src/rclcpp/timer.cpp
+++ b/rclcpp/src/rclcpp/timer.cpp
@@ -15,13 +15,19 @@
 #include "rclcpp/timer.hpp"

 #include <chrono>
+#include <string>

 using rclcpp::timer::TimerBase;

 TimerBase::TimerBase(std::chrono::nanoseconds period)
-: period_(period),
-  canceled_(false)
-{}
+{
+  if (rcl_timer_init(
+      &timer_handle_, period.count(), nullptr,
+      rcl_get_default_allocator()) != RCL_RET_OK)
+  {
+    fprintf(stderr, "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string_safe());
+  }
+}

 TimerBase::~TimerBase()
 {}
@@ -29,5 +35,35 @@ TimerBase::~TimerBase()
 void
 TimerBase::cancel()
 {
-  this->canceled_ = true;
+  if (rcl_timer_cancel(&timer_handle_) != RCL_RET_OK) {
+    throw std::runtime_error(std::string("Couldn't cancel timer: ") + rcl_get_error_string_safe());
+  }
+}
+
+bool
+TimerBase::is_ready()
+{
+  bool ready = false;
+  if (rcl_timer_is_ready(&timer_handle_, &ready) != RCL_RET_OK) {
+    throw std::runtime_error(std::string("Failed to check timer: ") + rcl_get_error_string_safe());
+  }
+  return ready;
+}
+
+std::chrono::nanoseconds
+TimerBase::time_until_trigger()
+{
+  int64_t time_until_next_call = 0;
+  if (rcl_timer_get_time_until_next_call(&timer_handle_, &time_until_next_call) != RCL_RET_OK) {
+    throw std::runtime_error(
+            std::string("Timer could not get time until next call: ") +
+            rcl_get_error_string_safe());
+  }
+  return std::chrono::nanoseconds(time_until_next_call);
+}
+
+const rcl_timer_t *
+TimerBase::get_timer_handle()
+{
+  return &timer_handle_;
 }
--- a/rclcpp/src/rclcpp/utilities.cpp
+++ b/rclcpp/src/rclcpp/utilities.cpp
@@ -19,8 +19,13 @@
 #include <csignal>
 #include <cstdio>
 #include <cstring>
+#include <map>
 #include <mutex>
 #include <string>
+#include <vector>
+
+#include "rcl/error_handling.h"
+#include "rcl/rcl.h"

 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
@@ -32,8 +37,10 @@

 /// Represent the status of the global interrupt signal.
 static volatile sig_atomic_t g_signal_status = 0;
-/// Guard condition for interrupting the rmw implementation when the global interrupt signal fired.
-static rmw_guard_condition_t * g_sigint_guard_cond_handle = rmw_create_guard_condition();
+/// Guard conditions for interrupting the rmw implementation when the global interrupt signal fired.
+static std::map<rcl_wait_set_t *, rcl_guard_condition_t> g_sigint_guard_cond_handles;
+/// Mutex to protect g_sigint_guard_cond_handles
+static std::mutex g_sigint_guard_cond_handles_mutex;
 /// Condition variable for timed sleep (see sleep_for).
 static std::condition_variable g_interrupt_condition_variable;
 static std::atomic<bool> g_is_interrupted(false);
@@ -77,10 +84,15 @@ signal_handler(int signal_value)
  }
 #endif
  g_signal_status = signal_value;
-  rmw_ret_t status = rmw_trigger_guard_condition(g_sigint_guard_cond_handle);
-  if (status != RMW_RET_OK) {
-    fprintf(stderr,
-      "[rclcpp::error] failed to trigger guard condition: %s\n", rmw_get_error_string_safe());
+  {
+    std::lock_guard<std::mutex> lock(g_sigint_guard_cond_handles_mutex);
+    for (auto const & kv : g_sigint_guard_cond_handles) {
+      rcl_ret_t status = rcl_trigger_guard_condition(&(kv.second));
+      if (status != RCL_RET_OK) {
+        fprintf(stderr,
+          "[rclcpp::error] failed to trigger guard condition: %s\n", rcl_get_error_string_safe());
+      }
+    }
  }
  g_is_interrupted.store(true);
  g_interrupt_condition_variable.notify_all();
@@ -89,14 +101,11 @@ signal_handler(int signal_value)
 void
 rclcpp::utilities::init(int argc, char * argv[])
 {
-  (void)argc;
-  (void)argv;
  g_is_interrupted.store(false);
-  rmw_ret_t status = rmw_init();
-  if (status != RMW_RET_OK) {
-    // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+  if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) {
+    // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
    throw std::runtime_error(
-      std::string("failed to initialize rmw implementation: ") + rmw_get_error_string_safe());
+      std::string("failed to initialize rmw implementation: ") + rcl_get_error_string_safe());
    // *INDENT-ON*
  }
 #ifdef HAS_SIGACTION
@@ -117,7 +126,7 @@ rclcpp::utilities::init(int argc, char * argv[])
    // NOLINTNEXTLINE(runtime/arrays)
    char error_string[error_length];
 #ifndef _WIN32
-#ifdef _GNU_SOURCE
+#if (defined(_GNU_SOURCE) && !defined(ANDROID))
    char * msg = strerror_r(errno, error_string, error_length);
    if (msg != error_string) {
      strncpy(error_string, msg, error_length);
@@ -132,7 +141,7 @@ rclcpp::utilities::init(int argc, char * argv[])
 #else
    strerror_s(error_string, error_length, errno);
 #endif
-    // *INDENT-OFF*
+    // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
    throw std::runtime_error(
      std::string("Failed to set SIGINT signal handler: (" + std::to_string(errno) + ")") +
      error_string);
@@ -146,23 +155,82 @@ rclcpp::utilities::ok()
  return ::g_signal_status == 0;
 }

+static std::mutex on_shutdown_mutex_;
+static std::vector<std::function<void(void)>> on_shutdown_callbacks_;
+
 void
 rclcpp::utilities::shutdown()
 {
  g_signal_status = SIGINT;
-  rmw_ret_t status = rmw_trigger_guard_condition(g_sigint_guard_cond_handle);
-  if (status != RMW_RET_OK) {
-    fprintf(stderr,
-      "[rclcpp::error] failed to trigger guard condition: %s\n", rmw_get_error_string_safe());
+  {
+    std::lock_guard<std::mutex> lock(g_sigint_guard_cond_handles_mutex);
+    for (auto const & kv : g_sigint_guard_cond_handles) {
+      if (rcl_trigger_guard_condition(&(kv.second)) != RCL_RET_OK) {
+        fprintf(stderr,
+          "[rclcpp::error] failed to trigger sigint guard condition: %s\n",
+          rcl_get_error_string_safe());
+      }
+    }
  }
  g_is_interrupted.store(true);
  g_interrupt_condition_variable.notify_all();
+  {
+    std::lock_guard<std::mutex> lock(on_shutdown_mutex_);
+    for (auto & on_shutdown_callback : on_shutdown_callbacks_) {
+      on_shutdown_callback();
+    }
+  }
 }

-rmw_guard_condition_t *
-rclcpp::utilities::get_global_sigint_guard_condition()
+void
+rclcpp::utilities::on_shutdown(std::function<void(void)> callback)
 {
-  return ::g_sigint_guard_cond_handle;
+  std::lock_guard<std::mutex> lock(on_shutdown_mutex_);
+  on_shutdown_callbacks_.push_back(callback);
+}
+
+rcl_guard_condition_t *
+rclcpp::utilities::get_sigint_guard_condition(rcl_wait_set_t * waitset)
+{
+  std::lock_guard<std::mutex> lock(g_sigint_guard_cond_handles_mutex);
+  auto kv = g_sigint_guard_cond_handles.find(waitset);
+  if (kv != g_sigint_guard_cond_handles.end()) {
+    return &kv->second;
+  } else {
+    rcl_guard_condition_t handle =
+      rcl_get_zero_initialized_guard_condition();
+    rcl_guard_condition_options_t options = rcl_guard_condition_get_default_options();
+    if (rcl_guard_condition_init(&handle, options) != RCL_RET_OK) {
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
+      throw std::runtime_error(std::string(
+        "Couldn't initialize guard condition: ") + rcl_get_error_string_safe());
+      // *INDENT-ON*
+    }
+    g_sigint_guard_cond_handles[waitset] = handle;
+    return &g_sigint_guard_cond_handles[waitset];
+  }
+}
+
+void
+rclcpp::utilities::release_sigint_guard_condition(rcl_wait_set_t * waitset)
+{
+  std::lock_guard<std::mutex> lock(g_sigint_guard_cond_handles_mutex);
+  auto kv = g_sigint_guard_cond_handles.find(waitset);
+  if (kv != g_sigint_guard_cond_handles.end()) {
+    if (rcl_guard_condition_fini(&kv->second) != RCL_RET_OK) {
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
+      throw std::runtime_error(std::string(
+        "Failed to destroy sigint guard condition: ") +
+        rcl_get_error_string_safe());
+      // *INDENT-ON*
+    }
+    g_sigint_guard_cond_handles.erase(kv);
+  } else {
+    // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
+    throw std::runtime_error(std::string(
+      "Tried to release sigint guard condition for nonexistent waitset"));
+    // *INDENT-ON*
+  }
 }

 bool
Author	SHA1	Message	Date
gerkey	fc0d539837	add parameter helpers (redo of #233 ) (#237 ) * add parameter helpers * respond to comments * remove unnecessary indent comments * replace temp variable assignment with explicit constructor invocation	2016-07-08 15:45:05 -07:00
gerkey	ea76716982	use the param profile for param services (#236 ) * use the param profile for param services * expose qos to users of param clients / services	2016-07-08 13:39:18 -07:00
Dirk Thomas	8251b84f68	update schema url	2016-06-28 20:02:25 -07:00
Esteve Fernandez	058de29628	Implement std::to_string for Android (#231 )	2016-06-28 16:20:59 -07:00
Dirk Thomas	e8600d1b80	add schema to manifest files	2016-06-26 17:15:05 -07:00
William Woodall	5e2a76cc20	add wait_for_service() and service_is_ready() to Client (#222 ) * add wait_for_service() and service_is_ready() to Client * fix compile on Linux (maybe Windows) * use visibility macros for Windows * prevent unreasonable uncrustify change * fixup comment * add GraphListener::is_shutdown() * disable copy on GraphListener * use weak_ptr<Node> in client, throw if invalid * ensure blocking wait_for_service wakes on rclcpp::shutdown/sigint * rethrow exceptions after reporting them in thread * lock ~Node() against notify_graph_change() this essentially protects the notify_guard_condition_ * adjust thread sync strategy * style * moving initialization of wait set around, fix double free * only fini wait set if started * use rclcpp::shutdown to ensure graph listener resources clean up before static destruction * uncrustify	2016-06-22 20:18:46 -07:00
gerkey	3553107823	Allow empty prefix list to get all params; check depth correctly. (#230 ) * Allow empty prefix list to get all params; check depth correctly. * use enum instead of constant	2016-06-20 17:55:57 -07:00
Dirk Thomas	759b063db5	Merge pull request #227 from ros2/cmake35 require CMake 3.5	2016-06-16 12:18:59 -07:00
Dirk Thomas	aeb3c55894	remove trailing spaces from comparisons, obsolete quotes and explicit variable expansion	2016-06-16 09:19:37 -07:00
gerkey	bf6394004c	Fix style (#229 )	2016-06-16 08:44:12 -07:00
gerkey	39f0a1b93f	Give a different signal guard condition for each waitset (#226 ) Fixes #225.	2016-06-15 13:14:44 -07:00
Dirk Thomas	7a5285a3d0	require CMake 3.5	2016-06-15 11:35:56 -07:00
gerkey	3c45a571e7	Merge pull request #223 from ros2/mutex_callbackgroup Add mutex to protect vectors of pointers in callbackgroup	2016-06-06 17:36:25 -07:00
Brian Gerkey	af0b1e6b07	Add mutex to protect vectors of pointers in callbackgroup	2016-06-06 17:29:32 -07:00
Dirk Thomas	0f58c5305c	fix spelling in comment	2016-05-31 09:06:07 -07:00
dhood	a5fa8277f3	Merge pull request #220 from dhood/test-linking-runtime Regression test for #213	2016-05-25 02:44:16 +01:00
dhood	458019bdff	Regression test for #213	2016-05-17 17:40:28 -07:00
Dirk Thomas	0a9f2e26a2	Merge pull request #215 from ros2/ctest_build_testing use CTest BUILD_TESTING	2016-05-02 10:11:35 -07:00
Dirk Thomas	45f43ef523	use CTest BUILD_TESTING	2016-04-28 18:45:08 -07:00
Dirk Thomas	c99b9b9734	Merge pull request #213 from ros2/fix_wrong_link_library fix wrong link library for test_rate gtest	2016-04-27 08:13:24 -07:00
Dirk Thomas	b34a5f5504	fix wrong link library for test_rate gtest	2016-04-26 17:42:57 -07:00
Jackie Kay	6adfb917a9	Virtual destructors (#212 )	2016-04-25 16:31:03 -07:00
Jackie Kay	e961189be8	Refactor to use rcl (#207 )	2016-04-24 21:25:19 +00:00
Jackie Kay	6bcd9db4d6	Remove fixed guard conditions, add notify guard conditions * No more fixed guard conditions. * Add notify guard condition to nodes	2016-04-01 14:07:07 -07:00
Jackie Kay	2be9568498	Merge pull request #208 from ros2/timer_specialization Only enable Timer if clock type is steady	2016-03-15 18:20:33 -07:00
Jackie Kay	e7833fa709	Only enable Timer if clock type is steady	2016-03-15 16:23:39 -07:00
William Woodall	02311dee4c	Merge pull request #202 from ros2/fix_flaky_subscription_and_spinning_test Fix flaky subscription and spinning test	2016-03-08 23:15:38 +00:00
William Woodall	82139f1a12	refactor and test spin_until_future_complete	2016-03-08 15:10:05 -08:00
William Woodall	e8f9344015	refactor executor.spin_until_future_complete	2016-03-03 18:14:18 -08:00
William Woodall	0c826497f1	convert enum to enum class and provide to_string	2016-03-03 18:13:46 -08:00
Jackie Kay	249b7d80d8	Merge pull request #199 from ros2/request_header Don't type-erase request header	2016-02-29 16:59:08 -08:00
Jackie Kay	f6ce2d8dc5	Don't type-erase request header	2016-02-29 16:57:53 -08:00