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

rclcpp/CMakeLists.txt

@@ -1,34 +1,124 @@
-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)
+find_package(rmw_implementation_cmake REQUIRED)
+find_package(rosidl_generator_cpp REQUIRED)
 
-ament_export_dependencies(rmw)
-ament_export_dependencies(rcl_interfaces)
+if(NOT WIN32)
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra")
+endif()
+
+include_directories(include)
+
+set(${PROJECT_NAME}_SRCS
+  src/rclcpp/any_executable.cpp
+  src/rclcpp/callback_group.cpp
+  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
+  src/rclcpp/memory_strategy.cpp
+  src/rclcpp/parameter.cpp
+  src/rclcpp/parameter_client.cpp
+  src/rclcpp/parameter_service.cpp
+  src/rclcpp/publisher.cpp
+  src/rclcpp/node.cpp
+  src/rclcpp/service.cpp
+  src/rclcpp/subscription.cpp
+  src/rclcpp/timer.cpp
+  src/rclcpp/type_support.cpp
+  src/rclcpp/utilities.cpp
+)
+
+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${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.
+  target_compile_definitions(${PROJECT_NAME}${target_suffix}
+    PRIVATE "RCLCPP_BUILDING_LIBRARY")
+
+  install(
+    TARGETS ${PROJECT_NAME}${target_suffix}
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION bin
+  )
+endmacro()
+
+call_for_each_rmw_implementation(target GENERATE_DEFAULT)
+
+ament_export_dependencies(ament_cmake)
+ament_export_dependencies(rcl)
+ament_export_dependencies(rosidl_generator_cpp)
 
 ament_export_include_directories(include)
 
-if(AMENT_ENABLE_TESTING)
+ament_export_libraries(${PROJECT_NAME})
+
+if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 
-  find_package(rmw REQUIRED)
-
-  if(NOT WIN32)
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra")
-  endif()
-
-  include_directories(include)
-
   ament_add_gtest(test_function_traits test/test_function_traits.cpp)
+  if(TARGET test_function_traits)
+    target_include_directories(test_function_traits PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+    )
+  endif()
   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}
+    )
+  endif()
   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_interfaces_INCLUDE_DIRS}"
-      "${rmw_INCLUDE_DIRS}")
+      ${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
+    ENV RCL_ASSERT_RMW_ID_MATCHES=${rmw_implementation})
+  if(TARGET test_rate)
+    target_include_directories(test_rate PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_rate
+      ${PROJECT_NAME}
+    )
   endif()
 endif()
 
@@ -37,11 +127,11 @@ ament_package(
 )
 
 install(
-  DIRECTORY include/
-  DESTINATION include
+  DIRECTORY cmake
+  DESTINATION share/${PROJECT_NAME}
 )
 
 install(
-  DIRECTORY src/
-  DESTINATION src/rclcpp
+  DIRECTORY include/
+  DESTINATION include
 )

rclcpp/cmake/get_rclcpp_information.cmake

@@ -0,0 +1,92 @@
+# Copyright 2015 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.
+
+#
+# Get all information about rclcpp for a specific RMW implementation.
+#
+# It sets the common variables _DEFINITIONS, _INCLUDE_DIRS and _LIBRARIES
+# with the given prefix.
+#
+# :param rmw_implementation: the RMW implementation name
+# :type target: string
+# :param var_prefix: the prefix of all output variable names
+# :type var_prefix: string
+#
+macro(get_rclcpp_information rmw_implementation var_prefix)
+  # pretend to be a "package"
+  # 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")
+
+  # libraries
+  set(_libs)
+  # search for library relative to this CMake file
+  set(_library_target "rclcpp")
+  get_available_rmw_implementations(_rmw_impls)
+  list(LENGTH _rmw_impls _rmw_impls_length)
+  if(_rmw_impls_length GREATER 1)
+    set(_library_target "${_library_target}__${rmw_implementation}")
+  endif()
+  set(_lib "NOTFOUND")
+  find_library(
+    _lib NAMES "${_library_target}"
+    PATHS "${rclcpp_DIR}/../../../lib"
+    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH
+  )
+  if(NOT _lib)
+    # warn about not existing library and ignore it
+    message(WARNING "Package 'rclcpp' doesn't contain the library '${_library_target}'")
+  elseif(NOT IS_ABSOLUTE "${_lib}")
+    # the found library must be an absolute path
+    message(FATAL_ERROR "Package 'rclcpp' found the library '${_library_target}' at '${_lib}' which is not an absolute path")
+  elseif(NOT EXISTS "${_lib}")
+    # the found library must exist
+    message(FATAL_ERROR "Package 'rclcpp' found the library '${_lib}' which doesn't exist")
+  else()
+    list(APPEND _libs "${_lib}")
+  endif()
+
+  # dependencies
+  set(_exported_dependencies
+    "rcl_interfaces"
+    "rcl${target_suffix}"
+    "rosidl_generator_cpp")
+  set(${var_prefix}_DEFINITIONS)
+  foreach(_dep ${_exported_dependencies})
+    if(NOT ${_dep}_FOUND)
+      find_package("${_dep}" QUIET REQUIRED)
+    endif()
+    if(${_dep}_DEFINITIONS)
+      list_append_unique(${var_prefix}_DEFINITIONS "${${_dep}_DEFINITIONS}")
+    endif()
+    if(${_dep}_INCLUDE_DIRS)
+      list_append_unique(${var_prefix}_INCLUDE_DIRS "${${_dep}_INCLUDE_DIRS}")
+    endif()
+    if(${_dep}_LIBRARIES)
+      list(APPEND _libs "${${_dep}_LIBRARIES}")
+    endif()
+  endforeach()
+  if(_libs)
+    ament_libraries_deduplicate(_libs "${_libs}")
+  endif()
+  set(${var_prefix}_LIBRARIES "${_libs}")
+endmacro()

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
 

rclcpp/include/rclcpp/allocator/allocator_deleter.hpp

@@ -16,10 +16,10 @@
 #define RCLCPP__ALLOCATOR__ALLOCATOR_DELETER_HPP_
 
 #include <memory>
+#include <stdexcept>
 
 namespace rclcpp
 {
-
 namespace allocator
 {
 

rclcpp/include/rclcpp/any_executable.hpp

@@ -12,13 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_ANY_EXECUTABLE_HPP_
-#define RCLCPP_RCLCPP_ANY_EXECUTABLE_HPP_
+#ifndef RCLCPP__ANY_EXECUTABLE_HPP_
+#define RCLCPP__ANY_EXECUTABLE_HPP_
 
 #include <memory>
 
-#include <rclcpp/macros.hpp>
-#include <rclcpp/node.hpp>
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
@@ -28,9 +29,13 @@ namespace executor
 struct AnyExecutable
 {
   RCLCPP_SMART_PTR_DEFINITIONS(AnyExecutable);
-  AnyExecutable()
-  : subscription(0), timer(0), callback_group(0), node(0)
-  {}
+
+  RCLCPP_PUBLIC
+  AnyExecutable();
+
+  RCLCPP_PUBLIC
+  virtual ~AnyExecutable();
+
   // Only one of the following pointers will be set.
   rclcpp::subscription::SubscriptionBase::SharedPtr subscription;
   rclcpp::subscription::SubscriptionBase::SharedPtr subscription_intra_process;
@@ -42,7 +47,7 @@ struct AnyExecutable
   rclcpp::node::Node::SharedPtr node;
 };
 
-} /* executor */
-} /* rclcpp */
+}  // namespace executor
+}  // namespace rclcpp
 
-#endif
+#endif  // RCLCPP__ANY_EXECUTABLE_HPP_

rclcpp/include/rclcpp/any_service_callback.hpp

@@ -12,16 +12,17 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_ANY_SERVICE_CALLBACK_HPP_
-#define RCLCPP_RCLCPP_ANY_SERVICE_CALLBACK_HPP_
-
-#include <rclcpp/function_traits.hpp>
+#ifndef RCLCPP__ANY_SERVICE_CALLBACK_HPP_
+#define RCLCPP__ANY_SERVICE_CALLBACK_HPP_
 
 #include <functional>
 #include <memory>
+#include <stdexcept>
 #include <type_traits>
 
-#include <rmw/types.h>
+#include "rclcpp/function_traits.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rmw/types.h"
 
 namespace rclcpp
 {
@@ -56,7 +57,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         SharedPtrCallback
       >::value
@@ -70,7 +71,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         SharedPtrWithRequestHeaderCallback
       >::value
@@ -97,7 +98,7 @@ public:
   }
 };
 
-} /* namespace any_service_callback */
-} /* namespace rclcpp */
+}  // namespace any_service_callback
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_ANY_SERVICE_CALLBACK_HPP_ */
+#endif  // RCLCPP__ANY_SERVICE_CALLBACK_HPP_

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -19,10 +19,12 @@
 
 #include <functional>
 #include <memory>
+#include <stdexcept>
 #include <type_traits>
 
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/function_traits.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
@@ -70,7 +72,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         SharedPtrCallback
       >::value
@@ -84,7 +86,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         SharedPtrWithInfoCallback
       >::value
@@ -98,7 +100,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         ConstSharedPtrCallback
       >::value
@@ -112,7 +114,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         ConstSharedPtrWithInfoCallback
       >::value
@@ -126,7 +128,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         UniquePtrCallback
       >::value
@@ -140,7 +142,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         UniquePtrWithInfoCallback
       >::value
@@ -181,16 +183,16 @@ public:
   {
     (void)message_info;
     if (shared_ptr_callback_) {
-      typename MessageT::SharedPtr shared_message = std::move(message);
+      typename std::shared_ptr<MessageT> shared_message = std::move(message);
       shared_ptr_callback_(shared_message);
     } else if (shared_ptr_with_info_callback_) {
-      typename MessageT::SharedPtr shared_message = std::move(message);
+      typename std::shared_ptr<MessageT> shared_message = std::move(message);
       shared_ptr_with_info_callback_(shared_message, message_info);
     } else if (const_shared_ptr_callback_) {
-      typename MessageT::ConstSharedPtr const_shared_message = std::move(message);
+      typename std::shared_ptr<MessageT const> const_shared_message = std::move(message);
       const_shared_ptr_callback_(const_shared_message);
     } else if (const_shared_ptr_with_info_callback_) {
-      typename MessageT::ConstSharedPtr const_shared_message = std::move(message);
+      typename std::shared_ptr<MessageT const> const_shared_message = std::move(message);
       const_shared_ptr_with_info_callback_(const_shared_message, message_info);
     } else if (unique_ptr_callback_) {
       unique_ptr_callback_(std::move(message));

rclcpp/include/rclcpp/callback_group.hpp

@@ -12,17 +12,19 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_
-#define RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_
+#ifndef RCLCPP__CALLBACK_GROUP_HPP_
+#define RCLCPP__CALLBACK_GROUP_HPP_
 
 #include <atomic>
+#include <mutex>
 #include <string>
 #include <vector>
 
-#include <rclcpp/subscription.hpp>
-#include <rclcpp/timer.hpp>
-#include <rclcpp/service.hpp>
-#include <rclcpp/client.hpp>
+#include "rclcpp/client.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/subscription.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
@@ -31,7 +33,7 @@ namespace rclcpp
 namespace node
 {
 class Node;
-} // namespace node
+}  // namespace node
 
 namespace callback_group
 {
@@ -49,82 +51,63 @@ class CallbackGroup
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup);
 
-  CallbackGroup(CallbackGroupType group_type)
-  : type_(group_type), can_be_taken_from_(true)
-  {}
+  RCLCPP_PUBLIC
+  explicit CallbackGroup(CallbackGroupType group_type);
 
-  const std::vector<subscription::SubscriptionBase::WeakPtr> &
-  get_subscription_ptrs() const
-  {
-    return subscription_ptrs_;
-  }
+  RCLCPP_PUBLIC
+  const std::vector<rclcpp::subscription::SubscriptionBase::WeakPtr> &
+  get_subscription_ptrs() const;
 
-  const std::vector<timer::TimerBase::WeakPtr> &
-  get_timer_ptrs() const
-  {
-    return timer_ptrs_;
-  }
+  RCLCPP_PUBLIC
+  const std::vector<rclcpp::timer::TimerBase::WeakPtr> &
+  get_timer_ptrs() const;
 
-  const std::vector<service::ServiceBase::SharedPtr> &
-  get_service_ptrs() const
-  {
-    return service_ptrs_;
-  }
+  RCLCPP_PUBLIC
+  const std::vector<rclcpp::service::ServiceBase::SharedPtr> &
+  get_service_ptrs() const;
 
-  const std::vector<client::ClientBase::SharedPtr> &
-  get_client_ptrs() const
-  {
-    return client_ptrs_;
-  }
+  RCLCPP_PUBLIC
+  const std::vector<rclcpp::client::ClientBase::WeakPtr> &
+  get_client_ptrs() const;
 
-  std::atomic_bool & can_be_taken_from()
-  {
-    return can_be_taken_from_;
-  }
+  RCLCPP_PUBLIC
+  std::atomic_bool &
+  can_be_taken_from();
 
-  const CallbackGroupType & type() const
-  {
-    return type_;
-  }
+  RCLCPP_PUBLIC
+  const CallbackGroupType &
+  type() const;
 
 private:
   RCLCPP_DISABLE_COPY(CallbackGroup);
 
+  RCLCPP_PUBLIC
   void
-  add_subscription(
-    const subscription::SubscriptionBase::SharedPtr subscription_ptr)
-  {
-    subscription_ptrs_.push_back(subscription_ptr);
-  }
+  add_subscription(const rclcpp::subscription::SubscriptionBase::SharedPtr subscription_ptr);
 
+  RCLCPP_PUBLIC
   void
-  add_timer(const timer::TimerBase::SharedPtr timer_ptr)
-  {
-    timer_ptrs_.push_back(timer_ptr);
-  }
+  add_timer(const rclcpp::timer::TimerBase::SharedPtr timer_ptr);
 
+  RCLCPP_PUBLIC
   void
-  add_service(const service::ServiceBase::SharedPtr service_ptr)
-  {
-    service_ptrs_.push_back(service_ptr);
-  }
+  add_service(const rclcpp::service::ServiceBase::SharedPtr service_ptr);
 
+  RCLCPP_PUBLIC
   void
-  add_client(const client::ClientBase::SharedPtr client_ptr)
-  {
-    client_ptrs_.push_back(client_ptr);
-  }
+  add_client(const rclcpp::client::ClientBase::SharedPtr client_ptr);
 
   CallbackGroupType type_;
-  std::vector<subscription::SubscriptionBase::WeakPtr> subscription_ptrs_;
-  std::vector<timer::TimerBase::WeakPtr> timer_ptrs_;
-  std::vector<service::ServiceBase::SharedPtr> service_ptrs_;
-  std::vector<client::ClientBase::SharedPtr> client_ptrs_;
+  // 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_;
+  std::vector<rclcpp::client::ClientBase::WeakPtr> client_ptrs_;
   std::atomic_bool can_be_taken_from_;
-
 };
 
-} /* namespace callback_group */
-} /* namespace rclcpp */
+}  // namespace callback_group
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_ */
+#endif  // RCLCPP__CALLBACK_GROUP_HPP_

rclcpp/include/rclcpp/client.hpp

@@ -12,113 +12,174 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_CLIENT_HPP_
-#define RCLCPP_RCLCPP_CLIENT_HPP_
+#ifndef RCLCPP__CLIENT_HPP_
+#define RCLCPP__CLIENT_HPP_
 
 #include <future>
-#include <iostream>
 #include <map>
 #include <memory>
 #include <sstream>
+#include <string>
+#include <tuple>
 #include <utility>
 
-#include <rmw/error_handling.h>
-#include <rmw/rmw.h>
+#include "rcl/client.h"
+#include "rcl/error_handling.h"
+#include "rcl/wait.h"
 
-#include <rclcpp/macros.hpp>
-#include <rclcpp/utilities.hpp>
+#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
 {
 
 class ClientBase
 {
-
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientBase);
 
+  RCLCPP_PUBLIC
   ClientBase(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_client_t * client_handle,
-    const std::string & service_name)
-  : node_handle_(node_handle), client_handle_(client_handle), service_name_(service_name)
-  {}
+    std::shared_ptr<rclcpp::node::Node> parent_node,
+    const std::string & service_name);
 
-  virtual ~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());
-      }
-    }
-  }
+  RCLCPP_PUBLIC
+  virtual ~ClientBase();
 
-  const std::string & get_service_name() const
-  {
-    return this->service_name_;
-  }
+  RCLCPP_PUBLIC
+  const std::string &
+  get_service_name() const;
 
-  const rmw_client_t * get_client_handle() const
+  RCLCPP_PUBLIC
+  const rcl_client_t *
+  get_client_handle() const;
+
+  RCLCPP_PUBLIC
+  bool
+  service_is_ready() const;
+
+  template<typename RatioT = std::milli>
+  bool
+  wait_for_service(
+    std::chrono::duration<int64_t, RatioT> timeout = std::chrono::duration<int64_t, RatioT>(-1))
   {
-    return this->client_handle_;
+    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<void> create_request_header() = 0;
+  virtual std::shared_ptr<rmw_request_id_t> create_request_header() = 0;
   virtual void handle_response(
-    std::shared_ptr<void> & request_header, std::shared_ptr<void> & response) = 0;
+    std::shared_ptr<rmw_request_id_t> request_header, std::shared_ptr<void> response) = 0;
 
-private:
+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_;
-
 };
 
 template<typename ServiceT>
 class Client : public ClientBase
 {
 public:
-  using Promise = std::promise<typename ServiceT::Response::SharedPtr>;
+  using SharedRequest = typename ServiceT::Request::SharedPtr;
+  using SharedResponse = typename ServiceT::Response::SharedPtr;
+
+  using Promise = std::promise<SharedResponse>;
+  using PromiseWithRequest = std::promise<std::pair<SharedRequest, SharedResponse>>;
+
   using SharedPromise = std::shared_ptr<Promise>;
-  using SharedFuture = std::shared_future<typename ServiceT::Response::SharedPtr>;
+  using SharedPromiseWithRequest = std::shared_ptr<PromiseWithRequest>;
+
+  using SharedFuture = std::shared_future<SharedResponse>;
+  using SharedFutureWithRequest = std::shared_future<std::pair<SharedRequest, SharedResponse>>;
 
   using CallbackType = std::function<void(SharedFuture)>;
+  using CallbackWithRequestType = std::function<void(SharedFutureWithRequest)>;
 
   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)
   {
-    auto typed_request_header = std::static_pointer_cast<rmw_request_id_t>(request_header);
+    std::lock_guard<std::mutex> lock(pending_requests_mutex_);
     auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(response);
-    int64_t sequence_number = typed_request_header->sequence_number;
-    // TODO this must check if the sequence_number is valid otherwise the call_promise will be null
+    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");
+      return;
+    }
     auto tuple = this->pending_requests_[sequence_number];
     auto call_promise = std::get<0>(tuple);
     auto callback = std::get<1>(tuple);
@@ -128,37 +189,74 @@ public:
     callback(future);
   }
 
-  SharedFuture async_send_request(
-    typename ServiceT::Request::SharedPtr request)
+  SharedFuture
+  async_send_request(SharedRequest request)
   {
     return async_send_request(request, [](SharedFuture) {});
   }
 
-  SharedFuture async_send_request(
-    typename ServiceT::Request::SharedPtr request,
-    CallbackType cb)
+  template<
+    typename CallbackT,
+    typename std::enable_if<
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        CallbackType
+      >::value
+    >::type * = nullptr
+  >
+  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*
     }
 
     SharedPromise call_promise = std::make_shared<Promise>();
     SharedFuture f(call_promise->get_future());
-    pending_requests_[sequence_number] = std::make_tuple(call_promise, cb, f);
+    pending_requests_[sequence_number] =
+      std::make_tuple(call_promise, std::forward<CallbackType>(cb), f);
     return f;
   }
 
+  template<
+    typename CallbackT,
+    typename std::enable_if<
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        CallbackWithRequestType
+      >::value
+    >::type * = nullptr
+  >
+  SharedFutureWithRequest
+  async_send_request(SharedRequest request, CallbackT && cb)
+  {
+    SharedPromiseWithRequest promise = std::make_shared<PromiseWithRequest>();
+    SharedFutureWithRequest future_with_request(promise->get_future());
+
+    auto wrapping_cb = [future_with_request, promise, request, &cb](SharedFuture future) {
+        auto response = future.get();
+        promise->set_value(std::make_pair(request, response));
+        cb(future_with_request);
+      };
+
+    async_send_request(request, wrapping_cb);
+
+    return future_with_request;
+  }
+
 private:
   RCLCPP_DISABLE_COPY(Client);
 
   std::map<int64_t, std::tuple<SharedPromise, CallbackType, SharedFuture>> pending_requests_;
+  std::mutex pending_requests_mutex_;
 };
 
-} /* namespace client */
-} /* namespace rclcpp */
+}  // namespace client
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_CLIENT_HPP_ */
+#endif  // RCLCPP__CLIENT_HPP_

rclcpp/include/rclcpp/context.hpp

@@ -12,20 +12,19 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_CONTEXT_HPP_
-#define RCLCPP_RCLCPP_CONTEXT_HPP_
-
-#include <rclcpp/macros.hpp>
+#ifndef RCLCPP__CONTEXT_HPP_
+#define RCLCPP__CONTEXT_HPP_
 
 #include <iostream>
-
 #include <memory>
 #include <mutex>
-#include <typeinfo>
 #include <typeindex>
+#include <typeinfo>
 #include <unordered_map>
 
-#include <rmw/rmw.h>
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rmw/rmw.h"
 
 namespace rclcpp
 {
@@ -38,7 +37,8 @@ class Context
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(Context);
 
-  Context() {}
+  RCLCPP_PUBLIC
+  Context();
 
   template<typename SubContext, typename ... Args>
   std::shared_ptr<SubContext>
@@ -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) {
@@ -71,10 +71,9 @@ private:
 
   std::unordered_map<std::type_index, std::shared_ptr<void>> sub_contexts_;
   std::mutex mutex_;
-
 };
 
-} /* namespace context */
-} /* namespace rclcpp */
+}  // namespace context
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_CONTEXT_HPP_ */
+#endif  // RCLCPP__CONTEXT_HPP_

rclcpp/include/rclcpp/contexts/default_context.hpp

@@ -12,10 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_
-#define RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_
+#ifndef RCLCPP__CONTEXTS__DEFAULT_CONTEXT_HPP_
+#define RCLCPP__CONTEXTS__DEFAULT_CONTEXT_HPP_
 
-#include <rclcpp/context.hpp>
+#include "rclcpp/context.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
@@ -29,19 +30,16 @@ class DefaultContext : public rclcpp::context::Context
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(DefaultContext);
 
-  DefaultContext() {}
-
+  RCLCPP_PUBLIC
+  DefaultContext();
 };
 
+RCLCPP_PUBLIC
 DefaultContext::SharedPtr
-get_global_default_context()
-{
-  static DefaultContext::SharedPtr default_context = DefaultContext::make_shared();
-  return default_context;
-}
+get_global_default_context();
 
-} // namespace default_context
-} // namespace contexts
-} // namespace rclcpp
+}  // namespace default_context
+}  // namespace contexts
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_ */
+#endif  // RCLCPP__CONTEXTS__DEFAULT_CONTEXT_HPP_

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_

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_

rclcpp/include/rclcpp/executor.hpp

@@ -17,13 +17,16 @@
 
 #include <algorithm>
 #include <cassert>
+#include <chrono>
 #include <cstdlib>
 #include <iostream>
 #include <list>
 #include <memory>
+#include <string>
 #include <vector>
 
-#include "rcl_interfaces/msg/intra_process_message.hpp"
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
 
 #include "rclcpp/any_executable.hpp"
 #include "rclcpp/macros.hpp"
@@ -31,12 +34,47 @@
 #include "rclcpp/memory_strategy.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/utilities.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
 namespace executor
 {
 
+/// Return codes to be used with spin_until_future_complete.
+/**
+ * SUCCESS: The future is complete and can be accessed with "get" without blocking.
+ * INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error.
+ * TIMEOUT: Spinning timed out.
+ */
+enum class FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT};
+
+RCLCPP_PUBLIC
+std::ostream &
+operator<<(std::ostream & os, const FutureReturnCode & future_return_code);
+
+RCLCPP_PUBLIC
+std::string
+to_string(const FutureReturnCode & future_return_code);
+
+///
+/**
+ * Options to be passed to the executor constructor.
+ */
+struct ExecutorArgs
+{
+  memory_strategy::MemoryStrategy::SharedPtr memory_strategy;
+  size_t max_conditions = 0;
+};
+
+static inline ExecutorArgs create_default_executor_arguments()
+{
+  ExecutorArgs args;
+  args.memory_strategy = memory_strategies::create_default_strategy();
+  args.max_conditions = 0;
+  return args;
+}
+
 /// Coordinate the order and timing of available communication tasks.
 /**
  * Executor provides spin functions (including spin_node_once and spin_some).
@@ -54,29 +92,17 @@ public:
 
   /// Default constructor.
   // \param[in] ms The memory strategy to be used with this executor.
-  explicit Executor(memory_strategy::MemoryStrategy::SharedPtr ms =
-    memory_strategies::create_default_strategy())
-  : interrupt_guard_condition_(rmw_create_guard_condition()),
-    memory_strategy_(ms)
-  {
-  }
+  RCLCPP_PUBLIC
+  explicit Executor(const ExecutorArgs & args = create_default_executor_arguments());
 
   /// Default destructor.
-  virtual ~Executor()
-  {
-    // 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());
-      }
-    }
-  }
+  RCLCPP_PUBLIC
+  virtual ~Executor();
 
   /// Do work periodically as it becomes available to us. Blocking call, may block indefinitely.
   // It is up to the implementation of Executor to implement spin.
-  virtual void spin() = 0;
+  virtual void
+  spin() = 0;
 
   /// Add a node to the executor.
   /**
@@ -86,26 +112,9 @@ public:
    * the executor is blocked at the rmw layer while waiting for work and it is notified that a new
    * node was added, it will wake up.
    */
+  RCLCPP_PUBLIC
   virtual void
-  add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true)
-  {
-    // Check to ensure node not already added
-    for (auto & weak_node : weak_nodes_) {
-      auto node = weak_node.lock();
-      if (node == node_ptr) {
-        // TODO(jacquelinekay): Use a different error here?
-        throw std::runtime_error("Cannot add node to executor, node already added.");
-      }
-    }
-    weak_nodes_.push_back(node_ptr);
-    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());
-      }
-    }
-  }
+  add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true);
 
   /// Remove a node from the executor.
   /**
@@ -114,29 +123,9 @@ public:
    * This is useful if the last node was removed from the executor while the executor was blocked
    * waiting for work in another thread, because otherwise the executor would never be notified.
    */
+  RCLCPP_PUBLIC
   virtual void
-  remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true)
-  {
-    bool node_removed = false;
-    weak_nodes_.erase(
-      std::remove_if(
-        weak_nodes_.begin(), weak_nodes_.end(),
-        [&](std::weak_ptr<rclcpp::node::Node> & i)
-        {
-          bool matched = (i.lock() == node_ptr);
-          node_removed |= matched;
-          return matched;
-        }));
-    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());
-        }
-      }
-    }
-  }
+  remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true);
 
   /// Add a node to executor, execute the next available unit of work, and remove the node.
   /**
@@ -146,28 +135,23 @@ public:
    * function to be non-blocking.
    */
   template<typename T = std::milli>
-  void spin_node_once(rclcpp::node::Node::SharedPtr node,
+  void
+  spin_node_once(rclcpp::node::Node::SharedPtr node,
     std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
   {
-    this->add_node(node, false);
-    // non-blocking = true
-    auto any_exec = get_next_executable(timeout);
-    if (any_exec) {
-      execute_any_executable(any_exec);
-    }
-    this->remove_node(node, false);
+    return spin_node_once_nanoseconds(
+      node,
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
   }
 
   /// Add a node, complete all immediately available work, and remove the node.
   /**
    * \param[in] node Shared pointer to the node to add.
    */
-  void spin_node_some(rclcpp::node::Node::SharedPtr node)
-  {
-    this->add_node(node, false);
-    spin_some();
-    this->remove_node(node, false);
-  }
+  RCLCPP_PUBLIC
+  void
+  spin_node_some(rclcpp::node::Node::SharedPtr node);
 
   /// Complete all available queued work without blocking.
   /**
@@ -176,429 +160,157 @@ public:
    * Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function
    * to block (which may have unintended consequences).
    */
-  virtual void spin_some()
+  RCLCPP_PUBLIC
+  virtual void
+  spin_some();
+
+  RCLCPP_PUBLIC
+  virtual void
+  spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+
+  /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
+  /**
+   * \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] 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.
+   * \return The return code, one of SUCCESS, INTERRUPTED, or TIMEOUT.
+   */
+  template<typename ResponseT, typename TimeT = std::milli>
+  FutureReturnCode
+  spin_until_future_complete(
+    std::shared_future<ResponseT> & future,
+    std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
   {
-    while (AnyExecutable::SharedPtr any_exec =
-      get_next_executable(std::chrono::milliseconds::zero()))
-    {
-      execute_any_executable(any_exec);
+    // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
+    // inside a callback executed by an executor.
+
+    // 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));
+    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;
   }
 
+  /// Cancel any running spin* function, causing it to return.
+  /* This function can be called asynchonously from any thread. */
+  RCLCPP_PUBLIC
+  void
+  cancel();
+
   /// Support dynamic switching of the memory strategy.
   /**
    * Switching the memory strategy while the executor is spinning in another threading could have
    * unintended consequences.
    * \param[in] memory_strategy Shared pointer to the memory strategy to set.
    */
+  RCLCPP_PUBLIC
   void
-  set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy)
-  {
-    if (memory_strategy == nullptr) {
-      throw std::runtime_error("Received NULL memory strategy in executor.");
-    }
-    memory_strategy_ = memory_strategy;
-  }
+  set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy);
 
 protected:
+  RCLCPP_PUBLIC
+  void
+  spin_node_once_nanoseconds(rclcpp::node::Node::SharedPtr node, std::chrono::nanoseconds timeout);
+
   /// Find the next available executable and do the work associated with it.
   /** \param[in] any_exec Union structure that can hold any executable type (timer, subscription,
    * service, client).
    */
+  RCLCPP_PUBLIC
   void
-  execute_any_executable(AnyExecutable::SharedPtr any_exec)
-  {
-    if (!any_exec) {
-      return;
-    }
-    if (any_exec->timer) {
-      execute_timer(any_exec->timer);
-    }
-    if (any_exec->subscription) {
-      execute_subscription(any_exec->subscription);
-    }
-    if (any_exec->subscription_intra_process) {
-      execute_intra_process_subscription(any_exec->subscription_intra_process);
-    }
-    if (any_exec->service) {
-      execute_service(any_exec->service);
-    }
-    if (any_exec->client) {
-      execute_client(any_exec->client);
-    }
-    // Reset the callback_group, regardless of type
-    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());
-    }
-  }
+  execute_any_executable(AnyExecutable::SharedPtr any_exec);
 
+  RCLCPP_PUBLIC
   static void
   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 {
-      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->return_message(message);
-  }
+    rclcpp::subscription::SubscriptionBase::SharedPtr subscription);
 
+  RCLCPP_PUBLIC
   static void
   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(
-      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 {
-      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());
-    }
-  }
+    rclcpp::subscription::SubscriptionBase::SharedPtr subscription);
 
+  RCLCPP_PUBLIC
   static void
-  execute_timer(
-    rclcpp::timer::TimerBase::SharedPtr timer)
-  {
-    timer->execute_callback();
-  }
+  execute_timer(rclcpp::timer::TimerBase::SharedPtr timer);
 
+  RCLCPP_PUBLIC
   static void
-  execute_service(
-    rclcpp::service::ServiceBase::SharedPtr service)
-  {
-    std::shared_ptr<void> request_header = service->create_request_header();
-    std::shared_ptr<void> request = service->create_request();
-    bool taken = false;
-    rmw_ret_t status = rmw_take_request(
-      service->get_service_handle(),
-      request_header.get(),
-      request.get(),
-      &taken);
-    if (status == RMW_RET_OK) {
-      if (taken) {
-        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());
-    }
-  }
+  execute_service(rclcpp::service::ServiceBase::SharedPtr service);
 
+  RCLCPP_PUBLIC
   static void
-  execute_client(
-    rclcpp::client::ClientBase::SharedPtr client)
-  {
-    std::shared_ptr<void> request_header = client->create_request_header();
-    std::shared_ptr<void> response = client->create_response();
-    bool taken = false;
-    rmw_ret_t status = rmw_take_response(
-      client->get_client_handle(),
-      request_header.get(),
-      response.get(),
-      &taken);
-    if (status == RMW_RET_OK) {
-      if (taken) {
-        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());
-    }
-  }
+  execute_client(rclcpp::client::ClientBase::SharedPtr client);
 
-/*** Populate class storage from stored weak node pointers and wait. ***/
-
-  template<typename T = std::milli>
+  RCLCPP_PUBLIC
   void
-  wait_for_work(std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
-  {
-    memory_strategy_->clear_active_entities();
+  wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
-    // Collect the subscriptions and timers to be waited on
-    bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_);
-
-    // Clean up any invalid nodes, if they were detected
-    if (has_invalid_weak_nodes) {
-      weak_nodes_.erase(
-        remove_if(weak_nodes_.begin(), weak_nodes_.end(),
-        [](std::weak_ptr<rclcpp::node::Node> i)
-          {
-            return i.expired();
-          }));
-    }
-
-    // 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);
-
-    // The number of guard conditions is fixed at 2: 1 for the ctrl-c guard cond,
-    // and one for the executor's guard cond (interrupt_guard_condition_)
-    size_t number_of_guard_conds = 2;
-    rmw_guard_conditions_t guard_condition_handles;
-    guard_condition_handles.guard_condition_count = number_of_guard_conds;
-    guard_condition_handles.guard_conditions = static_cast<void **>(guard_cond_handles_.data());
-    if (guard_condition_handles.guard_conditions == NULL &&
-      number_of_guard_conds > 0)
-    {
-      // TODO(wjwwood): Use a different error here? maybe std::bad_alloc?
-      throw std::runtime_error("Could not malloc for guard condition pointers.");
-    }
-    // Put the global ctrl-c guard condition in
-    assert(guard_condition_handles.guard_condition_count > 1);
-    guard_condition_handles.guard_conditions[0] = \
-      rclcpp::utilities::get_global_sigint_guard_condition()->data;
-    // Put the executor's guard condition in
-    guard_condition_handles.guard_conditions[1] = \
-      interrupt_guard_condition_->data;
-
-    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::duration<int64_t, T>::zero()) && has_valid_timer)
-    {
-      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::duration<int64_t, T>::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;
-    }
-
-    // Now wait on the waitable subscriptions and timers
-    rmw_ret_t status = rmw_wait(
-      &subscriber_handles,
-      &guard_condition_handles,
-      &service_handles,
-      &client_handles,
-      wait_timeout);
-    if (status != RMW_RET_OK && status != RMW_RET_TIMEOUT) {
-      throw std::runtime_error(rmw_get_error_string_safe());
-    }
-    // If ctrl-c guard condition, return directly
-    if (guard_condition_handles.guard_conditions[0] != 0) {
-      // Make sure to free or clean memory
-      memory_strategy_->clear_handles();
-      return;
-    }
-
-    memory_strategy_->revalidate_handles();
-  }
-
-/******************************/
+  RCLCPP_PUBLIC
   rclcpp::node::Node::SharedPtr
-  get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group)
-  {
-    if (!group) {
-      return rclcpp::node::Node::SharedPtr();
-    }
-    for (auto & weak_node : weak_nodes_) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto callback_group = weak_group.lock();
-        if (callback_group == group) {
-          return node;
-        }
-      }
-    }
-    return rclcpp::node::Node::SharedPtr();
-  }
+  get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group);
 
+  RCLCPP_PUBLIC
   rclcpp::callback_group::CallbackGroup::SharedPtr
-  get_group_by_timer(
-    rclcpp::timer::TimerBase::SharedPtr timer)
-  {
-    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) {
-          continue;
-        }
-        for (auto & weak_timer : group->get_timer_ptrs()) {
-          auto t = weak_timer.lock();
-          if (t == timer) {
-            return group;
-          }
-        }
-      }
-    }
-    return rclcpp::callback_group::CallbackGroup::SharedPtr();
-  }
+  get_group_by_timer(rclcpp::timer::TimerBase::SharedPtr timer);
 
+  RCLCPP_PUBLIC
   void
-  get_next_timer(AnyExecutable::SharedPtr any_exec)
-  {
-    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()) {
-          auto timer = timer_ref.lock();
-          if (timer && timer->check_and_trigger()) {
-            any_exec->timer = timer;
-            any_exec->callback_group = group;
-            node = get_node_by_group(group);
-            return;
-          }
-        }
-      }
-    }
-  }
-
-  std::chrono::nanoseconds
-  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;
-  }
+  get_next_timer(AnyExecutable::SharedPtr any_exec);
 
+  RCLCPP_PUBLIC
   AnyExecutable::SharedPtr
-  get_next_ready_executable()
-  {
-    auto any_exec = memory_strategy_->instantiate_next_executable();
-    // Check the timers to see if there are any that are ready, if so return
-    get_next_timer(any_exec);
-    if (any_exec->timer) {
-      return any_exec;
-    }
-    // Check the subscriptions to see if there are any that are ready
-    memory_strategy_->get_next_subscription(any_exec, weak_nodes_);
-    if (any_exec->subscription || any_exec->subscription_intra_process) {
-      return any_exec;
-    }
-    // Check the services to see if there are any that are ready
-    memory_strategy_->get_next_service(any_exec, weak_nodes_);
-    if (any_exec->service) {
-      return any_exec;
-    }
-    // Check the clients to see if there are any that are ready
-    memory_strategy_->get_next_client(any_exec, weak_nodes_);
-    if (any_exec->client) {
-      return any_exec;
-    }
-    // If there is no ready executable, return a null ptr
-    return nullptr;
-  }
+  get_next_ready_executable();
 
-  template<typename T = std::milli>
+  RCLCPP_PUBLIC
   AnyExecutable::SharedPtr
-  get_next_executable(std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t,
-    T>(-1))
-  {
-    // Check to see if there are any subscriptions or timers needing service
-    // TODO(wjwwood): improve run to run efficiency of this function
-    auto any_exec = get_next_ready_executable();
-    // If there are none
-    if (!any_exec) {
-      // Wait for subscriptions or timers to work on
-      wait_for_work(timeout);
-      // Try again
-      any_exec = get_next_ready_executable();
-    }
-    // At this point any_exec should be valid with either a valid subscription
-    // or a valid timer, or it should be a null shared_ptr
-    if (any_exec) {
-      // If it is valid, check to see if the group is mutually exclusive or
-      // not, then mark it accordingly
-      if (any_exec->callback_group && any_exec->callback_group->type() == \
-        callback_group::CallbackGroupType::MutuallyExclusive)
-      {
-        // It should not have been taken otherwise
-        assert(any_exec->callback_group->can_be_taken_from().load());
-        // Set to false to indicate something is being run from this group
-        any_exec->callback_group->can_be_taken_from().store(false);
-      }
-    }
-    return any_exec;
-  }
+  get_next_executable(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+
+  /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins.
+  std::atomic_bool spinning;
 
   /// Guard condition for signaling the rmw layer to wake up for special events.
-  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.
+  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_;
@@ -607,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

rclcpp/include/rclcpp/executors.hpp

@@ -12,33 +12,39 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_EXECUTORS_HPP_
-#define RCLCPP_RCLCPP_EXECUTORS_HPP_
+#ifndef RCLCPP__EXECUTORS_HPP_
+#define RCLCPP__EXECUTORS_HPP_
 
 #include <future>
-#include <rclcpp/executors/multi_threaded_executor.hpp>
-#include <rclcpp/executors/single_threaded_executor.hpp>
-#include <rclcpp/node.hpp>
-#include <rclcpp/utilities.hpp>
+
+#include "rclcpp/executors/multi_threaded_executor.hpp"
+#include "rclcpp/executors/single_threaded_executor.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/utilities.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
+
+/// Create a default single-threaded executor and execute any immediately available work.
+// \param[in] node_ptr Shared pointer to the node to spin.
+RCLCPP_PUBLIC
+void
+spin_some(node::Node::SharedPtr node_ptr);
+
+/// Create a default single-threaded executor and spin the specified node.
+// \param[in] node_ptr Shared pointer to the node to spin.
+RCLCPP_PUBLIC
+void
+spin(node::Node::SharedPtr node_ptr);
+
 namespace executors
 {
 
 using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor;
 using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor;
 
-/// Return codes to be used with spin_until_future_complete.
-/**
- * SUCCESS: The future is complete and can be accessed with "get" without blocking.
- * INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error.
- * TIMEOUT: Spinning timed out.
- */
-enum FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT};
-
-/// Spin (blocking) until the future is complete, until the function times out (if applicable),
-/// or until rclcpp is interrupted.
+/// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
 /**
  * \param[in] executor The executor which will spin the node.
  * \param[in] node_ptr The node to spin.
@@ -49,39 +55,32 @@ enum FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT};
  * \return The return code, one of SUCCESS, INTERRUPTED, or TIMEOUT.
  */
 template<typename ResponseT, typename TimeT = std::milli>
-FutureReturnCode
+rclcpp::executor::FutureReturnCode
 spin_node_until_future_complete(
   rclcpp::executor::Executor & executor, rclcpp::node::Node::SharedPtr node_ptr,
   std::shared_future<ResponseT> & future,
   std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
 {
-  // TODO(wjwwood): does not work recursively right, can't call spin_node_until_future_complete
+  // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
   // inside a callback executed by an executor.
-
-  // 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()) {
-    executor.spin_node_once(node_ptr, 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;
-  }
-  return FutureReturnCode::INTERRUPTED;
+  executor.add_node(node_ptr);
+  auto retcode = executor.spin_until_future_complete(future, timeout);
+  executor.remove_node(node_ptr);
+  return retcode;
 }
 
-} // namespace executors
-} // namespace rclcpp
+}  // namespace executors
 
-#endif /* RCLCPP_RCLCPP_EXECUTORS_HPP_ */
+template<typename FutureT, typename TimeT = std::milli>
+rclcpp::executor::FutureReturnCode
+spin_until_future_complete(
+  node::Node::SharedPtr node_ptr, std::shared_future<FutureT> & future,
+  std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
+{
+  rclcpp::executors::SingleThreadedExecutor executor;
+  return executors::spin_node_until_future_complete<FutureT>(executor, node_ptr, future, timeout);
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXECUTORS_HPP_

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -15,18 +15,14 @@
 #ifndef RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
 #define RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
 
-#include <rmw/rmw.h>
-
-#include <cassert>
-#include <cstdlib>
-#include <memory>
 #include <mutex>
-#include <vector>
+#include <thread>
+#include <unordered_map>
 
 #include "rclcpp/executor.hpp"
 #include "rclcpp/macros.hpp"
-#include "rclcpp/node.hpp"
-#include "rclcpp/utilities.hpp"
+#include "rclcpp/memory_strategies.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
@@ -40,59 +36,27 @@ class MultiThreadedExecutor : public executor::Executor
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(MultiThreadedExecutor);
 
-  MultiThreadedExecutor(memory_strategy::MemoryStrategy::SharedPtr ms =
-    memory_strategies::create_default_strategy())
-  : executor::Executor(ms)
-  {
-    number_of_threads_ = std::thread::hardware_concurrency();
-    if (number_of_threads_ == 0) {
-      number_of_threads_ = 1;
-    }
-  }
+  RCLCPP_PUBLIC
+  MultiThreadedExecutor(
+    const executor::ExecutorArgs & args = rclcpp::executor::create_default_executor_arguments());
 
-  virtual ~MultiThreadedExecutor() {}
+  RCLCPP_PUBLIC
+  virtual ~MultiThreadedExecutor();
 
+  RCLCPP_PUBLIC
   void
-  spin()
-  {
-    std::vector<std::thread> threads;
-    {
-      std::lock_guard<std::mutex> wait_lock(wait_mutex_);
-      size_t thread_id_ = 1;  // Use a _ suffix to avoid shadowing `rclcpp::thread_id`
-      for (size_t i = number_of_threads_; i > 0; --i) {
-        std::this_thread::sleep_for(std::chrono::milliseconds(100));
-        auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id_++);
-        threads.emplace_back(func);
-      }
-    }
-    for (auto & thread : threads) {
-      thread.join();
-    }
-  }
+  spin();
 
+  RCLCPP_PUBLIC
   size_t
-  get_number_of_threads()
-  {
-    return number_of_threads_;
-  }
+  get_number_of_threads();
+
+protected:
+  RCLCPP_PUBLIC
+  void
+  run(size_t this_thread_number);
 
 private:
-  void run(size_t this_thread_id)
-  {
-    rclcpp::thread_id = this_thread_id;
-    while (rclcpp::utilities::ok()) {
-      executor::AnyExecutable::SharedPtr any_exec;
-      {
-        std::lock_guard<std::mutex> wait_lock(wait_mutex_);
-        if (!rclcpp::utilities::ok()) {
-          return;
-        }
-        any_exec = get_next_executable();
-      }
-      execute_any_executable(any_exec);
-    }
-  }
-
   RCLCPP_DISABLE_COPY(MultiThreadedExecutor);
 
   std::mutex wait_mutex_;

rclcpp/include/rclcpp/executors/single_threaded_executor.hpp

@@ -28,6 +28,7 @@
 #include "rclcpp/node.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/rate.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
@@ -44,23 +45,20 @@ public:
   RCLCPP_SMART_PTR_DEFINITIONS(SingleThreadedExecutor);
 
   /// Default constructor. See the default constructor for Executor.
-  SingleThreadedExecutor(memory_strategy::MemoryStrategy::SharedPtr ms =
-    memory_strategies::create_default_strategy())
-  : executor::Executor(ms) {}
+  RCLCPP_PUBLIC
+  SingleThreadedExecutor(
+    const executor::ExecutorArgs & args = rclcpp::executor::create_default_executor_arguments());
 
   /// Default destrcutor.
-  virtual ~SingleThreadedExecutor() {}
+  RCLCPP_PUBLIC
+  virtual ~SingleThreadedExecutor();
 
   /// Single-threaded implementation of spin.
   // This function will block until work comes in, execute it, and keep blocking.
   // It will only be interrupt by a CTRL-C (managed by the global signal handler).
-  void spin()
-  {
-    while (rclcpp::utilities::ok()) {
-      auto any_exec = get_next_executable();
-      execute_any_executable(any_exec);
-    }
-  }
+  RCLCPP_PUBLIC
+  void
+  spin();
 
 private:
   RCLCPP_DISABLE_COPY(SingleThreadedExecutor);

rclcpp/include/rclcpp/function_traits.hpp

@@ -12,13 +12,19 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_FUNCTION_TRAITS_HPP_
-#define RCLCPP_RCLCPP_FUNCTION_TRAITS_HPP_
+#ifndef RCLCPP__FUNCTION_TRAITS_HPP_
+#define RCLCPP__FUNCTION_TRAITS_HPP_
+
+#include <functional>
 #include <memory>
+#include <tuple>
 
 namespace rclcpp
 {
 
+namespace function_traits
+{
+
 /* NOTE(esteve):
  * We support service callbacks that can optionally take the request id,
  * which should be possible with two overloaded create_service methods,
@@ -49,6 +55,8 @@ struct function_traits
 
   template<std::size_t N>
   using argument_type = typename std::tuple_element<N, arguments>::type;
+
+  using return_type = typename function_traits<decltype( & FunctionT::operator())>::return_type;
 };
 
 // Free functions
@@ -61,17 +69,57 @@ struct function_traits<ReturnTypeT(Args ...)>
 
   template<std::size_t N>
   using argument_type = typename std::tuple_element<N, arguments>::type;
+
+  using return_type = ReturnTypeT;
 };
 
 // Function pointers
 template<typename ReturnTypeT, typename ... Args>
-struct function_traits<ReturnTypeT (*)(Args ...)>: public function_traits<ReturnTypeT(Args ...)>
+struct function_traits<ReturnTypeT (*)(Args ...)>: function_traits<ReturnTypeT(Args ...)>
+{};
+
+// std::bind for object methods
+template<typename ClassT, typename ReturnTypeT, typename ... Args, typename ... FArgs>
+#if defined _LIBCPP_VERSION  // libc++ (Clang)
+struct function_traits<std::__1::__bind<ReturnTypeT (ClassT::*)(Args ...), FArgs ...>>
+#elif defined __GLIBCXX__  // glibc++ (GNU C++)
+struct function_traits<std::_Bind<std::_Mem_fn<ReturnTypeT (ClassT::*)(Args ...)>(FArgs ...)>>
+#elif defined _MSC_VER  // MS Visual Studio
+struct function_traits<
+  std::_Binder<std::_Unforced, ReturnTypeT(__cdecl ClassT::*)(Args ...), FArgs ...>
+>
+#else
+#error "Unsupported C++ compiler / standard library"
+#endif
+  : function_traits<ReturnTypeT(Args ...)>
+{};
+
+// std::bind for free functions
+template<typename ReturnTypeT, typename ... Args, typename ... FArgs>
+#if defined _LIBCPP_VERSION  // libc++ (Clang)
+struct function_traits<std::__1::__bind<ReturnTypeT( &)(Args ...), FArgs ...>>
+#elif defined __GLIBCXX__  // glibc++ (GNU C++)
+struct function_traits<std::_Bind<ReturnTypeT(*(FArgs ...))(Args ...)>>
+#elif defined _MSC_VER  // MS Visual Studio
+struct function_traits<std::_Binder<std::_Unforced, ReturnTypeT(__cdecl &)(Args ...), FArgs ...>>
+#else
+#error "Unsupported C++ compiler / standard library"
+#endif
+  : function_traits<ReturnTypeT(Args ...)>
 {};
 
 // Lambdas
 template<typename ClassT, typename ReturnTypeT, typename ... Args>
 struct function_traits<ReturnTypeT (ClassT::*)(Args ...) const>
-  : public function_traits<ReturnTypeT(ClassT &, Args ...)>
+  : function_traits<ReturnTypeT(ClassT &, Args ...)>
+{};
+
+template<typename FunctionT>
+struct function_traits<FunctionT &>: function_traits<FunctionT>
+{};
+
+template<typename FunctionT>
+struct function_traits<FunctionT &&>: function_traits<FunctionT>
 {};
 
 /* NOTE(esteve):
@@ -95,6 +143,9 @@ struct same_arguments : std::is_same<
     typename function_traits<FunctorBT>::arguments
   >
 {};
-} /* namespace rclcpp */
 
-#endif /* RCLCPP_RCLCPP_FUNCTION_TRAITS_HPP_ */
+}  // namespace function_traits
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__FUNCTION_TRAITS_HPP_

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_

rclcpp/include/rclcpp/intra_process_manager.hpp

@@ -27,11 +27,12 @@
 #include <set>
 
 #include "rclcpp/allocator/allocator_deleter.hpp"
-#include "rclcpp/intra_process_manager_state.hpp"
+#include "rclcpp/intra_process_manager_impl.hpp"
 #include "rclcpp/mapped_ring_buffer.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/publisher.hpp"
 #include "rclcpp/subscription.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
@@ -124,12 +125,12 @@ private:
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager);
 
+  RCLCPP_PUBLIC
   explicit IntraProcessManager(
-    IntraProcessManagerStateBase::SharedPtr state = create_default_state()
-  )
-  : state_(state)
-  {
-  }
+    IntraProcessManagerImplBase::SharedPtr state = create_default_impl());
+
+  RCLCPP_PUBLIC
+  virtual ~IntraProcessManager();
 
   /// Register a subscription with the manager, returns subscriptions unique id.
   /* In addition to generating a unique intra process id for the subscription,
@@ -143,24 +144,18 @@ public:
    * \param subscription the Subscription to register.
    * \return an unsigned 64-bit integer which is the subscription's unique id.
    */
+  RCLCPP_PUBLIC
   uint64_t
-  add_subscription(subscription::SubscriptionBase::SharedPtr subscription)
-  {
-    auto id = IntraProcessManager::get_next_unique_id();
-    state_->add_subscription(id, subscription);
-    return id;
-  }
+  add_subscription(subscription::SubscriptionBase::SharedPtr subscription);
 
   /// Unregister a subscription using the subscription's unique id.
   /* This method does not allocate memory.
    *
    * \param intra_process_subscription_id id of the subscription to remove.
    */
+  RCLCPP_PUBLIC
   void
-  remove_subscription(uint64_t intra_process_subscription_id)
-  {
-    state_->remove_subscription(intra_process_subscription_id);
-  }
+  remove_subscription(uint64_t intra_process_subscription_id);
 
   /// Register a publisher with the manager, returns the publisher unique id.
   /* In addition to generating and returning a unique id for the publisher,
@@ -194,7 +189,7 @@ public:
     auto mrb = mapped_ring_buffer::MappedRingBuffer<MessageT,
       typename publisher::Publisher<MessageT, Alloc>::MessageAlloc>::make_shared(
       size, publisher->get_allocator());
-    state_->add_publisher(id, publisher, mrb, size);
+    impl_->add_publisher(id, publisher, mrb, size);
     return id;
   }
 
@@ -203,11 +198,9 @@ public:
    *
    * \param intra_process_publisher_id id of the publisher to remove.
    */
+  RCLCPP_PUBLIC
   void
-  remove_publisher(uint64_t intra_process_publisher_id)
-  {
-    state_->remove_publisher(intra_process_publisher_id);
-  }
+  remove_publisher(uint64_t intra_process_publisher_id);
 
   /// Store a message in the manager, and return the message sequence number.
   /* The given message is stored in internal storage using the given publisher
@@ -249,7 +242,7 @@ public:
     using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
     using TypedMRB = typename mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc>;
     uint64_t message_seq = 0;
-    mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = state_->get_publisher_info_for_id(
+    mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = impl_->get_publisher_info_for_id(
       intra_process_publisher_id, message_seq);
     typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(buffer);
     if (!typed_buffer) {
@@ -261,7 +254,7 @@ public:
     // TODO(wjwwood): do something when a message was displaced. log debug?
     (void)did_replace;  // Avoid unused variable warning.
 
-    state_->store_intra_process_message(intra_process_publisher_id, message_seq);
+    impl_->store_intra_process_message(intra_process_publisher_id, message_seq);
 
     // Return the message sequence which is sent to the subscription.
     return message_seq;
@@ -315,7 +308,7 @@ public:
     message = nullptr;
 
     size_t target_subs_size = 0;
-    mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = state_->take_intra_process_message(
+    mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = impl_->take_intra_process_message(
       intra_process_publisher_id,
       message_sequence_number,
       requesting_subscriptions_intra_process_id,
@@ -336,41 +329,18 @@ public:
   }
 
   /// Return true if the given rmw_gid_t matches any stored Publishers.
+  RCLCPP_PUBLIC
   bool
-  matches_any_publishers(const rmw_gid_t * id) const
-  {
-    return state_->matches_any_publishers(id);
-  }
+  matches_any_publishers(const rmw_gid_t * id) const;
 
 private:
-  static uint64_t get_next_unique_id()
-  {
-    auto next_id = next_unique_id_.fetch_add(1, std::memory_order_relaxed);
-    // Check for rollover (we started at 1).
-    if (0 == next_id) {
-      // This puts a technical limit on the number of times you can add a publisher or subscriber.
-      // But even if you could add (and remove) them at 1 kHz (very optimistic rate)
-      // it would still be a very long time before you could exhaust the pool of id's:
-      //   2^64 / 1000 times per sec / 60 sec / 60 min / 24 hours / 365 days = 584,942,417 years
-      // 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)
-      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)");
-      // *INDENT-ON*
-    }
-    return next_id;
-  }
+  RCLCPP_PUBLIC
+  static uint64_t
+  get_next_unique_id();
 
-  static std::atomic<uint64_t> next_unique_id_;
-
-  IntraProcessManagerStateBase::SharedPtr state_;
+  IntraProcessManagerImplBase::SharedPtr impl_;
 };
 
-std::atomic<uint64_t> IntraProcessManager::next_unique_id_ {1};
-
 }  // namespace intra_process_manager
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/intra_process_manager_impl.hpp

@@ -12,33 +12,39 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_
-#define RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_
-
-#include <rclcpp/mapped_ring_buffer.hpp>
-#include <rclcpp/publisher.hpp>
-#include <rclcpp/subscription.hpp>
+#ifndef RCLCPP__INTRA_PROCESS_MANAGER_IMPL_HPP_
+#define RCLCPP__INTRA_PROCESS_MANAGER_IMPL_HPP_
 
 #include <algorithm>
 #include <atomic>
 #include <functional>
 #include <limits>
-#include <memory>
 #include <map>
-#include <unordered_map>
+#include <memory>
+#include <mutex>
 #include <set>
 #include <string>
+#include <unordered_map>
 #include <utility>
 
+#include "rclcpp/macros.hpp"
+#include "rclcpp/mapped_ring_buffer.hpp"
+#include "rclcpp/publisher.hpp"
+#include "rclcpp/subscription.hpp"
+#include "rclcpp/visibility_control.hpp"
+
 namespace rclcpp
 {
 namespace intra_process_manager
 {
 
-class IntraProcessManagerStateBase
+class IntraProcessManagerImplBase
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(IntraProcessManagerStateBase);
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(IntraProcessManagerImplBase);
+
+  IntraProcessManagerImplBase() = default;
+  ~IntraProcessManagerImplBase() = default;
 
   virtual void
   add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription) = 0;
@@ -70,12 +76,18 @@ public:
 
   virtual bool
   matches_any_publishers(const rmw_gid_t * id) const = 0;
+
+private:
+  RCLCPP_DISABLE_COPY(IntraProcessManagerImplBase);
 };
 
 template<typename Allocator = std::allocator<void>>
-class IntraProcessManagerState : public IntraProcessManagerStateBase
+class IntraProcessManagerImpl : public IntraProcessManagerImplBase
 {
 public:
+  IntraProcessManagerImpl() = default;
+  ~IntraProcessManagerImpl() = default;
+
   void
   add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription)
   {
@@ -127,13 +139,14 @@ public:
     uint64_t intra_process_publisher_id,
     uint64_t & message_seq)
   {
+    std::lock_guard<std::mutex> lock(runtime_mutex_);
     auto it = publishers_.find(intra_process_publisher_id);
     if (it == publishers_.end()) {
-      throw std::runtime_error("store_intra_process_message called with invalid publisher id");
+      throw std::runtime_error("get_publisher_info_for_id called with invalid publisher id");
     }
     PublisherInfo & info = it->second;
     // Calculate the next message sequence number.
-    message_seq = info.sequence_number.fetch_add(1, std::memory_order_relaxed);
+    message_seq = info.sequence_number.fetch_add(1);
 
     return info.buffer;
   }
@@ -141,6 +154,7 @@ public:
   void
   store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq)
   {
+    std::lock_guard<std::mutex> lock(runtime_mutex_);
     auto it = publishers_.find(intra_process_publisher_id);
     if (it == publishers_.end()) {
       throw std::runtime_error("store_intra_process_message called with invalid publisher id");
@@ -154,7 +168,12 @@ public:
     // Figure out what subscriptions should receive the message.
     auto & destined_subscriptions = subscription_ids_by_topic_[publisher->get_topic_name()];
     // Store the list for later comparison.
-    info.target_subscriptions_by_message_sequence[message_seq].clear();
+    if (info.target_subscriptions_by_message_sequence.count(message_seq) == 0) {
+      info.target_subscriptions_by_message_sequence.emplace(
+        message_seq, AllocSet(std::less<uint64_t>(), uint64_allocator));
+    } else {
+      info.target_subscriptions_by_message_sequence[message_seq].clear();
+    }
     std::copy(
       destined_subscriptions.begin(), destined_subscriptions.end(),
       // Memory allocation occurs in info.target_subscriptions_by_message_sequence[message_seq]
@@ -173,6 +192,7 @@ public:
     size_t & size
   )
   {
+    std::lock_guard<std::mutex> lock(runtime_mutex_);
     PublisherInfo * info;
     {
       auto it = publishers_.find(intra_process_publisher_id);
@@ -222,9 +242,13 @@ public:
   }
 
 private:
+  RCLCPP_DISABLE_COPY(IntraProcessManagerImpl);
+
   template<typename T>
   using RebindAlloc = typename std::allocator_traits<Allocator>::template rebind_alloc<T>;
 
+  RebindAlloc<uint64_t> uint64_allocator;
+
   using AllocSet = std::set<uint64_t, std::less<uint64_t>, RebindAlloc<uint64_t>>;
   using SubscriptionMap = std::unordered_map<uint64_t, subscription::SubscriptionBase::WeakPtr,
       std::hash<uint64_t>, std::equal_to<uint64_t>,
@@ -257,14 +281,15 @@ private:
       RebindAlloc<std::pair<const uint64_t, PublisherInfo>>>;
 
   PublisherMap publishers_;
+
+  std::mutex runtime_mutex_;
 };
 
-static IntraProcessManagerStateBase::SharedPtr create_default_state()
-{
-  return std::make_shared<IntraProcessManagerState<>>();
-}
+RCLCPP_PUBLIC
+IntraProcessManagerImplBase::SharedPtr
+create_default_impl();
 
 }  // namespace intra_process_manager
 }  // namespace rclcpp
 
-#endif  // RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_
+#endif  // RCLCPP__INTRA_PROCESS_MANAGER_IMPL_HPP_

rclcpp/include/rclcpp/macros.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_MACROS_HPP_
-#define RCLCPP_RCLCPP_MACROS_HPP_
+#ifndef RCLCPP__MACROS_HPP_
+#define RCLCPP__MACROS_HPP_
 
 /* Disables the copy constructor and operator= for the given class.
  *
@@ -76,11 +76,15 @@
   { \
     return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward<Args>(args) ...)); \
   }
+
 /// Defines aliases and static functions for using the Class with unique_ptrs.
 #define RCLCPP_UNIQUE_PTR_DEFINITIONS(...) \
   __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__) \
   __RCLCPP_MAKE_UNIQUE_DEFINITION(__VA_ARGS__)
 
+#define RCLCPP_STRING_JOIN(arg1, arg2) RCLCPP_DO_STRING_JOIN(arg1, arg2)
+#define RCLCPP_DO_STRING_JOIN(arg1, arg2) arg1 ## arg2
+
 #define RCLCPP_INFO(Args) std::cout << Args << std::endl;
 
-#endif /* RCLCPP_RCLCPP_MACROS_HPP_ */
+#endif  // RCLCPP__MACROS_HPP_

rclcpp/include/rclcpp/mapped_ring_buffer.hpp

@@ -19,17 +19,19 @@
 #include <cstddef>
 #include <cstdint>
 #include <memory>
+#include <mutex>
 #include <vector>
 
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
 namespace mapped_ring_buffer
 {
 
-class MappedRingBufferBase
+class RCLCPP_PUBLIC MappedRingBufferBase
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBufferBase);
@@ -96,6 +98,7 @@ public:
   void
   get_copy_at_key(uint64_t key, ElemUniquePtr & value)
   {
+    std::lock_guard<std::mutex> lock(data_mutex_);
     auto it = get_iterator_of_key(key);
     value = nullptr;
     if (it != elements_.end() && it->in_use) {
@@ -127,6 +130,7 @@ public:
   void
   get_ownership_at_key(uint64_t key, ElemUniquePtr & value)
   {
+    std::lock_guard<std::mutex> lock(data_mutex_);
     auto it = get_iterator_of_key(key);
     value = nullptr;
     if (it != elements_.end() && it->in_use) {
@@ -154,6 +158,7 @@ public:
   void
   pop_at_key(uint64_t key, ElemUniquePtr & value)
   {
+    std::lock_guard<std::mutex> lock(data_mutex_);
     auto it = get_iterator_of_key(key);
     value = nullptr;
     if (it != elements_.end() && it->in_use) {
@@ -176,6 +181,7 @@ public:
   bool
   push_and_replace(uint64_t key, ElemUniquePtr & value)
   {
+    std::lock_guard<std::mutex> lock(data_mutex_);
     bool did_replace = elements_[head_].in_use;
     elements_[head_].key = key;
     elements_[head_].value.swap(value);
@@ -195,6 +201,7 @@ public:
   bool
   has_key(uint64_t key)
   {
+    std::lock_guard<std::mutex> lock(data_mutex_);
     return elements_.end() != get_iterator_of_key(key);
   }
 
@@ -213,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;
     });
@@ -224,6 +231,7 @@ private:
   std::vector<element, VectorAlloc> elements_;
   size_t head_;
   std::shared_ptr<ElemAlloc> allocator_;
+  std::mutex data_mutex_;
 };
 
 }  // namespace mapped_ring_buffer

rclcpp/include/rclcpp/memory_strategies.hpp

@@ -15,20 +15,17 @@
 #ifndef RCLCPP__MEMORY_STRATEGIES_HPP_
 #define RCLCPP__MEMORY_STRATEGIES_HPP_
 
-#include <rclcpp/memory_strategy.hpp>
-#include <rclcpp/strategies/allocator_memory_strategy.hpp>
+#include "rclcpp/memory_strategy.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
 namespace memory_strategies
 {
 
-using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy;
-
-static memory_strategy::MemoryStrategy::SharedPtr create_default_strategy()
-{
-  return std::make_shared<AllocatorMemoryStrategy<>>();
-}
+RCLCPP_PUBLIC
+memory_strategy::MemoryStrategy::SharedPtr
+create_default_strategy();
 
 }  // namespace memory_strategies
 }  // namespace rclcpp

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -18,9 +18,13 @@
 #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"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
@@ -33,223 +37,77 @@ namespace memory_strategy
  * the rmw implementation after the executor waits for work, based on the number of entities that
  * come through.
  */
-class MemoryStrategy
+class RCLCPP_PUBLIC MemoryStrategy
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy);
-  using WeakNodeVector = std::vector<std::weak_ptr<node::Node>>;
+  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 revalidate_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 executor::AnyExecutable::SharedPtr instantiate_next_executable() = 0;
+  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(executor::AnyExecutable::SharedPtr any_exec,
+  get_next_subscription(rclcpp::executor::AnyExecutable::SharedPtr any_exec,
     const WeakNodeVector & weak_nodes) = 0;
 
   virtual void
-  get_next_service(executor::AnyExecutable::SharedPtr any_exec,
+  get_next_service(rclcpp::executor::AnyExecutable::SharedPtr any_exec,
     const WeakNodeVector & weak_nodes) = 0;
 
   virtual void
-  get_next_client(executor::AnyExecutable::SharedPtr any_exec,
+  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,
-    const WeakNodeVector & weak_nodes)
-  {
-    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) {
-          continue;
-        }
-        for (auto & weak_subscription : group->get_subscription_ptrs()) {
-          auto subscription = weak_subscription.lock();
-          if (subscription) {
-            if (subscription->get_subscription_handle()->data == subscriber_handle) {
-              return subscription;
-            }
-            if (subscription->get_intra_process_subscription_handle() &&
-              subscription->get_intra_process_subscription_handle()->data == subscriber_handle)
-            {
-              return subscription;
-            }
-          }
-        }
-      }
-    }
-    return nullptr;
-  }
+  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)
-  {
-    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) {
-          continue;
-        }
-        for (auto & service : group->get_service_ptrs()) {
-          if (service->get_service_handle()->data == service_handle) {
-            return service;
-          }
-        }
-      }
-    }
-    return nullptr;
-  }
+  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)
-  {
-    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) {
-          continue;
-        }
-        for (auto & client : group->get_client_ptrs()) {
-          if (client->get_client_handle()->data == client_handle) {
-            return client;
-          }
-        }
-      }
-    }
-    return nullptr;
-  }
+  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,
-    const WeakNodeVector & weak_nodes)
-  {
-    if (!group) {
-      return nullptr;
-    }
-    for (auto & weak_node : weak_nodes) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto callback_group = weak_group.lock();
-        if (callback_group == group) {
-          return node;
-        }
-      }
-    }
-    return nullptr;
-  }
+    const WeakNodeVector & weak_nodes);
 
   static rclcpp::callback_group::CallbackGroup::SharedPtr
   get_group_by_subscription(
     rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
-    const WeakNodeVector & weak_nodes)
-  {
-    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) {
-          continue;
-        }
-        for (auto & weak_sub : group->get_subscription_ptrs()) {
-          auto sub = weak_sub.lock();
-          if (sub == subscription) {
-            return group;
-          }
-        }
-      }
-    }
-    return nullptr;
-  }
+    const WeakNodeVector & weak_nodes);
 
   static rclcpp::callback_group::CallbackGroup::SharedPtr
   get_group_by_service(
     rclcpp::service::ServiceBase::SharedPtr service,
-    const WeakNodeVector & weak_nodes)
-  {
-    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) {
-          continue;
-        }
-        for (auto & serv : group->get_service_ptrs()) {
-          if (serv == service) {
-            return group;
-          }
-        }
-      }
-    }
-    return nullptr;
-  }
+    const WeakNodeVector & weak_nodes);
 
   static rclcpp::callback_group::CallbackGroup::SharedPtr
   get_group_by_client(rclcpp::client::ClientBase::SharedPtr client,
-    const WeakNodeVector & weak_nodes)
-  {
-    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) {
-          continue;
-        }
-        for (auto & cli : group->get_client_ptrs()) {
-          if (cli == client) {
-            return group;
-          }
-        }
-      }
-    }
-    return nullptr;
-  }
-
-
+    const WeakNodeVector & weak_nodes);
 };
 
 }  // namespace memory_strategy
-
 }  // namespace rclcpp
 
 #endif  // RCLCPP__MEMORY_STRATEGY_HPP_

rclcpp/include/rclcpp/message_memory_strategy.hpp

@@ -16,9 +16,11 @@
 #define RCLCPP__MESSAGE_MEMORY_STRATEGY_HPP_
 
 #include <memory>
+#include <stdexcept>
 
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {

rclcpp/include/rclcpp/node.hpp

@@ -15,22 +15,28 @@
 #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"
 #include "rcl_interfaces/msg/set_parameters_result.hpp"
-#include "rosidl_generator_cpp/message_type_support.hpp"
 
 #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"
@@ -38,21 +44,40 @@
 #include "rclcpp/service.hpp"
 #include "rclcpp/subscription.hpp"
 #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);
@@ -63,6 +88,7 @@ public:
    * \param[in] use_intra_process_comms True to use the optimized intra-process communication
    * pipeline to pass messages between nodes in the same process using shared memory.
    */
+  RCLCPP_PUBLIC
   explicit Node(const std::string & node_name, bool use_intra_process_comms = false);
 
   /// Create a node based on the node name and a rclcpp::context::Context.
@@ -72,16 +98,22 @@ public:
    * \param[in] use_intra_process_comms True to use the optimized intra-process communication
    * pipeline to pass messages between nodes in the same process using shared memory.
    */
+  RCLCPP_PUBLIC
   Node(
     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
   const std::string &
-  get_name() const {return name_; }
+  get_name() const;
 
   /// Create and return a callback group.
+  RCLCPP_PUBLIC
   rclcpp::callback_group::CallbackGroup::SharedPtr
   create_callback_group(rclcpp::callback_group::CallbackGroupType group_type);
 
@@ -128,7 +160,7 @@ public:
   typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
   create_subscription(
     const std::string & topic_name,
-    CallbackT callback,
+    CallbackT && callback,
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr,
     bool ignore_local_publications = false,
@@ -155,7 +187,7 @@ public:
   create_subscription(
     const std::string & topic_name,
     size_t qos_history_depth,
-    CallbackT callback,
+    CallbackT && callback,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr,
     bool ignore_local_publications = false,
     typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
@@ -168,13 +200,14 @@ public:
    * \param[in] callback User-defined callback function.
    * \param[in] group Callback group to execute this timer's callback in.
    */
-  rclcpp::timer::WallTimer::SharedPtr
+  template<typename CallbackType>
+  typename rclcpp::timer::WallTimer<CallbackType>::SharedPtr
   create_wall_timer(
     std::chrono::nanoseconds period,
-    rclcpp::timer::CallbackType callback,
+    CallbackType callback,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
-  /// Create a timer with a sub-nanosecond precision update period.
+  /// Create a timer.
   /**
    * \param[in] period Time interval between triggers of the callback.
    * \param[in] callback User-defined callback function.
@@ -196,53 +229,142 @@ public:
   typename rclcpp::client::Client<ServiceT>::SharedPtr
   create_client(
     const std::string & service_name,
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
   /* Create and return a Service. */
-  template<typename ServiceT, typename FunctorT>
+  template<typename ServiceT, typename CallbackT>
   typename rclcpp::service::Service<ServiceT>::SharedPtr
   create_service(
     const std::string & service_name,
-    FunctorT callback,
+    CallbackT && callback,
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
-  std::vector<rcl_interfaces::msg::SetParametersResult> set_parameters(
-    const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+  RCLCPP_PUBLIC
+  std::vector<rcl_interfaces::msg::SetParametersResult>
+  set_parameters(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
 
-  rcl_interfaces::msg::SetParametersResult set_parameters_atomically(
-    const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+  RCLCPP_PUBLIC
+  rcl_interfaces::msg::SetParametersResult
+  set_parameters_atomically(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
 
-  std::vector<rclcpp::parameter::ParameterVariant> get_parameters(
-    const std::vector<std::string> & names) const;
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::parameter::ParameterVariant>
+  get_parameters(const std::vector<std::string> & names) const;
 
-  std::vector<rcl_interfaces::msg::ParameterDescriptor> describe_parameters(
-    const std::vector<std::string> & names) const;
+  RCLCPP_PUBLIC
+  std::vector<rcl_interfaces::msg::ParameterDescriptor>
+  describe_parameters(const std::vector<std::string> & names) const;
 
-  std::vector<uint8_t> get_parameter_types(
-    const std::vector<std::string> & names) const;
+  RCLCPP_PUBLIC
+  std::vector<uint8_t>
+  get_parameter_types(const std::vector<std::string> & names) const;
 
-  rcl_interfaces::msg::ListParametersResult list_parameters(
-    const std::vector<std::string> & prefixes, uint64_t depth) const;
+  RCLCPP_PUBLIC
+  rcl_interfaces::msg::ListParametersResult
+  list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const;
 
-  std::map<std::string, std::string> get_topic_names_and_types() const;
+  RCLCPP_PUBLIC
+  std::map<std::string, std::string>
+  get_topic_names_and_types() const;
 
-  size_t count_publishers(const std::string & topic_name) const;
+  RCLCPP_PUBLIC
+  size_t
+  count_publishers(const std::string & topic_name) const;
 
-  size_t count_subscribers(const std::string & topic_name) const;
+  RCLCPP_PUBLIC
+  size_t
+  count_subscribers(const std::string & topic_name) const;
 
-  const CallbackGroupWeakPtrList & get_callback_groups() const;
+  RCLCPP_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:
   RCLCPP_DISABLE_COPY(Node);
 
-  static const rosidl_message_type_support_t * ipm_ts_;
-
+  RCLCPP_PUBLIC
   bool
   group_in_node(callback_group::CallbackGroup::SharedPtr group);
 
   std::string name_;
 
-  std::shared_ptr<rmw_node_t> node_handle_;
+  std::shared_ptr<rcl_node_t> node_handle_;
 
   rclcpp::context::Context::SharedPtr context_;
 
@@ -258,25 +380,33 @@ 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_;
 };
 
-const rosidl_message_type_support_t * Node::ipm_ts_ =
-  rosidl_generator_cpp::get_message_type_support_handle<rcl_interfaces::msg::IntraProcessMessage>();
-
-} /* namespace node */
-} /* namespace rclcpp */
-
-#define RCLCPP_REGISTER_NODE(Class) RMW_EXPORT \
-  rclcpp::node::Node::SharedPtr \
-  create_node() \
-  { \
-    return rclcpp::node::Node::SharedPtr(new Class( \
-               rclcpp::contexts::default_context::DefaultContext:: \
-               make_shared())); \
-  }
+}  // namespace node
+}  // namespace rclcpp
 
 #ifndef RCLCPP__NODE_IMPL_HPP_
 // Template implementations

rclcpp/include/rclcpp/node_impl.hpp

@@ -30,92 +30,25 @@
 #include <utility>
 #include <vector>
 
+#include "rcl/publisher.h"
+#include "rcl/subscription.h"
+
 #include "rcl_interfaces/msg/intra_process_message.hpp"
-#include "rosidl_generator_cpp/message_type_support.hpp"
-#include "rosidl_generator_cpp/service_type_support.hpp"
 
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/intra_process_manager.hpp"
 #include "rclcpp/parameter.hpp"
+#include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 #ifndef RCLCPP__NODE_HPP_
 #include "node.hpp"
 #endif
 
-using namespace rclcpp;
-using namespace node;
-
-Node::Node(const std::string & node_name, bool use_intra_process_comms)
-: Node(
-    node_name,
-    rclcpp::contexts::default_context::get_global_default_context(),
-    use_intra_process_comms)
-{}
-
-Node::Node(
-  const std::string & node_name,
-  context::Context::SharedPtr context,
-  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)
+namespace rclcpp
 {
-  size_t domain_id = 0;
-  char * ros_domain_id = nullptr;
-  const char * env_var = "ROS_DOMAIN_ID";
-#ifndef _WIN32
-  ros_domain_id = getenv(env_var);
-#else
-  size_t ros_domain_id_size;
-  _dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var);
-#endif
-  if (ros_domain_id) {
-    uint32_t number = strtoul(ros_domain_id, NULL, 0);
-    if (number == (std::numeric_limits<uint32_t>::max)()) {
-      throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number");
-    }
-    domain_id = static_cast<size_t>(number);
-#ifdef _WIN32
-    free(ros_domain_id);
-#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) {
-      fprintf(
-        stderr, "Error in destruction of rmw node handle: %s\n", rmw_get_error_string_safe());
-    }
-  });
-  // *INDENT-ON*
-
-  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);
-}
-
-rclcpp::callback_group::CallbackGroup::SharedPtr
-Node::create_callback_group(
-  rclcpp::callback_group::CallbackGroupType group_type)
+namespace node
 {
-  using rclcpp::callback_group::CallbackGroup;
-  using rclcpp::callback_group::CallbackGroupType;
-  auto group = CallbackGroup::SharedPtr(new CallbackGroup(group_type));
-  callback_groups_.push_back(group);
-  return group;
-}
 
 template<typename MessageT, typename Alloc>
 typename rclcpp::publisher::Publisher<MessageT, Alloc>::SharedPtr
@@ -140,32 +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(), ipm_ts_, (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 =
@@ -201,29 +122,21 @@ 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;
 }
 
-bool
-Node::group_in_node(callback_group::CallbackGroup::SharedPtr group)
-{
-  bool group_belongs_to_this_node = false;
-  for (auto & weak_group : this->callback_groups_) {
-    auto cur_group = weak_group.lock();
-    if (cur_group && (cur_group == group)) {
-      group_belongs_to_this_node = true;
-    }
-  }
-  return group_belongs_to_this_node;
-}
-
 template<typename MessageT, typename CallbackT, typename Alloc>
 typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
 Node::create_subscription(
   const std::string & topic_name,
-  CallbackT callback,
+  CallbackT && callback,
   const rmw_qos_profile_t & qos_profile,
   rclcpp::callback_group::CallbackGroup::SharedPtr group,
   bool ignore_local_publications,
@@ -237,47 +150,39 @@ Node::create_subscription(
 
   rclcpp::subscription::AnySubscriptionCallback<MessageT,
   Alloc> any_subscription_callback(allocator);
-  any_subscription_callback.set(callback);
-
-  using rosidl_generator_cpp::get_message_type_support_handle;
+  any_subscription_callback.set(std::forward<CallbackT>(callback));
 
   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 namespace rclcpp::subscription;
+  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(), ipm_ts_,
-      (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;
@@ -286,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,
@@ -309,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*
   }
@@ -324,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;
 }
 
@@ -332,7 +242,7 @@ typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
 Node::create_subscription(
   const std::string & topic_name,
   size_t qos_history_depth,
-  CallbackT callback,
+  CallbackT && callback,
   rclcpp::callback_group::CallbackGroup::SharedPtr group,
   bool ignore_local_publications,
   typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
@@ -343,7 +253,7 @@ Node::create_subscription(
   qos.depth = qos_history_depth;
   return this->create_subscription<MessageT, CallbackT, Alloc>(
     topic_name,
-    callback,
+    std::forward<CallbackT>(callback),
     qos,
     group,
     ignore_local_publications,
@@ -351,13 +261,15 @@ Node::create_subscription(
     allocator);
 }
 
-rclcpp::timer::WallTimer::SharedPtr
+template<typename CallbackType>
+typename rclcpp::timer::WallTimer<CallbackType>::SharedPtr
 Node::create_wall_timer(
   std::chrono::nanoseconds period,
-  rclcpp::timer::CallbackType callback,
+  CallbackType callback,
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
-  auto timer = rclcpp::timer::WallTimer::make_shared(period, callback);
+  auto timer = rclcpp::timer::WallTimer<CallbackType>::make_shared(
+    period, std::move(callback));
   if (group) {
     if (!group_in_node(group)) {
       // TODO(jacquelinekay): use custom exception
@@ -368,48 +280,31 @@ 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;
 }
 
-// TODO(wjwwood): reenable this once I figure out why the demo doesn't build with it.
-// rclcpp::timer::WallTimer::SharedPtr
-// Node::create_wall_timer(
-//   std::chrono::duration<long double, std::nano> period,
-//   rclcpp::timer::CallbackType callback,
-//   rclcpp::callback_group::CallbackGroup::SharedPtr group)
-// {
-//   return create_wall_timer(
-//     std::chrono::duration_cast<std::chrono::nanoseconds>(period),
-//     callback,
-//     group);
-// }
-
 template<typename ServiceT>
 typename client::Client<ServiceT>::SharedPtr
 Node::create_client(
   const std::string & service_name,
+  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>();
+  rcl_client_options_t options = rcl_client_get_default_options();
+  options.qos = qos_profile;
 
-  rmw_client_t * client_handle = rmw_create_client(
-    this->node_handle_.get(), service_type_support_handle, service_name.c_str());
-  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*
-  }
-
-  using namespace rclcpp::client;
+  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) {
@@ -423,35 +318,31 @@ 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;
 }
 
-template<typename ServiceT, typename FunctorT>
+template<typename ServiceT, typename CallbackT>
 typename rclcpp::service::Service<ServiceT>::SharedPtr
 Node::create_service(
   const std::string & service_name,
-  FunctorT callback,
+  CallbackT && callback,
+  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(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());
-  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)) {
@@ -463,214 +354,15 @@ 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;
 }
 
-std::vector<rcl_interfaces::msg::SetParametersResult>
-Node::set_parameters(
-  const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
-{
-  std::vector<rcl_interfaces::msg::SetParametersResult> results;
-  for (auto p : parameters) {
-    auto result = set_parameters_atomically({{p}});
-    results.push_back(result);
-  }
-  return results;
-}
-
-rcl_interfaces::msg::SetParametersResult
-Node::set_parameters_atomically(
-  const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  std::map<std::string, rclcpp::parameter::ParameterVariant> tmp_map;
-  auto parameter_event = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
-
-  for (auto p : parameters) {
-    if (parameters_.find(p.get_name()) == parameters_.end()) {
-      if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
-        parameter_event->new_parameters.push_back(p.to_parameter());
-      }
-    } else if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
-      parameter_event->changed_parameters.push_back(p.to_parameter());
-    } else {
-      parameter_event->deleted_parameters.push_back(p.to_parameter());
-    }
-    tmp_map[p.get_name()] = p;
-  }
-  // std::map::insert will not overwrite elements, so we'll keep the new
-  // ones and add only those that already exist in the Node's internal map
-  tmp_map.insert(parameters_.begin(), parameters_.end());
-  std::swap(tmp_map, parameters_);
-
-  // TODO(jacquelinekay): handle parameter constraints
-  rcl_interfaces::msg::SetParametersResult result;
-  result.successful = true;
-
-  events_publisher_->publish(parameter_event);
-
-  return result;
-}
-
-std::vector<rclcpp::parameter::ParameterVariant>
-Node::get_parameters(
-  const std::vector<std::string> & names) const
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  std::vector<rclcpp::parameter::ParameterVariant> results;
-
-  for (auto & name : names) {
-    if (std::any_of(parameters_.cbegin(), parameters_.cend(),
-      [&name](const std::pair<std::string, rclcpp::parameter::ParameterVariant> & kv) {
-      return name == kv.first;
-    }))
-    {
-      results.push_back(parameters_.at(name));
-    }
-  }
-  return results;
-}
-
-std::vector<rcl_interfaces::msg::ParameterDescriptor>
-Node::describe_parameters(
-  const std::vector<std::string> & names) const
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  std::vector<rcl_interfaces::msg::ParameterDescriptor> results;
-  for (auto & kv : parameters_) {
-    if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
-      return name == kv.first;
-    }))
-    {
-      rcl_interfaces::msg::ParameterDescriptor parameter_descriptor;
-      parameter_descriptor.name = kv.first;
-      parameter_descriptor.type = kv.second.get_type();
-      results.push_back(parameter_descriptor);
-    }
-  }
-  return results;
-}
-
-std::vector<uint8_t>
-Node::get_parameter_types(
-  const std::vector<std::string> & names) const
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  std::vector<uint8_t> results;
-  for (auto & kv : parameters_) {
-    if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
-      return name == kv.first;
-    }))
-    {
-      results.push_back(kv.second.get_type());
-    } else {
-      results.push_back(rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET);
-    }
-  }
-  return results;
-}
-
-rcl_interfaces::msg::ListParametersResult
-Node::list_parameters(
-  const std::vector<std::string> & prefixes, uint64_t depth) const
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  rcl_interfaces::msg::ListParametersResult result;
-
-  // 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 (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 false;
-    }))
-    {
-      result.names.push_back(kv.first);
-      size_t last_separator = kv.first.find_last_of('.');
-      if (std::string::npos != last_separator) {
-        std::string prefix = kv.first.substr(0, last_separator);
-        if (std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) ==
-          result.prefixes.cend())
-        {
-          result.prefixes.push_back(prefix);
-        }
-      }
-    }
-  }
-  return result;
-}
-
-std::map<std::string, std::string>
-Node::get_topic_names_and_types() const
-{
-  rmw_topic_names_and_types_t topic_names_and_types;
-  topic_names_and_types.topic_count = 0;
-  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);
-  if (ret != RMW_RET_OK) {
-    // *INDENT-OFF*
-    throw std::runtime_error(
-      std::string("could not get topic names and types: ") + rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
-
-  std::map<std::string, std::string> topics;
-  for (size_t i = 0; i < topic_names_and_types.topic_count; ++i) {
-    topics[topic_names_and_types.topic_names[i]] = topic_names_and_types.type_names[i];
-  }
-
-  ret = rmw_destroy_topic_names_and_types(&topic_names_and_types);
-  if (ret != RMW_RET_OK) {
-    // *INDENT-OFF*
-    throw std::runtime_error(
-      std::string("could not destroy topic names and types: ") + rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
-
-  return topics;
-}
-
-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);
-  if (ret != RMW_RET_OK) {
-    // *INDENT-OFF*
-    throw std::runtime_error(
-      std::string("could not count publishers: ") + rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
-  return count;
-}
-
-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);
-  if (ret != RMW_RET_OK) {
-    // *INDENT-OFF*
-    throw std::runtime_error(
-      std::string("could not count subscribers: ") + rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
-  return count;
-}
-
-
-const Node::CallbackGroupWeakPtrList &
-Node::get_callback_groups() const
-{
-  return callback_groups_;
-}
+}  // namespace node
+}  // namespace rclcpp
 
 #endif  // RCLCPP__NODE_IMPL_HPP_

rclcpp/include/rclcpp/parameter.hpp

@@ -12,253 +12,203 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_PARAMETER_HPP_
-#define RCLCPP_RCLCPP_PARAMETER_HPP_
+#ifndef RCLCPP__PARAMETER_HPP_
+#define RCLCPP__PARAMETER_HPP_
 
 #include <ostream>
 #include <sstream>
 #include <string>
+#include <vector>
 
-#include <rmw/rmw.h>
-
-#include <rcl_interfaces/msg/parameter.hpp>
-#include <rcl_interfaces/msg/parameter_type.hpp>
-#include <rcl_interfaces/msg/parameter_value.hpp>
+#include "rcl_interfaces/msg/parameter.hpp"
+#include "rcl_interfaces/msg/parameter_type.hpp"
+#include "rcl_interfaces/msg/parameter_value.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rmw/rmw.h"
 
 namespace rclcpp
 {
-
 namespace parameter
 {
 
 enum ParameterType
 {
-  PARAMETER_NOT_SET=rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET,
-  PARAMETER_BOOL=rcl_interfaces::msg::ParameterType::PARAMETER_BOOL,
-  PARAMETER_INTEGER=rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER,
-  PARAMETER_DOUBLE=rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE,
-  PARAMETER_STRING=rcl_interfaces::msg::ParameterType::PARAMETER_STRING,
-  PARAMETER_BYTES=rcl_interfaces::msg::ParameterType::PARAMETER_BYTES,
+  PARAMETER_NOT_SET = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET,
+  PARAMETER_BOOL = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL,
+  PARAMETER_INTEGER = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER,
+  PARAMETER_DOUBLE = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE,
+  PARAMETER_STRING = rcl_interfaces::msg::ParameterType::PARAMETER_STRING,
+  PARAMETER_BYTES = rcl_interfaces::msg::ParameterType::PARAMETER_BYTES,
 };
 
 // Structure to store an arbitrary parameter with templated get/set methods
 class ParameterVariant
 {
 public:
-  ParameterVariant()
-  : name_("")
-  {
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET;
-  }
-  explicit ParameterVariant(const std::string & name, const bool bool_value)
-  : name_(name)
-  {
-    value_.bool_value = bool_value;
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
-  }
-  explicit ParameterVariant(const std::string & name, const int int_value)
-  : name_(name)
-  {
-    value_.integer_value = int_value;
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
-  }
-  explicit ParameterVariant(const std::string & name, const int64_t int_value)
-  : name_(name)
-  {
-    value_.integer_value = int_value;
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
-  }
-  explicit ParameterVariant(const std::string & name, const float double_value)
-  : name_(name)
-  {
-    value_.double_value = double_value;
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
-  }
-  explicit ParameterVariant(const std::string & name, const double double_value)
-  : name_(name)
-  {
-    value_.double_value = double_value;
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
-  }
-  explicit ParameterVariant(const std::string & name, const std::string & string_value)
-  : name_(name)
-  {
-    value_.string_value = string_value;
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
-  }
-  explicit ParameterVariant(const std::string & name, const char * string_value)
-  : ParameterVariant(name, std::string(string_value)) {}
-  explicit ParameterVariant(const std::string & name, const std::vector<uint8_t> & bytes_value)
-  : name_(name)
-  {
-    value_.bytes_value = bytes_value;
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BYTES;
-  }
+  RCLCPP_PUBLIC
+  ParameterVariant();
+  RCLCPP_PUBLIC
+  explicit ParameterVariant(const std::string & name, const bool bool_value);
+  RCLCPP_PUBLIC
+  explicit ParameterVariant(const std::string & name, const int int_value);
+  RCLCPP_PUBLIC
+  explicit ParameterVariant(const std::string & name, const int64_t int_value);
+  RCLCPP_PUBLIC
+  explicit ParameterVariant(const std::string & name, const float double_value);
+  RCLCPP_PUBLIC
+  explicit ParameterVariant(const std::string & name, const double double_value);
+  RCLCPP_PUBLIC
+  explicit ParameterVariant(const std::string & name, const std::string & string_value);
+  RCLCPP_PUBLIC
+  explicit ParameterVariant(const std::string & name, const char * string_value);
+  RCLCPP_PUBLIC
+  explicit ParameterVariant(const std::string & name, const std::vector<uint8_t> & bytes_value);
 
-  inline ParameterType get_type() const {return static_cast<ParameterType>(value_.type); }
+  RCLCPP_PUBLIC
+  ParameterType
+  get_type() const;
 
-  inline std::string get_type_name() const
-  {
-    switch (get_type()) {
-      case rclcpp::parameter::ParameterType::PARAMETER_BOOL:
-        return "bool";
-      case rclcpp::parameter::ParameterType::PARAMETER_INTEGER:
-        return "integer";
-      case rclcpp::parameter::ParameterType::PARAMETER_DOUBLE:
-        return "double";
-      case rclcpp::parameter::ParameterType::PARAMETER_STRING:
-        return "string";
-      case rclcpp::parameter::ParameterType::PARAMETER_BYTES:
-        return "bytes";
-      case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET:
-        return "not set";
-      default:
-        // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-        throw std::runtime_error(
-          "Unexpected type from ParameterVariant: " + std::to_string(get_type()));
-        // *INDENT-ON*
-    }
-  }
+  RCLCPP_PUBLIC
+  std::string
+  get_type_name() const;
 
-  inline std::string get_name() const & {return name_; }
+  RCLCPP_PUBLIC
+  const std::string &
+  get_name() const;
 
-  inline rcl_interfaces::msg::ParameterValue get_parameter_value() const
-  {
-    return value_;
-  }
+  RCLCPP_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
   {
     if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) {
-      // TODO: use custom exception
+      // TODO(wjwwood): use custom exception
       throw std::runtime_error("Invalid type");
     }
     return value_.integer_value;
   }
+
   template<ParameterType type>
   typename std::enable_if<type == ParameterType::PARAMETER_DOUBLE, double>::type
   get_value() const
   {
     if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) {
-      // TODO: use custom exception
+      // TODO(wjwwood): use custom exception
       throw std::runtime_error("Invalid type");
     }
     return value_.double_value;
   }
+
   template<ParameterType type>
   typename std::enable_if<type == ParameterType::PARAMETER_STRING, const std::string &>::type
   get_value() const
   {
     if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_STRING) {
-      // TODO: use custom exception
+      // TODO(wjwwood): use custom exception
       throw std::runtime_error("Invalid type");
     }
     return value_.string_value;
   }
+
   template<ParameterType type>
   typename std::enable_if<type == ParameterType::PARAMETER_BOOL, bool>::type
   get_value() const
   {
     if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) {
-      // TODO: use custom exception
+      // TODO(wjwwood): use custom exception
       throw std::runtime_error("Invalid type");
     }
     return value_.bool_value;
   }
+
   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) {
-      // TODO: use custom exception
+      // TODO(wjwwood): use custom exception
       throw std::runtime_error("Invalid type");
     }
     return value_.bytes_value;
   }
 
-  int64_t as_int() const {return get_value<ParameterType::PARAMETER_INTEGER>(); }
+  // The following get_value() variants allow the use of primitive types
 
-  double as_double() const {return get_value<ParameterType::PARAMETER_DOUBLE>(); }
+  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>();
+  }
 
-  const std::string & as_string() const {return get_value<ParameterType::PARAMETER_STRING>(); }
+  template<typename type>
+  typename std::enable_if<std::is_floating_point<type>::value, double>::type
+  get_value() const
+  {
+    return get_value<ParameterType::PARAMETER_DOUBLE>();
+  }
 
-  bool as_bool() const {return get_value<ParameterType::PARAMETER_BOOL>(); }
+  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>();
+  }
 
-  const std::vector<uint8_t> & as_bytes() const
+  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>();
   }
 
-  static ParameterVariant from_parameter(const rcl_interfaces::msg::Parameter & parameter)
-  {
-    switch (parameter.value.type) {
-      case PARAMETER_BOOL:
-        return ParameterVariant(parameter.name, parameter.value.bool_value);
-      case PARAMETER_INTEGER:
-        return ParameterVariant(parameter.name, parameter.value.integer_value);
-      case PARAMETER_DOUBLE:
-        return ParameterVariant(parameter.name, parameter.value.double_value);
-      case PARAMETER_STRING:
-        return ParameterVariant(parameter.name, parameter.value.string_value);
-      case PARAMETER_BYTES:
-        return ParameterVariant(parameter.name, parameter.value.bytes_value);
-      case PARAMETER_NOT_SET:
-        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)
-        throw std::runtime_error(
-          "Unexpected type from ParameterVariant: " + std::to_string(parameter.value.type));
-        // *INDENT-ON*
-    }
-  }
+  RCLCPP_PUBLIC
+  int64_t
+  as_int() const;
 
-  rcl_interfaces::msg::Parameter to_parameter()
-  {
-    rcl_interfaces::msg::Parameter parameter;
-    parameter.name = name_;
-    parameter.value = value_;
-    return parameter;
-  }
+  RCLCPP_PUBLIC
+  double
+  as_double() const;
 
-  std::string value_to_string() const
-  {
-    switch (get_type()) {
-      case rclcpp::parameter::ParameterType::PARAMETER_BOOL:
-        return as_bool() ? "true" : "false";
-      case rclcpp::parameter::ParameterType::PARAMETER_INTEGER:
-        return std::to_string(as_int());
-      case rclcpp::parameter::ParameterType::PARAMETER_DOUBLE:
-        return std::to_string(as_double());
-      case rclcpp::parameter::ParameterType::PARAMETER_STRING:
-        return as_string();
-      case rclcpp::parameter::ParameterType::PARAMETER_BYTES:
-        {
-          std::stringstream bytes;
-          bool first_byte = true;
-          bytes << "[" << std::hex;
-          for (auto & byte : as_bytes()) {
-            bytes << "0x" << byte;
-            if (!first_byte) {
-              bytes << ", ";
-            } else {
-              first_byte = false;
-            }
-          }
-          return bytes.str();
-        }
-      case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET:
-        return "not set";
-      default:
-        // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-        throw std::runtime_error(
-          "Unexpected type from ParameterVariant: " + std::to_string(get_type()));
-        // *INDENT-ON*
-    }
-  }
+  RCLCPP_PUBLIC
+  const std::string &
+  as_string() const;
+
+  RCLCPP_PUBLIC
+  bool
+  as_bool() const;
+
+  RCLCPP_PUBLIC
+  const std::vector<uint8_t> &
+  as_bytes() const;
+
+  RCLCPP_PUBLIC
+  static ParameterVariant
+  from_parameter(const rcl_interfaces::msg::Parameter & parameter);
+
+  RCLCPP_PUBLIC
+  rcl_interfaces::msg::Parameter
+  to_parameter();
+
+  RCLCPP_PUBLIC
+  std::string
+  value_to_string() const;
 
 private:
   std::string name_;
@@ -267,71 +217,34 @@ private:
 
 
 /* Return a json encoded version of the parameter intended for a dict. */
-std::string _to_json_dict_entry(const ParameterVariant & param)
-{
-  std::stringstream ss;
-  ss << "\"" << param.get_name() << "\": ";
-  ss << "{\"type\": \"" << param.get_type_name() << "\", ";
-  ss << "\"value\": \"" << param.value_to_string() << "\"}";
-  return ss.str();
-}
+RCLCPP_PUBLIC
+std::string
+_to_json_dict_entry(const ParameterVariant & param);
 
+RCLCPP_PUBLIC
+std::ostream &
+operator<<(std::ostream & os, const rclcpp::parameter::ParameterVariant & pv);
 
-} /* namespace parameter */
+RCLCPP_PUBLIC
+std::ostream &
+operator<<(std::ostream & os, const std::vector<ParameterVariant> & parameters);
 
-} /* namespace rclcpp */
+}  // namespace parameter
+}  // namespace rclcpp
 
 namespace std
 {
+
 /* Return a json encoded version of the parameter intended for a list. */
-inline std::string to_string(const rclcpp::parameter::ParameterVariant & param)
-{
-  std::stringstream ss;
-  ss << "{\"name\": \"" << param.get_name() << "\", ";
-  ss << "\"type\": \"" << param.get_type_name() << "\", ";
-  ss << "\"value\": \"" << param.value_to_string() << "\"}";
-  return ss.str();
-}
+RCLCPP_PUBLIC
+std::string
+to_string(const rclcpp::parameter::ParameterVariant & param);
 
 /* Return a json encoded version of a vector of parameters, as a string*/
-inline std::string to_string(const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
-{
-  std::stringstream ss;
-  ss << "{";
-  bool first = true;
-  for (const auto & pv : parameters) {
-    if (first == false) {
-      ss << ", ";
-    } else {
-      first = false;
-    }
-    ss << rclcpp::parameter::_to_json_dict_entry(pv);
-  }
-  ss << "}";
-  return ss.str();
-}
+RCLCPP_PUBLIC
+std::string
+to_string(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
 
-} /* namespace std */
+}  // namespace std
 
-namespace rclcpp
-{
-namespace parameter
-{
-
-std::ostream & operator<<(std::ostream & os, const rclcpp::parameter::ParameterVariant & pv)
-{
-  os << std::to_string(pv);
-  return os;
-}
-
-std::ostream & operator<<(std::ostream & os, const std::vector<ParameterVariant> & parameters)
-{
-  os << std::to_string(parameters);
-  return os;
-}
-
-} /* namespace parameter */
-
-} /* namespace rclcpp */
-
-#endif /* RCLCPP_RCLCPP_PARAMETER_HPP_ */
+#endif  // RCLCPP__PARAMETER_HPP_

rclcpp/include/rclcpp/parameter_client.hpp

@@ -12,227 +12,92 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_PARAMETER_CLIENT_HPP_
-#define RCLCPP_RCLCPP_PARAMETER_CLIENT_HPP_
+#ifndef RCLCPP__PARAMETER_CLIENT_HPP_
+#define RCLCPP__PARAMETER_CLIENT_HPP_
 
 #include <string>
+#include <vector>
 
-#include <rmw/rmw.h>
-
-#include <rclcpp/executors.hpp>
-#include <rclcpp/macros.hpp>
-#include <rclcpp/node.hpp>
-#include <rclcpp/parameter.hpp>
-
-#include <rcl_interfaces/msg/parameter.hpp>
-#include <rcl_interfaces/msg/parameter_event.hpp>
-#include <rcl_interfaces/msg/parameter_value.hpp>
-#include <rcl_interfaces/srv/describe_parameters.hpp>
-#include <rcl_interfaces/srv/get_parameters.hpp>
-#include <rcl_interfaces/srv/get_parameter_types.hpp>
-#include <rcl_interfaces/srv/list_parameters.hpp>
-#include <rcl_interfaces/srv/set_parameters.hpp>
-#include <rcl_interfaces/srv/set_parameters_atomically.hpp>
+#include "rcl_interfaces/msg/parameter.hpp"
+#include "rcl_interfaces/msg/parameter_event.hpp"
+#include "rcl_interfaces/msg/parameter_value.hpp"
+#include "rcl_interfaces/srv/describe_parameters.hpp"
+#include "rcl_interfaces/srv/get_parameter_types.hpp"
+#include "rcl_interfaces/srv/get_parameters.hpp"
+#include "rcl_interfaces/srv/list_parameters.hpp"
+#include "rcl_interfaces/srv/set_parameters.hpp"
+#include "rcl_interfaces/srv/set_parameters_atomically.hpp"
+#include "rclcpp/executors.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rmw/rmw.h"
 
 namespace rclcpp
 {
-
 namespace parameter_client
 {
 
 class AsyncParametersClient
 {
-
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(AsyncParametersClient);
 
-  AsyncParametersClient(const rclcpp::node::Node::SharedPtr node,
-    const std::string & remote_node_name = "")
-  : node_(node)
-  {
-    if (remote_node_name != "") {
-      remote_node_name_ = remote_node_name;
-    } else {
-      remote_node_name_ = node_->get_name();
-    }
-    get_parameters_client_ = node_->create_client<rcl_interfaces::srv::GetParameters>(
-      remote_node_name_ + "__get_parameters");
-    get_parameter_types_client_ = node_->create_client<rcl_interfaces::srv::GetParameterTypes>(
-      remote_node_name_ + "__get_parameter_types");
-    set_parameters_client_ = node_->create_client<rcl_interfaces::srv::SetParameters>(
-      remote_node_name_ + "__set_parameters");
-    list_parameters_client_ = node_->create_client<rcl_interfaces::srv::ListParameters>(
-      remote_node_name_ + "__list_parameters");
-    describe_parameters_client_ = node_->create_client<rcl_interfaces::srv::DescribeParameters>(
-      remote_node_name_ + "__describe_parameters");
-  }
+  RCLCPP_PUBLIC
+  AsyncParametersClient(
+    const rclcpp::node::Node::SharedPtr node,
+    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>>
   get_parameters(
     const std::vector<std::string> & names,
-    std::function<void(
-      std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>)> callback = nullptr)
-  {
-    auto promise_result =
-      std::make_shared<std::promise<std::vector<rclcpp::parameter::ParameterVariant>>>();
-    auto future_result = promise_result->get_future().share();
-
-    auto request = std::make_shared<rcl_interfaces::srv::GetParameters::Request>();
-    request->names = names;
-
-    get_parameters_client_->async_send_request(
-      request,
-      [request, promise_result, future_result, &callback](
-        rclcpp::client::Client<rcl_interfaces::srv::GetParameters>::SharedFuture cb_f) {
-          std::vector<rclcpp::parameter::ParameterVariant> parameter_variants;
-          auto & pvalues = cb_f.get()->values;
-
-          for (auto & pvalue : pvalues) {
-            auto i = &pvalue - &pvalues[0];
-            rcl_interfaces::msg::Parameter parameter;
-            parameter.name = request->names[i];
-            parameter.value = pvalue;
-            parameter_variants.push_back(rclcpp::parameter::ParameterVariant::from_parameter(
-              parameter));
-          }
-
-          promise_result->set_value(parameter_variants);
-          if (callback != nullptr) {
-            callback(future_result);
-          }
-        }
-    );
-
-    return future_result;
-  }
+    std::function<
+      void(std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>)
+    > callback = nullptr);
 
+  RCLCPP_PUBLIC
   std::shared_future<std::vector<rclcpp::parameter::ParameterType>>
   get_parameter_types(
     const std::vector<std::string> & names,
-    std::function<void(
-      std::shared_future<std::vector<rclcpp::parameter::ParameterType>>)> callback = nullptr)
-  {
-    auto promise_result =
-      std::make_shared<std::promise<std::vector<rclcpp::parameter::ParameterType>>>();
-    auto future_result = promise_result->get_future().share();
-
-    auto request = std::make_shared<rcl_interfaces::srv::GetParameterTypes::Request>();
-    request->names = names;
-
-    get_parameter_types_client_->async_send_request(
-      request,
-      [promise_result, future_result, &callback](
-        rclcpp::client::Client<rcl_interfaces::srv::GetParameterTypes>::SharedFuture cb_f) {
-          std::vector<rclcpp::parameter::ParameterType> types;
-          auto & pts = cb_f.get()->types;
-          for (auto & pt : pts) {
-            pts.push_back(static_cast<rclcpp::parameter::ParameterType>(pt));
-          }
-          promise_result->set_value(types);
-          if (callback != nullptr) {
-            callback(future_result);
-          }
-        }
-    );
-
-    return future_result;
-  }
+    std::function<
+      void(std::shared_future<std::vector<rclcpp::parameter::ParameterType>>)
+    > callback = nullptr);
 
+  RCLCPP_PUBLIC
   std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>
   set_parameters(
     const std::vector<rclcpp::parameter::ParameterVariant> & parameters,
-    std::function<void(std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>)> callback =
-    nullptr)
-  {
-    auto promise_result =
-      std::make_shared<std::promise<std::vector<rcl_interfaces::msg::SetParametersResult>>>();
-    auto future_result = promise_result->get_future().share();
-
-    auto request = std::make_shared<rcl_interfaces::srv::SetParameters::Request>();
-
-    std::transform(parameters.begin(), parameters.end(), std::back_inserter(
-        request->parameters), [](
-        rclcpp::parameter::ParameterVariant p) {return p.to_parameter(); });
-
-    set_parameters_client_->async_send_request(
-      request,
-      [promise_result, future_result, &callback](
-        rclcpp::client::Client<rcl_interfaces::srv::SetParameters>::SharedFuture cb_f) {
-          promise_result->set_value(cb_f.get()->results);
-          if (callback != nullptr) {
-            callback(future_result);
-          }
-        }
-    );
-
-    return future_result;
-  }
+    std::function<
+      void(std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>)
+    > callback = nullptr);
 
+  RCLCPP_PUBLIC
   std::shared_future<rcl_interfaces::msg::SetParametersResult>
   set_parameters_atomically(
     const std::vector<rclcpp::parameter::ParameterVariant> & parameters,
-    std::function<void(std::shared_future<rcl_interfaces::msg::SetParametersResult>)> callback =
-    nullptr)
-  {
-    auto promise_result =
-      std::make_shared<std::promise<rcl_interfaces::msg::SetParametersResult>>();
-    auto future_result = promise_result->get_future().share();
-
-    auto request = std::make_shared<rcl_interfaces::srv::SetParametersAtomically::Request>();
-
-    std::transform(parameters.begin(), parameters.end(), std::back_inserter(
-        request->parameters), [](
-        rclcpp::parameter::ParameterVariant p) {return p.to_parameter(); });
-
-    set_parameters_atomically_client_->async_send_request(
-      request,
-      [promise_result, future_result, &callback](
-        rclcpp::client::Client<rcl_interfaces::srv::SetParametersAtomically>::SharedFuture cb_f) {
-          promise_result->set_value(cb_f.get()->result);
-          if (callback != nullptr) {
-            callback(future_result);
-          }
-        }
-    );
-
-    return future_result;
-  }
+    std::function<
+      void(std::shared_future<rcl_interfaces::msg::SetParametersResult>)
+    > callback = nullptr);
 
+  RCLCPP_PUBLIC
   std::shared_future<rcl_interfaces::msg::ListParametersResult>
   list_parameters(
     const std::vector<std::string> & prefixes,
     uint64_t depth,
-    std::function<void(std::shared_future<rcl_interfaces::msg::ListParametersResult>)> callback =
-    nullptr)
-  {
-    auto promise_result =
-      std::make_shared<std::promise<rcl_interfaces::msg::ListParametersResult>>();
-    auto future_result = promise_result->get_future().share();
+    std::function<
+      void(std::shared_future<rcl_interfaces::msg::ListParametersResult>)
+    > callback = nullptr);
 
-    auto request = std::make_shared<rcl_interfaces::srv::ListParameters::Request>();
-    request->prefixes = prefixes;
-    request->depth = depth;
-
-    list_parameters_client_->async_send_request(
-      request,
-      [promise_result, future_result, &callback](
-        rclcpp::client::Client<rcl_interfaces::srv::ListParameters>::SharedFuture cb_f) {
-          promise_result->set_value(cb_f.get()->result);
-          if (callback != nullptr) {
-            callback(future_result);
-          }
-        }
-    );
-
-    return future_result;
-  }
-
-  template<typename FunctorT>
+  template<typename CallbackT>
   typename rclcpp::subscription::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
-  on_parameter_event(FunctorT callback)
+  on_parameter_event(CallbackT && callback)
   {
     return node_->create_subscription<rcl_interfaces::msg::ParameterEvent>(
-      "parameter_events", callback, rmw_qos_profile_parameter_events);
+      "parameter_events", std::forward<CallbackT>(callback), rmw_qos_profile_parameter_events);
   }
 
 private:
@@ -254,96 +119,83 @@ class SyncParametersClient
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient);
 
-  SyncParametersClient(
-    rclcpp::node::Node::SharedPtr node)
-  : node_(node)
-  {
-    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
-    async_parameters_client_ = std::make_shared<AsyncParametersClient>(node);
-  }
+  RCLCPP_PUBLIC
+  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)
-  : executor_(executor), node_(node)
-  {
-    async_parameters_client_ = std::make_shared<AsyncParametersClient>(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)
+  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)
   {
-    auto f = async_parameters_client_->get_parameters(parameter_names);
-    if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
-      rclcpp::executors::FutureReturnCode::SUCCESS)
-    {
-      return f.get();
+    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>());
     }
-    // Return an empty vector if unsuccessful
-    return std::vector<rclcpp::parameter::ParameterVariant>();
   }
 
+  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)
-  {
-    auto f = async_parameters_client_->get_parameter_types(parameter_names);
-
-    if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
-      rclcpp::executors::FutureReturnCode::SUCCESS)
-    {
-      return f.get();
-    }
-    return std::vector<rclcpp::parameter::ParameterType>();
-  }
+  get_parameter_types(const std::vector<std::string> & parameter_names);
 
+  RCLCPP_PUBLIC
   std::vector<rcl_interfaces::msg::SetParametersResult>
-  set_parameters(const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
-  {
-    auto f = async_parameters_client_->set_parameters(parameters);
-
-    if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
-      rclcpp::executors::FutureReturnCode::SUCCESS)
-    {
-      return f.get();
-    }
-    return std::vector<rcl_interfaces::msg::SetParametersResult>();
-  }
+  set_parameters(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
 
+  RCLCPP_PUBLIC
   rcl_interfaces::msg::SetParametersResult
-  set_parameters_atomically(const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
-  {
-    auto f = async_parameters_client_->set_parameters_atomically(parameters);
-
-    if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
-      rclcpp::executors::FutureReturnCode::SUCCESS)
-    {
-      return f.get();
-    }
-
-    throw std::runtime_error("Unable to get result of set parameters service call.");
-  }
+  set_parameters_atomically(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
 
+  RCLCPP_PUBLIC
   rcl_interfaces::msg::ListParametersResult
   list_parameters(
     const std::vector<std::string> & parameter_prefixes,
-    uint64_t depth)
-  {
-    auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth);
+    uint64_t depth);
 
-    if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
-      rclcpp::executors::FutureReturnCode::SUCCESS)
-    {
-      return f.get();
-    }
-
-    throw std::runtime_error("Unable to get result of list parameters service call.");
-  }
-
-  template<typename FunctorT>
+  template<typename CallbackT>
   typename rclcpp::subscription::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
-  on_parameter_event(FunctorT callback)
+  on_parameter_event(CallbackT && callback)
   {
-    return async_parameters_client_->on_parameter_event(callback);
+    return async_parameters_client_->on_parameter_event(std::forward<CallbackT>(callback));
   }
 
 private:
@@ -352,8 +204,7 @@ private:
   AsyncParametersClient::SharedPtr async_parameters_client_;
 };
 
-} /* namespace parameter_client */
+}  // namespace parameter_client
+}  // namespace rclcpp
 
-} /* namespace rclcpp */
-
-#endif /* RCLCPP_RCLCPP_PARAMETER_CLIENT_HPP_ */
+#endif  // RCLCPP__PARAMETER_CLIENT_HPP_

rclcpp/include/rclcpp/parameter_service.hpp

@@ -12,150 +12,38 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_PARAMETER_SERVICE_HPP_
-#define RCLCPP_RCLCPP_PARAMETER_SERVICE_HPP_
+#ifndef RCLCPP__PARAMETER_SERVICE_HPP_
+#define RCLCPP__PARAMETER_SERVICE_HPP_
 
 #include <string>
 
-#include <rmw/rmw.h>
-
-#include <rclcpp/executors.hpp>
-#include <rclcpp/macros.hpp>
-#include <rclcpp/node.hpp>
-#include <rclcpp/parameter.hpp>
-
-#include <rcl_interfaces/srv/describe_parameters.hpp>
-#include <rcl_interfaces/srv/get_parameters.hpp>
-#include <rcl_interfaces/srv/get_parameter_types.hpp>
-#include <rcl_interfaces/srv/list_parameters.hpp>
-#include <rcl_interfaces/srv/set_parameters.hpp>
-#include <rcl_interfaces/srv/set_parameters_atomically.hpp>
+#include "rcl_interfaces/srv/describe_parameters.hpp"
+#include "rcl_interfaces/srv/get_parameter_types.hpp"
+#include "rcl_interfaces/srv/get_parameters.hpp"
+#include "rcl_interfaces/srv/list_parameters.hpp"
+#include "rcl_interfaces/srv/set_parameters.hpp"
+#include "rcl_interfaces/srv/set_parameters_atomically.hpp"
+#include "rclcpp/executors.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rmw/rmw.h"
 
 namespace rclcpp
 {
-
 namespace parameter_service
 {
 
 class ParameterService
 {
-
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(ParameterService);
 
-  ParameterService(const rclcpp::node::Node::SharedPtr node)
-  : node_(node)
-  {
-    std::weak_ptr<rclcpp::node::Node> captured_node = node_;
-    get_parameters_service_ = node_->create_service<rcl_interfaces::srv::GetParameters>(
-      node_->get_name() + "__get_parameters", [captured_node](
-        const std::shared_ptr<rmw_request_id_t>,
-        const std::shared_ptr<rcl_interfaces::srv::GetParameters::Request> request,
-        std::shared_ptr<rcl_interfaces::srv::GetParameters::Response> response)
-        {
-          auto node = captured_node.lock();
-          if (!node) {
-            return;
-          }
-          auto values = node->get_parameters(request->names);
-          for (auto & pvariant : values) {
-            response->values.push_back(pvariant.get_parameter_value());
-          }
-        }
-      );
-
-    get_parameter_types_service_ = node_->create_service<rcl_interfaces::srv::GetParameterTypes>(
-      node_->get_name() + "__get_parameter_types", [captured_node](
-        const std::shared_ptr<rmw_request_id_t>,
-        const std::shared_ptr<rcl_interfaces::srv::GetParameterTypes::Request> request,
-        std::shared_ptr<rcl_interfaces::srv::GetParameterTypes::Response> response)
-        {
-          auto node = captured_node.lock();
-          if (!node) {
-            return;
-          }
-          auto types = node->get_parameter_types(request->names);
-          std::transform(types.cbegin(), types.cend(),
-          std::back_inserter(response->types), [](const uint8_t & type) {
-            return static_cast<rclcpp::parameter::ParameterType>(type);
-          });
-        }
-      );
-
-    set_parameters_service_ = node_->create_service<rcl_interfaces::srv::SetParameters>(
-      node_->get_name() + "__set_parameters", [captured_node](
-        const std::shared_ptr<rmw_request_id_t>,
-        const std::shared_ptr<rcl_interfaces::srv::SetParameters::Request> request,
-        std::shared_ptr<rcl_interfaces::srv::SetParameters::Response> response)
-        {
-          auto node = captured_node.lock();
-          if (!node) {
-            return;
-          }
-          std::vector<rclcpp::parameter::ParameterVariant> pvariants;
-          for (auto & p : request->parameters) {
-            pvariants.push_back(rclcpp::parameter::ParameterVariant::from_parameter(p));
-          }
-          auto results = node->set_parameters(pvariants);
-          response->results = results;
-        }
-      );
-
-    set_parameters_atomically_service_ =
-      node_->create_service<rcl_interfaces::srv::SetParametersAtomically>(
-      node_->get_name() + "__set_parameters_atomically", [captured_node](
-        const std::shared_ptr<rmw_request_id_t>,
-        const std::shared_ptr<rcl_interfaces::srv::SetParametersAtomically::Request> request,
-        std::shared_ptr<rcl_interfaces::srv::SetParametersAtomically::Response> response)
-        {
-          auto node = captured_node.lock();
-          if (!node) {
-            return;
-          }
-          std::vector<rclcpp::parameter::ParameterVariant> pvariants;
-          std::transform(request->parameters.cbegin(), request->parameters.cend(),
-          std::back_inserter(pvariants),
-          [](const rcl_interfaces::msg::Parameter & p) {
-            return rclcpp::parameter::ParameterVariant::
-            from_parameter(p);
-          });
-          auto result = node->set_parameters_atomically(pvariants);
-          response->result = result;
-        }
-      );
-
-    describe_parameters_service_ = node_->create_service<rcl_interfaces::srv::DescribeParameters>(
-      node_->get_name() + "__describe_parameters", [captured_node](
-        const std::shared_ptr<rmw_request_id_t>,
-        const std::shared_ptr<rcl_interfaces::srv::DescribeParameters::Request> request,
-        std::shared_ptr<rcl_interfaces::srv::DescribeParameters::Response> response)
-        {
-          auto node = captured_node.lock();
-          if (!node) {
-            return;
-          }
-          auto descriptors = node->describe_parameters(request->names);
-          response->descriptors = descriptors;
-        }
-      );
-
-    list_parameters_service_ = node_->create_service<rcl_interfaces::srv::ListParameters>(
-      node_->get_name() + "__list_parameters", [captured_node](
-        const std::shared_ptr<rmw_request_id_t>,
-        const std::shared_ptr<rcl_interfaces::srv::ListParameters::Request> request,
-        std::shared_ptr<rcl_interfaces::srv::ListParameters::Response> response)
-        {
-          auto node = captured_node.lock();
-          if (!node) {
-            return;
-          }
-          auto result = node->list_parameters(request->prefixes, request->depth);
-          response->result = result;
-        }
-      );
-
-
-  }
+  RCLCPP_PUBLIC
+  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_;
@@ -170,8 +58,7 @@ private:
   rclcpp::service::Service<rcl_interfaces::srv::ListParameters>::SharedPtr list_parameters_service_;
 };
 
-} /* namespace parameter_service */
+}  // namespace parameter_service
+}  // namespace rclcpp
 
-} /* namespace rclcpp */
-
-#endif /* RCLCPP_RCLCPP_PARAMETER_SERVICE_HPP_ */
+#endif  // RCLCPP__PARAMETER_SERVICE_HPP_

rclcpp/include/rclcpp/publisher.hpp

@@ -18,17 +18,23 @@
 #include <rmw/error_handling.h>
 #include <rmw/rmw.h>
 
+#include <functional>
 #include <iostream>
 #include <memory>
-#include <mutex>
 #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
 {
@@ -50,82 +56,42 @@ 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)
-  : node_handle_(node_handle), publisher_handle_(publisher_handle),
-    intra_process_publisher_handle_(nullptr),
-    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*
-    }
-  }
+    size_t queue_size);
 
-  /// Default destructor.
-  virtual ~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());
-      }
-    }
-  }
+  RCLCPP_PUBLIC
+  virtual ~PublisherBase();
 
   /// Get the topic that this publisher publishes on.
   // \return The topic name.
+  RCLCPP_PUBLIC
   const std::string &
-  get_topic_name() const
-  {
-    return topic_;
-  }
+  get_topic_name() const;
 
   /// Get the queue size for this publisher.
   // \return The queue size.
+  RCLCPP_PUBLIC
   size_t
-  get_queue_size() const
-  {
-    return queue_size_;
-  }
+  get_queue_size() const;
 
   /// Get the global identifier for this publisher (used in rmw and by DDS).
   // \return The gid.
+  RCLCPP_PUBLIC
   const rmw_gid_t &
-  get_gid() const
-  {
-    return rmw_gid_;
-  }
+  get_gid() const;
 
   /// Get the global identifier for this publisher used by intra-process communication.
   // \return The intra-process gid.
+  RCLCPP_PUBLIC
   const rmw_gid_t &
-  get_intra_process_gid() const
-  {
-    return intra_process_rmw_gid_;
-  }
+  get_intra_process_gid() const;
 
   /// Compare this publisher to a gid.
   /**
@@ -133,11 +99,9 @@ public:
    * \param[in] gid Reference to a gid.
    * \return True if the publisher's gid matches the input.
    */
+  RCLCPP_PUBLIC
   bool
-  operator==(const rmw_gid_t & gid) const
-  {
-    return *this == &gid;
-  }
+  operator==(const rmw_gid_t & gid) const;
 
   /// Compare this publisher to a pointer gid.
   /**
@@ -145,52 +109,25 @@ public:
    * \param[in] gid A pointer to a gid.
    * \return True if this publisher's gid matches the input.
    */
+  RCLCPP_PUBLIC
   bool
-  operator==(const rmw_gid_t * gid) const
-  {
-    bool result = false;
-    auto ret = rmw_compare_gids_equal(gid, &this->get_gid(), &result);
-    if (ret != RMW_RET_OK) {
-      throw std::runtime_error(
-              std::string("failed to compare gids: ") + rmw_get_error_string_safe());
-    }
-    if (!result) {
-      ret = rmw_compare_gids_equal(gid, &this->get_intra_process_gid(), &result);
-      if (ret != RMW_RET_OK) {
-        throw std::runtime_error(
-                std::string("failed to compare gids: ") + rmw_get_error_string_safe());
-      }
-    }
-    return result;
-  }
+  operator==(const rmw_gid_t * gid) const;
 
   typedef std::function<uint64_t(uint64_t, void *, const std::type_info &)> StoreMessageCallbackT;
 
 protected:
+  RCLCPP_PUBLIC
   void
   setup_intra_process(
     uint64_t intra_process_publisher_id,
     StoreMessageCallbackT callback,
-    rmw_publisher_t * intra_process_publisher_handle)
-  {
-    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_);
-    if (ret != RMW_RET_OK) {
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-      throw std::runtime_error(
-        std::string("failed to create intra process publisher gid: ") +
-        rmw_get_error_string_safe());
-      // *INDENT-ON*
-    }
-  }
+    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_;
@@ -200,8 +137,6 @@ protected:
 
   rmw_gid_t rmw_gid_;
   rmw_gid_t intra_process_rmw_gid_;
-
-  std::mutex intra_process_publish_mutex_;
 };
 
 /// A publisher publishes messages of any type to a topic.
@@ -219,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());
+    }
   }
 
 
@@ -250,20 +224,16 @@ public:
       //   See: http://stackoverflow.com/questions/11002641/dynamic-casting-for-unique-ptr
       MessageT * msg_ptr = msg.get();
       msg.release();
-      uint64_t message_seq;
-      {
-        std::lock_guard<std::mutex> lock(intra_process_publish_mutex_);
-        message_seq =
-          store_intra_process_message_(intra_process_publisher_id_, msg_ptr, typeid(MessageT));
-      }
+      uint64_t message_seq =
+        store_intra_process_message_(intra_process_publisher_id_, msg_ptr, typeid(MessageT));
       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 {
@@ -334,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*
     }
   }

rclcpp/include/rclcpp/rate.hpp

@@ -12,15 +12,16 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_RATE_HPP_
-#define RCLCPP_RCLCPP_RATE_HPP_
+#ifndef RCLCPP__RATE_HPP_
+#define RCLCPP__RATE_HPP_
 
 #include <chrono>
 #include <memory>
 #include <thread>
 
-#include <rclcpp/macros.hpp>
-#include <rclcpp/utilities.hpp>
+#include "rclcpp/macros.hpp"
+#include "rclcpp/utilities.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
@@ -33,7 +34,7 @@ public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(RateBase);
 
   virtual bool sleep() = 0;
-  virtual bool is_steady() = 0;
+  virtual bool is_steady() const = 0;
   virtual void reset() = 0;
 };
 
@@ -47,11 +48,11 @@ class GenericRate : public RateBase
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(GenericRate);
 
-  GenericRate(double rate)
+  explicit GenericRate(double rate)
   : GenericRate<Clock>(
       duration_cast<nanoseconds>(duration<double>(1.0 / rate)))
   {}
-  GenericRate(std::chrono::nanoseconds period)
+  explicit GenericRate(std::chrono::nanoseconds period)
   : period_(period), last_interval_(Clock::now())
   {}
 
@@ -88,7 +89,7 @@ public:
   }
 
   virtual bool
-  is_steady()
+  is_steady() const
   {
     return Clock::is_steady;
   }
@@ -108,14 +109,14 @@ private:
   RCLCPP_DISABLE_COPY(GenericRate);
 
   std::chrono::nanoseconds period_;
-  std::chrono::time_point<Clock> last_interval_;
-
+  using ClockDurationNano = std::chrono::duration<typename Clock::rep, std::nano>;
+  std::chrono::time_point<Clock, ClockDurationNano> last_interval_;
 };
 
 using Rate = GenericRate<std::chrono::system_clock>;
 using WallRate = GenericRate<std::chrono::steady_clock>;
 
-} // namespace rate
-} // namespace rclcpp
+}  // namespace rate
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_RATE_HPP_ */
+#endif  // RCLCPP__RATE_HPP_

rclcpp/include/rclcpp/rclcpp.hpp

@@ -12,59 +12,57 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_RCLCPP_HPP_
-#define RCLCPP_RCLCPP_RCLCPP_HPP_
+#ifndef RCLCPP__RCLCPP_HPP_
+#define RCLCPP__RCLCPP_HPP_
 
 #include <csignal>
 #include <memory>
 
-#include <rclcpp/node.hpp>
-#include <rclcpp/parameter.hpp>
-#include <rclcpp/parameter_client.hpp>
-#include <rclcpp/parameter_service.hpp>
-#include <rclcpp/executors.hpp>
-#include <rclcpp/rate.hpp>
-#include <rclcpp/utilities.hpp>
+#include "rclcpp/executors.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/parameter_client.hpp"
+#include "rclcpp/parameter_service.hpp"
+#include "rclcpp/rate.hpp"
+#include "rclcpp/utilities.hpp"
+#include "rclcpp/visibility_control.hpp"
 
-namespace rclcpp
-{
 // NOLINTNEXTLINE(runtime/int)
-const std::chrono::seconds operator"" _s(unsigned long long s)
+inline const std::chrono::seconds operator"" _s(unsigned long long s)
 {
   return std::chrono::seconds(s);
 }
-const std::chrono::nanoseconds operator"" _s(long double s)
+inline const std::chrono::nanoseconds operator"" _s(long double s)
 {
   return std::chrono::duration_cast<std::chrono::nanoseconds>(
     std::chrono::duration<long double>(s));
 }
 
-const std::chrono::nanoseconds
 // NOLINTNEXTLINE(runtime/int)
-operator"" _ms(unsigned long long ms)
+inline const std::chrono::nanoseconds operator"" _ms(unsigned long long ms)
 {
   return std::chrono::milliseconds(ms);
 }
-const std::chrono::nanoseconds
-operator"" _ms(long double ms)
+inline const std::chrono::nanoseconds operator"" _ms(long double ms)
 {
   return std::chrono::duration_cast<std::chrono::nanoseconds>(
     std::chrono::duration<long double, std::milli>(ms));
 }
 
-const std::chrono::nanoseconds
 // NOLINTNEXTLINE(runtime/int)
-operator"" _ns(unsigned long long ns)
+inline const std::chrono::nanoseconds operator"" _ns(unsigned long long ns)
 {
   return std::chrono::nanoseconds(ns);
 }
-const std::chrono::nanoseconds
-operator"" _ns(long double ns)
+inline const std::chrono::nanoseconds operator"" _ns(long double ns)
 {
   return std::chrono::duration_cast<std::chrono::nanoseconds>(
     std::chrono::duration<long double, std::nano>(ns));
 }
 
+namespace rclcpp
+{
+
 // Namespace escalations.
 // For example, this next line escalates type "rclcpp:node::Node" to "rclcpp::Node"
 using rclcpp::node::Node;
@@ -82,34 +80,6 @@ using rclcpp::utilities::shutdown;
 using rclcpp::utilities::init;
 using rclcpp::utilities::sleep_for;
 
-/// Create a default single-threaded executor and execute any immediately available work.
-// \param[in] node_ptr Shared pointer to the node to spin.
-void spin_some(Node::SharedPtr node_ptr)
-{
-  rclcpp::executors::SingleThreadedExecutor executor;
-  executor.spin_node_some(node_ptr);
-}
+}  // namespace rclcpp
 
-/// Create a default single-threaded executor and spin the specified node.
-// \param[in] node_ptr Shared pointer to the node to spin.
-void spin(Node::SharedPtr node_ptr)
-{
-  rclcpp::executors::SingleThreadedExecutor executor;
-  executor.add_node(node_ptr);
-  executor.spin();
-}
-
-template<typename FutureT, typename TimeT = std::milli>
-rclcpp::executors::FutureReturnCode
-spin_until_future_complete(
-  Node::SharedPtr node_ptr, std::shared_future<FutureT> & future,
-  std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
-{
-  rclcpp::executors::SingleThreadedExecutor executor;
-  return rclcpp::executors::spin_node_until_future_complete<FutureT>(
-    executor, node_ptr, future, timeout);
-}
-
-} /* namespace rclcpp */
-
-#endif /* RCLCPP_RCLCPP_RCLCPP_HPP_ */
+#endif  // RCLCPP__RCLCPP_HPP_

rclcpp/include/rclcpp/scope_exit.hpp

@@ -0,0 +1,52 @@
+// Copyright 2015 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.
+
+// Based on: http://the-witness.net/news/2012/11/scopeexit-in-c11/
+// But I changed the lambda to include by reference rather than value, see:
+// http://the-witness.net/news/2012/11/scopeexit-in-c11/comment-page-1/#comment-86873
+
+#ifndef RCLCPP__SCOPE_EXIT_HPP_
+#define RCLCPP__SCOPE_EXIT_HPP_
+
+#include <functional>
+
+#include "rclcpp/macros.hpp"
+
+namespace rclcpp
+{
+
+template<typename Callable>
+struct ScopeExit
+{
+  explicit ScopeExit(Callable callable)
+  : callable_(callable) {}
+  ~ScopeExit() {callable_(); }
+
+private:
+  Callable callable_;
+};
+
+template<typename Callable>
+ScopeExit<Callable>
+make_scope_exit(Callable callable)
+{
+  return ScopeExit<Callable>(callable);
+}
+
+}  // namespace rclcpp
+
+#define RCLCPP_SCOPE_EXIT(code) \
+  auto RCLCPP_STRING_JOIN(scope_exit_, __LINE__) = rclcpp::make_scope_exit([&]() {code; })
+
+#endif  // RCLCPP__SCOPE_EXIT_HPP_

rclcpp/include/rclcpp/service.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_SERVICE_HPP_
-#define RCLCPP_RCLCPP_SERVICE_HPP_
+#ifndef RCLCPP__SERVICE_HPP_
+#define RCLCPP__SERVICE_HPP_
 
 #include <functional>
 #include <iostream>
@@ -21,70 +21,58 @@
 #include <sstream>
 #include <string>
 
-#include <rmw/error_handling.h>
-#include <rmw/rmw.h>
+#include "rcl/error_handling.h"
+#include "rcl/service.h"
 
-#include <rclcpp/macros.hpp>
-#include <rclcpp/any_service_callback.hpp>
+#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"
 
 namespace rclcpp
 {
-
 namespace service
 {
 
 class ServiceBase
 {
-
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase);
 
+  RCLCPP_PUBLIC
   ServiceBase(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_service_t * service_handle,
-    const std::string service_name)
-  : node_handle_(node_handle), service_handle_(service_handle), service_name_(service_name)
-  {}
+    std::shared_ptr<rcl_node_t> node_handle,
+    const std::string service_name);
 
-  virtual ~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();
-      }
-    }
-  }
+  RCLCPP_PUBLIC
+  virtual ~ServiceBase();
 
-  std::string get_service_name()
-  {
-    return this->service_name_;
-  }
+  RCLCPP_PUBLIC
+  std::string
+  get_service_name();
 
-  const rmw_service_t * get_service_handle()
-  {
-    return this->service_handle_;
-  }
+  RCLCPP_PUBLIC
+  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_;
-
 };
 
-using namespace any_service_callback;
+using any_service_callback::AnyServiceCallback;
 
 template<typename ServiceT>
 class Service : public ServiceBase
@@ -103,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*
     }
   }
@@ -152,7 +163,7 @@ private:
   AnyServiceCallback<ServiceT> any_callback_;
 };
 
-} /* namespace service */
-} /* namespace rclcpp */
+}  // namespace service
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_SERVICE_HPP_ */
+#endif  // RCLCPP__SERVICE_HPP_

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -18,16 +18,19 @@
 #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
 {
-
 namespace allocator_memory_strategy
 {
 
@@ -63,81 +66,76 @@ 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 revalidate_handles()
+  virtual void remove_null_handles(rcl_wait_set_t * wait_set)
   {
-    {
-      VectorRebind<void *> temp;
-      for (auto & subscriber_handle : subscriber_handles_) {
-        if (subscriber_handle) {
-          temp.push_back(subscriber_handle);
-        }
+    for (size_t i = 0; i < wait_set->size_of_subscriptions; ++i) {
+      if (!wait_set->subscriptions[i]) {
+        subscription_handles_[i] = nullptr;
       }
-      subscriber_handles_.swap(temp);
     }
-    {
-      VectorRebind<void *> temp;
-      for (auto & service_handle : service_handles_) {
-        if (service_handle) {
-          temp.push_back(service_handle);
-        }
+    for (size_t i = 0; i < wait_set->size_of_services; ++i) {
+      if (!wait_set->services[i]) {
+        service_handles_[i] = nullptr;
       }
-      service_handles_.swap(temp);
     }
-    {
-      VectorRebind<void *> temp;
-      for (auto & client_handle : client_handles_) {
-        if (client_handle) {
-          temp.push_back(client_handle);
-        }
+    for (size_t i = 0; i < wait_set->size_of_clients; ++i) {
+      if (!wait_set->clients[i]) {
+        client_handles_[i] = nullptr;
       }
-      client_handles_.swap(temp);
     }
+    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(
+      std::remove(service_handles_.begin(), service_handles_.end(), nullptr),
+      service_handles_.end()
+    );
+
+    client_handles_.erase(
+      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)
@@ -157,17 +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 & client : group->get_client_ptrs()) {
+        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());
           }
         }
       }
@@ -175,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.
@@ -187,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()) {
@@ -218,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);
     }
   }
 
@@ -293,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_;
@@ -312,7 +389,6 @@ private:
 
 }  // namespace allocator_memory_strategy
 }  // namespace memory_strategies
-
 }  // namespace rclcpp
 
 #endif  // RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_

rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp

@@ -15,8 +15,9 @@
 #ifndef RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_
 #define RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_
 
-#include <rclcpp/macros.hpp>
-#include <rclcpp/message_memory_strategy.hpp>
+#include "rclcpp/macros.hpp"
+#include "rclcpp/message_memory_strategy.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
@@ -32,9 +33,13 @@ namespace message_pool_memory_strategy
  * The size of the message pool should be at least the largest number of concurrent accesses to
  * the subscription (usually the number of threads).
  */
-template<typename MessageT, size_t Size,
-typename std::enable_if<rosidl_generator_traits::has_fixed_size<MessageT>::value>::type * =
-nullptr>
+template<
+  typename MessageT,
+  size_t Size,
+  typename std::enable_if<
+    rosidl_generator_traits::has_fixed_size<MessageT>::value
+  >::type * = nullptr
+>
 class MessagePoolMemoryStrategy
   : public message_memory_strategy::MessageMemoryStrategy<MessageT>
 {
@@ -101,4 +106,5 @@ protected:
 }  // namespace message_pool_memory_strategy
 }  // namespace strategies
 }  // namespace rclcpp
+
 #endif  // RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_

rclcpp/include/rclcpp/subscription.hpp

@@ -24,11 +24,16 @@
 #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
 {
@@ -50,97 +55,66 @@ 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)
-  : intra_process_subscription_handle_(nullptr),
-    node_handle_(node_handle),
-    subscription_handle_(subscription_handle),
-    topic_name_(topic_name),
-    ignore_local_publications_(ignore_local_publications)
-  {
-    // To avoid unused private member warnings.
-    (void)ignore_local_publications_;
-  }
+    bool ignore_local_publications);
 
   /// Default destructor.
-  virtual ~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 (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();
-      }
-    }
-  }
+  RCLCPP_PUBLIC
+  virtual ~SubscriptionBase();
 
   /// Get the topic that this subscription is subscribed on.
-  const std::string & get_topic_name() const
-  {
-    return this->topic_name_;
-  }
+  RCLCPP_PUBLIC
+  const std::string &
+  get_topic_name() const;
 
-  const rmw_subscription_t * get_subscription_handle() const
-  {
-    return subscription_handle_;
-  }
+  RCLCPP_PUBLIC
+  const rcl_subscription_t *
+  get_subscription_handle() const;
 
-  const rmw_subscription_t * get_intra_process_subscription_handle() const
-  {
-    return intra_process_subscription_handle_;
-  }
+  RCLCPP_PUBLIC
+  virtual const rcl_subscription_t *
+  get_intra_process_subscription_handle() const;
 
   /// Borrow a new message.
   // \return Shared pointer to the fresh message.
-  virtual std::shared_ptr<void> create_message() = 0;
+  virtual std::shared_ptr<void>
+  create_message() = 0;
   /// Check if we need to handle the message, and execute the callback if we do.
   /**
    * \param[in] message Shared pointer to the message to handle.
    * \param[in] message_info Metadata associated with this message.
    */
-  virtual void handle_message(
-    std::shared_ptr<void> & message,
-    const rmw_message_info_t & message_info) = 0;
+  virtual void
+  handle_message(std::shared_ptr<void> & message, const rmw_message_info_t & message_info) = 0;
 
   /// Return the message borrowed in create_message.
   // \param[in] Shared pointer to the returned message.
-  virtual void return_message(std::shared_ptr<void> & message) = 0;
-  virtual void handle_intra_process_message(
+  virtual void
+  return_message(std::shared_ptr<void> & message) = 0;
+  virtual void
+  handle_intra_process_message(
     rcl_interfaces::msg::IntraProcessMessage & ipm,
     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_;
 };
 
-using namespace any_subscription_callback;
+using any_subscription_callback::AnySubscriptionCallback;
 
 /// Subscription implementation, templated on the type of message this subscription receives.
 template<typename MessageT, typename Alloc = std::allocator<void>>
@@ -160,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,
+    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.
@@ -257,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_;

rclcpp/include/rclcpp/timer.hpp

@@ -12,93 +12,95 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_TIMER_HPP_
-#define RCLCPP_RCLCPP_TIMER_HPP_
+#ifndef RCLCPP__TIMER_HPP_
+#define RCLCPP__TIMER_HPP_
 
 #include <chrono>
 #include <functional>
 #include <memory>
 #include <sstream>
 #include <thread>
+#include <type_traits>
 
-#include <rmw/error_handling.h>
-#include <rmw/rmw.h>
+#include "rclcpp/function_traits.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/rate.hpp"
+#include "rclcpp/utilities.hpp"
+#include "rclcpp/visibility_control.hpp"
 
-#include <rclcpp/macros.hpp>
-#include <rclcpp/rate.hpp>
-#include <rclcpp/utilities.hpp>
+#include "rcl/error_handling.h"
+#include "rcl/timer.h"
+
+#include "rmw/error_handling.h"
+#include "rmw/rmw.h"
 
 namespace rclcpp
 {
-
 namespace timer
 {
 
-using CallbackType = std::function<void()>;
-
 class TimerBase
 {
-
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase);
 
-  TimerBase(std::chrono::nanoseconds period, CallbackType callback)
-  : period_(period),
-    callback_(callback),
-    canceled_(false)
-  {
-  }
+  RCLCPP_PUBLIC
+  explicit TimerBase(std::chrono::nanoseconds period);
 
-  virtual ~TimerBase()
-  {
-  }
+  RCLCPP_PUBLIC
+  ~TimerBase();
 
+  RCLCPP_PUBLIC
   void
-  cancel()
-  {
-    this->canceled_ = true;
-  }
+  cancel();
 
-  void execute_callback() const
-  {
-    callback_();
-  }
+  RCLCPP_PUBLIC
+  virtual void
+  execute_callback() = 0;
 
-  const CallbackType & get_callback() const
-  {
-    return callback_;
-  }
+  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_;
-  CallbackType callback_;
-
-  bool canceled_;
-
+  rcl_timer_t timer_handle_ = rcl_get_zero_initialized_timer();
 };
 
+
+using VoidCallbackType = std::function<void()>;
+using TimerCallbackType = std::function<void(TimerBase &)>;
+
 /// Generic timer templated on the clock type. Periodically executes a user-specified callback.
-template<class Clock = std::chrono::high_resolution_clock>
+template<
+  typename FunctorT,
+  class Clock,
+  typename std::enable_if<
+    (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
 {
-
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(GenericTimer);
 
@@ -107,12 +109,9 @@ public:
    * \param[in] period The interval at which the timer fires.
    * \param[in] callback User-specified callback function.
    */
-  GenericTimer(std::chrono::nanoseconds period, CallbackType callback)
-  : TimerBase(period, callback), loop_rate_(period)
+  GenericTimer(std::chrono::nanoseconds period, FunctorT && callback)
+  : TimerBase(period), callback_(std::forward<FunctorT>(callback))
   {
-    /* Subtracting the loop rate period ensures that the callback gets triggered
-       on the first call to check_and_trigger. */
-    last_triggered_time_ = Clock::now() - period;
   }
 
   /// Default destructor.
@@ -120,39 +119,47 @@ 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());
+    }
   }
 
-  bool
-  check_and_trigger()
+  void
+  execute_callback()
   {
-    if (canceled_) {
-      return false;
+    rcl_ret_t ret = rcl_timer_call(&timer_handle_);
+    if (ret == RCL_RET_TIMER_CANCELED) {
+      return;
     }
-    if (Clock::now() < last_triggered_time_) {
-      return false;
+    if (ret != RCL_RET_OK) {
+      throw std::runtime_error("Failed to notify timer that callback occurred");
     }
-    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;
+    execute_callback_delegate<>();
   }
 
-  std::chrono::nanoseconds
-  time_until_trigger()
+  // void specialization
+  template<
+    typename CallbackT = FunctorT,
+    typename std::enable_if<
+      rclcpp::function_traits::same_arguments<CallbackT, VoidCallbackType>::value
+    >::type * = nullptr
+  >
+  void
+  execute_callback_delegate()
   {
-    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;
+    callback_();
+  }
+
+  template<
+    typename CallbackT = FunctorT,
+    typename std::enable_if<
+      rclcpp::function_traits::same_arguments<CallbackT, TimerCallbackType>::value
+    >::type * = nullptr
+  >
+  void
+  execute_callback_delegate()
+  {
+    callback_(*this);
   }
 
   virtual bool
@@ -161,17 +168,16 @@ public:
     return Clock::is_steady;
   }
 
-private:
+protected:
   RCLCPP_DISABLE_COPY(GenericTimer);
 
-  rclcpp::rate::GenericRate<Clock> loop_rate_;
-  std::chrono::time_point<Clock> last_triggered_time_;
-
+  FunctorT callback_;
 };
 
-using WallTimer = GenericTimer<std::chrono::steady_clock>;
+template<typename CallbackType>
+using WallTimer = GenericTimer<CallbackType, std::chrono::steady_clock>;
 
-} /* namespace timer */
-} /* namespace rclcpp */
+}  // namespace timer
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_TIMER_HPP_ */
+#endif  // RCLCPP__TIMER_HPP_

rclcpp/include/rclcpp/type_support_decl.hpp

@@ -0,0 +1,78 @@
+// Copyright 2014 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__TYPE_SUPPORT_DECL_HPP_
+#define RCLCPP__TYPE_SUPPORT_DECL_HPP_
+
+#include "rosidl_generator_cpp/message_type_support_decl.hpp"
+#include "rosidl_generator_cpp/service_type_support_decl.hpp"
+
+#include "rosidl_generator_cpp/message_type_support.hpp"
+#include "rosidl_generator_cpp/service_type_support.hpp"
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace type_support
+{
+
+RCLCPP_PUBLIC
+const rosidl_message_type_support_t *
+get_intra_process_message_msg_type_support();
+
+RCLCPP_PUBLIC
+const rosidl_message_type_support_t *
+get_parameter_event_msg_type_support();
+
+RCLCPP_PUBLIC
+const rosidl_message_type_support_t *
+get_set_parameters_result_msg_type_support();
+
+RCLCPP_PUBLIC
+const rosidl_message_type_support_t *
+get_parameter_descriptor_msg_type_support();
+
+RCLCPP_PUBLIC
+const rosidl_message_type_support_t *
+get_list_parameters_result_msg_type_support();
+
+RCLCPP_PUBLIC
+const rosidl_service_type_support_t *
+get_get_parameters_srv_type_support();
+
+RCLCPP_PUBLIC
+const rosidl_service_type_support_t *
+get_get_parameter_types_srv_type_support();
+
+RCLCPP_PUBLIC
+const rosidl_service_type_support_t *
+get_set_parameters_srv_type_support();
+
+RCLCPP_PUBLIC
+const rosidl_service_type_support_t *
+get_list_parameters_srv_type_support();
+
+RCLCPP_PUBLIC
+const rosidl_service_type_support_t *
+get_describe_parameters_srv_type_support();
+
+RCLCPP_PUBLIC
+const rosidl_service_type_support_t *
+get_set_parameters_atomically_srv_type_support();
+
+}  // namespace type_support
+}  // namespace rclcpp
+
+#endif  // RCLCPP__TYPE_SUPPORT_DECL_HPP_

rclcpp/include/rclcpp/utilities.hpp

@@ -12,100 +12,38 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_UTILITIES_HPP_
-#define RCLCPP_RCLCPP_UTILITIES_HPP_
+#ifndef RCLCPP__UTILITIES_HPP_
+#define RCLCPP__UTILITIES_HPP_
 
-// TODO(wjwwood): remove
-#include <iostream>
-
-#include <cerrno>
 #include <chrono>
-#include <condition_variable>
-#include <csignal>
-#include <cstring>
-#include <mutex>
-#include <string.h>
-#include <thread>
+#include <functional>
 
-#include <rmw/error_handling.h>
-#include <rmw/macros.h>
-#include <rmw/rmw.h>
+#include "rclcpp/visibility_control.hpp"
 
-// Determine if sigaction is available
-#if __APPLE__ || _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _POSIX_SOURCE
-#define HAS_SIGACTION
-#endif
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
 
-namespace
+#include "rmw/macros.h"
+#include "rmw/rmw.h"
+
+#ifdef ANDROID
+#include <string>
+#include <sstream>
+
+namespace std
 {
-/// Represent the status of the global interrupt signal.
-volatile sig_atomic_t g_signal_status = 0;
-/// Guard condition for interrupting the rmw implementation when the global interrupt signal fired.
-rmw_guard_condition_t * g_sigint_guard_cond_handle = \
-  rmw_create_guard_condition();
-/// Condition variable for timed sleep (see sleep_for).
-std::condition_variable g_interrupt_condition_variable;
-std::atomic<bool> g_is_interrupted(false);
-/// Mutex for protecting the global condition variable.
-std::mutex g_interrupt_mutex;
-
-#ifdef HAS_SIGACTION
-struct sigaction old_action;
-#else
-void (* old_signal_handler)(int) = 0;
-#endif
-
-/// Handle the interrupt signal.
-/** When the interrupt signal fires, the signal handler notifies the condition variable to wake up
- * and triggers the interrupt guard condition, so that all global threads managed by rclcpp
- * are interrupted.
- */
-void
-#ifdef HAS_SIGACTION
-signal_handler(int signal_value, siginfo_t * siginfo, void * context)
-#else
-signal_handler(int signal_value)
-#endif
+template<typename T>
+std::string to_string(T value)
 {
-  // TODO(wjwwood): remove
-  std::cout << "signal_handler(" << signal_value << ")" << std::endl;
-#ifdef HAS_SIGACTION
-  if (old_action.sa_flags & SA_SIGINFO) {
-    if (old_action.sa_sigaction != NULL) {
-      old_action.sa_sigaction(signal_value, siginfo, context);
-    }
-  } else {
-    // *INDENT-OFF*
-    if (
-      old_action.sa_handler != NULL && // Is set
-      old_action.sa_handler != SIG_DFL && // Is not default
-      old_action.sa_handler != SIG_IGN) // Is not ignored
-    // *INDENT-ON*
-    {
-      old_action.sa_handler(signal_value);
-    }
-  }
-#else
-  if (old_signal_handler) {
-    old_signal_handler(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());
-  }
-  g_is_interrupted.store(true);
-  g_interrupt_condition_variable.notify_all();
+  std::ostringstream os;
+  os << value;
+  return os.str();
 }
-} // namespace
+}
+#endif
 
 namespace rclcpp
 {
-
-RMW_THREAD_LOCAL size_t thread_id = 0;
-
 namespace utilities
 {
 
@@ -114,113 +52,66 @@ namespace utilities
  * \param[in] argc Number of arguments.
  * \param[in] argv Argument vector. Will eventually be used for passing options to rclcpp.
  */
+RCLCPP_PUBLIC
 void
-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)
-    throw std::runtime_error(
-      std::string("failed to initialize rmw implementation: ") + rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
-#ifdef HAS_SIGACTION
-  struct sigaction action;
-  memset(&action, 0, sizeof(action));
-  sigemptyset(&action.sa_mask);
-  action.sa_sigaction = ::signal_handler;
-  action.sa_flags = SA_SIGINFO;
-  ssize_t ret = sigaction(SIGINT, &action, &old_action);
-  if (ret == -1)
-#else
-  ::old_signal_handler = std::signal(SIGINT, ::signal_handler);
-  if (::old_signal_handler == SIG_ERR)
-#endif
-  {
-    const size_t error_length = 1024;
-    char error_string[error_length];
-#ifndef _WIN32
-    auto rc = strerror_r(errno, error_string, error_length);
-    if (rc) {
-      // *INDENT-OFF*
-      throw std::runtime_error(
-        "Failed to set SIGINT signal handler: (" + std::to_string(errno) +
-        ") unable to retrieve error string");
-      // *INDENT-ON*
-    }
-
-#else
-    strerror_s(error_string, error_length, errno);
-#endif
-    // *INDENT-OFF*
-    throw std::runtime_error(
-      std::string("Failed to set SIGINT signal handler: (" + std::to_string(errno) + ")") +
-      error_string);
-    // *INDENT-ON*
-  }
-}
+init(int argc, char * argv[]);
 
 /// Check rclcpp's status.
 // \return True if SIGINT hasn't fired yet, false otherwise.
+RCLCPP_PUBLIC
 bool
-ok()
-{
-  return ::g_signal_status == 0;
-}
+ok();
 
 /// Notify the signal handler and rmw that rclcpp is shutting down.
+RCLCPP_PUBLIC
 void
-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());
-  }
-  g_is_interrupted.store(true);
-  g_interrupt_condition_variable.notify_all();
-}
+shutdown();
 
+/// Register a function to be called when shutdown is called.
+/* Calling the callbacks is the last thing shutdown() does. */
+RCLCPP_PUBLIC
+void
+on_shutdown(std::function<void(void)> callback);
 
 /// Get a handle to the rmw guard condition that manages the signal handler.
-rmw_guard_condition_t *
-get_global_sigint_guard_condition()
-{
-  return ::g_sigint_guard_cond_handle;
-}
+/**
+ * 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.
 /**
  * \param[in] nanoseconds A std::chrono::duration representing how long to sleep for.
  * \return True if the condition variable did not timeout.
  */
+RCLCPP_PUBLIC
 bool
-sleep_for(const std::chrono::nanoseconds & nanoseconds)
-{
-  // TODO: determine if posix's nanosleep(2) is more efficient here
-  std::chrono::nanoseconds time_left = nanoseconds;
-  {
-    std::unique_lock<std::mutex> lock(::g_interrupt_mutex);
-    auto start = std::chrono::steady_clock::now();
-    ::g_interrupt_condition_variable.wait_for(lock, nanoseconds);
-    time_left -= std::chrono::steady_clock::now() - start;
-  }
-  if (time_left > std::chrono::nanoseconds::zero() && !g_is_interrupted) {
-    return sleep_for(time_left);
-  }
-  // Return true if the timeout elapsed successfully, otherwise false.
-  return !g_is_interrupted;
-}
+sleep_for(const std::chrono::nanoseconds & nanoseconds);
 
-} /* namespace utilities */
-} /* namespace rclcpp */
+}  // namespace utilities
+}  // namespace rclcpp
 
-#ifdef HAS_SIGACTION
-#undef HAS_SIGACTION
-#endif
-
-#endif /* RCLCPP_RCLCPP_UTILITIES_HPP_ */
+#endif  // RCLCPP__UTILITIES_HPP_

rclcpp/include/rclcpp/visibility_control.hpp

@@ -0,0 +1,58 @@
+// Copyright 2015 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.
+
+/* This header must be included by all rclcpp headers which declare symbols
+ * which are defined in the rclcpp library. When not building the rclcpp
+ * library, i.e. when using the headers in other package's code, the contents
+ * of this header change the visibility of certain symbols which the rclcpp
+ * library cannot have, but the consuming code must have inorder to link.
+ */
+
+#ifndef RCLCPP__VISIBILITY_CONTROL_HPP_
+#define RCLCPP__VISIBILITY_CONTROL_HPP_
+
+#include "rmw/rmw.h"
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+  #ifdef __GNUC__
+    #define RCLCPP_EXPORT __attribute__ ((dllexport))
+    #define RCLCPP_IMPORT __attribute__ ((dllimport))
+  #else
+    #define RCLCPP_EXPORT __declspec(dllexport)
+    #define RCLCPP_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef RCLCPP_BUILDING_LIBRARY
+    #define RCLCPP_PUBLIC RCLCPP_EXPORT
+  #else
+    #define RCLCPP_PUBLIC RCLCPP_IMPORT
+  #endif
+  #define RCLCPP_PUBLIC_TYPE RCLCPP_PUBLIC
+  #define RCLCPP_LOCAL
+#else
+  #define RCLCPP_EXPORT __attribute__ ((visibility("default")))
+  #define RCLCPP_IMPORT
+  #if __GNUC__ >= 4
+    #define RCLCPP_PUBLIC __attribute__ ((visibility("default")))
+    #define RCLCPP_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define RCLCPP_PUBLIC
+    #define RCLCPP_LOCAL
+  #endif
+  #define RCLCPP_PUBLIC_TYPE
+#endif
+
+#endif  // RCLCPP__VISIBILITY_CONTROL_HPP_

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>
@@ -11,7 +12,15 @@
   <build_export_depend>rmw</build_export_depend>
 
   <build_depend>rcl_interfaces</build_depend>
+  <build_depend>rmw_implementation_cmake</build_depend>
+  <build_depend>rosidl_generator_cpp</build_depend>
   <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>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>

rclcpp/rclcpp-extras.cmake

@@ -14,6 +14,8 @@
 
 # copied from rclcpp/rclcpp-extras.cmake
 
+include("${rclcpp_DIR}/get_rclcpp_information.cmake")
+
 set(rclcpp_node_main_SRC "${rclcpp_DIR}/../../../src/rclcpp/node_main.cpp")
 
 function(rclcpp_create_node_main node_library_target)

rclcpp/src/rclcpp/any_executable.cpp

@@ -0,0 +1,37 @@
+// Copyright 2015 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/any_executable.hpp"
+
+using rclcpp::executor::AnyExecutable;
+
+AnyExecutable::AnyExecutable()
+: subscription(nullptr),
+  subscription_intra_process(nullptr),
+  timer(nullptr),
+  service(nullptr),
+  client(nullptr),
+  callback_group(nullptr),
+  node(nullptr)
+{}
+
+AnyExecutable::~AnyExecutable()
+{
+  // Make sure that discarded (taken but not executed) AnyExecutable's have
+  // their callback groups reset. This can happen when an executor is canceled
+  // between taking an AnyExecutable and executing it.
+  if (callback_group) {
+    callback_group->can_be_taken_from().store(true);
+  }
+}

rclcpp/src/rclcpp/callback_group.cpp

@@ -0,0 +1,93 @@
+// Copyright 2015 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/callback_group.hpp"
+
+#include <vector>
+
+using rclcpp::callback_group::CallbackGroup;
+using rclcpp::callback_group::CallbackGroupType;
+
+CallbackGroup::CallbackGroup(CallbackGroupType group_type)
+: type_(group_type), can_be_taken_from_(true)
+{}
+
+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_;
+}
+
+std::atomic_bool &
+CallbackGroup::can_be_taken_from()
+{
+  return can_be_taken_from_;
+}
+
+const CallbackGroupType &
+CallbackGroup::type() const
+{
+  return type_;
+}
+
+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);
+}

rclcpp/src/rclcpp/client.cpp

@@ -0,0 +1,113 @@
+// Copyright 2015 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/client.hpp"
+
+#include <chrono>
+#include <cstdio>
+#include <string>
+
+#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<rclcpp::node::Node> parent_node,
+  const std::string & service_name)
+: node_(parent_node), node_handle_(parent_node->get_shared_node_handle()),
+  service_name_(service_name)
+{}
+
+ClientBase::~ClientBase() {}
+
+const std::string &
+ClientBase::get_service_name() const
+{
+  return this->service_name_;
+}
+
+const rcl_client_t *
+ClientBase::get_client_handle() const
+{
+  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();
+}

rclcpp/src/rclcpp/context.cpp

@@ -0,0 +1,19 @@
+// Copyright 2015 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/context.hpp"
+
+using rclcpp::context::Context;
+
+Context::Context() {}

rclcpp/src/rclcpp/contexts/default_context.cpp

@@ -1,4 +1,4 @@
-// Copyright 2014 Open Source Robotics Foundation, Inc.
+// Copyright 2015 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.
@@ -12,17 +12,16 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <rclcpp/rclcpp.hpp>
+#include "rclcpp/contexts/default_context.hpp"
 
-// This forward declaration is implemented by the RCLCPP_REGISTER_NODE macro
-RMW_IMPORT rclcpp::Node::SharedPtr create_node();
+using rclcpp::contexts::default_context::DefaultContext;
 
-int main(int argc, char ** argv)
+DefaultContext::DefaultContext()
+{}
+
+DefaultContext::SharedPtr
+rclcpp::contexts::default_context::get_global_default_context()
 {
-  rclcpp::init(argc, argv);
-  rclcpp::executors::SingleThreadedExecutor executor;
-  rclcpp::Node::SharedPtr node = create_node();
-  executor.add_node(node);
-  executor.spin();
-  return 0;
+  static DefaultContext::SharedPtr default_context = DefaultContext::make_shared();
+  return default_context;
 }

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

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

rclcpp/src/rclcpp/executor.cpp

@@ -0,0 +1,576 @@
+// Copyright 2015 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 <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)
+{
+  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 always at least 2: 1 for the ctrl-c guard cond,
+  // and one for the executor's guard cond (interrupt_guard_condition_)
+
+  // Put the global ctrl-c guard condition in
+  memory_strategy_->add_guard_condition(rclcpp::utilities::get_sigint_guard_condition(&waitset_));
+
+  // Put the executor's guard condition in
+  memory_strategy_->add_guard_condition(&interrupt_guard_condition_);
+  rcl_allocator_t allocator = memory_strategy_->get_allocator();
+
+  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", 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", rcl_get_error_string_safe());
+    }
+    throw std::runtime_error("Failed to create waitset in Executor constructor");
+  }
+}
+
+Executor::~Executor()
+{
+  // 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());
+  }
+  // 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
+Executor::add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify)
+{
+  // If the node already has an executor
+  if (node_ptr->has_executor.exchange(true)) {
+    throw std::runtime_error("Node has already been added to an executor.");
+  }
+  // Check to ensure node not already added
+  for (auto & weak_node : weak_nodes_) {
+    auto node = weak_node.lock();
+    if (node == node_ptr) {
+      // TODO(jacquelinekay): Use a different error here?
+      throw std::runtime_error("Cannot add node to executor, node already added.");
+    }
+  }
+  weak_nodes_.push_back(node_ptr);
+  if (notify) {
+    // Interrupt waiting to handle new node
+    if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+      throw std::runtime_error(rcl_get_error_string_safe());
+    }
+  }
+  // Add the node's notify condition to the guard condition handles
+  memory_strategy_->add_guard_condition(node_ptr->get_notify_guard_condition());
+}
+
+void
+Executor::remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify)
+{
+  bool node_removed = false;
+  weak_nodes_.erase(
+    std::remove_if(
+      weak_nodes_.begin(), weak_nodes_.end(),
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
+      [&](std::weak_ptr<rclcpp::node::Node> & i)
+      {
+        bool matched = (i.lock() == node_ptr);
+        node_removed |= matched;
+        return matched;
+      }
+      // *INDENT-ON*
+    )
+  );
+  node_ptr->has_executor.store(false);
+  if (notify) {
+    // If the node was matched and removed, interrupt waiting
+    if (node_removed) {
+      if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+        throw std::runtime_error(rcl_get_error_string_safe());
+      }
+    }
+  }
+  memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition());
+}
+
+void
+Executor::spin_node_once_nanoseconds(
+  rclcpp::node::Node::SharedPtr node,
+  std::chrono::nanoseconds timeout)
+{
+  this->add_node(node, false);
+  // non-blocking = true
+  spin_once(timeout);
+  this->remove_node(node, false);
+}
+
+void
+Executor::spin_node_some(rclcpp::node::Node::SharedPtr node)
+{
+  this->add_node(node, false);
+  spin_some();
+  this->remove_node(node, false);
+}
+
+void
+Executor::spin_some()
+{
+  if (spinning.exchange(true)) {
+    throw std::runtime_error("spin_some() called while already spinning");
+  }
+  RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
+  AnyExecutable::SharedPtr any_exec;
+  while ((any_exec = get_next_executable(std::chrono::milliseconds::zero())) && spinning.load()) {
+    execute_any_executable(any_exec);
+  }
+}
+
+void
+Executor::spin_once(std::chrono::nanoseconds timeout)
+{
+  if (spinning.exchange(true)) {
+    throw std::runtime_error("spin_once() called while already spinning");
+  }
+  RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
+  auto any_exec = get_next_executable(timeout);
+  if (any_exec) {
+    execute_any_executable(any_exec);
+  }
+}
+
+void
+Executor::cancel()
+{
+  spinning.store(false);
+  if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+    throw std::runtime_error(rcl_get_error_string_safe());
+  }
+}
+
+void
+Executor::set_memory_strategy(rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy)
+{
+  if (memory_strategy == nullptr) {
+    throw std::runtime_error("Received NULL memory strategy in executor.");
+  }
+  memory_strategy_ = memory_strategy;
+}
+
+void
+Executor::execute_any_executable(AnyExecutable::SharedPtr any_exec)
+{
+  if (!any_exec || !spinning.load()) {
+    return;
+  }
+  if (any_exec->timer) {
+    execute_timer(any_exec->timer);
+  }
+  if (any_exec->subscription) {
+    execute_subscription(any_exec->subscription);
+  }
+  if (any_exec->subscription_intra_process) {
+    execute_intra_process_subscription(any_exec->subscription_intra_process);
+  }
+  if (any_exec->service) {
+    execute_service(any_exec->service);
+  }
+  if (any_exec->client) {
+    execute_client(any_exec->client);
+  }
+  // Reset the callback_group, regardless of type
+  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.
+  if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+    throw std::runtime_error(rcl_get_error_string_safe());
+  }
+}
+
+void
+Executor::execute_subscription(
+  rclcpp::subscription::SubscriptionBase::SharedPtr subscription)
+{
+  std::shared_ptr<void> message = subscription->create_message();
+  rmw_message_info_t message_info;
+
+  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(), rcl_get_error_string_safe());
+  }
+  subscription->return_message(message);
+}
+
+void
+Executor::execute_intra_process_subscription(
+  rclcpp::subscription::SubscriptionBase::SharedPtr subscription)
+{
+  rcl_interfaces::msg::IntraProcessMessage ipm;
+  rmw_message_info_t message_info;
+  rcl_ret_t status = rcl_take(
+    subscription->get_intra_process_subscription_handle(),
+    &ipm,
+    &message_info);
+
+  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(), rcl_get_error_string_safe());
+  }
+}
+
+void
+Executor::execute_timer(
+  rclcpp::timer::TimerBase::SharedPtr timer)
+{
+  timer->execute_callback();
+}
+
+void
+Executor::execute_service(
+  rclcpp::service::ServiceBase::SharedPtr service)
+{
+  auto request_header = service->create_request_header();
+  std::shared_ptr<void> request = service->create_request();
+  rcl_ret_t status = rcl_take_request(
+    service->get_service_handle(),
+    request_header.get(),
+    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(), rcl_get_error_string_safe());
+  }
+}
+
+void
+Executor::execute_client(
+  rclcpp::client::ClientBase::SharedPtr client)
+{
+  auto request_header = client->create_request_header();
+  std::shared_ptr<void> response = client->create_response();
+  rcl_ret_t status = rcl_take_response(
+    client->get_client_handle(),
+    request_header.get(),
+    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(), rcl_get_error_string_safe());
+  }
+}
+
+void
+Executor::wait_for_work(std::chrono::nanoseconds timeout)
+{
+  // 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
+  if (has_invalid_weak_nodes) {
+    weak_nodes_.erase(
+      remove_if(
+        weak_nodes_.begin(), weak_nodes_.end(),
+        // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
+        [](std::weak_ptr<rclcpp::node::Node> i)
+        {
+          return i.expired();
+        }
+        // *INDENT-ON*
+      )
+    );
+  }
+
+  if (rcl_wait_set_resize_subscriptions(
+      &waitset_, memory_strategy_->number_of_ready_subscriptions()) != RCL_RET_OK)
+  {
+    throw std::runtime_error(
+            std::string("Couldn't resize the number of subscriptions in waitset : ") +
+            rcl_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());
+  }
+
+  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
+Executor::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group)
+{
+  if (!group) {
+    return rclcpp::node::Node::SharedPtr();
+  }
+  for (auto & weak_node : weak_nodes_) {
+    auto node = weak_node.lock();
+    if (!node) {
+      continue;
+    }
+    for (auto & weak_group : node->get_callback_groups()) {
+      auto callback_group = weak_group.lock();
+      if (callback_group == group) {
+        return node;
+      }
+    }
+  }
+  return rclcpp::node::Node::SharedPtr();
+}
+
+rclcpp::callback_group::CallbackGroup::SharedPtr
+Executor::get_group_by_timer(rclcpp::timer::TimerBase::SharedPtr timer)
+{
+  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) {
+        continue;
+      }
+      for (auto & weak_timer : group->get_timer_ptrs()) {
+        auto t = weak_timer.lock();
+        if (t == timer) {
+          return group;
+        }
+      }
+    }
+  }
+  return rclcpp::callback_group::CallbackGroup::SharedPtr();
+}
+
+void
+Executor::get_next_timer(AnyExecutable::SharedPtr any_exec)
+{
+  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()) {
+        auto timer = timer_ref.lock();
+        if (timer && timer->is_ready()) {
+          any_exec->timer = timer;
+          any_exec->callback_group = group;
+          node = get_node_by_group(group);
+          return;
+        }
+      }
+    }
+  }
+}
+
+AnyExecutable::SharedPtr
+Executor::get_next_ready_executable()
+{
+  auto any_exec = memory_strategy_->instantiate_next_executable();
+  // Check the timers to see if there are any that are ready, if so return
+  get_next_timer(any_exec);
+  if (any_exec->timer) {
+    return any_exec;
+  }
+  // Check the subscriptions to see if there are any that are ready
+  memory_strategy_->get_next_subscription(any_exec, weak_nodes_);
+  if (any_exec->subscription || any_exec->subscription_intra_process) {
+    return any_exec;
+  }
+  // Check the services to see if there are any that are ready
+  memory_strategy_->get_next_service(any_exec, weak_nodes_);
+  if (any_exec->service) {
+    return any_exec;
+  }
+  // Check the clients to see if there are any that are ready
+  memory_strategy_->get_next_client(any_exec, weak_nodes_);
+  if (any_exec->client) {
+    return any_exec;
+  }
+  // If there is no ready executable, return a null ptr
+  return nullptr;
+}
+
+AnyExecutable::SharedPtr
+Executor::get_next_executable(std::chrono::nanoseconds timeout)
+{
+  // Check to see if there are any subscriptions or timers needing service
+  // TODO(wjwwood): improve run to run efficiency of this function
+  auto any_exec = get_next_ready_executable();
+  // If there are none
+  if (!any_exec) {
+    // Wait for subscriptions or timers to work on
+    wait_for_work(timeout);
+    if (!spinning.load()) {
+      return nullptr;
+    }
+    // Try again
+    any_exec = get_next_ready_executable();
+  }
+  // At this point any_exec should be valid with either a valid subscription
+  // or a valid timer, or it should be a null shared_ptr
+  if (any_exec) {
+    // If it is valid, check to see if the group is mutually exclusive or
+    // not, then mark it accordingly
+    if (any_exec->callback_group && any_exec->callback_group->type() == \
+      callback_group::CallbackGroupType::MutuallyExclusive)
+    {
+      // It should not have been taken otherwise
+      assert(any_exec->callback_group->can_be_taken_from().load());
+      // Set to false to indicate something is being run from this group
+      // This is reset to true either when the any_exec is executed or when the
+      // any_exec is destructued
+      any_exec->callback_group->can_be_taken_from().store(false);
+    }
+  }
+  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 + ")";
+}

rclcpp/src/rclcpp/executors.cpp

@@ -0,0 +1,31 @@
+// Copyright 2015 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/executors.hpp"
+
+void
+rclcpp::spin_some(node::Node::SharedPtr node_ptr)
+{
+  rclcpp::executors::SingleThreadedExecutor exec;
+  exec.spin_node_some(node_ptr);
+}
+
+void
+rclcpp::spin(node::Node::SharedPtr node_ptr)
+{
+  rclcpp::executors::SingleThreadedExecutor exec;
+  exec.add_node(node_ptr);
+  exec.spin();
+  exec.remove_node(node_ptr);
+}

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -0,0 +1,80 @@
+// Copyright 2015 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/executors/multi_threaded_executor.hpp"
+
+#include <chrono>
+#include <functional>
+#include <vector>
+
+#include "rclcpp/utilities.hpp"
+#include "rclcpp/scope_exit.hpp"
+
+using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor;
+
+MultiThreadedExecutor::MultiThreadedExecutor(const rclcpp::executor::ExecutorArgs & args)
+: executor::Executor(args)
+{
+  number_of_threads_ = std::thread::hardware_concurrency();
+  if (number_of_threads_ == 0) {
+    number_of_threads_ = 1;
+  }
+}
+
+MultiThreadedExecutor::~MultiThreadedExecutor() {}
+
+void
+MultiThreadedExecutor::spin()
+{
+  if (spinning.exchange(true)) {
+    throw std::runtime_error("spin() called while already spinning");
+  }
+  RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
+  std::vector<std::thread> threads;
+  size_t thread_id = 0;
+  {
+    std::lock_guard<std::mutex> wait_lock(wait_mutex_);
+    for (; thread_id < number_of_threads_ - 1; ++thread_id) {
+      auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id);
+      threads.emplace_back(func);
+    }
+  }
+
+  run(thread_id);
+  for (auto & thread : threads) {
+    thread.join();
+  }
+}
+
+size_t
+MultiThreadedExecutor::get_number_of_threads()
+{
+  return number_of_threads_;
+}
+
+void
+MultiThreadedExecutor::run(size_t)
+{
+  while (rclcpp::utilities::ok() && spinning.load()) {
+    executor::AnyExecutable::SharedPtr any_exec;
+    {
+      std::lock_guard<std::mutex> wait_lock(wait_mutex_);
+      if (!rclcpp::utilities::ok() || !spinning.load()) {
+        return;
+      }
+      any_exec = get_next_executable();
+    }
+    execute_any_executable(any_exec);
+  }
+}

rclcpp/src/rclcpp/executors/single_threaded_executor.cpp

@@ -0,0 +1,36 @@
+// Copyright 2015 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/executors/single_threaded_executor.hpp"
+#include "rclcpp/scope_exit.hpp"
+
+using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor;
+
+SingleThreadedExecutor::SingleThreadedExecutor(const rclcpp::executor::ExecutorArgs & args)
+: executor::Executor(args) {}
+
+SingleThreadedExecutor::~SingleThreadedExecutor() {}
+
+void
+SingleThreadedExecutor::spin()
+{
+  if (spinning.exchange(true)) {
+    throw std::runtime_error("spin_some() called while already spinning");
+  }
+  RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
+  while (rclcpp::utilities::ok() && spinning.load()) {
+    auto any_exec = get_next_executable();
+    execute_any_executable(any_exec);
+  }
+}

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

rclcpp/src/rclcpp/intra_process_manager.cpp

@@ -0,0 +1,82 @@
+// Copyright 2015 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/intra_process_manager.hpp"
+
+namespace rclcpp
+{
+namespace intra_process_manager
+{
+
+static std::atomic<uint64_t> _next_unique_id {1};
+
+IntraProcessManager::IntraProcessManager(
+  rclcpp::intra_process_manager::IntraProcessManagerImplBase::SharedPtr impl)
+: impl_(impl)
+{}
+
+IntraProcessManager::~IntraProcessManager()
+{}
+
+uint64_t
+IntraProcessManager::add_subscription(
+  rclcpp::subscription::SubscriptionBase::SharedPtr subscription)
+{
+  auto id = IntraProcessManager::get_next_unique_id();
+  impl_->add_subscription(id, subscription);
+  return id;
+}
+
+void
+IntraProcessManager::remove_subscription(uint64_t intra_process_subscription_id)
+{
+  impl_->remove_subscription(intra_process_subscription_id);
+}
+
+void
+IntraProcessManager::remove_publisher(uint64_t intra_process_publisher_id)
+{
+  impl_->remove_publisher(intra_process_publisher_id);
+}
+
+bool
+IntraProcessManager::matches_any_publishers(const rmw_gid_t * id) const
+{
+  return impl_->matches_any_publishers(id);
+}
+
+uint64_t
+IntraProcessManager::get_next_unique_id()
+{
+  auto next_id = _next_unique_id.fetch_add(1, std::memory_order_relaxed);
+  // Check for rollover (we started at 1).
+  if (0 == next_id) {
+    // This puts a technical limit on the number of times you can add a publisher or subscriber.
+    // But even if you could add (and remove) them at 1 kHz (very optimistic rate)
+    // it would still be a very long time before you could exhaust the pool of id's:
+    //   2^64 / 1000 times per sec / 60 sec / 60 min / 24 hours / 365 days = 584,942,417 years
+    // 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 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)");
+    // *INDENT-ON*
+  }
+  return next_id;
+}
+
+}  // namespace intra_process_manager
+}  // namespace rclcpp

rclcpp/src/rclcpp/intra_process_manager_impl.cpp

@@ -0,0 +1,23 @@
+// Copyright 2015 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/intra_process_manager_impl.hpp"
+
+#include <memory>
+
+rclcpp::intra_process_manager::IntraProcessManagerImplBase::SharedPtr
+rclcpp::intra_process_manager::create_default_impl()
+{
+  return std::make_shared<IntraProcessManagerImpl<>>();
+}

rclcpp/src/rclcpp/memory_strategies.cpp

@@ -0,0 +1,25 @@
+// Copyright 2015 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/memory_strategies.hpp"
+
+#include "rclcpp/strategies/allocator_memory_strategy.hpp"
+
+using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy;
+
+rclcpp::memory_strategy::MemoryStrategy::SharedPtr
+rclcpp::memory_strategies::create_default_strategy()
+{
+  return std::make_shared<AllocatorMemoryStrategy<>>();
+}

rclcpp/src/rclcpp/memory_strategy.cpp

@@ -0,0 +1,195 @@
+// Copyright 2015 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/memory_strategy.hpp"
+
+using rclcpp::memory_strategy::MemoryStrategy;
+
+rclcpp::subscription::SubscriptionBase::SharedPtr
+MemoryStrategy::get_subscription_by_handle(
+  const rcl_subscription_t * subscriber_handle, const WeakNodeVector & weak_nodes)
+{
+  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) {
+        continue;
+      }
+      for (auto & weak_subscription : group->get_subscription_ptrs()) {
+        auto subscription = weak_subscription.lock();
+        if (subscription) {
+          if (subscription->get_subscription_handle() == subscriber_handle) {
+            return subscription;
+          }
+          if (subscription->get_intra_process_subscription_handle() == subscriber_handle) {
+            return subscription;
+          }
+        }
+      }
+    }
+  }
+  return nullptr;
+}
+
+rclcpp::service::ServiceBase::SharedPtr
+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();
+    if (!node) {
+      continue;
+    }
+    for (auto & weak_group : node->get_callback_groups()) {
+      auto group = weak_group.lock();
+      if (!group) {
+        continue;
+      }
+      for (auto & service : group->get_service_ptrs()) {
+        if (service->get_service_handle() == service_handle) {
+          return service;
+        }
+      }
+    }
+  }
+  return nullptr;
+}
+
+rclcpp::client::ClientBase::SharedPtr
+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();
+    if (!node) {
+      continue;
+    }
+    for (auto & weak_group : node->get_callback_groups()) {
+      auto group = weak_group.lock();
+      if (!group) {
+        continue;
+      }
+      for (auto & weak_client : group->get_client_ptrs()) {
+        auto client = weak_client.lock();
+        if (client && client->get_client_handle() == client_handle) {
+          return client;
+        }
+      }
+    }
+  }
+  return nullptr;
+}
+
+rclcpp::node::Node::SharedPtr
+MemoryStrategy::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group,
+  const WeakNodeVector & weak_nodes)
+{
+  if (!group) {
+    return nullptr;
+  }
+  for (auto & weak_node : weak_nodes) {
+    auto node = weak_node.lock();
+    if (!node) {
+      continue;
+    }
+    for (auto & weak_group : node->get_callback_groups()) {
+      auto callback_group = weak_group.lock();
+      if (callback_group == group) {
+        return node;
+      }
+    }
+  }
+  return nullptr;
+}
+
+rclcpp::callback_group::CallbackGroup::SharedPtr
+MemoryStrategy::get_group_by_subscription(
+  rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
+  const WeakNodeVector & weak_nodes)
+{
+  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) {
+        continue;
+      }
+      for (auto & weak_sub : group->get_subscription_ptrs()) {
+        auto sub = weak_sub.lock();
+        if (sub == subscription) {
+          return group;
+        }
+      }
+    }
+  }
+  return nullptr;
+}
+
+rclcpp::callback_group::CallbackGroup::SharedPtr
+MemoryStrategy::get_group_by_service(
+  rclcpp::service::ServiceBase::SharedPtr service,
+  const WeakNodeVector & weak_nodes)
+{
+  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) {
+        continue;
+      }
+      for (auto & serv : group->get_service_ptrs()) {
+        if (serv == service) {
+          return group;
+        }
+      }
+    }
+  }
+  return nullptr;
+}
+
+rclcpp::callback_group::CallbackGroup::SharedPtr
+MemoryStrategy::get_group_by_client(
+  rclcpp::client::ClientBase::SharedPtr client,
+  const WeakNodeVector & weak_nodes)
+{
+  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) {
+        continue;
+      }
+      for (auto & weak_client : group->get_client_ptrs()) {
+        auto cli = weak_client.lock();
+        if (cli && cli == client) {
+          return group;
+        }
+      }
+    }
+  }
+  return nullptr;
+}

rclcpp/src/rclcpp/node.cpp

@@ -0,0 +1,511 @@
+// Copyright 2015 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 <algorithm>
+#include <limits>
+#include <map>
+#include <string>
+#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(
+    node_name,
+    rclcpp::contexts::default_context::get_global_default_context(),
+    use_intra_process_comms)
+{}
+
+Node::Node(
+  const std::string & node_name,
+  rclcpp::context::Context::SharedPtr context,
+  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), 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;
+  const char * env_var = "ROS_DOMAIN_ID";
+#ifndef _WIN32
+  ros_domain_id = getenv(env_var);
+#else
+  size_t ros_domain_id_size;
+  _dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var);
+#endif
+  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);
+#ifdef _WIN32
+    free(ros_domain_id);
+#endif
+  }
+
+  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", rcl_get_error_string_safe());
+    }
+    delete node;
+  });
+  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 &
+Node::get_name() const
+{
+  return name_;
+}
+
+rclcpp::callback_group::CallbackGroup::SharedPtr
+Node::create_callback_group(
+  rclcpp::callback_group::CallbackGroupType group_type)
+{
+  using rclcpp::callback_group::CallbackGroup;
+  using rclcpp::callback_group::CallbackGroupType;
+  auto group = CallbackGroup::SharedPtr(new CallbackGroup(group_type));
+  callback_groups_.push_back(group);
+  return group;
+}
+
+bool
+Node::group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group)
+{
+  bool group_belongs_to_this_node = false;
+  for (auto & weak_group : this->callback_groups_) {
+    auto cur_group = weak_group.lock();
+    if (cur_group && (cur_group == group)) {
+      group_belongs_to_this_node = true;
+    }
+  }
+  return group_belongs_to_this_node;
+}
+
+// TODO(wjwwood): reenable this once I figure out why the demo doesn't build with it.
+// rclcpp::timer::WallTimer::SharedPtr
+// Node::create_wall_timer(
+//   std::chrono::duration<long double, std::nano> period,
+//   rclcpp::timer::CallbackType callback,
+//   rclcpp::callback_group::CallbackGroup::SharedPtr group)
+// {
+//   return create_wall_timer(
+//     std::chrono::duration_cast<std::chrono::nanoseconds>(period),
+//     callback,
+//     group);
+// }
+
+std::vector<rcl_interfaces::msg::SetParametersResult>
+Node::set_parameters(
+  const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
+{
+  std::vector<rcl_interfaces::msg::SetParametersResult> results;
+  for (auto p : parameters) {
+    auto result = set_parameters_atomically({{p}});
+    results.push_back(result);
+  }
+  return results;
+}
+
+rcl_interfaces::msg::SetParametersResult
+Node::set_parameters_atomically(
+  const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  std::map<std::string, rclcpp::parameter::ParameterVariant> tmp_map;
+  auto parameter_event = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
+
+  for (auto p : parameters) {
+    if (parameters_.find(p.get_name()) == parameters_.end()) {
+      if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
+        parameter_event->new_parameters.push_back(p.to_parameter());
+      }
+    } else if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
+      parameter_event->changed_parameters.push_back(p.to_parameter());
+    } else {
+      parameter_event->deleted_parameters.push_back(p.to_parameter());
+    }
+    tmp_map[p.get_name()] = p;
+  }
+  // std::map::insert will not overwrite elements, so we'll keep the new
+  // ones and add only those that already exist in the Node's internal map
+  tmp_map.insert(parameters_.begin(), parameters_.end());
+  std::swap(tmp_map, parameters_);
+
+  // TODO(jacquelinekay): handle parameter constraints
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+
+  events_publisher_->publish(parameter_event);
+
+  return result;
+}
+
+std::vector<rclcpp::parameter::ParameterVariant>
+Node::get_parameters(
+  const std::vector<std::string> & names) const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  std::vector<rclcpp::parameter::ParameterVariant> results;
+
+  for (auto & name : names) {
+    if (std::any_of(parameters_.cbegin(), parameters_.cend(),
+      [&name](const std::pair<std::string, rclcpp::parameter::ParameterVariant> & kv) {
+      return name == kv.first;
+    }))
+    {
+      results.push_back(parameters_.at(name));
+    }
+  }
+  return results;
+}
+
+std::vector<rcl_interfaces::msg::ParameterDescriptor>
+Node::describe_parameters(
+  const std::vector<std::string> & names) const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  std::vector<rcl_interfaces::msg::ParameterDescriptor> results;
+  for (auto & kv : parameters_) {
+    if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
+      return name == kv.first;
+    }))
+    {
+      rcl_interfaces::msg::ParameterDescriptor parameter_descriptor;
+      parameter_descriptor.name = kv.first;
+      parameter_descriptor.type = kv.second.get_type();
+      results.push_back(parameter_descriptor);
+    }
+  }
+  return results;
+}
+
+std::vector<uint8_t>
+Node::get_parameter_types(
+  const std::vector<std::string> & names) const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  std::vector<uint8_t> results;
+  for (auto & kv : parameters_) {
+    if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
+      return name == kv.first;
+    }))
+    {
+      results.push_back(kv.second.get_type());
+    } else {
+      results.push_back(rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET);
+    }
+  }
+  return results;
+}
+
+rcl_interfaces::msg::ListParametersResult
+Node::list_parameters(
+  const std::vector<std::string> & prefixes, uint64_t depth) const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  rcl_interfaces::msg::ListParametersResult result;
+
+  // TODO(esteve): define parameter separator, use "." for now
+  for (auto & kv : parameters_) {
+    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 (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('.');
+      if (std::string::npos != last_separator) {
+        std::string prefix = kv.first.substr(0, last_separator);
+        if (std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) ==
+          result.prefixes.cend())
+        {
+          result.prefixes.push_back(prefix);
+        }
+      }
+    }
+  }
+  return result;
+}
+
+std::map<std::string, std::string>
+Node::get_topic_names_and_types() const
+{
+  rmw_topic_names_and_types_t topic_names_and_types;
+  topic_names_and_types.topic_count = 0;
+  topic_names_and_types.topic_names = nullptr;
+  topic_names_and_types.type_names = nullptr;
+
+  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(
+      std::string("could not get topic names and types: ") + rmw_get_error_string_safe());
+    // *INDENT-ON*
+  }
+
+  std::map<std::string, std::string> topics;
+  for (size_t i = 0; i < topic_names_and_types.topic_count; ++i) {
+    topics[topic_names_and_types.topic_names[i]] = topic_names_and_types.type_names[i];
+  }
+
+  ret = rmw_destroy_topic_names_and_types(&topic_names_and_types);
+  if (ret != RMW_RET_OK) {
+    // *INDENT-OFF*
+    throw std::runtime_error(
+      std::string("could not destroy topic names and types: ") + rmw_get_error_string_safe());
+    // *INDENT-ON*
+  }
+
+  return topics;
+}
+
+size_t
+Node::count_publishers(const std::string & topic_name) const
+{
+  size_t 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(
+      std::string("could not count publishers: ") + rmw_get_error_string_safe());
+    // *INDENT-ON*
+  }
+  return count;
+}
+
+size_t
+Node::count_subscribers(const std::string & topic_name) const
+{
+  size_t 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(
+      std::string("could not count subscribers: ") + rmw_get_error_string_safe());
+    // *INDENT-ON*
+  }
+  return count;
+}
+
+
+const Node::CallbackGroupWeakPtrList &
+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();
+}

rclcpp/src/rclcpp/parameter.cpp

@@ -0,0 +1,279 @@
+// Copyright 2015 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 <ostream>
+#include <sstream>
+#include <string>
+#include <vector>
+
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/utilities.hpp"
+
+using rclcpp::parameter::ParameterType;
+using rclcpp::parameter::ParameterVariant;
+
+ParameterVariant::ParameterVariant()
+: name_("")
+{
+  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET;
+}
+
+ParameterVariant::ParameterVariant(const std::string & name, const bool bool_value)
+: name_(name)
+{
+  value_.bool_value = bool_value;
+  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
+}
+
+ParameterVariant::ParameterVariant(const std::string & name, const int int_value)
+: name_(name)
+{
+  value_.integer_value = int_value;
+  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
+}
+
+ParameterVariant::ParameterVariant(const std::string & name, const int64_t int_value)
+: name_(name)
+{
+  value_.integer_value = int_value;
+  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
+}
+
+ParameterVariant::ParameterVariant(const std::string & name, const float double_value)
+: name_(name)
+{
+  value_.double_value = double_value;
+  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
+}
+
+ParameterVariant::ParameterVariant(const std::string & name, const double double_value)
+: name_(name)
+{
+  value_.double_value = double_value;
+  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
+}
+
+ParameterVariant::ParameterVariant(const std::string & name, const std::string & string_value)
+: name_(name)
+{
+  value_.string_value = string_value;
+  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
+}
+
+ParameterVariant::ParameterVariant(const std::string & name, const char * string_value)
+: ParameterVariant(name, std::string(string_value))
+{}
+
+ParameterVariant::ParameterVariant(
+  const std::string & name, const std::vector<uint8_t> & bytes_value)
+: name_(name)
+{
+  value_.bytes_value = bytes_value;
+  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BYTES;
+}
+
+ParameterType
+ParameterVariant::get_type() const
+{
+  return static_cast<ParameterType>(value_.type);
+}
+
+std::string
+ParameterVariant::get_type_name() const
+{
+  switch (get_type()) {
+    case rclcpp::parameter::ParameterType::PARAMETER_BOOL:
+      return "bool";
+    case rclcpp::parameter::ParameterType::PARAMETER_INTEGER:
+      return "integer";
+    case rclcpp::parameter::ParameterType::PARAMETER_DOUBLE:
+      return "double";
+    case rclcpp::parameter::ParameterType::PARAMETER_STRING:
+      return "string";
+    case rclcpp::parameter::ParameterType::PARAMETER_BYTES:
+      return "bytes";
+    case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET:
+      return "not set";
+    default:
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
+      throw std::runtime_error(
+        "Unexpected type from ParameterVariant: " + std::to_string(get_type()));
+      // *INDENT-ON*
+  }
+}
+
+const std::string &
+ParameterVariant::get_name() const
+{
+  return name_;
+}
+
+rcl_interfaces::msg::ParameterValue
+ParameterVariant::get_parameter_value() const
+{
+  return value_;
+}
+
+int64_t
+ParameterVariant::as_int() const
+{
+  return get_value<ParameterType::PARAMETER_INTEGER>();
+}
+
+double
+ParameterVariant::as_double() const
+{
+  return get_value<ParameterType::PARAMETER_DOUBLE>();
+}
+
+const std::string &
+ParameterVariant::as_string() const
+{
+  return get_value<ParameterType::PARAMETER_STRING>();
+}
+
+bool
+ParameterVariant::as_bool() const
+{
+  return get_value<ParameterType::PARAMETER_BOOL>();
+}
+
+const std::vector<uint8_t> &
+ParameterVariant::as_bytes() const
+{
+  return get_value<ParameterType::PARAMETER_BYTES>();
+}
+
+ParameterVariant
+ParameterVariant::from_parameter(const rcl_interfaces::msg::Parameter & parameter)
+{
+  switch (parameter.value.type) {
+    case PARAMETER_BOOL:
+      return ParameterVariant(parameter.name, parameter.value.bool_value);
+    case PARAMETER_INTEGER:
+      return ParameterVariant(parameter.name, parameter.value.integer_value);
+    case PARAMETER_DOUBLE:
+      return ParameterVariant(parameter.name, parameter.value.double_value);
+    case PARAMETER_STRING:
+      return ParameterVariant(parameter.name, parameter.value.string_value);
+    case PARAMETER_BYTES:
+      return ParameterVariant(parameter.name, parameter.value.bytes_value);
+    case PARAMETER_NOT_SET:
+      throw std::runtime_error("Type from ParameterValue is not set");
+    default:
+      // TODO(wjwwood): use custom exception
+      // *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*
+  }
+}
+
+rcl_interfaces::msg::Parameter
+ParameterVariant::to_parameter()
+{
+  rcl_interfaces::msg::Parameter parameter;
+  parameter.name = name_;
+  parameter.value = value_;
+  return parameter;
+}
+
+std::string
+ParameterVariant::value_to_string() const
+{
+  switch (get_type()) {
+    case rclcpp::parameter::ParameterType::PARAMETER_BOOL:
+      return as_bool() ? "true" : "false";
+    case rclcpp::parameter::ParameterType::PARAMETER_INTEGER:
+      return std::to_string(as_int());
+    case rclcpp::parameter::ParameterType::PARAMETER_DOUBLE:
+      return std::to_string(as_double());
+    case rclcpp::parameter::ParameterType::PARAMETER_STRING:
+      return as_string();
+    case rclcpp::parameter::ParameterType::PARAMETER_BYTES:
+      {
+        std::stringstream bytes;
+        bool first_byte = true;
+        bytes << "[" << std::hex;
+        for (auto & byte : as_bytes()) {
+          bytes << "0x" << byte;
+          if (!first_byte) {
+            bytes << ", ";
+          } else {
+            first_byte = false;
+          }
+        }
+        return bytes.str();
+      }
+    case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET:
+      return "not set";
+    default:
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
+      throw std::runtime_error(
+        "Unexpected type from ParameterVariant: " + std::to_string(get_type()));
+      // *INDENT-ON*
+  }
+}
+
+std::string
+rclcpp::parameter::_to_json_dict_entry(const ParameterVariant & param)
+{
+  std::stringstream ss;
+  ss << "\"" << param.get_name() << "\": ";
+  ss << "{\"type\": \"" << param.get_type_name() << "\", ";
+  ss << "\"value\": \"" << param.value_to_string() << "\"}";
+  return ss.str();
+}
+
+std::ostream &
+rclcpp::parameter::operator<<(std::ostream & os, const rclcpp::parameter::ParameterVariant & pv)
+{
+  os << std::to_string(pv);
+  return os;
+}
+
+std::ostream &
+rclcpp::parameter::operator<<(std::ostream & os, const std::vector<ParameterVariant> & parameters)
+{
+  os << std::to_string(parameters);
+  return os;
+}
+
+std::string
+std::to_string(const rclcpp::parameter::ParameterVariant & param)
+{
+  std::stringstream ss;
+  ss << "{\"name\": \"" << param.get_name() << "\", ";
+  ss << "\"type\": \"" << param.get_type_name() << "\", ";
+  ss << "\"value\": \"" << param.value_to_string() << "\"}";
+  return ss.str();
+}
+
+std::string
+std::to_string(const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
+{
+  std::stringstream ss;
+  ss << "{";
+  bool first = true;
+  for (const auto & pv : parameters) {
+    if (first == false) {
+      ss << ", ";
+    } else {
+      first = false;
+    }
+    ss << rclcpp::parameter::_to_json_dict_entry(pv);
+  }
+  ss << "}";
+  return ss.str();
+}

rclcpp/src/rclcpp/parameter_client.cpp

@@ -0,0 +1,327 @@
+// Copyright 2015 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/parameter_client.hpp"
+
+#include <algorithm>
+#include <string>
+#include <vector>
+
+using rclcpp::parameter_client::AsyncParametersClient;
+using rclcpp::parameter_client::SyncParametersClient;
+
+AsyncParametersClient::AsyncParametersClient(
+  const rclcpp::node::Node::SharedPtr node,
+  const std::string & remote_node_name,
+  const rmw_qos_profile_t & qos_profile)
+: node_(node)
+{
+  if (remote_node_name != "") {
+    remote_node_name_ = remote_node_name;
+  } else {
+    remote_node_name_ = node_->get_name();
+  }
+  get_parameters_client_ = node_->create_client<rcl_interfaces::srv::GetParameters>(
+    remote_node_name_ + "__get_parameters", qos_profile);
+  get_parameter_types_client_ = node_->create_client<rcl_interfaces::srv::GetParameterTypes>(
+    remote_node_name_ + "__get_parameter_types", qos_profile);
+  set_parameters_client_ = node_->create_client<rcl_interfaces::srv::SetParameters>(
+    remote_node_name_ + "__set_parameters", qos_profile);
+  list_parameters_client_ = node_->create_client<rcl_interfaces::srv::ListParameters>(
+    remote_node_name_ + "__list_parameters", qos_profile);
+  describe_parameters_client_ = node_->create_client<rcl_interfaces::srv::DescribeParameters>(
+    remote_node_name_ + "__describe_parameters", qos_profile);
+}
+
+std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>
+AsyncParametersClient::get_parameters(
+  const std::vector<std::string> & names,
+  std::function<
+    void(std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>)
+  > callback)
+{
+  auto promise_result =
+    std::make_shared<std::promise<std::vector<rclcpp::parameter::ParameterVariant>>>();
+  auto future_result = promise_result->get_future().share();
+
+  auto request = std::make_shared<rcl_interfaces::srv::GetParameters::Request>();
+  request->names = names;
+
+  // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
+  get_parameters_client_->async_send_request(
+    request,
+    [request, promise_result, future_result, &callback](
+      rclcpp::client::Client<rcl_interfaces::srv::GetParameters>::SharedFuture cb_f)
+    {
+      std::vector<rclcpp::parameter::ParameterVariant> parameter_variants;
+      auto & pvalues = cb_f.get()->values;
+
+      for (auto & pvalue : pvalues) {
+        auto i = &pvalue - &pvalues[0];
+        rcl_interfaces::msg::Parameter parameter;
+        parameter.name = request->names[i];
+        parameter.value = pvalue;
+        parameter_variants.push_back(rclcpp::parameter::ParameterVariant::from_parameter(
+          parameter));
+      }
+
+      promise_result->set_value(parameter_variants);
+      if (callback != nullptr) {
+        callback(future_result);
+      }
+    }
+  );
+  // *INDENT-ON*
+
+  return future_result;
+}
+
+std::shared_future<std::vector<rclcpp::parameter::ParameterType>>
+AsyncParametersClient::get_parameter_types(
+  const std::vector<std::string> & names,
+  std::function<
+    void(std::shared_future<std::vector<rclcpp::parameter::ParameterType>>)
+  > callback)
+{
+  auto promise_result =
+    std::make_shared<std::promise<std::vector<rclcpp::parameter::ParameterType>>>();
+  auto future_result = promise_result->get_future().share();
+
+  auto request = std::make_shared<rcl_interfaces::srv::GetParameterTypes::Request>();
+  request->names = names;
+
+  // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
+  get_parameter_types_client_->async_send_request(
+    request,
+    [promise_result, future_result, &callback](
+      rclcpp::client::Client<rcl_interfaces::srv::GetParameterTypes>::SharedFuture cb_f)
+    {
+      std::vector<rclcpp::parameter::ParameterType> types;
+      auto & pts = cb_f.get()->types;
+      for (auto & pt : pts) {
+        pts.push_back(static_cast<rclcpp::parameter::ParameterType>(pt));
+      }
+      promise_result->set_value(types);
+      if (callback != nullptr) {
+        callback(future_result);
+      }
+    }
+  );
+  // *INDENT-ON*
+
+  return future_result;
+}
+
+std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>
+AsyncParametersClient::set_parameters(
+  const std::vector<rclcpp::parameter::ParameterVariant> & parameters,
+  std::function<
+    void(std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>)
+  > callback)
+{
+  auto promise_result =
+    std::make_shared<std::promise<std::vector<rcl_interfaces::msg::SetParametersResult>>>();
+  auto future_result = promise_result->get_future().share();
+
+  auto request = std::make_shared<rcl_interfaces::srv::SetParameters::Request>();
+
+  // *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();
+    }
+  );
+
+  set_parameters_client_->async_send_request(
+    request,
+    [promise_result, future_result, &callback](
+      rclcpp::client::Client<rcl_interfaces::srv::SetParameters>::SharedFuture cb_f)
+    {
+      promise_result->set_value(cb_f.get()->results);
+      if (callback != nullptr) {
+        callback(future_result);
+      }
+    }
+  );
+  // *INDENT-ON*
+
+  return future_result;
+}
+
+std::shared_future<rcl_interfaces::msg::SetParametersResult>
+AsyncParametersClient::set_parameters_atomically(
+  const std::vector<rclcpp::parameter::ParameterVariant> & parameters,
+  std::function<
+    void(std::shared_future<rcl_interfaces::msg::SetParametersResult>)
+  > callback)
+{
+  auto promise_result =
+    std::make_shared<std::promise<rcl_interfaces::msg::SetParametersResult>>();
+  auto future_result = promise_result->get_future().share();
+
+  auto request = std::make_shared<rcl_interfaces::srv::SetParametersAtomically::Request>();
+
+  // *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();
+    }
+  );
+
+  set_parameters_atomically_client_->async_send_request(
+    request,
+    [promise_result, future_result, &callback](
+      rclcpp::client::Client<rcl_interfaces::srv::SetParametersAtomically>::SharedFuture cb_f)
+    {
+      promise_result->set_value(cb_f.get()->result);
+      if (callback != nullptr) {
+        callback(future_result);
+      }
+    }
+  );
+  // *INDENT-ON*
+
+  return future_result;
+}
+
+std::shared_future<rcl_interfaces::msg::ListParametersResult>
+AsyncParametersClient::list_parameters(
+  const std::vector<std::string> & prefixes,
+  uint64_t depth,
+  std::function<
+    void(std::shared_future<rcl_interfaces::msg::ListParametersResult>)
+  > callback)
+{
+  auto promise_result =
+    std::make_shared<std::promise<rcl_interfaces::msg::ListParametersResult>>();
+  auto future_result = promise_result->get_future().share();
+
+  auto request = std::make_shared<rcl_interfaces::srv::ListParameters::Request>();
+  request->prefixes = prefixes;
+  request->depth = depth;
+
+  // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
+  list_parameters_client_->async_send_request(
+    request,
+    [promise_result, future_result, &callback](
+      rclcpp::client::Client<rcl_interfaces::srv::ListParameters>::SharedFuture cb_f)
+    {
+      promise_result->set_value(cb_f.get()->result);
+      if (callback != nullptr) {
+        callback(future_result);
+      }
+    }
+  );
+  // *INDENT-ON*
+
+  return future_result;
+}
+
+SyncParametersClient::SyncParametersClient(
+  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, "", qos_profile);
+}
+
+SyncParametersClient::SyncParametersClient(
+  rclcpp::executor::Executor::SharedPtr executor,
+  rclcpp::node::Node::SharedPtr node,
+  const rmw_qos_profile_t & qos_profile)
+: executor_(executor), node_(node)
+{
+  async_parameters_client_ = std::make_shared<AsyncParametersClient>(node, "", qos_profile);
+}
+
+std::vector<rclcpp::parameter::ParameterVariant>
+SyncParametersClient::get_parameters(const std::vector<std::string> & parameter_names)
+{
+  auto f = async_parameters_client_->get_parameters(parameter_names);
+  if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
+    rclcpp::executor::FutureReturnCode::SUCCESS)
+  {
+    return f.get();
+  }
+  // Return an empty vector if unsuccessful
+  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)
+{
+  auto f = async_parameters_client_->get_parameter_types(parameter_names);
+
+  if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
+    rclcpp::executor::FutureReturnCode::SUCCESS)
+  {
+    return f.get();
+  }
+  return std::vector<rclcpp::parameter::ParameterType>();
+}
+
+std::vector<rcl_interfaces::msg::SetParametersResult>
+SyncParametersClient::set_parameters(
+  const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
+{
+  auto f = async_parameters_client_->set_parameters(parameters);
+
+  if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
+    rclcpp::executor::FutureReturnCode::SUCCESS)
+  {
+    return f.get();
+  }
+  return std::vector<rcl_interfaces::msg::SetParametersResult>();
+}
+
+rcl_interfaces::msg::SetParametersResult
+SyncParametersClient::set_parameters_atomically(
+  const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
+{
+  auto f = async_parameters_client_->set_parameters_atomically(parameters);
+
+  if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
+    rclcpp::executor::FutureReturnCode::SUCCESS)
+  {
+    return f.get();
+  }
+
+  throw std::runtime_error("Unable to get result of set parameters service call.");
+}
+
+rcl_interfaces::msg::ListParametersResult
+SyncParametersClient::list_parameters(
+  const std::vector<std::string> & parameter_prefixes,
+  uint64_t depth)
+{
+  auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth);
+
+  if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
+    rclcpp::executor::FutureReturnCode::SUCCESS)
+  {
+    return f.get();
+  }
+
+  throw std::runtime_error("Unable to get result of list parameters service call.");
+}

rclcpp/src/rclcpp/parameter_service.cpp

@@ -0,0 +1,148 @@
+// Copyright 2015 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/parameter_service.hpp"
+
+#include <algorithm>
+#include <vector>
+
+using rclcpp::parameter_service::ParameterService;
+
+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 unnecessary indents here)
+  get_parameters_service_ = node_->create_service<rcl_interfaces::srv::GetParameters>(
+    node_->get_name() + "__get_parameters",
+    [captured_node](
+      const std::shared_ptr<rmw_request_id_t>,
+      const std::shared_ptr<rcl_interfaces::srv::GetParameters::Request> request,
+      std::shared_ptr<rcl_interfaces::srv::GetParameters::Response> response)
+    {
+      auto node = captured_node.lock();
+      if (!node) {
+        return;
+      }
+      auto values = node->get_parameters(request->names);
+      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>(
+    node_->get_name() + "__get_parameter_types",
+    [captured_node](
+      const std::shared_ptr<rmw_request_id_t>,
+      const std::shared_ptr<rcl_interfaces::srv::GetParameterTypes::Request> request,
+      std::shared_ptr<rcl_interfaces::srv::GetParameterTypes::Response> response)
+    {
+      auto node = captured_node.lock();
+      if (!node) {
+        return;
+      }
+      auto types = node->get_parameter_types(request->names);
+      std::transform(types.cbegin(), types.cend(),
+      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>(
+    node_->get_name() + "__set_parameters",
+    [captured_node](
+      const std::shared_ptr<rmw_request_id_t>,
+      const std::shared_ptr<rcl_interfaces::srv::SetParameters::Request> request,
+      std::shared_ptr<rcl_interfaces::srv::SetParameters::Response> response)
+    {
+      auto node = captured_node.lock();
+      if (!node) {
+        return;
+      }
+      std::vector<rclcpp::parameter::ParameterVariant> pvariants;
+      for (auto & p : request->parameters) {
+        pvariants.push_back(rclcpp::parameter::ParameterVariant::from_parameter(p));
+      }
+      auto results = node->set_parameters(pvariants);
+      response->results = results;
+    },
+    qos_profile
+  );
+
+  set_parameters_atomically_service_ =
+    node_->create_service<rcl_interfaces::srv::SetParametersAtomically>(
+    node_->get_name() + "__set_parameters_atomically",
+    [captured_node](
+      const std::shared_ptr<rmw_request_id_t>,
+      const std::shared_ptr<rcl_interfaces::srv::SetParametersAtomically::Request> request,
+      std::shared_ptr<rcl_interfaces::srv::SetParametersAtomically::Response> response)
+    {
+      auto node = captured_node.lock();
+      if (!node) {
+        return;
+      }
+      std::vector<rclcpp::parameter::ParameterVariant> pvariants;
+      std::transform(request->parameters.cbegin(), request->parameters.cend(),
+      std::back_inserter(pvariants),
+      [](const rcl_interfaces::msg::Parameter & p) {
+        return rclcpp::parameter::ParameterVariant::
+        from_parameter(p);
+      });
+      auto result = node->set_parameters_atomically(pvariants);
+      response->result = result;
+    },
+    qos_profile
+  );
+
+  describe_parameters_service_ = node_->create_service<rcl_interfaces::srv::DescribeParameters>(
+    node_->get_name() + "__describe_parameters",
+    [captured_node](
+      const std::shared_ptr<rmw_request_id_t>,
+      const std::shared_ptr<rcl_interfaces::srv::DescribeParameters::Request> request,
+      std::shared_ptr<rcl_interfaces::srv::DescribeParameters::Response> response)
+    {
+      auto node = captured_node.lock();
+      if (!node) {
+        return;
+      }
+      auto descriptors = node->describe_parameters(request->names);
+      response->descriptors = descriptors;
+    },
+    qos_profile
+  );
+
+  list_parameters_service_ = node_->create_service<rcl_interfaces::srv::ListParameters>(
+    node_->get_name() + "__list_parameters",
+    [captured_node](
+      const std::shared_ptr<rmw_request_id_t>,
+      const std::shared_ptr<rcl_interfaces::srv::ListParameters::Request> request,
+      std::shared_ptr<rcl_interfaces::srv::ListParameters::Response> response)
+    {
+      auto node = captured_node.lock();
+      if (!node) {
+        return;
+      }
+      auto result = node->list_parameters(request->prefixes, request->depth);
+      response->result = result;
+    },
+    qos_profile
+  );
+  // *INDENT-ON*
+}

rclcpp/src/rclcpp/publisher.cpp

@@ -0,0 +1,132 @@
+// Copyright 2015 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/publisher.hpp"
+
+#include <rmw/error_handling.h>
+#include <rmw/rmw.h>
+
+#include <iostream>
+#include <memory>
+#include <mutex>
+#include <sstream>
+#include <string>
+
+#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"
+
+using rclcpp::publisher::PublisherBase;
+
+PublisherBase::PublisherBase(
+  std::shared_ptr<rcl_node_t> node_handle,
+  std::string topic,
+  size_t queue_size)
+: node_handle_(node_handle),
+  topic_(topic), queue_size_(queue_size),
+  intra_process_publisher_id_(0), store_intra_process_message_(nullptr)
+{
+}
+
+PublisherBase::~PublisherBase()
+{
+}
+
+const std::string &
+PublisherBase::get_topic_name() const
+{
+  return topic_;
+}
+
+size_t
+PublisherBase::get_queue_size() const
+{
+  return queue_size_;
+}
+
+const rmw_gid_t &
+PublisherBase::get_gid() const
+{
+  return rmw_gid_;
+}
+
+const rmw_gid_t &
+PublisherBase::get_intra_process_gid() const
+{
+  return intra_process_rmw_gid_;
+}
+
+bool
+PublisherBase::operator==(const rmw_gid_t & gid) const
+{
+  return *this == &gid;
+}
+
+bool
+PublisherBase::operator==(const rmw_gid_t * gid) const
+{
+  bool result = false;
+  auto ret = rmw_compare_gids_equal(gid, &this->get_gid(), &result);
+  if (ret != RMW_RET_OK) {
+    throw std::runtime_error(
+            std::string("failed to compare gids: ") + rmw_get_error_string_safe());
+  }
+  if (!result) {
+    ret = rmw_compare_gids_equal(gid, &this->get_intra_process_gid(), &result);
+    if (ret != RMW_RET_OK) {
+      throw std::runtime_error(
+              std::string("failed to compare gids: ") + rmw_get_error_string_safe());
+    }
+  }
+  return result;
+}
+
+void
+PublisherBase::setup_intra_process(
+  uint64_t intra_process_publisher_id,
+  StoreMessageCallbackT callback,
+  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;
+  // Life time of this object is tied to the publisher handle.
+  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 unnecessary indents here)
+    throw std::runtime_error(
+      std::string("failed to create intra process publisher gid: ") +
+      rmw_get_error_string_safe());
+    // *INDENT-ON*
+  }
+}

rclcpp/src/rclcpp/service.cpp

@@ -0,0 +1,49 @@
+// Copyright 2015 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/service.hpp"
+
+#include <functional>
+#include <iostream>
+#include <memory>
+#include <sstream>
+#include <string>
+
+#include "rclcpp/any_service_callback.hpp"
+#include "rclcpp/macros.hpp"
+#include "rmw/error_handling.h"
+#include "rmw/rmw.h"
+
+using rclcpp::service::ServiceBase;
+
+ServiceBase::ServiceBase(
+  std::shared_ptr<rcl_node_t> node_handle,
+  const std::string service_name)
+: node_handle_(node_handle), service_name_(service_name)
+{}
+
+ServiceBase::~ServiceBase()
+{}
+
+std::string
+ServiceBase::get_service_name()
+{
+  return this->service_name_;
+}
+
+const rcl_service_t *
+ServiceBase::get_service_handle()
+{
+  return &service_handle_;
+}

rclcpp/src/rclcpp/subscription.cpp

@@ -0,0 +1,72 @@
+// Copyright 2015 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/subscription.hpp"
+
+#include <cstdio>
+#include <string>
+
+#include "rmw/error_handling.h"
+#include "rmw/rmw.h"
+
+
+using rclcpp::subscription::SubscriptionBase;
+
+SubscriptionBase::SubscriptionBase(
+  std::shared_ptr<rcl_node_t> node_handle,
+  const std::string & topic_name,
+  bool ignore_local_publications)
+:   node_handle_(node_handle),
+  topic_name_(topic_name),
+  ignore_local_publications_(ignore_local_publications)
+{
+  // To avoid unused private member warnings.
+  (void)ignore_local_publications_;
+}
+
+SubscriptionBase::~SubscriptionBase()
+{
+  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 (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();
+  }
+}
+
+const std::string &
+SubscriptionBase::get_topic_name() const
+{
+  return this->topic_name_;
+}
+
+const rcl_subscription_t *
+SubscriptionBase::get_subscription_handle() const
+{
+  return &subscription_handle_;
+}
+
+const rcl_subscription_t *
+SubscriptionBase::get_intra_process_subscription_handle() const
+{
+  return &intra_process_subscription_handle_;
+}

rclcpp/src/rclcpp/timer.cpp

@@ -0,0 +1,69 @@
+// Copyright 2015 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/timer.hpp"
+
+#include <chrono>
+#include <string>
+
+using rclcpp::timer::TimerBase;
+
+TimerBase::TimerBase(std::chrono::nanoseconds period)
+{
+  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()
+{}
+
+void
+TimerBase::cancel()
+{
+  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_;
+}

rclcpp/src/rclcpp/type_support.cpp

@@ -0,0 +1,116 @@
+// Copyright 2014 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcl_interfaces/msg/intra_process_message.hpp"
+#include "rcl_interfaces/msg/list_parameters_result.hpp"
+#include "rcl_interfaces/msg/parameter_descriptor.hpp"
+#include "rcl_interfaces/msg/parameter_event.hpp"
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
+#include "rcl_interfaces/srv/describe_parameters.hpp"
+#include "rcl_interfaces/srv/get_parameter_types.hpp"
+#include "rcl_interfaces/srv/get_parameters.hpp"
+#include "rcl_interfaces/srv/list_parameters.hpp"
+#include "rcl_interfaces/srv/set_parameters.hpp"
+#include "rcl_interfaces/srv/set_parameters_atomically.hpp"
+
+const rosidl_message_type_support_t *
+rclcpp::type_support::get_intra_process_message_msg_type_support()
+{
+  return rosidl_generator_cpp::get_message_type_support_handle<
+    rcl_interfaces::msg::IntraProcessMessage
+  >();
+}
+
+const rosidl_message_type_support_t *
+rclcpp::type_support::get_parameter_event_msg_type_support()
+{
+  return rosidl_generator_cpp::get_message_type_support_handle<
+    rcl_interfaces::msg::ParameterEvent
+  >();
+}
+
+const rosidl_message_type_support_t *
+rclcpp::type_support::get_set_parameters_result_msg_type_support()
+{
+  return rosidl_generator_cpp::get_message_type_support_handle<
+    rcl_interfaces::msg::SetParametersResult
+  >();
+}
+
+const rosidl_message_type_support_t *
+rclcpp::type_support::get_parameter_descriptor_msg_type_support()
+{
+  return rosidl_generator_cpp::get_message_type_support_handle<
+    rcl_interfaces::msg::ParameterDescriptor
+  >();
+}
+
+const rosidl_message_type_support_t *
+rclcpp::type_support::get_list_parameters_result_msg_type_support()
+{
+  return rosidl_generator_cpp::get_message_type_support_handle<
+    rcl_interfaces::msg::ListParametersResult
+  >();
+}
+
+const rosidl_service_type_support_t *
+rclcpp::type_support::get_get_parameters_srv_type_support()
+{
+  return rosidl_generator_cpp::get_service_type_support_handle<
+    rcl_interfaces::srv::GetParameters
+  >();
+}
+
+const rosidl_service_type_support_t *
+rclcpp::type_support::get_get_parameter_types_srv_type_support()
+{
+  return rosidl_generator_cpp::get_service_type_support_handle<
+    rcl_interfaces::srv::GetParameterTypes
+  >();
+}
+
+const rosidl_service_type_support_t *
+rclcpp::type_support::get_set_parameters_srv_type_support()
+{
+  return rosidl_generator_cpp::get_service_type_support_handle<
+    rcl_interfaces::srv::SetParameters
+  >();
+}
+
+const rosidl_service_type_support_t *
+rclcpp::type_support::get_list_parameters_srv_type_support()
+{
+  return rosidl_generator_cpp::get_service_type_support_handle<
+    rcl_interfaces::srv::ListParameters
+  >();
+}
+
+const rosidl_service_type_support_t *
+rclcpp::type_support::get_describe_parameters_srv_type_support()
+{
+  return rosidl_generator_cpp::get_service_type_support_handle<
+    rcl_interfaces::srv::DescribeParameters
+  >();
+}
+
+const rosidl_service_type_support_t *
+rclcpp::type_support::get_set_parameters_atomically_srv_type_support()
+{
+  return rosidl_generator_cpp::get_service_type_support_handle<
+    rcl_interfaces::srv::SetParametersAtomically
+  >();
+}

rclcpp/src/rclcpp/utilities.cpp

@@ -0,0 +1,251 @@
+// Copyright 2015 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/utilities.hpp"
+
+#include <atomic>
+#include <condition_variable>
+#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"
+
+// Determine if sigaction is available
+#if __APPLE__ || _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _POSIX_SOURCE
+#define HAS_SIGACTION
+#endif
+
+/// Represent the status of the global interrupt signal.
+static volatile sig_atomic_t g_signal_status = 0;
+/// 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);
+/// Mutex for protecting the global condition variable.
+static std::mutex g_interrupt_mutex;
+
+#ifdef HAS_SIGACTION
+static struct sigaction old_action;
+#else
+static void (* old_signal_handler)(int) = 0;
+#endif
+
+void
+#ifdef HAS_SIGACTION
+signal_handler(int signal_value, siginfo_t * siginfo, void * context)
+#else
+signal_handler(int signal_value)
+#endif
+{
+  // TODO(wjwwood): remove? move to console logging at some point?
+  printf("signal_handler(%d)\n", signal_value);
+#ifdef HAS_SIGACTION
+  if (old_action.sa_flags & SA_SIGINFO) {
+    if (old_action.sa_sigaction != NULL) {
+      old_action.sa_sigaction(signal_value, siginfo, context);
+    }
+  } else {
+    // *INDENT-OFF*
+    if (
+      old_action.sa_handler != NULL &&  // Is set
+      old_action.sa_handler != SIG_DFL &&  // Is not default
+      old_action.sa_handler != SIG_IGN)  // Is not ignored
+    // *INDENT-ON*
+    {
+      old_action.sa_handler(signal_value);
+    }
+  }
+#else
+  if (old_signal_handler) {
+    old_signal_handler(signal_value);
+  }
+#endif
+  g_signal_status = signal_value;
+  {
+    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();
+}
+
+void
+rclcpp::utilities::init(int argc, char * argv[])
+{
+  g_is_interrupted.store(false);
+  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: ") + rcl_get_error_string_safe());
+    // *INDENT-ON*
+  }
+#ifdef HAS_SIGACTION
+  struct sigaction action;
+  memset(&action, 0, sizeof(action));
+  sigemptyset(&action.sa_mask);
+  action.sa_sigaction = ::signal_handler;
+  action.sa_flags = SA_SIGINFO;
+  ssize_t ret = sigaction(SIGINT, &action, &old_action);
+  if (ret == -1)
+#else
+  ::old_signal_handler = std::signal(SIGINT, ::signal_handler);
+  // NOLINTNEXTLINE(readability/braces)
+  if (::old_signal_handler == SIG_ERR)
+#endif
+  {
+    const size_t error_length = 1024;
+    // NOLINTNEXTLINE(runtime/arrays)
+    char error_string[error_length];
+#ifndef _WIN32
+#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);
+      msg[error_length - 1] = '\0';
+    }
+#else
+    int error_status = strerror_r(errno, error_string, error_length);
+    if (error_status != 0) {
+      throw std::runtime_error("Failed to get error string for errno: " + std::to_string(errno));
+    }
+#endif
+#else
+    strerror_s(error_string, error_length, errno);
+#endif
+    // *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);
+    // *INDENT-ON*
+  }
+}
+
+bool
+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;
+  {
+    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();
+    }
+  }
+}
+
+void
+rclcpp::utilities::on_shutdown(std::function<void(void)> callback)
+{
+  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
+rclcpp::utilities::sleep_for(const std::chrono::nanoseconds & nanoseconds)
+{
+  std::chrono::nanoseconds time_left = nanoseconds;
+  {
+    std::unique_lock<std::mutex> lock(::g_interrupt_mutex);
+    auto start = std::chrono::steady_clock::now();
+    ::g_interrupt_condition_variable.wait_for(lock, nanoseconds);
+    time_left -= std::chrono::steady_clock::now() - start;
+  }
+  if (time_left > std::chrono::nanoseconds::zero() && !g_is_interrupted) {
+    return sleep_for(time_left);
+  }
+  // Return true if the timeout elapsed successfully, otherwise false.
+  return !g_is_interrupted;
+}

rclcpp/test/test_function_traits.cpp

@@ -14,13 +14,17 @@
 
 #include <gtest/gtest.h>
 
-#include <rclcpp/function_traits.hpp>
+#include <functional>
+#include <string>
+
+#include "rclcpp/function_traits.hpp"
 
 int func_no_args()
 {
   return 0;
 }
 
+// NOLINTNEXTLINE(readability/casting)
 int func_one_int(int)
 {
   return 1;
@@ -68,10 +72,34 @@ struct FunctionObjectOneIntOneChar
   }
 };
 
+struct ObjectMember
+{
+  int callback_one_bool(bool a)
+  {
+    (void)a;
+    return 7;
+  }
+
+  int callback_two_bools(bool a, bool b)
+  {
+    (void)a;
+    (void)b;
+    return 8;
+  }
+
+  int callback_one_bool_one_float(bool a, float b)
+  {
+    (void)a;
+    (void)b;
+    return 9;
+  }
+};
+
 template<
   typename FunctorT,
   std::size_t Arity = 0,
-  typename std::enable_if<rclcpp::arity_comparator<Arity, FunctorT>::value>::type * = nullptr
+  typename std::enable_if<
+    rclcpp::function_traits::arity_comparator<Arity, FunctorT>::value>::type * = nullptr
 >
 int func_accept_callback(FunctorT callback)
 {
@@ -81,7 +109,7 @@ int func_accept_callback(FunctorT callback)
 template<
   typename FunctorT,
   typename std::enable_if<
-    rclcpp::check_arguments<FunctorT, int>::value
+    rclcpp::function_traits::check_arguments<FunctorT, int>::value
   >::type * = nullptr
 >
 int func_accept_callback(FunctorT callback)
@@ -93,7 +121,7 @@ int func_accept_callback(FunctorT callback)
 template<
   typename FunctorT,
   typename std::enable_if<
-    rclcpp::check_arguments<FunctorT, int, int>::value
+    rclcpp::function_traits::check_arguments<FunctorT, int, int>::value
   >::type * = nullptr
 >
 int func_accept_callback(FunctorT callback)
@@ -106,7 +134,7 @@ int func_accept_callback(FunctorT callback)
 template<
   typename FunctorT,
   typename std::enable_if<
-    rclcpp::check_arguments<FunctorT, int, char>::value
+    rclcpp::function_traits::check_arguments<FunctorT, int, char>::value
   >::type * = nullptr
 >
 int func_accept_callback(FunctorT callback)
@@ -116,25 +144,61 @@ int func_accept_callback(FunctorT callback)
   return callback(a, b);
 }
 
+template<
+  typename FunctorT,
+  std::size_t Arity = 0,
+  typename std::enable_if<
+    rclcpp::function_traits::arity_comparator<Arity, FunctorT>::value
+  >::type * = nullptr,
+  typename std::enable_if<
+    std::is_same<
+      typename rclcpp::function_traits::function_traits<FunctorT>::return_type,
+      double
+    >::value
+  >::type * = nullptr
+>
+double func_accept_callback_return_type(FunctorT callback)
+{
+  return callback();
+}
+
+template<
+  typename FunctorT,
+  std::size_t Arity = 0,
+  typename std::enable_if<
+    rclcpp::function_traits::arity_comparator<Arity, FunctorT>::value
+  >::type * = nullptr,
+  typename std::enable_if<
+    std::is_same<
+      typename rclcpp::function_traits::function_traits<FunctorT>::return_type,
+      std::string
+    >::value
+  >::type * = nullptr
+>
+std::string func_accept_callback_return_type(FunctorT callback)
+{
+  return callback();
+}
+
 /*
    Tests that funcion_traits calculates arity of several functors.
  */
 TEST(TestFunctionTraits, arity) {
   // Test regular functions
   static_assert(
-    rclcpp::function_traits<decltype(func_no_args)>::arity == 0,
+    rclcpp::function_traits::function_traits<decltype(func_no_args)>::arity == 0,
     "Functor does not accept arguments");
 
   static_assert(
-    rclcpp::function_traits<decltype(func_one_int)>::arity == 1,
+    rclcpp::function_traits::function_traits<decltype(func_one_int)>::arity == 1,
     "Functor only accepts one argument");
 
   static_assert(
-    rclcpp::function_traits<decltype(func_two_ints)>::arity == 2,
+    rclcpp::function_traits::function_traits<decltype(func_two_ints)>::arity == 2,
     "Functor only accepts two arguments");
 
   static_assert(
-    rclcpp::function_traits<decltype(func_one_int_one_char)>::arity == 2,
+    rclcpp::function_traits::function_traits<decltype(func_one_int_one_char)>::arity == 2,
     "Functor only accepts two arguments");
 
   // Test lambdas
@@ -142,49 +206,54 @@ TEST(TestFunctionTraits, arity) {
       return 0;
     };
 
-  auto lambda_one_int = [](int) {
+  auto lambda_one_int = [](int one) {
+      (void)one;
       return 1;
     };
 
-  auto lambda_two_ints = [](int, int) {
+  auto lambda_two_ints = [](int one, int two) {
+      (void)one;
+      (void)two;
       return 2;
     };
 
-  auto lambda_one_int_one_char = [](int, char) {
+  auto lambda_one_int_one_char = [](int one, char two) {
+      (void)one;
+      (void)two;
       return 3;
     };
 
   static_assert(
-    rclcpp::function_traits<decltype(lambda_no_args)>::arity == 0,
+    rclcpp::function_traits::function_traits<decltype(lambda_no_args)>::arity == 0,
     "Functor does not accept arguments");
 
   static_assert(
-    rclcpp::function_traits<decltype(lambda_one_int)>::arity == 1,
+    rclcpp::function_traits::function_traits<decltype(lambda_one_int)>::arity == 1,
     "Functor only accepts one argument");
 
   static_assert(
-    rclcpp::function_traits<decltype(lambda_two_ints)>::arity == 2,
+    rclcpp::function_traits::function_traits<decltype(lambda_two_ints)>::arity == 2,
     "Functor only accepts two arguments");
 
   static_assert(
-    rclcpp::function_traits<decltype(lambda_one_int_one_char)>::arity == 2,
+    rclcpp::function_traits::function_traits<decltype(lambda_one_int_one_char)>::arity == 2,
     "Functor only accepts two arguments");
 
   // Test objects that have a call operator
   static_assert(
-    rclcpp::function_traits<FunctionObjectNoArgs>::arity == 0,
+    rclcpp::function_traits::function_traits<FunctionObjectNoArgs>::arity == 0,
     "Functor does not accept arguments");
 
   static_assert(
-    rclcpp::function_traits<FunctionObjectOneInt>::arity == 1,
+    rclcpp::function_traits::function_traits<FunctionObjectOneInt>::arity == 1,
     "Functor only accepts one argument");
 
   static_assert(
-    rclcpp::function_traits<FunctionObjectTwoInts>::arity == 2,
+    rclcpp::function_traits::function_traits<FunctionObjectTwoInts>::arity == 2,
     "Functor only accepts two arguments");
 
   static_assert(
-    rclcpp::function_traits<FunctionObjectOneIntOneChar>::arity == 2,
+    rclcpp::function_traits::function_traits<FunctionObjectOneIntOneChar>::arity == 2,
     "Functor only accepts two arguments");
 }
 
@@ -196,105 +265,210 @@ TEST(TestFunctionTraits, argument_types) {
   static_assert(
     std::is_same<
       int,
-      rclcpp::function_traits<decltype(func_one_int)>::template argument_type<0>
+      rclcpp::function_traits::function_traits<decltype(func_one_int)>::template argument_type<0>
     >::value, "Functor accepts an int as first argument");
 
   static_assert(
     std::is_same<
       int,
-      rclcpp::function_traits<decltype(func_two_ints)>::template argument_type<0>
+      rclcpp::function_traits::function_traits<decltype(func_two_ints)>::template argument_type<0>
     >::value, "Functor accepts an int as first argument");
 
   static_assert(
     std::is_same<
       int,
-      rclcpp::function_traits<decltype(func_two_ints)>::template argument_type<1>
+      rclcpp::function_traits::function_traits<decltype(func_two_ints)>::template argument_type<1>
     >::value, "Functor accepts an int as second argument");
 
   static_assert(
     std::is_same<
       int,
-      rclcpp::function_traits<decltype(func_one_int_one_char)>::template argument_type<0>
+      rclcpp::function_traits::function_traits<
+        decltype(func_one_int_one_char)
+      >::template argument_type<0>
     >::value, "Functor accepts an int as first argument");
 
   static_assert(
     std::is_same<
       char,
-      rclcpp::function_traits<decltype(func_one_int_one_char)>::template argument_type<1>
+      rclcpp::function_traits::function_traits<
+        decltype(func_one_int_one_char)
+      >::template argument_type<1>
     >::value, "Functor accepts a char as second argument");
 
   // Test lambdas
-  auto lambda_one_int = [](int) {
+  auto lambda_one_int = [](int one) {
+      (void)one;
       return 1;
     };
 
-  auto lambda_two_ints = [](int, int) {
+  auto lambda_two_ints = [](int one, int two) {
+      (void)one;
+      (void)two;
       return 2;
     };
 
-  auto lambda_one_int_one_char = [](int, char) {
+  auto lambda_one_int_one_char = [](int one, char two) {
+      (void)one;
+      (void)two;
       return 3;
     };
 
   static_assert(
     std::is_same<
       int,
-      rclcpp::function_traits<decltype(lambda_one_int)>::template argument_type<0>
+      rclcpp::function_traits::function_traits<decltype(lambda_one_int)>::template argument_type<0>
     >::value, "Functor accepts an int as first argument");
 
   static_assert(
     std::is_same<
       int,
-      rclcpp::function_traits<decltype(lambda_two_ints)>::template argument_type<0>
+      rclcpp::function_traits::function_traits<decltype(lambda_two_ints)>::template argument_type<0>
     >::value, "Functor accepts an int as first argument");
 
   static_assert(
     std::is_same<
       int,
-      rclcpp::function_traits<decltype(lambda_two_ints)>::template argument_type<1>
+      rclcpp::function_traits::function_traits<decltype(lambda_two_ints)>::template argument_type<1>
     >::value, "Functor accepts an int as second argument");
 
   static_assert(
     std::is_same<
       int,
-      rclcpp::function_traits<decltype(lambda_one_int_one_char)>::template argument_type<0>
+      rclcpp::function_traits::function_traits<
+        decltype(lambda_one_int_one_char)
+      >::template argument_type<0>
     >::value, "Functor accepts an int as first argument");
 
   static_assert(
     std::is_same<
       char,
-      rclcpp::function_traits<decltype(lambda_one_int_one_char)>::template argument_type<1>
+      rclcpp::function_traits::function_traits<
+        decltype(lambda_one_int_one_char)
+      >::template argument_type<1>
     >::value, "Functor accepts a char as second argument");
 
   // Test objects that have a call operator
   static_assert(
     std::is_same<
       int,
-      rclcpp::function_traits<FunctionObjectOneInt>::template argument_type<0>
+      rclcpp::function_traits::function_traits<FunctionObjectOneInt>::template argument_type<0>
     >::value, "Functor accepts an int as first argument");
 
   static_assert(
     std::is_same<
       int,
-      rclcpp::function_traits<FunctionObjectTwoInts>::template argument_type<0>
+      rclcpp::function_traits::function_traits<FunctionObjectTwoInts>::template argument_type<0>
     >::value, "Functor accepts an int as first argument");
 
   static_assert(
     std::is_same<
       int,
-      rclcpp::function_traits<FunctionObjectTwoInts>::template argument_type<1>
+      rclcpp::function_traits::function_traits<FunctionObjectTwoInts>::template argument_type<1>
     >::value, "Functor accepts an int as second argument");
 
   static_assert(
     std::is_same<
       int,
-      rclcpp::function_traits<FunctionObjectOneIntOneChar>::template argument_type<0>
+      rclcpp::function_traits::function_traits<
+        FunctionObjectOneIntOneChar
+      >::template argument_type<0>
     >::value, "Functor accepts an int as first argument");
 
   static_assert(
     std::is_same<
       char,
-      rclcpp::function_traits<FunctionObjectOneIntOneChar>::template argument_type<1>
+      rclcpp::function_traits::function_traits<
+        FunctionObjectOneIntOneChar
+      >::template argument_type<1>
+    >::value, "Functor accepts a char as second argument");
+
+  ObjectMember object_member;
+
+  auto bind_one_bool = std::bind(
+    &ObjectMember::callback_one_bool, &object_member, std::placeholders::_1);
+
+  static_assert(
+    std::is_same<
+      bool,
+      rclcpp::function_traits::function_traits<decltype(bind_one_bool)>::template argument_type<0>
+    >::value, "Functor accepts a bool as first argument");
+
+  auto bind_two_bools = std::bind(
+    &ObjectMember::callback_two_bools, &object_member, std::placeholders::_1,
+    std::placeholders::_2);
+
+  static_assert(
+    std::is_same<
+      bool,
+      rclcpp::function_traits::function_traits<decltype(bind_two_bools)>::template argument_type<0>
+    >::value, "Functor accepts a bool as first argument");
+
+  static_assert(
+    std::is_same<
+      bool,
+      rclcpp::function_traits::function_traits<decltype(bind_two_bools)>::template argument_type<1>
+    >::value, "Functor accepts a bool as second argument");
+
+  auto bind_one_bool_one_float = std::bind(
+    &ObjectMember::callback_one_bool_one_float, &object_member, std::placeholders::_1,
+    std::placeholders::_2);
+
+  static_assert(
+    std::is_same<
+      bool,
+      rclcpp::function_traits::function_traits<
+        decltype(bind_one_bool_one_float)
+      >::template argument_type<0>
+    >::value, "Functor accepts a bool as first argument");
+
+  static_assert(
+    std::is_same<
+      float,
+      rclcpp::function_traits::function_traits<
+        decltype(bind_one_bool_one_float)
+      >::template argument_type<1>
+    >::value, "Functor accepts a float as second argument");
+
+  auto bind_one_int = std::bind(func_one_int, std::placeholders::_1);
+
+  static_assert(
+    std::is_same<
+      int,
+      rclcpp::function_traits::function_traits<decltype(bind_one_int)>::template argument_type<0>
+    >::value, "Functor accepts an int as first argument");
+
+  auto bind_two_ints = std::bind(func_two_ints, std::placeholders::_1, std::placeholders::_2);
+
+  static_assert(
+    std::is_same<
+      int,
+      rclcpp::function_traits::function_traits<decltype(bind_two_ints)>::template argument_type<0>
+    >::value, "Functor accepts an int as first argument");
+
+  static_assert(
+    std::is_same<
+      int,
+      rclcpp::function_traits::function_traits<decltype(bind_two_ints)>::template argument_type<1>
+    >::value, "Functor accepts an int as second argument");
+
+  auto bind_one_int_one_char = std::bind(
+    func_one_int_one_char, std::placeholders::_1, std::placeholders::_2);
+
+  static_assert(
+    std::is_same<
+      int,
+      rclcpp::function_traits::function_traits<
+        decltype(bind_one_int_one_char)
+      >::template argument_type<0>
+    >::value, "Functor accepts an int as first argument");
+
+  static_assert(
+    std::is_same<
+      char,
+      rclcpp::function_traits::function_traits<
+        decltype(bind_one_int_one_char)
+      >::template argument_type<1>
     >::value, "Functor accepts a char as second argument");
 }
 
@@ -304,113 +478,173 @@ TEST(TestFunctionTraits, argument_types) {
 TEST(TestFunctionTraits, check_arguments) {
   // Test regular functions
   static_assert(
-    rclcpp::check_arguments<decltype(func_one_int), int>::value,
+    rclcpp::function_traits::check_arguments<decltype(func_one_int), int>::value,
     "Functor accepts a single int as arguments");
 
   static_assert(
-    !rclcpp::check_arguments<decltype(func_one_int), char>::value,
+    !rclcpp::function_traits::check_arguments<decltype(func_one_int), char>::value,
     "Functor does not accept a char as argument");
 
   static_assert(
-    !rclcpp::check_arguments<decltype(func_one_int), char, int>::value,
+    !rclcpp::function_traits::check_arguments<decltype(func_one_int), char, int>::value,
     "Functor does not accept two arguments");
 
   static_assert(
-    !rclcpp::check_arguments<decltype(func_two_ints), int>::value,
+    !rclcpp::function_traits::check_arguments<decltype(func_two_ints), int>::value,
     "Functor does not accept a single int as argument, requires two ints");
 
   static_assert(
-    rclcpp::check_arguments<decltype(func_two_ints), int, int>::value,
+    rclcpp::function_traits::check_arguments<decltype(func_two_ints), int, int>::value,
     "Functor accepts two ints as arguments");
 
   static_assert(
-    !rclcpp::check_arguments<decltype(func_two_ints), bool, int>::value,
+    !rclcpp::function_traits::check_arguments<decltype(func_two_ints), bool, int>::value,
     "Functor does not accept a bool and an int as arguments, requires two ints");
 
   static_assert(
-    !rclcpp::check_arguments<decltype(func_two_ints), int, char>::value,
+    !rclcpp::function_traits::check_arguments<decltype(func_two_ints), int, char>::value,
     "Functor does not accept an int and a char as arguments, requires two ints");
 
   static_assert(
-    rclcpp::check_arguments<decltype(func_one_int_one_char), int, char>::value,
+    rclcpp::function_traits::check_arguments<decltype(func_one_int_one_char), int, char>::value,
     "Functor accepts an int and a char as arguments");
 
   // Test lambdas
-  auto lambda_one_int = [](int) {
+  auto lambda_one_int = [](int one) {
+      (void)one;
       return 1;
     };
 
-  auto lambda_two_ints = [](int, int) {
+  auto lambda_two_ints = [](int one, int two) {
+      (void)one;
+      (void)two;
       return 2;
     };
 
-  auto lambda_one_int_one_char = [](int, char) {
+  auto lambda_one_int_one_char = [](int one, char two) {
+      (void)one;
+      (void)two;
       return 3;
     };
 
   static_assert(
-    rclcpp::check_arguments<decltype(lambda_one_int), int>::value,
+    rclcpp::function_traits::check_arguments<decltype(lambda_one_int), int>::value,
     "Functor accepts an int as the only argument");
 
   static_assert(
-    rclcpp::check_arguments<decltype(lambda_two_ints), int, int>::value,
+    rclcpp::function_traits::check_arguments<decltype(lambda_two_ints), int, int>::value,
     "Functor accepts two ints as arguments");
 
   static_assert(
-    rclcpp::check_arguments<decltype(lambda_one_int_one_char), int, char>::value,
+    rclcpp::function_traits::check_arguments<decltype(lambda_one_int_one_char), int, char>::value,
     "Functor accepts an int and a char as arguments");
 
   // Test objects that have a call operator
   static_assert(
-    rclcpp::check_arguments<FunctionObjectOneInt, int>::value,
+    rclcpp::function_traits::check_arguments<FunctionObjectOneInt, int>::value,
     "Functor accepts an int as the only argument");
 
   static_assert(
-    rclcpp::check_arguments<FunctionObjectTwoInts, int, int>::value,
+    rclcpp::function_traits::check_arguments<FunctionObjectTwoInts, int, int>::value,
     "Functor accepts two ints as arguments");
 
   static_assert(
-    rclcpp::check_arguments<FunctionObjectOneIntOneChar, int, char>::value,
+    rclcpp::function_traits::check_arguments<FunctionObjectOneIntOneChar, int, char>::value,
     "Functor accepts an int and a char as arguments");
+
+  ObjectMember object_member;
+
+  auto bind_one_bool = std::bind(
+    &ObjectMember::callback_one_bool, &object_member, std::placeholders::_1);
+
+  // Test std::bind functions
+  static_assert(
+    rclcpp::function_traits::check_arguments<decltype(bind_one_bool), bool>::value,
+    "Functor accepts a single bool as arguments");
 }
 
 /*
    Tests that same_arguments work.
 */
 TEST(TestFunctionTraits, same_arguments) {
-  auto lambda_one_int = [](int) {
+  auto lambda_one_int = [](int one) {
+      (void)one;
       return 1;
     };
 
-  auto lambda_two_ints = [](int, int) {
+  auto lambda_two_ints = [](int one, int two) {
+      (void)one;
+      (void)two;
       return 1;
     };
 
   static_assert(
-    rclcpp::same_arguments<decltype(lambda_one_int), decltype(func_one_int)>::value,
+    rclcpp::function_traits::same_arguments<
+      decltype(lambda_one_int), decltype(func_one_int)
+    >::value,
     "Lambda and function have the same arguments");
 
   static_assert(
-    !rclcpp::same_arguments<decltype(lambda_two_ints), decltype(func_one_int)>::value,
+    !rclcpp::function_traits::same_arguments<
+      decltype(lambda_two_ints), decltype(func_one_int)
+    >::value,
     "Lambda and function have different arguments");
 
   static_assert(
-    !rclcpp::same_arguments<decltype(func_one_int_one_char), decltype(func_two_ints)>::value,
+    !rclcpp::function_traits::same_arguments<
+      decltype(func_one_int_one_char), decltype(func_two_ints)
+    >::value,
     "Functions have different arguments");
 
   static_assert(
-    !rclcpp::same_arguments<decltype(lambda_one_int), decltype(lambda_two_ints)>::value,
+    !rclcpp::function_traits::same_arguments<
+      decltype(lambda_one_int), decltype(lambda_two_ints)
+    >::value,
     "Lambdas have different arguments");
 
   static_assert(
-    rclcpp::same_arguments<FunctionObjectTwoInts, decltype(func_two_ints)>::value,
+    rclcpp::function_traits::same_arguments<FunctionObjectTwoInts, decltype(func_two_ints)>::value,
     "Functor and function have the same arguments");
 
   static_assert(
-    rclcpp::same_arguments<FunctionObjectTwoInts, decltype(lambda_two_ints)>::value,
+    rclcpp::function_traits::same_arguments<
+      FunctionObjectTwoInts, decltype(lambda_two_ints)>::value,
     "Functor and lambda have the same arguments");
 }
 
+TEST(TestFunctionTraits, return_type) {
+  // Test regular function
+  static_assert(
+    std::is_same<
+      rclcpp::function_traits::function_traits<decltype(func_no_args)>::return_type,
+      int
+    >::value,
+    "Functor return ints");
+
+  // Test lambda
+  auto lambda_one_int_return_double = [](int one) -> double {
+      (void)one;
+      return 1.0;
+    };
+
+  static_assert(
+    std::is_same<
+      rclcpp::function_traits::function_traits<
+        decltype(lambda_one_int_return_double)
+      >::return_type,
+      double
+    >::value,
+    "Lambda returns a double");
+
+  // Test objects that have a call operator
+  static_assert(
+    std::is_same<
+      rclcpp::function_traits::function_traits<FunctionObjectNoArgs>::return_type,
+      int
+    >::value,
+    "Functor return ints");
+}
+
 /*
    Tests that functions are matched via SFINAE.
  */
@@ -427,15 +661,20 @@ TEST(TestFunctionTraits, sfinae_match) {
       return 0;
     };
 
-  auto lambda_one_int = [](int) {
+  auto lambda_one_int = [](int one) {
+      (void)one;
       return 1;
     };
 
-  auto lambda_two_ints = [](int, int) {
+  auto lambda_two_ints = [](int one, int two) {
+      (void)one;
+      (void)two;
       return 2;
     };
 
-  auto lambda_one_int_one_char = [](int, char) {
+  auto lambda_one_int_one_char = [](int one, char two) {
+      (void)one;
+      (void)two;
       return 3;
     };
 
@@ -454,4 +693,16 @@ TEST(TestFunctionTraits, sfinae_match) {
   EXPECT_EQ(2, func_accept_callback(FunctionObjectTwoInts()));
 
   EXPECT_EQ(3, func_accept_callback(FunctionObjectOneIntOneChar()));
+
+  auto lambda_no_args_double = []() -> double {
+      return 123.45;
+    };
+
+  auto lambda_no_args_string = []() -> std::string {
+      return std::string("foo");
+    };
+
+  EXPECT_EQ(123.45, func_accept_callback_return_type(lambda_no_args_double));
+
+  EXPECT_EQ("foo", func_accept_callback_return_type(lambda_no_args_string));
 }

rclcpp/test/test_intra_process_manager.cpp

@@ -13,12 +13,12 @@
 // limitations under the License.
 
 #include <memory>
+#include <string>
 
-#include <gtest/gtest.h>
-
-#include <rclcpp/allocator/allocator_common.hpp>
-#include <rclcpp/macros.hpp>
-#include <rmw/types.h>
+#include "gtest/gtest.h"
+#include "rclcpp/allocator/allocator_common.hpp"
+#include "rclcpp/macros.hpp"
+#include "rmw/types.h"
 
 // Mock up publisher and subscription base to avoid needing an rmw impl.
 namespace rclcpp
@@ -79,9 +79,9 @@ public:
   }
 };
 
-}
-}
-}
+}  // namespace mock
+}  // namespace publisher
+}  // namespace rclcpp
 
 namespace rclcpp
 {
@@ -111,22 +111,25 @@ public:
   size_t mock_queue_size;
 };
 
-}
-}
-}
+}  // namespace mock
+}  // namespace subscription
+}  // namespace rclcpp
 
 // Prevent rclcpp/publisher.hpp and rclcpp/subscription.hpp from being imported.
 #define RCLCPP__PUBLISHER_HPP_
 #define RCLCPP__SUBSCRIPTION_HPP_
+#define RCLCPP_BUILDING_LIBRARY 1
 // Force ipm to use our mock publisher class.
 #define Publisher mock::Publisher
 #define PublisherBase mock::PublisherBase
 #define SubscriptionBase mock::SubscriptionBase
-#include <rclcpp/intra_process_manager.hpp>
+#include "../src/rclcpp/intra_process_manager.cpp"
+#include "../src/rclcpp/intra_process_manager_impl.cpp"
 #undef SubscriptionBase
 #undef Publisher
 #undef PublisherBase
 
+// NOLINTNEXTLINE(build/include_order)
 #include <rcl_interfaces/msg/intra_process_message.hpp>
 
 /*
@@ -142,13 +145,15 @@ public:
 TEST(TestIntraProcessManager, nominal) {
   rclcpp::intra_process_manager::IntraProcessManager ipm;
 
-  auto p1 =
-    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
+  auto p1 = std::make_shared<
+    rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
+    >();
   p1->mock_topic_name = "nominal1";
   p1->mock_queue_size = 2;
 
-  auto p2 =
-    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
+  auto p2 = std::make_shared<
+    rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
+    >();
   p2->mock_topic_name = "nominal2";
   p2->mock_queue_size = 10;
 
@@ -231,8 +236,9 @@ TEST(TestIntraProcessManager, nominal) {
 TEST(TestIntraProcessManager, remove_publisher_before_trying_to_take) {
   rclcpp::intra_process_manager::IntraProcessManager ipm;
 
-  auto p1 =
-    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
+  auto p1 = std::make_shared<
+    rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
+    >();
   p1->mock_topic_name = "nominal1";
   p1->mock_queue_size = 10;
 
@@ -272,8 +278,9 @@ TEST(TestIntraProcessManager, remove_publisher_before_trying_to_take) {
 TEST(TestIntraProcessManager, removed_subscription_affects_take) {
   rclcpp::intra_process_manager::IntraProcessManager ipm;
 
-  auto p1 =
-    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
+  auto p1 = std::make_shared<
+    rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
+    >();
   p1->mock_topic_name = "nominal1";
   p1->mock_queue_size = 10;
 
@@ -342,8 +349,9 @@ TEST(TestIntraProcessManager, removed_subscription_affects_take) {
 TEST(TestIntraProcessManager, multiple_subscriptions_one_publisher) {
   rclcpp::intra_process_manager::IntraProcessManager ipm;
 
-  auto p1 =
-    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
+  auto p1 = std::make_shared<
+    rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
+    >();
   p1->mock_topic_name = "nominal1";
   p1->mock_queue_size = 10;
 
@@ -413,18 +421,21 @@ TEST(TestIntraProcessManager, multiple_subscriptions_one_publisher) {
 TEST(TestIntraProcessManager, multiple_publishers_one_subscription) {
   rclcpp::intra_process_manager::IntraProcessManager ipm;
 
-  auto p1 =
-    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
+  auto p1 = std::make_shared<
+    rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
+    >();
   p1->mock_topic_name = "nominal1";
   p1->mock_queue_size = 10;
 
-  auto p2 =
-    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
+  auto p2 = std::make_shared<
+    rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
+    >();
   p2->mock_topic_name = "nominal1";
   p2->mock_queue_size = 10;
 
-  auto p3 =
-    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
+  auto p3 = std::make_shared<
+    rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
+    >();
   p3->mock_topic_name = "nominal1";
   p3->mock_queue_size = 10;
 
@@ -510,18 +521,21 @@ TEST(TestIntraProcessManager, multiple_publishers_one_subscription) {
 TEST(TestIntraProcessManager, multiple_publishers_multiple_subscription) {
   rclcpp::intra_process_manager::IntraProcessManager ipm;
 
-  auto p1 =
-    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
+  auto p1 = std::make_shared<
+    rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
+    >();
   p1->mock_topic_name = "nominal1";
   p1->mock_queue_size = 10;
 
-  auto p2 =
-    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
+  auto p2 = std::make_shared<
+    rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
+    >();
   p2->mock_topic_name = "nominal1";
   p2->mock_queue_size = 10;
 
-  auto p3 =
-    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
+  auto p3 = std::make_shared<
+    rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
+    >();
   p3->mock_topic_name = "nominal1";
   p3->mock_queue_size = 10;
 
@@ -674,8 +688,9 @@ TEST(TestIntraProcessManager, multiple_publishers_multiple_subscription) {
 TEST(TestIntraProcessManager, ring_buffer_displacement) {
   rclcpp::intra_process_manager::IntraProcessManager ipm;
 
-  auto p1 =
-    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
+  auto p1 = std::make_shared<
+    rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
+    >();
   p1->mock_topic_name = "nominal1";
   p1->mock_queue_size = 2;
 
@@ -744,8 +759,9 @@ TEST(TestIntraProcessManager, ring_buffer_displacement) {
 TEST(TestIntraProcessManager, subscription_creation_race_condition) {
   rclcpp::intra_process_manager::IntraProcessManager ipm;
 
-  auto p1 =
-    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
+  auto p1 = std::make_shared<
+    rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
+    >();
   p1->mock_topic_name = "nominal1";
   p1->mock_queue_size = 2;
 
@@ -792,8 +808,9 @@ TEST(TestIntraProcessManager, publisher_out_of_scope_take) {
   uint64_t p1_id;
   uint64_t p1_m1_id;
   {
-    auto p1 =
-      std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
+    auto p1 = std::make_shared<
+      rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
+      >();
     p1->mock_topic_name = "nominal1";
     p1->mock_queue_size = 2;
 
@@ -830,8 +847,9 @@ TEST(TestIntraProcessManager, publisher_out_of_scope_store) {
 
   uint64_t p1_id;
   {
-    auto p1 =
-      std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
+    auto p1 = std::make_shared<
+      rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>
+      >();
     p1->mock_topic_name = "nominal1";
     p1->mock_queue_size = 2;
 

rclcpp/test/test_mapped_ring_buffer.cpp

@@ -14,6 +14,7 @@
 
 #include <gtest/gtest.h>
 
+#define RCLCPP_BUILDING_LIBRARY 1  // Prevent including unavailable symbols
 #include <rclcpp/mapped_ring_buffer.hpp>
 
 /*

rclcpp/test/test_rate.cpp

@@ -0,0 +1,100 @@
+// Copyright 2015 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <string>
+
+#include "rclcpp/rate.hpp"
+
+/*
+   Basic tests for the Rate and WallRate clases.
+ */
+TEST(TestRate, rate_basics) {
+  auto period = std::chrono::milliseconds(100);
+  auto offset = std::chrono::milliseconds(50);
+  auto epsilon = std::chrono::milliseconds(1);
+  double overrun_ratio = 1.5;
+
+  auto start = std::chrono::system_clock::now();
+  rclcpp::rate::Rate r(period);
+  ASSERT_FALSE(r.is_steady());
+  ASSERT_TRUE(r.sleep());
+  auto one = std::chrono::system_clock::now();
+  auto delta = one - start;
+  ASSERT_TRUE(period < delta);
+  ASSERT_TRUE(period * overrun_ratio > delta);
+
+  rclcpp::utilities::sleep_for(offset);
+  ASSERT_TRUE(r.sleep());
+  auto two = std::chrono::system_clock::now();
+  delta = two - start;
+  ASSERT_TRUE(2 * period < delta);
+  ASSERT_TRUE(2 * period * overrun_ratio > delta);
+
+  rclcpp::utilities::sleep_for(offset);
+  auto two_offset = std::chrono::system_clock::now();
+  r.reset();
+  ASSERT_TRUE(r.sleep());
+  auto three = std::chrono::system_clock::now();
+  delta = three - two_offset;
+  ASSERT_TRUE(period < delta);
+  ASSERT_TRUE(period * overrun_ratio > delta);
+
+  rclcpp::utilities::sleep_for(offset + period);
+  auto four = std::chrono::system_clock::now();
+  ASSERT_FALSE(r.sleep());
+  auto five = std::chrono::system_clock::now();
+  delta = five - four;
+  ASSERT_TRUE(epsilon > delta);
+}
+
+TEST(TestRate, wall_rate_basics) {
+  auto period = std::chrono::milliseconds(100);
+  auto offset = std::chrono::milliseconds(50);
+  auto epsilon = std::chrono::milliseconds(1);
+  double overrun_ratio = 1.5;
+
+  auto start = std::chrono::steady_clock::now();
+  rclcpp::rate::WallRate r(period);
+  ASSERT_TRUE(r.is_steady());
+  ASSERT_TRUE(r.sleep());
+  auto one = std::chrono::steady_clock::now();
+  auto delta = one - start;
+  ASSERT_TRUE(period < delta);
+  ASSERT_TRUE(period * overrun_ratio > delta);
+
+  rclcpp::utilities::sleep_for(offset);
+  ASSERT_TRUE(r.sleep());
+  auto two = std::chrono::steady_clock::now();
+  delta = two - start;
+  ASSERT_TRUE(2 * period < delta + epsilon);
+  ASSERT_TRUE(2 * period * overrun_ratio > delta);
+
+  rclcpp::utilities::sleep_for(offset);
+  auto two_offset = std::chrono::steady_clock::now();
+  r.reset();
+  ASSERT_TRUE(r.sleep());
+  auto three = std::chrono::steady_clock::now();
+  delta = three - two_offset;
+  ASSERT_TRUE(period < delta);
+  ASSERT_TRUE(period * overrun_ratio > delta);
+
+  rclcpp::utilities::sleep_for(offset + period);
+  auto four = std::chrono::steady_clock::now();
+  ASSERT_FALSE(r.sleep());
+  auto five = std::chrono::steady_clock::now();
+  delta = five - four;
+  ASSERT_TRUE(epsilon > delta);
+}