0.0.3

* parameter client takes node interfaces

* correct wrong copy paste

* correctly fetch node name

* use node_topics_interface for creating parameter event

* fix typos

.github/ISSUE_TEMPLATE.md

@@ -0,0 +1,44 @@
+<!--
+For general questions, please post on discourse: https://discourse.ros.org/c/ng-ros
+Not sure if this is the right repository? Open an issue on https://github.com/ros2/ros2/issues
+For Bug report or feature requests, please fill out the relevant category below
+-->
+
+## Bug report
+
+**Required Info:**
+
+- Operating System:
+  - <!-- OS and version (e.g. Windows 10, Ubuntu 16.04...) -->
+- Installation type:
+  - <!-- binaries or from source  -->
+- Version or commit hash:
+  - <!-- Output of git rev-parse HEAD, release version, or repos file  -->
+- DDS implementation:
+  - <!-- rmw_implementation used (e.g. Fast-RTPS, RTI Connext, etc -->
+- Client library (if applicable):
+  - <!-- e.g. rclcpp, rclpy, or N/A -->
+
+#### Steps to reproduce issue
+<!-- Detailed instructions on how to reliably reproduce this issue http://sscce.org/
+``` code that can be copy-pasted is preferred ``` -->
+```
+
+```
+
+#### Expected behavior
+
+#### Actual behavior
+
+#### Additional information
+
+<!-- If you are reporting a bug delete everything below
+     If you are requesting a feature deleted everything above this line -->
+----
+## Feature request
+
+#### Feature description
+<!-- Description in a few sentences what the feature consists of and what problem it will solve -->
+
+#### Implementation considerations
+<!-- Relevant information on how the feature could be implemented and pros and cons of the different solutions -->

rclcpp/.gitignore

@@ -0,0 +1 @@
+doc_output

rclcpp/CMakeLists.txt

@@ -1,16 +1,28 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 
 project(rclcpp)
 
 find_package(ament_cmake REQUIRED)
+find_package(builtin_interfaces 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)
+find_package(rosidl_typesupport_c REQUIRED)
+find_package(rosidl_typesupport_cpp REQUIRED)
 
-if(NOT WIN32)
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra")
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  # we dont use add_compile_options with pedantic in message packages
+  # because the Python C extensions dont comply with it
+  # TODO(mikaelarguedas) change to add_compile_options
+  # once this is not a message package anymore
+  # https://github.com/ros2/system_tests/issues/191
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
 endif()
 
 include_directories(include)
@@ -21,99 +33,224 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/client.cpp
   src/rclcpp/context.cpp
   src/rclcpp/contexts/default_context.cpp
+  src/rclcpp/event.cpp
+  src/rclcpp/exceptions.cpp
   src/rclcpp/executor.cpp
   src/rclcpp/executors.cpp
+  src/rclcpp/expand_topic_or_service_name.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/node.cpp
+  src/rclcpp/node_interfaces/node_base.cpp
+  src/rclcpp/node_interfaces/node_graph.cpp
+  src/rclcpp/node_interfaces/node_parameters.cpp
+  src/rclcpp/node_interfaces/node_services.cpp
+  src/rclcpp/node_interfaces/node_timers.cpp
+  src/rclcpp/node_interfaces/node_topics.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/time.cpp
   src/rclcpp/timer.cpp
   src/rclcpp/type_support.cpp
   src/rclcpp/utilities.cpp
 )
 
-macro(target)
-  add_library(${PROJECT_NAME}${target_suffix} SHARED
-    ${${PROJECT_NAME}_SRCS})
-  ament_target_dependencies(${PROJECT_NAME}${target_suffix}
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_generator_cpp"
-    "${rmw_implementation}")
+add_library(${PROJECT_NAME} SHARED
+  ${${PROJECT_NAME}_SRCS})
+ament_target_dependencies(${PROJECT_NAME}
+  "builtin_interfaces"
+  "rcl"
+  "rosidl_generator_cpp"
+  "rosidl_typesupport_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")
+# 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}
+  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)
+install(
+  TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
 
 ament_export_dependencies(ament_cmake)
-ament_export_dependencies(rcl_interfaces)
-ament_export_dependencies(rmw)
-ament_export_dependencies(rmw_implementation)
+ament_export_dependencies(builtin_interfaces)
+ament_export_dependencies(rcl)
 ament_export_dependencies(rosidl_generator_cpp)
+ament_export_dependencies(rosidl_typesupport_c)
+ament_export_dependencies(rosidl_typesupport_cpp)
 
 ament_export_include_directories(include)
 
 ament_export_libraries(${PROJECT_NAME})
 
-if(AMENT_ENABLE_TESTING)
+if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 
+  find_package(rmw_implementation_cmake REQUIRED)
+
+  ament_add_gtest(test_client test/test_client.cpp)
+  if(TARGET test_client)
+    target_include_directories(test_client PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_client ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_expand_topic_or_service_name test/test_expand_topic_or_service_name.cpp)
+  if(TARGET test_expand_topic_or_service_name)
+    target_include_directories(test_expand_topic_or_service_name PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME})
+  endif()
   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}
+      ${rosidl_typesupport_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}
+      ${rosidl_typesupport_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_INCLUDE_DIRS}
       ${rcl_interfaces_INCLUDE_DIRS}
       ${rmw_INCLUDE_DIRS}
       ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
     )
   endif()
-  ament_add_gtest(test_rate test/test_rate.cpp)
+  ament_add_gtest(test_node test/test_node.cpp)
+  if(TARGET test_node)
+    target_include_directories(test_node PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_node ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_publisher test/test_publisher.cpp)
+  if(TARGET test_publisher)
+    target_include_directories(test_publisher PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_publisher ${PROJECT_NAME})
+  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}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
     )
     target_link_libraries(test_rate
-      ${PROJECT_NAME}${target_suffix}
+      ${PROJECT_NAME}
     )
   endif()
+  ament_add_gtest(test_service test/test_service.cpp)
+  if(TARGET test_service)
+    target_include_directories(test_service PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_service ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_subscription test/test_subscription.cpp)
+  if(TARGET test_subscription)
+    target_include_directories(test_subscription PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_subscription ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_find_weak_nodes test/test_find_weak_nodes.cpp)
+  if(TARGET test_find_weak_nodes)
+    target_include_directories(test_find_weak_nodes PUBLIC
+      ${rcl_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_find_weak_nodes ${PROJECT_NAME})
+  endif()
+
+  get_default_rmw_implementation(default_rmw)
+  find_package(${default_rmw} REQUIRED)
+  get_rmw_typesupport(typesupport_impls_cpp "${default_rmw}" LANGUAGE "cpp")
+  get_rmw_typesupport(typesupport_impls_c "${default_rmw}" LANGUAGE "c")
+  set(mock_msg_files
+    "test/mock_msgs/srv/Mock.srv")
+  rosidl_generate_interfaces(mock_msgs
+    ${mock_msg_files}
+    LIBRARY_NAME "rclcpp"
+    SKIP_INSTALL)
+
+  set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}")
+  if(WIN32)
+    set(append_library_dirs "${append_library_dirs}/$<CONFIG>")
+  endif()
+
+  ament_add_gtest(test_externally_defined_services test/test_externally_defined_services.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_externally_defined_services)
+    target_include_directories(test_externally_defined_services PUBLIC
+      ${rcl_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_externally_defined_services ${PROJECT_NAME})
+    foreach(typesupport_impl_cpp ${typesupport_impls_cpp})
+      rosidl_target_interfaces(test_externally_defined_services
+        mock_msgs ${typesupport_impl_cpp})
+    endforeach()
+    foreach(typesupport_impl_c ${typesupport_impls_c})
+      rosidl_target_interfaces(test_externally_defined_services
+        mock_msgs ${typesupport_impl_c})
+    endforeach()
+  endif()
+
+  ament_add_gtest(test_time test/test_time.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_time)
+    ament_target_dependencies(test_time
+      "rcl")
+    target_link_libraries(test_time ${PROJECT_NAME})
+  endif()
 endif()
 
 ament_package(

rclcpp/Doxyfile

@@ -0,0 +1,28 @@
+# All settings not listed here will use the Doxygen default values.
+
+PROJECT_NAME           = "rclcpp"
+PROJECT_NUMBER         = master
+PROJECT_BRIEF          = "C++ ROS Client Library API"
+
+INPUT                  = ./include
+RECURSIVE              = YES
+OUTPUT_DIRECTORY       = doc_output
+
+EXTRACT_ALL            = YES
+SORT_MEMBER_DOCS       = NO
+
+GENERATE_LATEX         = NO
+
+ENABLE_PREPROCESSING   = YES
+MACRO_EXPANSION        = YES
+EXPAND_ONLY_PREDEF     = YES
+PREDEFINED             = RCLCPP_PUBLIC=
+
+# Tag files that do not exist will produce a warning and cross-project linking will not work.
+TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/"
+# Consider changing "latest" to the version you want to reference (e.g. beta1 or 1.0.0)
+TAGFILES += "../../../../doxygen_tag_files/rcl.tag=http://docs.ros2.org/latest/api/rcl/"
+TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/"
+TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/"
+# Uncomment to generate tag files for cross-project linking.
+#GENERATE_TAGFILE = "../../../../doxygen_tag_files/rclcpp.tag"

rclcpp/cmake/get_rclcpp_information.cmake

@@ -1,88 +0,0 @@
-# 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)
-
-  # 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"
-    "rmw"
-    "${rmw_implementation}"
-    "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/cmake/rclcpp_create_node_main.cmake

@@ -0,0 +1,26 @@
+# 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.
+
+
+set(rclcpp_node_main_SRC "${rclcpp_DIR}/../../../src/rclcpp/node_main.cpp")
+
+function(rclcpp_create_node_main node_library_target)
+  if(NOT TARGET ${node_library_target})
+    message(FATAL_ERROR "rclcpp_create_node_main() the first argument must be a valid target name")
+  endif()
+  set(executable_name_ ${node_library_target}_node)
+  add_executable(${executable_name_} ${rclcpp_node_main_SRC})
+  target_link_libraries(${executable_name_} ${node_library_target})
+  install(TARGETS ${executable_name_} DESTINATION bin)
+endfunction()

rclcpp/cmake/rclcpp_package_hook.cmake

@@ -0,0 +1,17 @@
+# 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.
+
+# register node plugins
+ament_index_register_resource(
+  "node_plugin" CONTENT "${_RCLCPP__NODE_PLUGINS}")

rclcpp/cmake/rclcpp_register_node_plugins.cmake

@@ -0,0 +1,61 @@
+# 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.
+
+#
+# Register a node plugin with the ament resource index.
+#
+# The passed library can contain multiple plugins extending the node interface.
+#
+# :param target: the shared library target
+# :type target: string
+# :param ARGN: the unique plugin names being exported using class_loader
+# :type ARGN: list of strings
+#
+macro(rclcpp_register_node_plugins target)
+  if(NOT TARGET ${target})
+    message(
+      FATAL_ERROR
+      "rclcpp_register_node_plugins() first argument "
+      "'${target}' is not a target")
+  endif()
+  get_target_property(_target_type ${target} TYPE)
+  if(NOT _target_type STREQUAL "SHARED_LIBRARY")
+    message(
+      FATAL_ERROR
+      "rclcpp_register_node_plugins() first argument "
+      "'${target}' is not a shared library target")
+  endif()
+
+  if(${ARGC} GREATER 0)
+    _rclcpp_register_package_hook()
+    set(_unique_names)
+    foreach(_arg ${ARGN})
+      if(_arg IN_LIST _unique_names)
+        message(
+          FATAL_ERROR
+          "rclcpp_register_node_plugins() the plugin names "
+          "must be unique (multiple '${_arg}')")
+      endif()
+      list(APPEND _unique_names "${_arg}")
+
+      if(WIN32)
+        set(_path "bin")
+      else()
+        set(_path "lib")
+      endif()
+      set(_RCLCPP__NODE_PLUGINS
+        "${_RCLCPP__NODE_PLUGINS}${_arg};${_path}/$<TARGET_FILE_NAME:${target}>\n")
+    endforeach()
+  endif()
+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,66 @@ 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;
+#else
+  (void)allocator;  // Remove warning
+#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/any_executable.hpp

@@ -17,8 +17,13 @@
 
 #include <memory>
 
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/client.hpp"
 #include "rclcpp/macros.hpp"
-#include "rclcpp/node.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/subscription.hpp"
+#include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -28,7 +33,7 @@ namespace executor
 
 struct AnyExecutable
 {
-  RCLCPP_SMART_PTR_DEFINITIONS(AnyExecutable);
+  RCLCPP_SMART_PTR_DEFINITIONS(AnyExecutable)
 
   RCLCPP_PUBLIC
   AnyExecutable();
@@ -44,7 +49,7 @@ struct AnyExecutable
   rclcpp::client::ClientBase::SharedPtr client;
   // These are used to keep the scope on the containing items
   rclcpp::callback_group::CallbackGroup::SharedPtr callback_group;
-  rclcpp::node::Node::SharedPtr node;
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base;
 };
 
 }  // namespace executor

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -21,6 +21,7 @@
 #include <memory>
 #include <stdexcept>
 #include <type_traits>
+#include <utility>
 
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/function_traits.hpp"

rclcpp/include/rclcpp/callback_group.hpp

@@ -16,6 +16,7 @@
 #define RCLCPP__CALLBACK_GROUP_HPP_
 
 #include <atomic>
+#include <mutex>
 #include <string>
 #include <vector>
 
@@ -29,10 +30,12 @@ namespace rclcpp
 {
 
 // Forward declarations for friend statement in class CallbackGroup
-namespace node
+namespace node_interfaces
 {
-class Node;
-}  // namespace node
+class NodeServices;
+class NodeTimers;
+class NodeTopics;
+}  // namespace node_interfaces
 
 namespace callback_group
 {
@@ -45,10 +48,12 @@ enum class CallbackGroupType
 
 class CallbackGroup
 {
-  friend class rclcpp::node::Node;
+  friend class rclcpp::node_interfaces::NodeServices;
+  friend class rclcpp::node_interfaces::NodeTimers;
+  friend class rclcpp::node_interfaces::NodeTopics;
 
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup);
+  RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup)
 
   RCLCPP_PUBLIC
   explicit CallbackGroup(CallbackGroupType group_type);
@@ -62,7 +67,7 @@ public:
   get_timer_ptrs() const;
 
   RCLCPP_PUBLIC
-  const std::vector<rclcpp::service::ServiceBase::SharedPtr> &
+  const std::vector<rclcpp::service::ServiceBase::WeakPtr> &
   get_service_ptrs() const;
 
   RCLCPP_PUBLIC
@@ -77,8 +82,8 @@ public:
   const CallbackGroupType &
   type() const;
 
-private:
-  RCLCPP_DISABLE_COPY(CallbackGroup);
+protected:
+  RCLCPP_DISABLE_COPY(CallbackGroup)
 
   RCLCPP_PUBLIC
   void
@@ -97,9 +102,11 @@ private:
   add_client(const rclcpp::client::ClientBase::SharedPtr client_ptr);
 
   CallbackGroupType type_;
+  // Mutex to protect the subsequent vectors of pointers.
+  mutable std::mutex mutex_;
   std::vector<rclcpp::subscription::SubscriptionBase::WeakPtr> subscription_ptrs_;
   std::vector<rclcpp::timer::TimerBase::WeakPtr> timer_ptrs_;
-  std::vector<rclcpp::service::ServiceBase::SharedPtr> service_ptrs_;
+  std::vector<rclcpp::service::ServiceBase::WeakPtr> service_ptrs_;
   std::vector<rclcpp::client::ClientBase::WeakPtr> client_ptrs_;
   std::atomic_bool can_be_taken_from_;
 };

rclcpp/include/rclcpp/client.hpp

@@ -23,27 +23,42 @@
 #include <tuple>
 #include <utility>
 
+#include "rcl/client.h"
+#include "rcl/error_handling.h"
+#include "rcl/wait.h"
+
+#include "rclcpp/exceptions.hpp"
 #include "rclcpp/function_traits.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_graph_interface.hpp"
+#include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/utilities.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/visibility_control.hpp"
+
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 
 namespace rclcpp
 {
+
+namespace node_interfaces
+{
+class NodeBaseInterface;
+}  // namespace node_interfaces
+
 namespace client
 {
 
 class ClientBase
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientBase);
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientBase)
 
   RCLCPP_PUBLIC
   ClientBase(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_client_t * client_handle,
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
     const std::string & service_name);
 
   RCLCPP_PUBLIC
@@ -54,20 +69,51 @@ public:
   get_service_name() const;
 
   RCLCPP_PUBLIC
-  const rmw_client_t *
+  rcl_client_t *
+  get_client_handle();
+
+  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 wait_for_service_nanoseconds(
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
+  }
+
   virtual std::shared_ptr<void> create_response() = 0;
   virtual std::shared_ptr<rmw_request_id_t> create_request_header() = 0;
   virtual void handle_response(
     std::shared_ptr<rmw_request_id_t> request_header, std::shared_ptr<void> response) = 0;
 
-private:
-  RCLCPP_DISABLE_COPY(ClientBase);
+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();
+
+  RCLCPP_PUBLIC
+  const rcl_node_t *
+  get_rcl_node_handle() const;
+
+  rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
+  std::shared_ptr<rcl_node_t> node_handle_;
+
+  rcl_client_t client_handle_ = rcl_get_zero_initialized_client();
   std::string service_name_;
 };
 
@@ -90,28 +136,64 @@ public:
   using CallbackType = std::function<void(SharedFuture)>;
   using CallbackWithRequestType = std::function<void(SharedFutureWithRequest)>;
 
-  RCLCPP_SMART_PTR_DEFINITIONS(Client);
+  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)
-  {}
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
+    const std::string & service_name,
+    rcl_client_options_t & client_options)
+  : ClientBase(node_base, node_graph, service_name)
+  {
+    using rosidl_typesupport_cpp::get_service_type_support_handle;
+    auto service_type_support_handle =
+      get_service_type_support_handle<ServiceT>();
+    rcl_ret_t ret = rcl_client_init(
+      &client_handle_,
+      this->get_rcl_node_handle(),
+      service_type_support_handle,
+      service_name.c_str(),
+      &client_options);
+    if (ret != RCL_RET_OK) {
+      if (ret == RCL_RET_SERVICE_NAME_INVALID) {
+        auto rcl_node_handle = this->get_rcl_node_handle();
+        // this will throw on any validation problem
+        rcl_reset_error();
+        expand_topic_or_service_name(
+          service_name,
+          rcl_node_get_name(rcl_node_handle),
+          rcl_node_get_namespace(rcl_node_handle),
+          true);
+      }
+      rclcpp::exceptions::throw_from_rcl_error(ret, "could not create client");
+    }
+  }
 
-  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 rcl client handle: %s\n", rcl_get_error_string_safe());
+      rcl_reset_error();
+    }
+  }
+
+  std::shared_ptr<void>
+  create_response()
   {
     return std::shared_ptr<void>(new typename ServiceT::Response());
   }
 
-  std::shared_ptr<rmw_request_id_t> 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<rmw_request_id_t>(new rmw_request_id_t);
   }
 
-  void handle_response(std::shared_ptr<rmw_request_id_t> request_header,
+  void
+  handle_response(std::shared_ptr<rmw_request_id_t> request_header,
     std::shared_ptr<void> response)
   {
     std::lock_guard<std::mutex> lock(pending_requests_mutex_);
@@ -131,7 +213,8 @@ public:
     callback(future);
   }
 
-  SharedFuture async_send_request(SharedRequest request)
+  SharedFuture
+  async_send_request(SharedRequest request)
   {
     return async_send_request(request, [](SharedFuture) {});
   }
@@ -145,15 +228,14 @@ public:
       >::value
     >::type * = nullptr
   >
-  SharedFuture async_send_request(SharedRequest request, CallbackT && cb)
+  SharedFuture
+  async_send_request(SharedRequest request, CallbackT && cb)
   {
     std::lock_guard<std::mutex> lock(pending_requests_mutex_);
     int64_t sequence_number;
-    if (RMW_RET_OK != rmw_send_request(get_client_handle(), request.get(), &sequence_number)) {
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-      throw std::runtime_error(
-        std::string("failed to send request: ") + rmw_get_error_string_safe());
-      // *INDENT-ON*
+    rcl_ret_t ret = rcl_send_request(get_client_handle(), request.get(), &sequence_number);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");
     }
 
     SharedPromise call_promise = std::make_shared<Promise>();
@@ -172,7 +254,8 @@ public:
       >::value
     >::type * = nullptr
   >
-  SharedFutureWithRequest async_send_request(SharedRequest request, CallbackT && cb)
+  SharedFutureWithRequest
+  async_send_request(SharedRequest request, CallbackT && cb)
   {
     SharedPromiseWithRequest promise = std::make_shared<PromiseWithRequest>();
     SharedFutureWithRequest future_with_request(promise->get_future());
@@ -189,7 +272,7 @@ public:
   }
 
 private:
-  RCLCPP_DISABLE_COPY(Client);
+  RCLCPP_DISABLE_COPY(Client)
 
   std::map<int64_t, std::tuple<SharedPromise, CallbackType, SharedFuture>> pending_requests_;
   std::mutex pending_requests_mutex_;

rclcpp/include/rclcpp/context.hpp

@@ -21,6 +21,7 @@
 #include <typeindex>
 #include <typeinfo>
 #include <unordered_map>
+#include <utility>
 
 #include "rclcpp/macros.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -35,7 +36,7 @@ namespace context
 class Context
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(Context);
+  RCLCPP_SMART_PTR_DEFINITIONS(Context)
 
   RCLCPP_PUBLIC
   Context();
@@ -51,7 +52,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) {
@@ -67,7 +68,7 @@ public:
   }
 
 private:
-  RCLCPP_DISABLE_COPY(Context);
+  RCLCPP_DISABLE_COPY(Context)
 
   std::unordered_map<std::type_index, std::shared_ptr<void>> sub_contexts_;
   std::mutex mutex_;

rclcpp/include/rclcpp/contexts/default_context.hpp

@@ -28,7 +28,7 @@ namespace default_context
 class DefaultContext : public rclcpp::context::Context
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(DefaultContext);
+  RCLCPP_SMART_PTR_DEFINITIONS(DefaultContext)
 
   RCLCPP_PUBLIC
   DefaultContext();

rclcpp/include/rclcpp/create_publisher.hpp

@@ -0,0 +1,51 @@
+// 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__CREATE_PUBLISHER_HPP_
+#define RCLCPP__CREATE_PUBLISHER_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/publisher_factory.hpp"
+#include "rmw/qos_profiles.h"
+
+namespace rclcpp
+{
+
+template<typename MessageT, typename AllocatorT, typename PublisherT>
+std::shared_ptr<PublisherT>
+create_publisher(
+  rclcpp::node_interfaces::NodeTopicsInterface * node_topics,
+  const std::string & topic_name,
+  const rmw_qos_profile_t & qos_profile,
+  bool use_intra_process_comms,
+  std::shared_ptr<AllocatorT> allocator)
+{
+  auto publisher_options = rcl_publisher_get_default_options();
+  publisher_options.qos = qos_profile;
+
+  auto pub = node_topics->create_publisher(
+    topic_name,
+    rclcpp::create_publisher_factory<MessageT, AllocatorT, PublisherT>(allocator),
+    publisher_options,
+    use_intra_process_comms);
+  node_topics->add_publisher(pub);
+  return std::dynamic_pointer_cast<PublisherT>(pub);
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CREATE_PUBLISHER_HPP_

rclcpp/include/rclcpp/create_subscription.hpp

@@ -0,0 +1,62 @@
+// 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__CREATE_SUBSCRIPTION_HPP_
+#define RCLCPP__CREATE_SUBSCRIPTION_HPP_
+
+#include <memory>
+#include <string>
+#include <utility>
+
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/subscription_factory.hpp"
+#include "rmw/qos_profiles.h"
+
+namespace rclcpp
+{
+
+template<typename MessageT, typename CallbackT, typename AllocatorT, typename SubscriptionT>
+typename rclcpp::subscription::Subscription<MessageT, AllocatorT>::SharedPtr
+create_subscription(
+  rclcpp::node_interfaces::NodeTopicsInterface * node_topics,
+  const std::string & topic_name,
+  CallbackT && callback,
+  const rmw_qos_profile_t & qos_profile,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group,
+  bool ignore_local_publications,
+  bool use_intra_process_comms,
+  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, AllocatorT>::SharedPtr
+  msg_mem_strat,
+  typename std::shared_ptr<AllocatorT> allocator)
+{
+  auto subscription_options = rcl_subscription_get_default_options();
+  subscription_options.qos = qos_profile;
+  subscription_options.ignore_local_publications = ignore_local_publications;
+
+  auto factory =
+    rclcpp::create_subscription_factory<MessageT, CallbackT, AllocatorT, SubscriptionT>(
+    std::forward<CallbackT>(callback), msg_mem_strat, allocator);
+
+  auto sub = node_topics->create_subscription(
+    topic_name,
+    factory,
+    subscription_options,
+    use_intra_process_comms);
+  node_topics->add_subscription(sub, group);
+  return std::dynamic_pointer_cast<SubscriptionT>(sub);
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CREATE_SUBSCRIPTION_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,187 @@
+// 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") {}
+};
+
+/// Thrown when a any kind of name (node, namespace, topic, etc.) is invalid.
+class NameValidationError : public std::invalid_argument
+{
+public:
+  NameValidationError(
+    const char * name_type_,
+    const char * name_,
+    const char * error_msg_,
+    size_t invalid_index_)
+  : std::invalid_argument(format_error(name_type_, name_, error_msg_, invalid_index_)),
+    name_type(name_type_), name(name_), error_msg(error_msg_), invalid_index(invalid_index_)
+  {}
+
+  static std::string
+  format_error(
+    const char * name_type,
+    const char * name,
+    const char * error_msg,
+    size_t invalid_index);
+
+  const std::string name_type;
+  const std::string name;
+  const std::string error_msg;
+  const size_t invalid_index;
+};
+
+/// Thrown when a node name is invalid.
+class InvalidNodeNameError : public NameValidationError
+{
+public:
+  InvalidNodeNameError(const char * node_name, const char * error_msg, size_t invalid_index)
+  : NameValidationError("node name", node_name, error_msg, invalid_index)
+  {}
+};
+
+/// Thrown when a node namespace is invalid.
+class InvalidNamespaceError : public NameValidationError
+{
+public:
+  InvalidNamespaceError(const char * namespace_, const char * error_msg, size_t invalid_index)
+  : NameValidationError("namespace", namespace_, error_msg, invalid_index)
+  {}
+};
+
+/// Thrown when a topic name is invalid.
+class InvalidTopicNameError : public NameValidationError
+{
+public:
+  InvalidTopicNameError(const char * namespace_, const char * error_msg, size_t invalid_index)
+  : NameValidationError("topic name", namespace_, error_msg, invalid_index)
+  {}
+};
+
+/// Thrown when a service name is invalid.
+class InvalidServiceNameError : public NameValidationError
+{
+public:
+  InvalidServiceNameError(const char * namespace_, const char * error_msg, size_t invalid_index)
+  : NameValidationError("service name", namespace_, error_msg, invalid_index)
+  {}
+};
+
+/// Throw a C++ std::exception which was created based on an rcl error.
+/**
+ * Passing nullptr for reset_error is safe and will avoid calling any function
+ * to reset the error.
+ *
+ * \param ret the return code for the current error state
+ * \param prefix string to prefix to the error if applicable (not all errors have custom messages)
+ * \param error_state error state to create exception from, if nullptr rcl_get_error_state is used
+ * \param reset_error function to be called before throwing which whill clear the error state
+ * \throws std::invalid_argument if ret is RCL_RET_OK
+ * \throws std::runtime_error if the rcl_get_error_state returns 0
+ * \throws RCLErrorBase some child class exception based on ret
+ */
+RCLCPP_PUBLIC
+void
+throw_from_rcl_error(
+  rcl_ret_t ret,
+  const std::string & prefix = "",
+  const rcl_error_state_t * error_state = nullptr,
+  void (* reset_error)() = rcl_reset_error);
+
+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);
+};
+
+/// Thrown when an invalid rclcpp::Event object or SharedPtr is encountered.
+class InvalidEventError : public std::runtime_error
+{
+public:
+  InvalidEventError()
+  : std::runtime_error("event is invalid") {}
+};
+
+/// Thrown when an unregistered rclcpp::Event is encountered where a registered one was expected.
+class EventNotRegisteredError : public std::runtime_error
+{
+public:
+  EventNotRegisteredError()
+  : std::runtime_error("event already registered") {}
+};
+
+}  // namespace exceptions
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXCEPTIONS_HPP_

rclcpp/include/rclcpp/executor.hpp

@@ -25,16 +25,24 @@
 #include <string>
 #include <vector>
 
-#include "rclcpp/any_executable.hpp"
-#include "rclcpp/macros.hpp"
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
+
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/memory_strategies.hpp"
 #include "rclcpp/memory_strategy.hpp"
-#include "rclcpp/node.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
+
+// Forward declaration is used in convenience method signature.
+namespace node
+{
+class Node;
+}  // namespace node
+
 namespace executor
 {
 
@@ -85,7 +93,7 @@ static inline ExecutorArgs create_default_executor_arguments()
 class Executor
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Executor);
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Executor)
 
   /// Default constructor.
   // \param[in] ms The memory strategy to be used with this executor.
@@ -111,7 +119,12 @@ public:
    */
   RCLCPP_PUBLIC
   virtual void
-  add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true);
+  add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true);
+
+  /// Convenience function which takes Node and forwards NodeBaseInterface.
+  RCLCPP_PUBLIC
+  virtual void
+  add_node(std::shared_ptr<rclcpp::node::Node> node_ptr, bool notify = true);
 
   /// Remove a node from the executor.
   /**
@@ -122,7 +135,12 @@ public:
    */
   RCLCPP_PUBLIC
   virtual void
-  remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true);
+  remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true);
+
+  /// Convenience function which takes Node and forwards NodeBaseInterface.
+  RCLCPP_PUBLIC
+  virtual void
+  remove_node(std::shared_ptr<rclcpp::node::Node> node_ptr, bool notify = true);
 
   /// Add a node to executor, execute the next available unit of work, and remove the node.
   /**
@@ -133,7 +151,7 @@ public:
    */
   template<typename T = std::milli>
   void
-  spin_node_once(rclcpp::node::Node::SharedPtr node,
+  spin_node_once(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
     std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
   {
     return spin_node_once_nanoseconds(
@@ -142,13 +160,30 @@ public:
     );
   }
 
+  /// Convenience function which takes Node and forwards NodeBaseInterface.
+  template<typename NodeT = rclcpp::node::Node, typename T = std::milli>
+  void
+  spin_node_once(std::shared_ptr<NodeT> node,
+    std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
+  {
+    return spin_node_once_nanoseconds(
+      node->get_node_base_interface(),
+      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.
    */
   RCLCPP_PUBLIC
   void
-  spin_node_some(rclcpp::node::Node::SharedPtr node);
+  spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node);
+
+  /// Convenience function which takes Node and forwards NodeBaseInterface.
+  RCLCPP_PUBLIC
+  void
+  spin_node_some(std::shared_ptr<rclcpp::node::Node> node);
 
   /// Complete all available queued work without blocking.
   /**
@@ -167,14 +202,13 @@ public:
 
   /// 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.
+   *   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.
+   *   `-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
@@ -193,10 +227,12 @@ public:
     }
 
     auto end_time = std::chrono::steady_clock::now();
-    if (timeout > std::chrono::nanoseconds::zero()) {
-      end_time += timeout;
+    std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+      timeout);
+    if (timeout_ns > std::chrono::nanoseconds::zero()) {
+      end_time += timeout_ns;
     }
-    auto timeout_left = timeout;
+    std::chrono::nanoseconds timeout_left = timeout_ns;
 
     while (rclcpp::utilities::ok()) {
       // Do one item of work.
@@ -207,7 +243,7 @@ public:
         return FutureReturnCode::SUCCESS;
       }
       // If the original timeout is < 0, then this is blocking, never TIMEOUT.
-      if (timeout < std::chrono::nanoseconds::zero()) {
+      if (timeout_ns < std::chrono::nanoseconds::zero()) {
         continue;
       }
       // Otherwise check if we still have time to wait, return TIMEOUT if not.
@@ -216,8 +252,7 @@ public:
         return FutureReturnCode::TIMEOUT;
       }
       // Subtract the elapsed time from the original timeout.
-      using duration_type = std::chrono::duration<int64_t, TimeT>;
-      timeout_left = std::chrono::duration_cast<duration_type>(end_time - now);
+      timeout_left = std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - now);
     }
 
     // The future did not complete before ok() returned false, return INTERRUPTED.
@@ -243,7 +278,9 @@ public:
 protected:
   RCLCPP_PUBLIC
   void
-  spin_node_once_nanoseconds(rclcpp::node::Node::SharedPtr node, std::chrono::nanoseconds timeout);
+  spin_node_once_nanoseconds(
+    rclcpp::node_interfaces::NodeBaseInterface::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,
@@ -280,7 +317,7 @@ protected:
   wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
   RCLCPP_PUBLIC
-  rclcpp::node::Node::SharedPtr
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
   get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group);
 
   RCLCPP_PUBLIC
@@ -291,10 +328,6 @@ protected:
   void
   get_next_timer(AnyExecutable::SharedPtr any_exec);
 
-  RCLCPP_PUBLIC
-  std::chrono::nanoseconds
-  get_earliest_timer();
-
   RCLCPP_PUBLIC
   AnyExecutable::SharedPtr
   get_next_ready_executable();
@@ -307,18 +340,18 @@ protected:
   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.
-  rmw_waitset_t * waitset_;
+  rcl_wait_set_t waitset_ = rcl_get_zero_initialized_wait_set();
 
   /// The memory strategy: an interface for handling user-defined memory allocation strategies.
   memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;
 
 private:
-  RCLCPP_DISABLE_COPY(Executor);
+  RCLCPP_DISABLE_COPY(Executor)
 
-  std::vector<std::weak_ptr<rclcpp::node::Node>> weak_nodes_;
+  std::vector<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
 };
 
 }  // namespace executor

rclcpp/include/rclcpp/executors.hpp

@@ -16,6 +16,7 @@
 #define RCLCPP__EXECUTORS_HPP_
 
 #include <future>
+#include <memory>
 
 #include "rclcpp/executors/multi_threaded_executor.hpp"
 #include "rclcpp/executors/single_threaded_executor.hpp"
@@ -27,16 +28,24 @@ 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.
+/** \param[in] node_ptr Shared pointer to the node to spin. */
 RCLCPP_PUBLIC
 void
-spin_some(node::Node::SharedPtr node_ptr);
+spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+RCLCPP_PUBLIC
+void
+spin_some(rclcpp::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.
+/** \param[in] node_ptr Shared pointer to the node to spin. */
 RCLCPP_PUBLIC
 void
-spin(node::Node::SharedPtr node_ptr);
+spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+RCLCPP_PUBLIC
+void
+spin(rclcpp::node::Node::SharedPtr node_ptr);
 
 namespace executors
 {
@@ -48,16 +57,19 @@ using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor;
 /**
  * \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.
+ * \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>
 rclcpp::executor::FutureReturnCode
 spin_node_until_future_complete(
-  rclcpp::executor::Executor & executor, rclcpp::node::Node::SharedPtr node_ptr,
+  rclcpp::executor::Executor & executor,
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
   std::shared_future<ResponseT> & future,
   std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
 {
@@ -69,18 +81,44 @@ spin_node_until_future_complete(
   return retcode;
 }
 
+template<typename NodeT = rclcpp::node::Node, typename ResponseT, typename TimeT = std::milli>
+rclcpp::executor::FutureReturnCode
+spin_node_until_future_complete(
+  rclcpp::executor::Executor & executor,
+  std::shared_ptr<NodeT> node_ptr,
+  std::shared_future<ResponseT> & future,
+  std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
+{
+  return rclcpp::executors::spin_node_until_future_complete(
+    executor,
+    node_ptr->get_node_base_interface(),
+    future,
+    timeout);
+}
+
 }  // namespace executors
 
 template<typename FutureT, typename TimeT = std::milli>
 rclcpp::executor::FutureReturnCode
 spin_until_future_complete(
-  node::Node::SharedPtr node_ptr, std::shared_future<FutureT> & future,
+  rclcpp::node_interfaces::NodeBaseInterface::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);
 }
 
+template<typename NodeT = rclcpp::node::Node, typename FutureT, typename TimeT = std::milli>
+rclcpp::executor::FutureReturnCode
+spin_until_future_complete(
+  std::shared_ptr<NodeT> node_ptr,
+  std::shared_future<FutureT> & future,
+  std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
+{
+  return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout);
+}
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__EXECUTORS_HPP_

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -34,7 +34,7 @@ namespace multi_threaded_executor
 class MultiThreadedExecutor : public executor::Executor
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(MultiThreadedExecutor);
+  RCLCPP_SMART_PTR_DEFINITIONS(MultiThreadedExecutor)
 
   RCLCPP_PUBLIC
   MultiThreadedExecutor(
@@ -57,7 +57,7 @@ protected:
   run(size_t this_thread_number);
 
 private:
-  RCLCPP_DISABLE_COPY(MultiThreadedExecutor);
+  RCLCPP_DISABLE_COPY(MultiThreadedExecutor)
 
   std::mutex wait_mutex_;
   size_t number_of_threads_;

rclcpp/include/rclcpp/executors/single_threaded_executor.hpp

@@ -42,7 +42,7 @@ namespace single_threaded_executor
 class SingleThreadedExecutor : public executor::Executor
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(SingleThreadedExecutor);
+  RCLCPP_SMART_PTR_DEFINITIONS(SingleThreadedExecutor)
 
   /// Default constructor. See the default constructor for Executor.
   RCLCPP_PUBLIC
@@ -61,7 +61,7 @@ public:
   spin();
 
 private:
-  RCLCPP_DISABLE_COPY(SingleThreadedExecutor);
+  RCLCPP_DISABLE_COPY(SingleThreadedExecutor)
 };
 
 }  // namespace single_threaded_executor

rclcpp/include/rclcpp/expand_topic_or_service_name.hpp

@@ -0,0 +1,63 @@
+// Copyright 2017 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__EXPAND_TOPIC_OR_SERVICE_NAME_HPP_
+#define RCLCPP__EXPAND_TOPIC_OR_SERVICE_NAME_HPP_
+
+#include <string>
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Expand a topic or service name and throw if it is not valid.
+/**
+ * This function can be used to "just" validate a topic or service name too,
+ * since expanding the topic name is required to fully validate a name.
+ *
+ * If the name is invalid, then InvalidTopicNameError is thrown or
+ * InvalidServiceNameError if is_service is true.
+ *
+ * This function can take any form of a topic or service name, i.e. it does not
+ * have to be a fully qualified name.
+ * The node name and namespace are used to expand it if necessary while
+ * validating it.
+ *
+ * Expansion is done with rcl_expand_topic_name.
+ * The validation is doen with rcl_validate_topic_name and
+ * rmw_validate_full_topic_name, so details about failures can be found in the
+ * documentation for those functions.
+ *
+ * \param name the topic or service name to be validated
+ * \param node_name the name of the node associated with the name
+ * \param namespace_ the namespace of the node associated with the name
+ * \param is_service if true InvalidServiceNameError is thrown instead
+ * \returns expanded (and validated) topic name
+ * \throws InvalidTopicNameError if name is invalid and is_service is false
+ * \throws InvalidServiceNameError if name is invalid and is_service is true
+ * \throws std::bad_alloc if memory cannot be allocated
+ * \throws RCLError if an unexpect error occurs
+ */
+RCLCPP_PUBLIC
+std::string
+expand_topic_or_service_name(
+  const std::string & name,
+  const std::string & node_name,
+  const std::string & namespace_,
+  bool is_service = false);
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPAND_TOPIC_OR_SERVICE_NAME_HPP_

rclcpp/include/rclcpp/function_traits.hpp

@@ -82,6 +82,8 @@ struct function_traits<ReturnTypeT (*)(Args ...)>: function_traits<ReturnTypeT(A
 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_RELEASE  // glibc++ (GNU C++ >= 7.1)
+struct function_traits<std::_Bind<ReturnTypeT(ClassT::*(ClassT *, FArgs ...))(Args ...)>>
 #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

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/node_interfaces/node_graph_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+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_interfaces::NodeGraphInterface * node_graph);
+
+  /// 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_interfaces::NodeGraphInterface * node_graph);
+
+  /// 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_interfaces::NodeGraphInterface * node_graph);
+
+  /// 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 node_graph_interfaces_barrier_mutex_;
+  mutable std::mutex node_graph_interfaces_mutex_;
+  std::vector<rclcpp::node_interfaces::NodeGraphInterface *> node_graph_interfaces_;
+
+  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

@@ -23,6 +23,7 @@
 #include <exception>
 #include <map>
 #include <memory>
+#include <mutex>
 #include <unordered_map>
 #include <set>
 
@@ -40,7 +41,8 @@ namespace intra_process_manager
 {
 
 /// This class facilitates intra process communication between nodes.
-/* This class is used in the creation of publishers and subscriptions.
+/**
+ * This class is used in the creation of publishers and subscriptions.
  * A singleton instance of this class is owned by a rclcpp::Context and a
  * rclcpp::Node can use an associated Context to get an instance of this class.
  * Nodes which do not have a common Context will not exchange intra process
@@ -54,7 +56,7 @@ namespace intra_process_manager
  * When a publisher is created, it advertises on the topic the user provided,
  * as well as a "shadowing" topic of type rcl_interfaces/IntraProcessMessage.
  * For instance, if the user specified the topic '/namespace/chatter', then the
- * corresponding intra process topic might be '/namespace/chatter__intra'.
+ * corresponding intra process topic might be '/namespace/chatter/_intra'.
  * The publisher is also allocated an id which is unique among all publishers
  * and subscriptions in this process.
  * Additionally, when registered with this class a ring buffer is created and
@@ -120,10 +122,10 @@ namespace intra_process_manager
 class IntraProcessManager
 {
 private:
-  RCLCPP_DISABLE_COPY(IntraProcessManager);
+  RCLCPP_DISABLE_COPY(IntraProcessManager)
 
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager);
+  RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager)
 
   RCLCPP_PUBLIC
   explicit IntraProcessManager(
@@ -133,7 +135,8 @@ 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,
+  /**
+   * In addition to generating a unique intra process id for the subscription,
    * this method also stores the topic name of the subscription.
    *
    * This method is normally called during the creation of a subscription,
@@ -149,7 +152,8 @@ public:
   add_subscription(subscription::SubscriptionBase::SharedPtr subscription);
 
   /// Unregister a subscription using the subscription's unique id.
-  /* This method does not allocate memory.
+  /**
+   * This method does not allocate memory.
    *
    * \param intra_process_subscription_id id of the subscription to remove.
    */
@@ -158,7 +162,8 @@ public:
   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,
+  /**
+   * In addition to generating and returning a unique id for the publisher,
    * this method creates internal ring buffer storage for "in-flight" intra
    * process messages which are stored when store_intra_process_message is
    * called with this publisher's unique id.
@@ -194,7 +199,8 @@ public:
   }
 
   /// Unregister a publisher using the publisher's unique id.
-  /* This method does not allocate memory.
+  /**
+   * This method does not allocate memory.
    *
    * \param intra_process_publisher_id id of the publisher to remove.
    */
@@ -203,7 +209,8 @@ public:
   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
+  /**
+   * The given message is stored in internal storage using the given publisher
    * id and the newly generated message sequence, which is also returned.
    * The combination of publisher id and message sequence number can later
    * be used with a subscription id to retrieve the message by calling
@@ -261,7 +268,8 @@ public:
   }
 
   /// Take an intra process message.
-  /* The intra_process_publisher_id and message_sequence_number parameters
+  /**
+   * The intra_process_publisher_id and message_sequence_number parameters
    * uniquely identify a message instance, which should be taken.
    *
    * The requesting_subscriptions_intra_process_id parameter is used to make
@@ -308,6 +316,7 @@ public:
     message = nullptr;
 
     size_t target_subs_size = 0;
+    std::lock_guard<std::mutex> lock(take_mutex_);
     mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = impl_->take_intra_process_message(
       intra_process_publisher_id,
       message_sequence_number,
@@ -339,6 +348,7 @@ private:
   get_next_unique_id();
 
   IntraProcessManagerImplBase::SharedPtr impl_;
+  std::mutex take_mutex_;
 };
 
 }  // namespace intra_process_manager

rclcpp/include/rclcpp/intra_process_manager_impl.hpp

@@ -17,6 +17,7 @@
 
 #include <algorithm>
 #include <atomic>
+#include <cstring>
 #include <functional>
 #include <limits>
 #include <map>
@@ -41,7 +42,7 @@ namespace intra_process_manager
 class IntraProcessManagerImplBase
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(IntraProcessManagerImplBase);
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(IntraProcessManagerImplBase)
 
   IntraProcessManagerImplBase() = default;
   ~IntraProcessManagerImplBase() = default;
@@ -78,7 +79,7 @@ public:
   matches_any_publishers(const rmw_gid_t * id) const = 0;
 
 private:
-  RCLCPP_DISABLE_COPY(IntraProcessManagerImplBase);
+  RCLCPP_DISABLE_COPY(IntraProcessManagerImplBase)
 };
 
 template<typename Allocator = std::allocator<void>>
@@ -92,6 +93,8 @@ public:
   add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription)
   {
     subscriptions_[id] = subscription;
+    // subscription->get_topic_name() -> const char * can be used as the key,
+    // since subscriptions_ shares the ownership of subscription
     subscription_ids_by_topic_[subscription->get_topic_name()].insert(id);
   }
 
@@ -242,7 +245,7 @@ public:
   }
 
 private:
-  RCLCPP_DISABLE_COPY(IntraProcessManagerImpl);
+  RCLCPP_DISABLE_COPY(IntraProcessManagerImpl)
 
   template<typename T>
   using RebindAlloc = typename std::allocator_traits<Allocator>::template rebind_alloc<T>;
@@ -253,8 +256,20 @@ private:
   using SubscriptionMap = std::unordered_map<uint64_t, subscription::SubscriptionBase::WeakPtr,
       std::hash<uint64_t>, std::equal_to<uint64_t>,
       RebindAlloc<std::pair<const uint64_t, subscription::SubscriptionBase::WeakPtr>>>;
-  using IDTopicMap = std::map<std::string, AllocSet,
-      std::less<std::string>, RebindAlloc<std::pair<std::string, AllocSet>>>;
+
+  struct strcmp_wrapper : public std::binary_function<const char *, const char *, bool>
+  {
+    bool
+    operator()(const char * lhs, const char * rhs) const
+    {
+      return std::strcmp(lhs, rhs) < 0;
+    }
+  };
+  using IDTopicMap = std::map<
+      const char *,
+      AllocSet,
+      strcmp_wrapper,
+      RebindAlloc<std::pair<const std::string, AllocSet>>>;
 
   SubscriptionMap subscriptions_;
 
@@ -262,7 +277,7 @@ private:
 
   struct PublisherInfo
   {
-    RCLCPP_DISABLE_COPY(PublisherInfo);
+    RCLCPP_DISABLE_COPY(PublisherInfo)
 
     PublisherInfo() = default;
 

rclcpp/include/rclcpp/macros.hpp

@@ -12,10 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <memory>
+#include <utility>
+
 #ifndef RCLCPP__MACROS_HPP_
 #define RCLCPP__MACROS_HPP_
 
-/* Disables the copy constructor and operator= for the given class.
+/**
+ * Disables the copy constructor and operator= for the given class.
  *
  * Use in the private section of the class.
  */
@@ -23,31 +27,50 @@
   __VA_ARGS__(const __VA_ARGS__ &) = delete; \
   __VA_ARGS__ & operator=(const __VA_ARGS__ &) = delete;
 
-/* Defines aliases and static functions for using the Class with smart pointers.
+/**
+ * Defines aliases and static functions for using the Class with smart pointers.
  *
  * Use in the public section of the class.
- * Make sure to include <memory> in the header when using this.
+ * Make sure to include `<memory>` in the header when using this.
  */
 #define RCLCPP_SMART_PTR_DEFINITIONS(...) \
   RCLCPP_SHARED_PTR_DEFINITIONS(__VA_ARGS__) \
   RCLCPP_WEAK_PTR_DEFINITIONS(__VA_ARGS__) \
   RCLCPP_UNIQUE_PTR_DEFINITIONS(__VA_ARGS__)
 
-/* Defines aliases and static functions for using the Class with smart pointers.
+/**
+ * Defines aliases and static functions for using the Class with smart pointers.
  *
  * Same as RCLCPP_SMART_PTR_DEFINITIONS expect it excludes the static
  * Class::make_unique() method definition which does not work on classes which
  * are not CopyConstructable.
  *
  * Use in the public section of the class.
- * Make sure to include <memory> in the header when using this.
+ * Make sure to include `<memory>` in the header when using this.
  */
 #define RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(...) \
   RCLCPP_SHARED_PTR_DEFINITIONS(__VA_ARGS__) \
   RCLCPP_WEAK_PTR_DEFINITIONS(__VA_ARGS__) \
   __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__)
 
-#define __RCLCPP_SHARED_PTR_ALIAS(...) using SharedPtr = std::shared_ptr<__VA_ARGS__>;
+/**
+ * Defines aliases only for using the Class with smart pointers.
+ *
+ * Same as RCLCPP_SMART_PTR_DEFINITIONS expect it excludes the static
+ * method definitions which do not work on pure virtual classes and classes
+ * which are not CopyConstructable.
+ *
+ * Use in the public section of the class.
+ * Make sure to include `<memory>` in the header when using this.
+ */
+#define RCLCPP_SMART_PTR_ALIASES_ONLY(...) \
+  __RCLCPP_SHARED_PTR_ALIAS(__VA_ARGS__) \
+  __RCLCPP_WEAK_PTR_ALIAS(__VA_ARGS__) \
+  __RCLCPP_MAKE_SHARED_DEFINITION(__VA_ARGS__)
+
+#define __RCLCPP_SHARED_PTR_ALIAS(...) \
+  using SharedPtr = std::shared_ptr<__VA_ARGS__>; \
+  using ConstSharedPtr = std::shared_ptr<const __VA_ARGS__>;
 
 #define __RCLCPP_MAKE_SHARED_DEFINITION(...) \
   template<typename ... Args> \
@@ -62,7 +85,9 @@
   __RCLCPP_SHARED_PTR_ALIAS(__VA_ARGS__) \
   __RCLCPP_MAKE_SHARED_DEFINITION(__VA_ARGS__)
 
-#define __RCLCPP_WEAK_PTR_ALIAS(...) using WeakPtr = std::weak_ptr<__VA_ARGS__>;
+#define __RCLCPP_WEAK_PTR_ALIAS(...) \
+  using WeakPtr = std::weak_ptr<__VA_ARGS__>; \
+  using ConstWeakPtr = std::weak_ptr<const __VA_ARGS__>;
 
 /// Defines aliases and static functions for using the Class with weak_ptrs.
 #define RCLCPP_WEAK_PTR_DEFINITIONS(...) __RCLCPP_WEAK_PTR_ALIAS(__VA_ARGS__)

rclcpp/include/rclcpp/mapped_ring_buffer.hpp

@@ -20,6 +20,7 @@
 #include <cstdint>
 #include <memory>
 #include <mutex>
+#include <utility>
 #include <vector>
 
 #include "rclcpp/allocator/allocator_common.hpp"
@@ -34,11 +35,12 @@ namespace mapped_ring_buffer
 class RCLCPP_PUBLIC MappedRingBufferBase
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBufferBase);
+  RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBufferBase)
 };
 
 /// Ring buffer container of unique_ptr's of T, which can be accessed by a key.
-/* T must be a CopyConstructable and CopyAssignable.
+/**
+ * T must be a CopyConstructable and CopyAssignable.
  * This class can be used in a container by using the base class MappedRingBufferBase.
  * This class must have a positive, non-zero size.
  * This class cannot be resized nor can it reserve additional space after construction.
@@ -57,7 +59,7 @@ template<typename T, typename Alloc = std::allocator<void>>
 class MappedRingBuffer : public MappedRingBufferBase
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBuffer<T, Alloc>);
+  RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBuffer<T, Alloc>)
   using ElemAllocTraits = allocator::AllocRebind<T, Alloc>;
   using ElemAlloc = typename ElemAllocTraits::allocator_type;
   using ElemDeleter = allocator::Deleter<ElemAlloc, T>;
@@ -65,9 +67,11 @@ public:
   using ElemUniquePtr = std::unique_ptr<T, ElemDeleter>;
 
   /// Constructor.
-  /* The constructor will allocate memory while reserving space.
+  /**
+   * The constructor will allocate memory while reserving space.
    *
    * \param size size of the ring buffer; must be positive and non-zero.
+   * \param allocator optional custom allocator
    */
   explicit MappedRingBuffer(size_t size, std::shared_ptr<Alloc> allocator = nullptr)
   : elements_(size), head_(0)
@@ -85,7 +89,8 @@ public:
   virtual ~MappedRingBuffer() {}
 
   /// Return a copy of the value stored in the ring buffer at the given key.
-  /* The key is matched if an element in the ring buffer has a matching key.
+  /**
+   * The key is matched if an element in the ring buffer has a matching key.
    * This method will allocate in order to return a copy.
    *
    * The key is not guaranteed to be unique, see the class docs for more.
@@ -109,7 +114,8 @@ public:
   }
 
   /// Return ownership of the value stored in the ring buffer, leaving a copy.
-  /* The key is matched if an element in the ring bufer has a matching key.
+  /**
+   * The key is matched if an element in the ring bufer has a matching key.
    * This method will allocate in order to store a copy.
    *
    * The key is not guaranteed to be unique, see the class docs for more.
@@ -146,7 +152,8 @@ public:
   }
 
   /// Return ownership of the value stored in the ring buffer at the given key.
-  /* The key is matched if an element in the ring buffer has a matching key.
+  /**
+   * The key is matched if an element in the ring buffer has a matching key.
    *
    * The key is not guaranteed to be unique, see the class docs for more.
    *
@@ -168,7 +175,8 @@ public:
   }
 
   /// Insert a key-value pair, displacing an existing pair if necessary.
-  /* The key's uniqueness is not checked on insertion.
+  /**
+   * The key's uniqueness is not checked on insertion.
    * It is up to the user to ensure the key is unique.
    * This method should not allocate memory.
    *
@@ -206,7 +214,7 @@ public:
   }
 
 private:
-  RCLCPP_DISABLE_COPY(MappedRingBuffer<T, Alloc>);
+  RCLCPP_DISABLE_COPY(MappedRingBuffer<T, Alloc>)
 
   struct element
   {
@@ -220,7 +228,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;
     });

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -18,9 +18,12 @@
 #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/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -37,34 +40,28 @@ namespace memory_strategy
 class RCLCPP_PUBLIC MemoryStrategy
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy);
-  using WeakNodeVector = std::vector<std::weak_ptr<rclcpp::node::Node>>;
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy)
+  using WeakNodeVector = std::vector<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>;
 
-  // 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;
-
-  // return the new number of guard_conditions
-  virtual size_t fill_guard_condition_handles(void ** & ptr) = 0;
-
-  virtual void clear_active_entities() = 0;
-
-  virtual void clear_handles() = 0;
-  virtual void remove_null_handles() = 0;
   virtual bool collect_entities(const WeakNodeVector & weak_nodes) = 0;
 
+  virtual size_t number_of_ready_subscriptions() const = 0;
+  virtual size_t number_of_ready_services() const = 0;
+  virtual size_t number_of_ready_clients() const = 0;
+  virtual size_t number_of_ready_timers() const = 0;
+  virtual size_t number_of_guard_conditions() const = 0;
+
+  virtual bool add_handles_to_waitset(rcl_wait_set_t * wait_set) = 0;
+  virtual void clear_handles() = 0;
+  virtual void remove_null_handles(rcl_wait_set_t * wait_set) = 0;
+
   /// Provide a newly initialized AnyExecutable object.
   // \return Shared pointer to the fresh executable.
   virtual rclcpp::executor::AnyExecutable::SharedPtr instantiate_next_executable() = 0;
 
-  virtual void add_guard_condition(const rmw_guard_condition_t * guard_condition) = 0;
+  virtual void add_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
 
-  virtual void remove_guard_condition(const rmw_guard_condition_t * guard_condition) = 0;
+  virtual void remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
 
   virtual void
   get_next_subscription(rclcpp::executor::AnyExecutable::SharedPtr any_exec,
@@ -78,17 +75,20 @@ public:
   get_next_client(rclcpp::executor::AnyExecutable::SharedPtr any_exec,
     const WeakNodeVector & weak_nodes) = 0;
 
+  virtual rcl_allocator_t
+  get_allocator() = 0;
+
   static rclcpp::subscription::SubscriptionBase::SharedPtr
-  get_subscription_by_handle(void * subscriber_handle,
+  get_subscription_by_handle(const rcl_subscription_t * subscriber_handle,
     const WeakNodeVector & weak_nodes);
 
   static rclcpp::service::ServiceBase::SharedPtr
-  get_service_by_handle(void * service_handle, const WeakNodeVector & weak_nodes);
+  get_service_by_handle(const rcl_service_t * service_handle, const WeakNodeVector & weak_nodes);
 
   static rclcpp::client::ClientBase::SharedPtr
-  get_client_by_handle(void * client_handle, const WeakNodeVector & weak_nodes);
+  get_client_by_handle(const rcl_client_t * client_handle, const WeakNodeVector & weak_nodes);
 
-  static rclcpp::node::Node::SharedPtr
+  static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
   get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group,
     const WeakNodeVector & weak_nodes);
 

rclcpp/include/rclcpp/message_memory_strategy.hpp

@@ -28,12 +28,12 @@ namespace message_memory_strategy
 {
 
 /// Default allocation strategy for messages received by subscriptions.
-// A message memory strategy must be templated on the type of the subscription it belongs to.
+/** A message memory strategy must be templated on the type of the subscription it belongs to. */
 template<typename MessageT, typename Alloc = std::allocator<void>>
 class MessageMemoryStrategy
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(MessageMemoryStrategy);
+  RCLCPP_SMART_PTR_DEFINITIONS(MessageMemoryStrategy)
 
   using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
   using MessageAlloc = typename MessageAllocTraits::allocator_type;
@@ -56,14 +56,14 @@ public:
   }
 
   /// By default, dynamically allocate a new message.
-  // \return Shared pointer to the new message.
+  /** \return Shared pointer to the new message. */
   virtual std::shared_ptr<MessageT> borrow_message()
   {
     return std::allocate_shared<MessageT, MessageAlloc>(*message_allocator_.get());
   }
 
   /// Release ownership of the message, which will deallocate it if it has no more owners.
-  // \param[in] Shared pointer to the message we are returning.
+  /** \param[in] msg Shared pointer to the message we are returning. */
   virtual void return_message(std::shared_ptr<MessageT> & msg)
   {
     msg.reset();

rclcpp/include/rclcpp/node.hpp

@@ -15,13 +15,19 @@
 #ifndef RCLCPP__NODE_HPP_
 #define RCLCPP__NODE_HPP_
 
+#include <atomic>
+#include <condition_variable>
 #include <list>
 #include <map>
 #include <memory>
+#include <mutex>
 #include <string>
 #include <tuple>
 #include <vector>
 
+#include "rcl/error_handling.h"
+#include "rcl/node.h"
+
 #include "rcl_interfaces/msg/list_parameters_result.hpp"
 #include "rcl_interfaces/msg/parameter_descriptor.hpp"
 #include "rcl_interfaces/msg/parameter_event.hpp"
@@ -30,8 +36,15 @@
 #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/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_graph_interface.hpp"
+#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/node_interfaces/node_timers_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/publisher.hpp"
 #include "rclcpp/service.hpp"
@@ -39,66 +52,82 @@
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
 
-// Forward declaration of ROS middleware class
-namespace rmw
-{
-struct rmw_node_t;
-}  // namespace rmw
-
 namespace rclcpp
 {
 
 namespace node
 {
+
 /// 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);
+  RCLCPP_SMART_PTR_DEFINITIONS(Node)
 
   /// Create a new node with the specified name.
   /**
    * \param[in] node_name Name of the node.
+   * \param[in] namespace_ Namespace of the node.
    * \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);
+  explicit Node(
+    const std::string & node_name,
+    const std::string & namespace_ = "",
+    bool use_intra_process_comms = false);
 
   /// Create a node based on the node name and a rclcpp::context::Context.
   /**
    * \param[in] node_name Name of the node.
+   * \param[in] namespace_ Namespace of the node.
    * \param[in] context The context for the node (usually represents the state of a process).
    * \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,
+    const std::string & node_name,
+    const std::string & namespace_,
+    rclcpp::context::Context::SharedPtr context,
     bool use_intra_process_comms = false);
 
   RCLCPP_PUBLIC
-  ~Node();
+  virtual ~Node();
 
   /// Get the name of the node.
-  // \return The name of the node.
+  /** \return The name of the node. */
   RCLCPP_PUBLIC
-  const std::string &
+  const char *
   get_name() const;
 
+  /// Get the namespace of the node.
+  /** \return The namespace of the node. */
+  RCLCPP_PUBLIC
+  const char *
+  get_namespace() const;
+
   /// Create and return a callback group.
   RCLCPP_PUBLIC
   rclcpp::callback_group::CallbackGroup::SharedPtr
   create_callback_group(rclcpp::callback_group::CallbackGroupType group_type);
 
+  /// Return the list of callback groups in the node.
+  RCLCPP_PUBLIC
+  const std::vector<rclcpp::callback_group::CallbackGroup::WeakPtr> &
+  get_callback_groups() const;
+
   /// Create and return a Publisher.
   /**
    * \param[in] topic_name The topic for this publisher to publish on.
    * \param[in] qos_history_depth The depth of the publisher message queue.
+   * \param[in] allocator Optional custom allocator.
    * \return Shared pointer to the created publisher.
    */
-  template<typename MessageT, typename Alloc = std::allocator<void>>
-  typename rclcpp::publisher::Publisher<MessageT, Alloc>::SharedPtr
+  template<
+    typename MessageT, typename Alloc = std::allocator<void>,
+    typename PublisherT = ::rclcpp::publisher::Publisher<MessageT, Alloc>>
+  std::shared_ptr<PublisherT>
   create_publisher(
     const std::string & topic_name, size_t qos_history_depth,
     std::shared_ptr<Alloc> allocator = nullptr);
@@ -107,10 +136,13 @@ public:
   /**
    * \param[in] topic_name The topic for this publisher to publish on.
    * \param[in] qos_profile The quality of service profile to pass on to the rmw implementation.
+   * \param[in] allocator Optional custom allocator.
    * \return Shared pointer to the created publisher.
    */
-  template<typename MessageT, typename Alloc = std::allocator<void>>
-  typename rclcpp::publisher::Publisher<MessageT, Alloc>::SharedPtr
+  template<
+    typename MessageT, typename Alloc = std::allocator<void>,
+    typename PublisherT = ::rclcpp::publisher::Publisher<MessageT, Alloc>>
+  std::shared_ptr<PublisherT>
   create_publisher(
     const std::string & topic_name,
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default,
@@ -124,14 +156,19 @@ public:
    * \param[in] group The callback group for this subscription. NULL for no callback group.
    * \param[in] ignore_local_publications True to ignore local publications.
    * \param[in] msg_mem_strat The message memory strategy to use for allocating messages.
+   * \param[in] allocator Optional custom allocator.
    * \return Shared pointer to the created subscription.
    */
   /* TODO(jacquelinekay):
      Windows build breaks when static member function passed as default
      argument to msg_mem_strat, nullptr is a workaround.
    */
-  template<typename MessageT, typename CallbackT, typename Alloc = std::allocator<void>>
-  typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
+  template<
+    typename MessageT,
+    typename CallbackT,
+    typename Alloc = std::allocator<void>,
+    typename SubscriptionT = rclcpp::subscription::Subscription<MessageT, Alloc>>
+  std::shared_ptr<SubscriptionT>
   create_subscription(
     const std::string & topic_name,
     CallbackT && callback,
@@ -150,14 +187,19 @@ public:
    * \param[in] group The callback group for this subscription. NULL for no callback group.
    * \param[in] ignore_local_publications True to ignore local publications.
    * \param[in] msg_mem_strat The message memory strategy to use for allocating messages.
+   * \param[in] allocator Optional custom allocator.
    * \return Shared pointer to the created subscription.
    */
   /* TODO(jacquelinekay):
      Windows build breaks when static member function passed as default
      argument to msg_mem_strat, nullptr is a workaround.
    */
-  template<typename MessageT, typename CallbackT, typename Alloc = std::allocator<void>>
-  typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
+  template<
+    typename MessageT,
+    typename CallbackT,
+    typename Alloc = std::allocator<void>,
+    typename SubscriptionT = rclcpp::subscription::Subscription<MessageT, Alloc>>
+  std::shared_ptr<SubscriptionT>
   create_subscription(
     const std::string & topic_name,
     size_t qos_history_depth,
@@ -174,30 +216,13 @@ public:
    * \param[in] callback User-defined callback function.
    * \param[in] group Callback group to execute this timer's callback in.
    */
-  template<typename CallbackType>
-  typename rclcpp::timer::WallTimer<CallbackType>::SharedPtr
+  template<typename DurationT = std::milli, typename CallbackT>
+  typename rclcpp::timer::WallTimer<CallbackT>::SharedPtr
   create_wall_timer(
-    std::chrono::nanoseconds period,
-    CallbackType callback,
+    std::chrono::duration<int64_t, DurationT> period,
+    CallbackT callback,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
-  /// Create a timer.
-  /**
-   * \param[in] period Time interval between triggers of the callback.
-   * \param[in] callback User-defined callback function.
-   * \param[in] group Callback group to execute this timer's callback in.
-   */
-  // TODO(wjwwood): reenable this once I figure out why the demo doesn't build with it.
-  // rclcpp::timer::WallTimer::SharedPtr
-  // create_wall_timer(
-  //   std::chrono::duration<long double, std::nano> period,
-  //   rclcpp::timer::CallbackType callback,
-  //   rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
-
-  using CallbackGroup = rclcpp::callback_group::CallbackGroup;
-  using CallbackGroupWeakPtr = std::weak_ptr<CallbackGroup>;
-  using CallbackGroupWeakPtrList = std::list<CallbackGroupWeakPtr>;
-
   /* Create and return a Client. */
   template<typename ServiceT>
   typename rclcpp::client::Client<ServiceT>::SharedPtr
@@ -223,10 +248,56 @@ public:
   rcl_interfaces::msg::SetParametersResult
   set_parameters_atomically(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
 
+  template<typename ParameterT>
+  void
+  set_parameter_if_not_set(
+    const std::string & name,
+    const ParameterT & value);
+
   RCLCPP_PUBLIC
   std::vector<rclcpp::parameter::ParameterVariant>
   get_parameters(const std::vector<std::string> & names) const;
 
+  RCLCPP_PUBLIC
+  rclcpp::parameter::ParameterVariant
+  get_parameter(const std::string & name) const;
+
+  RCLCPP_PUBLIC
+  bool
+  get_parameter(
+    const std::string & name,
+    rclcpp::parameter::ParameterVariant & parameter) const;
+
+  /// Assign the value of the parameter if set into the parameter argument.
+  /**
+   * If the parameter was not set, then the "parameter" argument is never assigned a value.
+   *
+   * \param[in] name The name of the parameter to get.
+   * \param[out] parameter The output where the value of the parameter should be assigned.
+   * \returns true if the parameter was set, false otherwise
+   */
+  template<typename ParameterT>
+  bool
+  get_parameter(const std::string & name, ParameterT & parameter) const;
+
+  /// Get the parameter value, or the "alternative value" if not set, and assign it to "value".
+  /**
+   * If the parameter was not set, then the "value" argument is assigned
+   * the "alternative_value".
+   * In all cases, the parameter remains not set after this function is called.
+   *
+   * \param[in] name The name of the parameter to get.
+   * \param[out] value The output where the value of the parameter should be assigned.
+   * \param[in] alternative_value Value to be stored in output if the parameter was not set.
+   * \returns true if the parameter was set, false otherwise
+   */
+  template<typename ParameterT>
+  bool
+  get_parameter_or(
+    const std::string & name,
+    ParameterT & value,
+    const ParameterT & alternative_value) const;
+
   RCLCPP_PUBLIC
   std::vector<rcl_interfaces::msg::ParameterDescriptor>
   describe_parameters(const std::vector<std::string> & names) const;
@@ -239,10 +310,24 @@ public:
   rcl_interfaces::msg::ListParametersResult
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const;
 
+  /// Register the callback for parameter changes
+  /**
+   * \param[in] callback User defined callback function.
+   *   It is expected to atomically set parameters.
+   * \note Repeated invocations of this function will overwrite previous callbacks
+   */
+  template<typename CallbackT>
+  void
+  register_param_change_callback(CallbackT && callback);
+
   RCLCPP_PUBLIC
-  std::map<std::string, std::string>
+  std::map<std::string, std::vector<std::string>>
   get_topic_names_and_types() const;
 
+  RCLCPP_PUBLIC
+  std::map<std::string, std::vector<std::string>>
+  get_service_names_and_types() const;
+
   RCLCPP_PUBLIC
   size_t
   count_publishers(const std::string & topic_name) const;
@@ -251,46 +336,74 @@ public:
   size_t
   count_subscribers(const std::string & topic_name) const;
 
+  /// Return a graph event, which will be set anytime a graph change occurs.
+  /* The graph Event object is a loan which must be returned.
+   * The Event object is scoped and therefore to return the load just let it go
+   * out of scope.
+   */
   RCLCPP_PUBLIC
-  const CallbackGroupWeakPtrList &
-  get_callback_groups() const;
+  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
-  rmw_guard_condition_t * get_notify_guard_condition() const;
+  void
+  wait_for_graph_change(
+    rclcpp::event::Event::SharedPtr event,
+    std::chrono::nanoseconds timeout);
 
-  std::atomic_bool has_executor;
+  /// Return the Node's internal NodeBaseInterface implementation.
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
+  get_node_base_interface();
+
+  /// Return the Node's internal NodeGraphInterface implementation.
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr
+  get_node_graph_interface();
+
+  /// Return the Node's internal NodeTimersInterface implementation.
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeTimersInterface::SharedPtr
+  get_node_timers_interface();
+
+  /// Return the Node's internal NodeTopicsInterface implementation.
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr
+  get_node_topics_interface();
+
+  /// Return the Node's internal NodeServicesInterface implementation.
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr
+  get_node_services_interface();
+
+  /// Return the Node's internal NodeParametersInterface implementation.
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr
+  get_node_parameters_interface();
 
 private:
-  RCLCPP_DISABLE_COPY(Node);
+  RCLCPP_DISABLE_COPY(Node)
 
   RCLCPP_PUBLIC
   bool
   group_in_node(callback_group::CallbackGroup::SharedPtr group);
 
-  std::string name_;
-
-  std::shared_ptr<rmw_node_t> node_handle_;
-
-  rclcpp::context::Context::SharedPtr context_;
-
-  CallbackGroup::SharedPtr default_callback_group_;
-  CallbackGroupWeakPtrList callback_groups_;
-
-  size_t number_of_subscriptions_;
-  size_t number_of_timers_;
-  size_t number_of_services_;
-  size_t number_of_clients_;
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
+  rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_;
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
+  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
 
   bool use_intra_process_comms_;
-
-  mutable std::mutex mutex_;
-
-  /// Guard condition for notifying the Executor of changes to this node.
-  rmw_guard_condition_t * notify_guard_condition_;
-
-  std::map<std::string, rclcpp::parameter::ParameterVariant> parameters_;
-
-  publisher::Publisher<rcl_interfaces::msg::ParameterEvent>::SharedPtr events_publisher_;
 };
 
 }  // namespace node

rclcpp/include/rclcpp/node_impl.hpp

@@ -30,11 +30,16 @@
 #include <utility>
 #include <vector>
 
+#include "rcl/publisher.h"
+#include "rcl/subscription.h"
+
 #include "rcl_interfaces/msg/intra_process_message.hpp"
 
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/intra_process_manager.hpp"
 #include "rclcpp/parameter.hpp"
+#include "rclcpp/create_publisher.hpp"
+#include "rclcpp/create_subscription.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -47,8 +52,8 @@ namespace rclcpp
 namespace node
 {
 
-template<typename MessageT, typename Alloc>
-typename rclcpp::publisher::Publisher<MessageT, Alloc>::SharedPtr
+template<typename MessageT, typename Alloc, typename PublisherT>
+std::shared_ptr<PublisherT>
 Node::create_publisher(
   const std::string & topic_name, size_t qos_history_depth,
   std::shared_ptr<Alloc> allocator)
@@ -58,11 +63,11 @@ Node::create_publisher(
   }
   rmw_qos_profile_t qos = rmw_qos_profile_default;
   qos.depth = qos_history_depth;
-  return this->create_publisher<MessageT, Alloc>(topic_name, qos, allocator);
+  return this->create_publisher<MessageT, Alloc, PublisherT>(topic_name, qos, allocator);
 }
 
-template<typename MessageT, typename Alloc>
-typename publisher::Publisher<MessageT, Alloc>::SharedPtr
+template<typename MessageT, typename Alloc, typename PublisherT>
+std::shared_ptr<PublisherT>
 Node::create_publisher(
   const std::string & topic_name, const rmw_qos_profile_t & qos_profile,
   std::shared_ptr<Alloc> allocator)
@@ -70,80 +75,16 @@ 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 = publisher::Publisher<MessageT, Alloc>::make_shared(
-    node_handle_, publisher_handle, topic_name, qos_profile.depth, allocator);
-
-  if (use_intra_process_comms_) {
-    rmw_publisher_t * intra_process_publisher_handle = rmw_create_publisher(
-      node_handle_.get(), rclcpp::type_support::get_intra_process_message_msg_type_support(),
-      (topic_name + "__intra").c_str(), &qos_profile);
-    if (!intra_process_publisher_handle) {
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-      throw std::runtime_error(
-        std::string("could not create intra process publisher: ") +
-        rmw_get_error_string_safe());
-      // *INDENT-ON*
-    }
-
-    auto intra_process_manager =
-      context_->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
-    uint64_t intra_process_publisher_id =
-      intra_process_manager->add_publisher<MessageT, Alloc>(publisher);
-    rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = intra_process_manager;
-    // *INDENT-OFF*
-    auto shared_publish_callback =
-      [weak_ipm](uint64_t publisher_id, void * msg, const std::type_info & type_info) -> uint64_t
-    {
-      auto ipm = weak_ipm.lock();
-      if (!ipm) {
-        // TODO(wjwwood): should this just return silently? Or maybe return with a logged warning?
-        throw std::runtime_error(
-          "intra process publish called after destruction of intra process manager");
-      }
-      if (!msg) {
-        throw std::runtime_error("cannot publisher msg which is a null pointer");
-      }
-      auto & message_type_info = typeid(MessageT);
-      if (message_type_info != type_info) {
-        throw std::runtime_error(
-          std::string("published type '") + type_info.name() +
-          "' is incompatible from the publisher type '" + message_type_info.name() + "'");
-      }
-      MessageT * typed_message_ptr = static_cast<MessageT *>(msg);
-      using MessageDeleter = typename publisher::Publisher<MessageT, Alloc>::MessageDeleter;
-      std::unique_ptr<MessageT, MessageDeleter> unique_msg(typed_message_ptr);
-      uint64_t message_seq =
-        ipm->store_intra_process_message<MessageT, Alloc>(publisher_id, unique_msg);
-      return message_seq;
-    };
-    // *INDENT-ON*
-    publisher->setup_intra_process(
-      intra_process_publisher_id,
-      shared_publish_callback,
-      intra_process_publisher_handle);
-  }
-  if (rmw_trigger_guard_condition(notify_guard_condition_) != RMW_RET_OK) {
-    throw std::runtime_error(
-            std::string(
-              "Failed to notify waitset on publisher creation: ") + rmw_get_error_string());
-  }
-  return publisher;
+  return rclcpp::create_publisher<MessageT, Alloc, PublisherT>(
+    this->node_topics_.get(),
+    topic_name,
+    qos_profile,
+    use_intra_process_comms_,
+    allocator);
 }
 
-template<typename MessageT, typename CallbackT, typename Alloc>
-typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
+template<typename MessageT, typename CallbackT, typename Alloc, typename SubscriptionT>
+std::shared_ptr<SubscriptionT>
 Node::create_subscription(
   const std::string & topic_name,
   CallbackT && callback,
@@ -152,112 +93,31 @@ Node::create_subscription(
   bool ignore_local_publications,
   typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
   msg_mem_strat,
-  typename std::shared_ptr<Alloc> allocator)
+  std::shared_ptr<Alloc> allocator)
 {
   if (!allocator) {
     allocator = std::make_shared<Alloc>();
   }
 
-  rclcpp::subscription::AnySubscriptionCallback<MessageT,
-  Alloc> any_subscription_callback(allocator);
-  any_subscription_callback.set(std::forward<CallbackT>(callback));
-
-  using rosidl_generator_cpp::get_message_type_support_handle;
-
   if (!msg_mem_strat) {
-    msg_mem_strat =
-      rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::create_default();
+    using rclcpp::message_memory_strategy::MessageMemoryStrategy;
+    msg_mem_strat = MessageMemoryStrategy<MessageT, Alloc>::create_default();
   }
 
-  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*
-  }
-
-  using rclcpp::subscription::Subscription;
-  using rclcpp::subscription::SubscriptionBase;
-
-  auto sub = Subscription<MessageT, Alloc>::make_shared(
-    node_handle_,
-    subscriber_handle,
+  return rclcpp::create_subscription<MessageT, CallbackT, Alloc, SubscriptionT>(
+    this->node_topics_.get(),
     topic_name,
+    std::forward<CallbackT>(callback),
+    qos_profile,
+    group,
     ignore_local_publications,
-    any_subscription_callback,
-    msg_mem_strat);
-  auto sub_base_ptr = std::dynamic_pointer_cast<SubscriptionBase>(sub);
-  // Setup intra process.
-  if (use_intra_process_comms_) {
-    rmw_subscription_t * intra_process_subscriber_handle = rmw_create_subscription(
-      node_handle_.get(), rclcpp::type_support::get_intra_process_message_msg_type_support(),
-      (topic_name + "__intra").c_str(), &qos_profile, false);
-    if (!subscriber_handle) {
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-      throw std::runtime_error(
-        std::string("could not create intra process subscription: ") + rmw_get_error_string_safe());
-      // *INDENT-ON*
-    }
-    auto intra_process_manager =
-      context_->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
-    rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = intra_process_manager;
-    uint64_t intra_process_subscription_id =
-      intra_process_manager->add_subscription(sub_base_ptr);
-    // *INDENT-OFF*
-    sub->setup_intra_process(
-      intra_process_subscription_id,
-      intra_process_subscriber_handle,
-      [weak_ipm](
-        uint64_t publisher_id,
-        uint64_t message_sequence,
-        uint64_t subscription_id,
-        typename Subscription<MessageT, Alloc>::MessageUniquePtr & message)
-      {
-        auto ipm = weak_ipm.lock();
-        if (!ipm) {
-          // TODO(wjwwood): should this just return silently? Or maybe return with a logged warning?
-          throw std::runtime_error(
-            "intra process take called after destruction of intra process manager");
-        }
-        ipm->take_intra_process_message<MessageT, Alloc>(
-          publisher_id, message_sequence, subscription_id, message);
-      },
-      [weak_ipm](const rmw_gid_t * sender_gid) -> bool {
-        auto ipm = weak_ipm.lock();
-        if (!ipm) {
-          throw std::runtime_error(
-            "intra process publisher check called after destruction of intra process manager");
-        }
-        return ipm->matches_any_publishers(sender_gid);
-      }
-    );
-    // *INDENT-ON*
-  }
-  // Assign to a group.
-  if (group) {
-    if (!group_in_node(group)) {
-      // TODO(jacquelinekay): use custom exception
-      throw std::runtime_error("Cannot create subscription, group not in node.");
-    }
-    group->add_subscription(sub_base_ptr);
-  } else {
-    default_callback_group_->add_subscription(sub_base_ptr);
-  }
-  number_of_subscriptions_++;
-  if (rmw_trigger_guard_condition(notify_guard_condition_) != RMW_RET_OK) {
-    throw std::runtime_error(
-            std::string(
-              "Failed to notify waitset on subscription creation: ") + rmw_get_error_string());
-  }
-  return sub;
+    use_intra_process_comms_,
+    msg_mem_strat,
+    allocator);
 }
 
-template<typename MessageT, typename CallbackT, typename Alloc>
-typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
+template<typename MessageT, typename CallbackT, typename Alloc, typename SubscriptionT>
+std::shared_ptr<SubscriptionT>
 Node::create_subscription(
   const std::string & topic_name,
   size_t qos_history_depth,
@@ -270,7 +130,7 @@ Node::create_subscription(
 {
   rmw_qos_profile_t qos = rmw_qos_profile_default;
   qos.depth = qos_history_depth;
-  return this->create_subscription<MessageT, CallbackT, Alloc>(
+  return this->create_subscription<MessageT, CallbackT, Alloc, SubscriptionT>(
     topic_name,
     std::forward<CallbackT>(callback),
     qos,
@@ -280,30 +140,17 @@ Node::create_subscription(
     allocator);
 }
 
-template<typename CallbackType>
-typename rclcpp::timer::WallTimer<CallbackType>::SharedPtr
+template<typename DurationT, typename CallbackT>
+typename rclcpp::timer::WallTimer<CallbackT>::SharedPtr
 Node::create_wall_timer(
-  std::chrono::nanoseconds period,
-  CallbackType callback,
+  std::chrono::duration<int64_t, DurationT> period,
+  CallbackT callback,
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
-  auto timer = rclcpp::timer::WallTimer<CallbackType>::make_shared(
-    period, std::move(callback));
-  if (group) {
-    if (!group_in_node(group)) {
-      // TODO(jacquelinekay): use custom exception
-      throw std::runtime_error("Cannot create timer, group not in node.");
-    }
-    group->add_timer(timer);
-  } else {
-    default_callback_group_->add_timer(timer);
-  }
-  number_of_timers_++;
-  if (rmw_trigger_guard_condition(notify_guard_condition_) != RMW_RET_OK) {
-    throw std::runtime_error(
-            std::string(
-              "Failed to notify waitset on timer creation: ") + rmw_get_error_string());
-  }
+  auto timer = rclcpp::timer::WallTimer<CallbackT>::make_shared(
+    std::chrono::duration_cast<std::chrono::nanoseconds>(period),
+    std::move(callback));
+  node_timers_->add_timer(timer, group);
   return timer;
 }
 
@@ -314,45 +161,20 @@ Node::create_client(
   const rmw_qos_profile_t & qos_profile,
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
-  using rosidl_generator_cpp::get_service_type_support_handle;
-  auto service_type_support_handle =
-    get_service_type_support_handle<ServiceT>();
-
-  rmw_client_t * client_handle = rmw_create_client(
-    this->node_handle_.get(), service_type_support_handle, service_name.c_str(), &qos_profile);
-  if (!client_handle) {
-    // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-    throw std::runtime_error(
-      std::string("could not create client: ") +
-      rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
+  rcl_client_options_t options = rcl_client_get_default_options();
+  options.qos = qos_profile;
 
   using rclcpp::client::Client;
   using rclcpp::client::ClientBase;
 
   auto cli = Client<ServiceT>::make_shared(
-    node_handle_,
-    client_handle,
-    service_name);
+    node_base_.get(),
+    node_graph_,
+    service_name,
+    options);
 
   auto cli_base_ptr = std::dynamic_pointer_cast<ClientBase>(cli);
-  if (group) {
-    if (!group_in_node(group)) {
-      // TODO(esteve): use custom exception
-      throw std::runtime_error("Cannot create client, group not in node.");
-    }
-    group->add_client(cli_base_ptr);
-  } else {
-    default_callback_group_->add_client(cli_base_ptr);
-  }
-  number_of_clients_++;
-
-  if (rmw_trigger_guard_condition(notify_guard_condition_) != RMW_RET_OK) {
-    throw std::runtime_error(
-            std::string(
-              "Failed to notify waitset on client creation: ") + rmw_get_error_string());
-  }
+  node_services_->add_client(cli_base_ptr, group);
   return cli;
 }
 
@@ -364,44 +186,68 @@ Node::create_service(
   const rmw_qos_profile_t & qos_profile,
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
-  using rosidl_generator_cpp::get_service_type_support_handle;
-  auto service_type_support_handle =
-    get_service_type_support_handle<ServiceT>();
-
   rclcpp::service::AnyServiceCallback<ServiceT> any_service_callback;
   any_service_callback.set(std::forward<CallbackT>(callback));
 
-  rmw_service_t * service_handle = rmw_create_service(
-    node_handle_.get(), service_type_support_handle, service_name.c_str(), &qos_profile);
-  if (!service_handle) {
-    // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-    throw std::runtime_error(
-      std::string("could not create service: ") +
-      rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
+  rcl_service_options_t service_options = rcl_service_get_default_options();
+  service_options.qos = qos_profile;
 
   auto serv = service::Service<ServiceT>::make_shared(
-    node_handle_, service_handle, service_name, any_service_callback);
+    node_base_->get_shared_rcl_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)) {
-      // TODO(jacquelinekay): use custom exception
-      throw std::runtime_error("Cannot create service, group not in node.");
-    }
-    group->add_service(serv_base_ptr);
-  } else {
-    default_callback_group_->add_service(serv_base_ptr);
-  }
-  number_of_services_++;
-  if (rmw_trigger_guard_condition(notify_guard_condition_) != RMW_RET_OK) {
-    throw std::runtime_error(
-            std::string(
-              "Failed to notify waitset on service creation: ") + rmw_get_error_string());
-  }
+  node_services_->add_service(serv_base_ptr, group);
   return serv;
 }
 
+template<typename CallbackT>
+void
+Node::register_param_change_callback(CallbackT && callback)
+{
+  this->node_parameters_->register_param_change_callback(std::forward<CallbackT>(callback));
+}
+
+template<typename ParameterT>
+void
+Node::set_parameter_if_not_set(
+  const std::string & name,
+  const ParameterT & value)
+{
+  rclcpp::parameter::ParameterVariant parameter_variant;
+  if (!this->get_parameter(name, parameter_variant)) {
+    this->set_parameters({
+          rclcpp::parameter::ParameterVariant(name, value),
+        });
+  }
+}
+
+template<typename ParameterT>
+bool
+Node::get_parameter(const std::string & name, ParameterT & value) const
+{
+  rclcpp::parameter::ParameterVariant parameter_variant;
+  bool result = get_parameter(name, parameter_variant);
+  if (result) {
+    value = parameter_variant.get_value<ParameterT>();
+  }
+
+  return result;
+}
+
+template<typename ParameterT>
+bool
+Node::get_parameter_or(
+  const std::string & name,
+  ParameterT & value,
+  const ParameterT & alternative_value) const
+{
+  bool got_parameter = get_parameter(name, value);
+  if (!got_parameter) {
+    value = alternative_value;
+  }
+  return got_parameter;
+}
+
 }  // namespace node
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -0,0 +1,139 @@
+// 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__NODE_INTERFACES__NODE_BASE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/context.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeBase part of the Node API.
+class NodeBase : public NodeBaseInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBaseInterface)
+
+  RCLCPP_PUBLIC
+  NodeBase(
+    const std::string & node_name,
+    const std::string & namespace_,
+    rclcpp::context::Context::SharedPtr context);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeBase();
+
+  RCLCPP_PUBLIC
+  virtual
+  const char *
+  get_name() const;
+
+  RCLCPP_PUBLIC
+  virtual
+  const char *
+  get_namespace() const;
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::context::Context::SharedPtr
+  get_context();
+
+  RCLCPP_PUBLIC
+  virtual
+  rcl_node_t *
+  get_rcl_node_handle();
+
+  RCLCPP_PUBLIC
+  virtual
+  const rcl_node_t *
+  get_rcl_node_handle() const;
+
+  RCLCPP_PUBLIC
+  virtual
+  std::shared_ptr<rcl_node_t>
+  get_shared_rcl_node_handle();
+
+  RCLCPP_PUBLIC
+  virtual
+  std::shared_ptr<const rcl_node_t>
+  get_shared_rcl_node_handle() const;
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::callback_group::CallbackGroup::SharedPtr
+  create_callback_group(rclcpp::callback_group::CallbackGroupType group_type);
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::callback_group::CallbackGroup::SharedPtr
+  get_default_callback_group();
+
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group);
+
+  RCLCPP_PUBLIC
+  virtual
+  const std::vector<rclcpp::callback_group::CallbackGroup::WeakPtr> &
+  get_callback_groups() const;
+
+  RCLCPP_PUBLIC
+  virtual
+  std::atomic_bool &
+  get_associated_with_executor_atomic();
+
+  RCLCPP_PUBLIC
+  virtual
+  rcl_guard_condition_t *
+  get_notify_guard_condition();
+
+  RCLCPP_PUBLIC
+  virtual
+  std::unique_lock<std::recursive_mutex>
+  acquire_notify_guard_condition_lock() const;
+
+private:
+  RCLCPP_DISABLE_COPY(NodeBase)
+
+  rclcpp::context::Context::SharedPtr context_;
+
+  std::shared_ptr<rcl_node_t> node_handle_;
+
+  rclcpp::callback_group::CallbackGroup::SharedPtr default_callback_group_;
+  std::vector<rclcpp::callback_group::CallbackGroup::WeakPtr> callback_groups_;
+
+  std::atomic_bool associated_with_executor_;
+
+  /// Guard condition for notifying the Executor of changes to this node.
+  mutable std::recursive_mutex notify_guard_condition_mutex_;
+  rcl_guard_condition_t notify_guard_condition_ = rcl_get_zero_initialized_guard_condition();
+  bool notify_guard_condition_is_valid_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -0,0 +1,146 @@
+// 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__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_
+
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+#include "rcl/node.h"
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/context.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeBase part of the Node API.
+class NodeBaseInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBaseInterface)
+
+  /// Return the name of the node.
+  /** \return The name of the node. */
+  RCLCPP_PUBLIC
+  virtual
+  const char *
+  get_name() const = 0;
+
+  /// Return the namespace of the node.
+  /** \return The namespace of the node. */
+  RCLCPP_PUBLIC
+  virtual
+  const char *
+  get_namespace() const = 0;
+
+  /// Return the context of the node.
+  /** \return SharedPtr to the node's context. */
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::context::Context::SharedPtr
+  get_context() = 0;
+
+  /// Return the rcl_node_t node handle (non-const version).
+  RCLCPP_PUBLIC
+  virtual
+  rcl_node_t *
+  get_rcl_node_handle() = 0;
+
+  /// Return the rcl_node_t node handle (const version).
+  RCLCPP_PUBLIC
+  virtual
+  const rcl_node_t *
+  get_rcl_node_handle() const = 0;
+
+  /// 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
+  virtual
+  std::shared_ptr<rcl_node_t>
+  get_shared_rcl_node_handle() = 0;
+
+  /// 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
+  virtual
+  std::shared_ptr<const rcl_node_t>
+  get_shared_rcl_node_handle() const = 0;
+
+  /// Create and return a callback group.
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::callback_group::CallbackGroup::SharedPtr
+  create_callback_group(rclcpp::callback_group::CallbackGroupType group_type) = 0;
+
+  /// Return the default callback group.
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::callback_group::CallbackGroup::SharedPtr
+  get_default_callback_group() = 0;
+
+  /// Return true if the given callback group is associated with this node.
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0;
+
+  /// Return list of callback groups associated with this node.
+  RCLCPP_PUBLIC
+  virtual
+  const std::vector<rclcpp::callback_group::CallbackGroup::WeakPtr> &
+  get_callback_groups() const = 0;
+
+  /// Return the atomic bool which is used to ensure only one executor is used.
+  RCLCPP_PUBLIC
+  virtual
+  std::atomic_bool &
+  get_associated_with_executor_atomic() = 0;
+
+  /// Return guard condition that should be notified when the internal node state changes.
+  /**
+   * For example, this should be notified when a publisher is added or removed.
+   *
+   * \return the rcl_guard_condition_t if it is valid, else nullptr
+   */
+  RCLCPP_PUBLIC
+  virtual
+  rcl_guard_condition_t *
+  get_notify_guard_condition() = 0;
+
+  /// Acquire and return a scoped lock that protects the notify guard condition.
+  /** This should be used when triggering the notify guard condition. */
+  RCLCPP_PUBLIC
+  virtual
+  std::unique_lock<std::recursive_mutex>
+  acquire_notify_guard_condition_lock() const = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

@@ -0,0 +1,139 @@
+// Copyright 2016-2017 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__NODE_INTERFACES__NODE_GRAPH_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_
+
+#include <chrono>
+#include <map>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+#include "rcl/guard_condition.h"
+
+#include "rclcpp/event.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_graph_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+namespace graph_listener
+{
+class GraphListener;
+}  // namespace graph_listener
+
+namespace node_interfaces
+{
+
+/// Implementation the NodeGraph part of the Node API.
+class NodeGraph : public NodeGraphInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeGraph)
+
+  RCLCPP_PUBLIC
+  explicit NodeGraph(rclcpp::node_interfaces::NodeBaseInterface * node_base);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeGraph();
+
+  RCLCPP_PUBLIC
+  virtual
+  std::map<std::string, std::vector<std::string>>
+  get_topic_names_and_types(bool no_demangle = false) const;
+
+  RCLCPP_PUBLIC
+  virtual
+  std::map<std::string, std::vector<std::string>>
+  get_service_names_and_types() const;
+
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<std::string>
+  get_node_names() const;
+
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  count_publishers(const std::string & topic_name) const;
+
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  count_subscribers(const std::string & topic_name) const;
+
+  RCLCPP_PUBLIC
+  virtual
+  const rcl_guard_condition_t *
+  get_graph_guard_condition() const;
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  notify_graph_change();
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  notify_shutdown();
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::event::Event::SharedPtr
+  get_graph_event();
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  wait_for_graph_change(
+    rclcpp::event::Event::SharedPtr event,
+    std::chrono::nanoseconds timeout);
+
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  count_graph_users();
+
+private:
+  RCLCPP_DISABLE_COPY(NodeGraph)
+
+  /// Handle to the NodeBaseInterface given in the constructor.
+  rclcpp::node_interfaces::NodeBaseInterface * node_base_;
+
+  /// 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.
+  mutable 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_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

@@ -0,0 +1,147 @@
+// Copyright 2016-2017 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__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_
+
+#include <chrono>
+#include <map>
+#include <string>
+#include <vector>
+
+#include "rcl/guard_condition.h"
+
+#include "rclcpp/event.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeGraph part of the Node API.
+class NodeGraphInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeGraphInterface)
+
+  /// Return a map of existing topic names to list of topic types.
+  /**
+   * A topic is considered to exist when at least one publisher or subscriber
+   * exists for it, whether they be local or remote to this process.
+   *
+   * \param[in] no_demangle if true, topic names and types are not demangled
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::map<std::string, std::vector<std::string>>
+  get_topic_names_and_types(bool no_demangle = false) const = 0;
+
+  /// Return a map of existing service names to list of service types.
+  /**
+   * A service is considered to exist when at least one service server or
+   * service client exists for it, whether they be local or remote to this
+   * process.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::map<std::string, std::vector<std::string>>
+  get_service_names_and_types() const = 0;
+
+  /// Return a vector of existing node names (string).
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<std::string>
+  get_node_names() const = 0;
+
+  /// Return the number of publishers that are advertised on a given topic.
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  count_publishers(const std::string & topic_name) const = 0;
+
+  /// Return the number of subscribers who have created a subscription for a given topic.
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  count_subscribers(const std::string & topic_name) const = 0;
+
+  /// Return the rcl guard condition which is triggered when the ROS graph changes.
+  RCLCPP_PUBLIC
+  virtual
+  const rcl_guard_condition_t *
+  get_graph_guard_condition() const = 0;
+
+  /// 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
+  virtual
+  void
+  notify_graph_change() = 0;
+
+  /// Notify any and all blocking node actions that shutdown has occurred.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  notify_shutdown() = 0;
+
+  /// 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
+  virtual
+  rclcpp::event::Event::SharedPtr
+  get_graph_event() = 0;
+
+  /// 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
+  virtual
+  void
+  wait_for_graph_change(
+    rclcpp::event::Event::SharedPtr event,
+    std::chrono::nanoseconds timeout) = 0;
+
+  /// 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
+  virtual
+  size_t
+  count_graph_users() = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -0,0 +1,122 @@
+// 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__NODE_INTERFACES__NODE_PARAMETERS_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_
+
+#include <map>
+#include <string>
+#include <vector>
+
+#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 "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/publisher.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeParameters part of the Node API.
+class NodeParameters : public NodeParametersInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeParameters)
+
+  RCLCPP_PUBLIC
+  NodeParameters(
+    rclcpp::node_interfaces::NodeTopicsInterface * node_topics,
+    bool use_intra_process);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeParameters();
+
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<rcl_interfaces::msg::SetParametersResult>
+  set_parameters(
+    const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+
+  RCLCPP_PUBLIC
+  virtual
+  rcl_interfaces::msg::SetParametersResult
+  set_parameters_atomically(
+    const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<rclcpp::parameter::ParameterVariant>
+  get_parameters(const std::vector<std::string> & names) const;
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::parameter::ParameterVariant
+  get_parameter(const std::string & name) const;
+
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  get_parameter(
+    const std::string & name,
+    rclcpp::parameter::ParameterVariant & parameter) const;
+
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<rcl_interfaces::msg::ParameterDescriptor>
+  describe_parameters(const std::vector<std::string> & names) const;
+
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<uint8_t>
+  get_parameter_types(const std::vector<std::string> & names) const;
+
+  RCLCPP_PUBLIC
+  virtual
+  rcl_interfaces::msg::ListParametersResult
+  list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const;
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  register_param_change_callback(ParametersCallbackFunction callback);
+
+private:
+  RCLCPP_DISABLE_COPY(NodeParameters)
+
+  rclcpp::node_interfaces::NodeTopicsInterface * node_topics_;
+
+  mutable std::mutex mutex_;
+
+  ParametersCallbackFunction parameters_callback_ = nullptr;
+
+  std::map<std::string, rclcpp::parameter::ParameterVariant> parameters_;
+
+  publisher::Publisher<rcl_interfaces::msg::ParameterEvent>::SharedPtr events_publisher_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -0,0 +1,97 @@
+// 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__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
+
+#include <string>
+#include <vector>
+
+#include "rcl_interfaces/msg/list_parameters_result.hpp"
+#include "rcl_interfaces/msg/parameter_descriptor.hpp"
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeParameters part of the Node API.
+class NodeParametersInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeParametersInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<rcl_interfaces::msg::SetParametersResult>
+  set_parameters(
+    const std::vector<rclcpp::parameter::ParameterVariant> & parameters) = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  rcl_interfaces::msg::SetParametersResult
+  set_parameters_atomically(
+    const std::vector<rclcpp::parameter::ParameterVariant> & parameters) = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<rclcpp::parameter::ParameterVariant>
+  get_parameters(const std::vector<std::string> & names) const = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::parameter::ParameterVariant
+  get_parameter(const std::string & name) const = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  get_parameter(
+    const std::string & name,
+    rclcpp::parameter::ParameterVariant & parameter) const = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<rcl_interfaces::msg::ParameterDescriptor>
+  describe_parameters(const std::vector<std::string> & names) const = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<uint8_t>
+  get_parameter_types(const std::vector<std::string> & names) const = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  rcl_interfaces::msg::ListParametersResult
+  list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const = 0;
+
+  using ParametersCallbackFunction =
+      std::function<rcl_interfaces::msg::SetParametersResult(
+        const std::vector<rclcpp::parameter::ParameterVariant> &)>;
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  register_param_change_callback(ParametersCallbackFunction callback) = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_services.hpp

@@ -0,0 +1,67 @@
+// 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__NODE_INTERFACES__NODE_SERVICES_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_SERVICES_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/client.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeServices part of the Node API.
+class NodeServices : public NodeServicesInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeServices)
+
+  RCLCPP_PUBLIC
+  explicit NodeServices(rclcpp::node_interfaces::NodeBaseInterface * node_base);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeServices();
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_client(
+    rclcpp::client::ClientBase::SharedPtr client_base_ptr,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group);
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_service(
+    rclcpp::service::ServiceBase::SharedPtr service_base_ptr,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group);
+
+private:
+  RCLCPP_DISABLE_COPY(NodeServices)
+
+  rclcpp::node_interfaces::NodeBaseInterface * node_base_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_SERVICES_HPP_

rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp

@@ -0,0 +1,53 @@
+// 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__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/client.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeServices part of the Node API.
+class NodeServicesInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeServicesInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_client(
+    rclcpp::client::ClientBase::SharedPtr client_base_ptr,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_service(
+    rclcpp::service::ServiceBase::SharedPtr service_base_ptr,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_timers.hpp

@@ -0,0 +1,60 @@
+// 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__NODE_INTERFACES__NODE_TIMERS_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_TIMERS_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_timers_interface.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeTimers part of the Node API.
+class NodeTimers : public NodeTimersInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimers)
+
+  RCLCPP_PUBLIC
+  explicit NodeTimers(rclcpp::node_interfaces::NodeBaseInterface * node_base);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTimers();
+
+  /// Add a timer to the node.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_timer(
+    rclcpp::timer::TimerBase::SharedPtr timer,
+    rclcpp::callback_group::CallbackGroup::SharedPtr callback_group);
+
+private:
+  RCLCPP_DISABLE_COPY(NodeTimers)
+
+  rclcpp::node_interfaces::NodeBaseInterface * node_base_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_TIMERS_HPP_

rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp

@@ -0,0 +1,46 @@
+// 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__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeTimers part of the Node API.
+class NodeTimersInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimersInterface)
+
+  /// Add a timer to the node.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_timer(
+    rclcpp::timer::TimerBase::SharedPtr timer,
+    rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_topics.hpp

@@ -0,0 +1,88 @@
+// 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__NODE_INTERFACES__NODE_TOPICS_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_TOPICS_HPP_
+
+#include <string>
+
+#include "rcl/publisher.h"
+#include "rcl/subscription.h"
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/publisher.hpp"
+#include "rclcpp/publisher_factory.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeTopics part of the Node API.
+class NodeTopics : public NodeTopicsInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTopicsInterface)
+
+  RCLCPP_PUBLIC
+  explicit NodeTopics(rclcpp::node_interfaces::NodeBaseInterface * node_base);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTopics();
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::publisher::PublisherBase::SharedPtr
+  create_publisher(
+    const std::string & topic_name,
+    const rclcpp::PublisherFactory & publisher_factory,
+    rcl_publisher_options_t & publisher_options,
+    bool use_intra_process);
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_publisher(
+    rclcpp::publisher::PublisherBase::SharedPtr publisher);
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::subscription::SubscriptionBase::SharedPtr
+  create_subscription(
+    const std::string & topic_name,
+    const rclcpp::SubscriptionFactory & subscription_factory,
+    rcl_subscription_options_t & subscription_options,
+    bool use_intra_process);
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_subscription(
+    rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
+    rclcpp::callback_group::CallbackGroup::SharedPtr callback_group);
+
+private:
+  RCLCPP_DISABLE_COPY(NodeTopics)
+
+  NodeBaseInterface * node_base_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_TOPICS_HPP_

rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp

@@ -0,0 +1,78 @@
+// 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__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include "rcl/publisher.h"
+#include "rcl/subscription.h"
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/publisher.hpp"
+#include "rclcpp/publisher_factory.hpp"
+#include "rclcpp/subscription.hpp"
+#include "rclcpp/subscription_factory.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeTopics part of the Node API.
+class NodeTopicsInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTopicsInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::publisher::PublisherBase::SharedPtr
+  create_publisher(
+    const std::string & topic_name,
+    const rclcpp::PublisherFactory & publisher_factory,
+    rcl_publisher_options_t & publisher_options,
+    bool use_intra_process) = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_publisher(
+    rclcpp::publisher::PublisherBase::SharedPtr publisher) = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::subscription::SubscriptionBase::SharedPtr
+  create_subscription(
+    const std::string & topic_name,
+    const rclcpp::SubscriptionFactory & subscription_factory,
+    rcl_subscription_options_t & subscription_options,
+    bool use_intra_process) = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_subscription(
+    rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
+    rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_

rclcpp/include/rclcpp/parameter.hpp

@@ -80,6 +80,8 @@ public:
   rcl_interfaces::msg::ParameterValue
   get_parameter_value() const;
 
+  // The following get_value() variants require the use of ParameterType
+
   template<ParameterType type>
   typename std::enable_if<type == ParameterType::PARAMETER_INTEGER, int64_t>::type
   get_value() const
@@ -125,8 +127,8 @@ public:
   }
 
   template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_BYTES,
-  const std::vector<uint8_t> &>::type
+  typename std::enable_if<
+    type == ParameterType::PARAMETER_BYTES, const std::vector<uint8_t> &>::type
   get_value() const
   {
     if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BYTES) {
@@ -136,6 +138,46 @@ public:
     return value_.bytes_value;
   }
 
+  // The following get_value() variants allow the use of primitive types
+
+  template<typename type>
+  typename std::enable_if<
+    std::is_integral<type>::value && !std::is_same<type, bool>::value, int64_t>::type
+  get_value() const
+  {
+    return get_value<ParameterType::PARAMETER_INTEGER>();
+  }
+
+  template<typename type>
+  typename std::enable_if<std::is_floating_point<type>::value, double>::type
+  get_value() const
+  {
+    return get_value<ParameterType::PARAMETER_DOUBLE>();
+  }
+
+  template<typename type>
+  typename std::enable_if<std::is_convertible<type, std::string>::value, const std::string &>::type
+  get_value() const
+  {
+    return get_value<ParameterType::PARAMETER_STRING>();
+  }
+
+  template<typename type>
+  typename std::enable_if<std::is_same<type, bool>::value, bool>::type
+  get_value() const
+  {
+    return get_value<ParameterType::PARAMETER_BOOL>();
+  }
+
+  template<typename type>
+  typename std::enable_if<
+    std::is_convertible<
+      type, const std::vector<uint8_t> &>::value, const std::vector<uint8_t> &>::type
+  get_value() const
+  {
+    return get_value<ParameterType::PARAMETER_BYTES>();
+  }
+
   RCLCPP_PUBLIC
   int64_t
   as_int() const;
@@ -174,7 +216,7 @@ private:
 };
 
 
-/* Return a json encoded version of the parameter intended for a dict. */
+/// Return a json encoded version of the parameter intended for a dict.
 RCLCPP_PUBLIC
 std::string
 _to_json_dict_entry(const ParameterVariant & param);
@@ -193,12 +235,12 @@ operator<<(std::ostream & os, const std::vector<ParameterVariant> & parameters);
 namespace std
 {
 
-/* Return a json encoded version of the parameter intended for a list. */
+/// Return a json encoded version of the parameter intended for a list.
 RCLCPP_PUBLIC
 std::string
 to_string(const rclcpp::parameter::ParameterVariant & param);
 
-/* Return a json encoded version of a vector of parameters, as a string*/
+/// Return a json encoded version of a vector of parameters, as a string.
 RCLCPP_PUBLIC
 std::string
 to_string(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);

rclcpp/include/rclcpp/parameter_client.hpp

@@ -15,7 +15,9 @@
 #ifndef RCLCPP__PARAMETER_CLIENT_HPP_
 #define RCLCPP__PARAMETER_CLIENT_HPP_
 
+#include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 #include "rcl_interfaces/msg/parameter.hpp"
@@ -28,6 +30,7 @@
 #include "rcl_interfaces/srv/set_parameters.hpp"
 #include "rcl_interfaces/srv/set_parameters_atomically.hpp"
 #include "rclcpp/executors.hpp"
+#include "rclcpp/create_subscription.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/parameter.hpp"
@@ -43,12 +46,22 @@ namespace parameter_client
 class AsyncParametersClient
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(AsyncParametersClient);
+  RCLCPP_SMART_PTR_DEFINITIONS(AsyncParametersClient)
+
+  RCLCPP_PUBLIC
+  AsyncParametersClient(
+    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+    const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
+    const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
+    const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
   RCLCPP_PUBLIC
   AsyncParametersClient(
     const rclcpp::node::Node::SharedPtr node,
-    const std::string & remote_node_name = "");
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
   RCLCPP_PUBLIC
   std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>
@@ -91,16 +104,52 @@ public:
       void(std::shared_future<rcl_interfaces::msg::ListParametersResult>)
     > callback = nullptr);
 
-  template<typename CallbackT>
+  template<
+    typename CallbackT,
+    typename Alloc = std::allocator<void>,
+    typename SubscriptionT =
+    rclcpp::subscription::Subscription<rcl_interfaces::msg::ParameterEvent, Alloc>>
   typename rclcpp::subscription::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
   on_parameter_event(CallbackT && callback)
   {
-    return node_->create_subscription<rcl_interfaces::msg::ParameterEvent>(
-      "parameter_events", std::forward<CallbackT>(callback), rmw_qos_profile_parameter_events);
+    using rclcpp::message_memory_strategy::MessageMemoryStrategy;
+    auto msg_mem_strat =
+      MessageMemoryStrategy<rcl_interfaces::msg::ParameterEvent, Alloc>::create_default();
+
+    return rclcpp::create_subscription<
+      rcl_interfaces::msg::ParameterEvent, CallbackT, Alloc, SubscriptionT>(
+      this->node_topics_interface_.get(),
+      "parameter_events",
+      std::forward<CallbackT>(callback),
+      rmw_qos_profile_default,
+      nullptr,  // group,
+      false,  // ignore_local_publications,
+      false,  // use_intra_process_comms_,
+      msg_mem_strat,
+      std::make_shared<Alloc>());
   }
 
+  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 wait_for_service_nanoseconds(
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
+  }
+
+protected:
+  RCLCPP_PUBLIC
+  bool
+  wait_for_service_nanoseconds(std::chrono::nanoseconds timeout);
+
 private:
-  const rclcpp::node::Node::SharedPtr node_;
+  const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface_;
   rclcpp::client::Client<rcl_interfaces::srv::GetParameters>::SharedPtr get_parameters_client_;
   rclcpp::client::Client<rcl_interfaces::srv::GetParameterTypes>::SharedPtr
     get_parameter_types_client_;
@@ -116,21 +165,62 @@ private:
 class SyncParametersClient
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient);
+  RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient)
 
   RCLCPP_PUBLIC
-  SyncParametersClient(
-    rclcpp::node::Node::SharedPtr node);
+  explicit SyncParametersClient(
+    rclcpp::node::Node::SharedPtr node,
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
   RCLCPP_PUBLIC
   SyncParametersClient(
     rclcpp::executor::Executor::SharedPtr executor,
-    rclcpp::node::Node::SharedPtr node);
+    rclcpp::node::Node::SharedPtr node,
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
   RCLCPP_PUBLIC
   std::vector<rclcpp::parameter::ParameterVariant>
   get_parameters(const std::vector<std::string> & parameter_names);
 
+  RCLCPP_PUBLIC
+  bool
+  has_parameter(const std::string & parameter_name);
+
+  template<typename T>
+  T
+  get_parameter_impl(
+    const std::string & parameter_name, std::function<T()> parameter_not_found_handler)
+  {
+    std::vector<std::string> names;
+    names.push_back(parameter_name);
+    auto vars = get_parameters(names);
+    if ((vars.size() != 1) || (vars[0].get_type() == rclcpp::parameter::PARAMETER_NOT_SET)) {
+      return parameter_not_found_handler();
+    } else {
+      return static_cast<T>(vars[0].get_value<T>());
+    }
+  }
+
+  template<typename T>
+  T
+  get_parameter(const std::string & parameter_name, const T & default_value)
+  {
+    // *INDENT-OFF*
+    return get_parameter_impl(parameter_name,
+      std::function<T()>([&default_value]() -> T {return default_value; }));
+    // *INDENT-ON*
+  }
+
+  template<typename T>
+  T
+  get_parameter(const std::string & parameter_name)
+  {
+    // *INDENT-OFF*
+    return get_parameter_impl(parameter_name,
+      std::function<T()>([]() -> T {throw std::runtime_error("Parameter not set"); }));
+    // *INDENT-ON*
+  }
+
   RCLCPP_PUBLIC
   std::vector<rclcpp::parameter::ParameterType>
   get_parameter_types(const std::vector<std::string> & parameter_names);
@@ -156,6 +246,21 @@ public:
     return async_parameters_client_->on_parameter_event(std::forward<CallbackT>(callback));
   }
 
+  RCLCPP_PUBLIC
+  bool
+  service_is_ready() const
+  {
+    return async_parameters_client_->service_is_ready();
+  }
+
+  template<typename RatioT = std::milli>
+  bool
+  wait_for_service(
+    std::chrono::duration<int64_t, RatioT> timeout = std::chrono::duration<int64_t, RatioT>(-1))
+  {
+    return async_parameters_client_->wait_for_service(timeout);
+  }
+
 private:
   rclcpp::executor::Executor::SharedPtr executor_;
   rclcpp::node::Node::SharedPtr node_;

rclcpp/include/rclcpp/parameter_service.hpp

@@ -38,10 +38,12 @@ namespace parameter_service
 class ParameterService
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(ParameterService);
+  RCLCPP_SMART_PTR_DEFINITIONS(ParameterService)
 
   RCLCPP_PUBLIC
-  explicit ParameterService(const rclcpp::node::Node::SharedPtr node);
+  explicit ParameterService(
+    const rclcpp::node::Node::SharedPtr node,
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
 private:
   const rclcpp::node::Node::SharedPtr node_;

rclcpp/include/rclcpp/publisher.hpp

@@ -24,74 +24,92 @@
 #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/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
 
-// Forward declaration for friend statement
-namespace node
+// Forward declaration is used for friend statement.
+namespace node_interfaces
 {
-class Node;
-}  // namespace node
+class NodeTopicsInterface;
+}
 
 namespace publisher
 {
 
 class PublisherBase
 {
+  friend rclcpp::node_interfaces::NodeTopicsInterface;
+
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase);
+  RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase)
+
   /// Default constructor.
   /**
    * 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_base A pointer to the NodeBaseInterface for the parent node.
    * \param[in] topic The topic that this publisher publishes on.
-   * \param[in] queue_size The maximum number of unpublished messages to queue.
+   * \param[in] type_support The type support structure for the type to be published.
+   * \param[in] publisher_options QoS settings for this publisher.
    */
   RCLCPP_PUBLIC
   PublisherBase(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_publisher_t * publisher_handle,
-    std::string topic,
-    size_t queue_size);
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    const std::string & topic,
+    const rosidl_message_type_support_t & type_support,
+    const rcl_publisher_options_t & publisher_options);
 
-  /// Default destructor.
   RCLCPP_PUBLIC
   virtual ~PublisherBase();
 
   /// Get the topic that this publisher publishes on.
-  // \return The topic name.
+  /** \return The topic name. */
   RCLCPP_PUBLIC
-  const std::string &
+  const char *
   get_topic_name() const;
 
   /// Get the queue size for this publisher.
-  // \return The queue size.
+  /** \return The queue size. */
   RCLCPP_PUBLIC
   size_t
   get_queue_size() const;
 
   /// Get the global identifier for this publisher (used in rmw and by DDS).
-  // \return The gid.
+  /** \return The gid. */
   RCLCPP_PUBLIC
   const rmw_gid_t &
   get_gid() const;
 
   /// Get the global identifier for this publisher used by intra-process communication.
-  // \return The intra-process gid.
+  /** \return The intra-process gid. */
   RCLCPP_PUBLIC
   const rmw_gid_t &
   get_intra_process_gid() const;
 
+  /// Get the rcl publisher handle.
+  /** \return The rcl publisher handle. */
+  RCLCPP_PUBLIC
+  rcl_publisher_t *
+  get_publisher_handle();
+
+  /// Get the rcl publisher handle.
+  /** \return The rcl publisher handle. */
+  RCLCPP_PUBLIC
+  const rcl_publisher_t *
+  get_publisher_handle() const;
+
   /// Compare this publisher to a gid.
   /**
    * Note that this function calls the next function.
@@ -112,23 +130,21 @@ public:
   bool
   operator==(const rmw_gid_t * gid) const;
 
-  typedef std::function<uint64_t(uint64_t, void *, const std::type_info &)> StoreMessageCallbackT;
+  using StoreMessageCallbackT = std::function<uint64_t(uint64_t, void *, const std::type_info &)>;
 
-protected:
+  /// Implementation utility function used to setup intra process publishing after creation.
   RCLCPP_PUBLIC
   void
   setup_intra_process(
     uint64_t intra_process_publisher_id,
     StoreMessageCallbackT callback,
-    rmw_publisher_t * intra_process_publisher_handle);
+    const rcl_publisher_options_t & intra_process_options);
 
-  std::shared_ptr<rmw_node_t> node_handle_;
+protected:
+  std::shared_ptr<rcl_node_t> rcl_node_handle_;
 
-  rmw_publisher_t * publisher_handle_;
-  rmw_publisher_t * intra_process_publisher_handle_;
-
-  std::string topic_;
-  size_t queue_size_;
+  rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher();
+  rcl_publisher_t intra_process_publisher_handle_ = rcl_get_zero_initialized_publisher();
 
   uint64_t intra_process_publisher_id_;
   StoreMessageCallbackT store_intra_process_message_;
@@ -141,35 +157,38 @@ protected:
 template<typename MessageT, typename Alloc = std::allocator<void>>
 class Publisher : public PublisherBase
 {
-  friend rclcpp::node::Node;
-
 public:
   using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
   using MessageAlloc = typename MessageAllocTraits::allocator_type;
   using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
   using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
 
-  RCLCPP_SMART_PTR_DEFINITIONS(Publisher<MessageT, Alloc>);
+  RCLCPP_SMART_PTR_DEFINITIONS(Publisher<MessageT, Alloc>)
 
   Publisher(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_publisher_t * publisher_handle,
-    std::string topic,
-    size_t queue_size,
-    std::shared_ptr<Alloc> allocator)
-  : PublisherBase(node_handle, publisher_handle, topic, queue_size)
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    const std::string & topic,
+    const rcl_publisher_options_t & publisher_options,
+    const std::shared_ptr<MessageAlloc> & allocator)
+  : PublisherBase(
+      node_base,
+      topic,
+      *rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>(),
+      publisher_options),
+    message_allocator_(allocator)
   {
-    message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
     allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get());
   }
 
+  virtual ~Publisher()
+  {}
 
   /// Send a message to the topic for this publisher.
   /**
    * This function is templated on the input message type, MessageT.
    * \param[in] msg A shared pointer to the message to send.
    */
-  void
+  virtual void
   publish(std::unique_ptr<MessageT, MessageDeleter> & msg)
   {
     this->do_inter_process_publish(msg.get());
@@ -188,11 +207,11 @@ public:
       rcl_interfaces::msg::IntraProcessMessage ipm;
       ipm.publisher_id = intra_process_publisher_id_;
       ipm.message_sequence = message_seq;
-      auto status = rmw_publish(intra_process_publisher_handle_, &ipm);
-      if (status != RMW_RET_OK) {
-        // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+      auto status = rcl_publish(&intra_process_publisher_handle_, &ipm);
+      if (status != RCL_RET_OK) {
+        // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
         throw std::runtime_error(
-          std::string("failed to publish intra process message: ") + rmw_get_error_string_safe());
+          std::string("failed to publish intra process message: ") + rcl_get_error_string_safe());
         // *INDENT-ON*
       }
     } else {
@@ -201,7 +220,7 @@ public:
     }
   }
 
-  void
+  virtual void
   publish(const std::shared_ptr<MessageT> & msg)
   {
     // Avoid allocating when not using intra process.
@@ -220,7 +239,7 @@ public:
     return this->publish(unique_msg);
   }
 
-  void
+  virtual void
   publish(std::shared_ptr<const MessageT> msg)
   {
     // Avoid allocating when not using intra process.
@@ -239,7 +258,7 @@ public:
     return this->publish(unique_msg);
   }
 
-  void
+  virtual void
   publish(const MessageT & msg)
   {
     // Avoid allocating when not using intra process.
@@ -254,6 +273,15 @@ public:
     return this->publish(unique_msg);
   }
 
+  virtual void
+  publish(const MessageT * msg)
+  {
+    if (!msg) {
+      throw std::runtime_error("msg argument is nullptr");
+    }
+    return this->publish(*msg);
+  }
+
   std::shared_ptr<MessageAlloc> get_allocator() const
   {
     return message_allocator_;
@@ -263,11 +291,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*
     }
   }
@@ -278,6 +306,7 @@ protected:
 };
 
 }  // namespace publisher
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__PUBLISHER_HPP_

rclcpp/include/rclcpp/publisher_factory.hpp

@@ -0,0 +1,148 @@
+// 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__PUBLISHER_FACTORY_HPP_
+#define RCLCPP__PUBLISHER_FACTORY_HPP_
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include "rcl/publisher.h"
+
+#include "rosidl_typesupport_cpp/message_type_support.hpp"
+
+#include "rclcpp/publisher.hpp"
+#include "rclcpp/intra_process_manager.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Factory with functions used to create a MessageT specific PublisherT.
+/**
+ * This factory class is used to encapsulate the template generated functions
+ * which are used during the creation of a Message type specific publisher
+ * within a non-templated class.
+ *
+ * It is created using the create_publisher_factory function, which is usually
+ * called from a templated "create_publisher" method on the Node class, and
+ * is passed to the non-templated "create_publisher" method on the NodeTopics
+ * class where it is used to create and setup the Publisher.
+ */
+struct PublisherFactory
+{
+  // Creates a PublisherT<MessageT, ...> publisher object and returns it as a PublisherBase.
+  using PublisherFactoryFunction = std::function<
+      rclcpp::publisher::PublisherBase::SharedPtr(
+        rclcpp::node_interfaces::NodeBaseInterface * node_base,
+        const std::string & topic_name,
+        rcl_publisher_options_t & publisher_options)>;
+
+  PublisherFactoryFunction create_typed_publisher;
+
+  // Adds the PublisherBase to the intraprocess manager with the correctly
+  // templated call to IntraProcessManager::store_intra_process_message.
+  using AddPublisherToIntraProcessManagerFunction = std::function<
+      uint64_t(
+        rclcpp::intra_process_manager::IntraProcessManager * ipm,
+        rclcpp::publisher::PublisherBase::SharedPtr publisher)>;
+
+  AddPublisherToIntraProcessManagerFunction add_publisher_to_intra_process_manager;
+
+  // Creates the callback function which is called on each
+  // PublisherT::publish() and which handles the intra process transmission of
+  // the message being published.
+  using SharedPublishCallbackFactoryFunction = std::function<
+      rclcpp::publisher::PublisherBase::StoreMessageCallbackT(
+        rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm)>;
+
+  SharedPublishCallbackFactoryFunction create_shared_publish_callback;
+};
+
+/// Return a PublisherFactory with functions setup for creating a PublisherT<MessageT, Alloc>.
+template<typename MessageT, typename Alloc, typename PublisherT>
+PublisherFactory
+create_publisher_factory(std::shared_ptr<Alloc> allocator)
+{
+  PublisherFactory factory;
+
+  // factory function that creates a MessageT specific PublisherT
+  factory.create_typed_publisher =
+    [allocator](
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    const std::string & topic_name,
+    rcl_publisher_options_t & publisher_options) -> std::shared_ptr<PublisherT>
+    {
+      auto message_alloc = std::make_shared<typename PublisherT::MessageAlloc>(*allocator.get());
+      publisher_options.allocator = allocator::get_rcl_allocator<MessageT>(*message_alloc.get());
+
+      return std::make_shared<PublisherT>(node_base, topic_name, publisher_options, message_alloc);
+    };
+
+  // function to add a publisher to the intra process manager
+  factory.add_publisher_to_intra_process_manager =
+    [](
+    rclcpp::intra_process_manager::IntraProcessManager * ipm,
+    rclcpp::publisher::PublisherBase::SharedPtr publisher) -> uint64_t
+    {
+      return ipm->add_publisher<MessageT, Alloc>(std::dynamic_pointer_cast<PublisherT>(publisher));
+    };
+
+  // function to create a shared publish callback std::function
+  using StoreMessageCallbackT = rclcpp::publisher::PublisherBase::StoreMessageCallbackT;
+  factory.create_shared_publish_callback =
+    [](rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm) -> StoreMessageCallbackT
+    {
+      rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = ipm;
+
+      // this function is called on each call to publish() and handles storing
+      // of the published message in the intra process manager
+      auto shared_publish_callback =
+        [weak_ipm](uint64_t publisher_id, void * msg, const std::type_info & type_info) -> uint64_t
+        {
+          auto ipm = weak_ipm.lock();
+          if (!ipm) {
+            // TODO(wjwwood): should this just return silently? Or maybe return with a warning?
+            throw std::runtime_error(
+                    "intra process publish called after destruction of intra process manager");
+          }
+          if (!msg) {
+            throw std::runtime_error("cannot publisher msg which is a null pointer");
+          }
+          auto & message_type_info = typeid(MessageT);
+          if (message_type_info != type_info) {
+            throw std::runtime_error(
+                    std::string("published type '") + type_info.name() +
+                    "' is incompatible from the publisher type '" + message_type_info.name() + "'");
+          }
+          MessageT * typed_message_ptr = static_cast<MessageT *>(msg);
+          using MessageDeleter = typename publisher::Publisher<MessageT, Alloc>::MessageDeleter;
+          std::unique_ptr<MessageT, MessageDeleter> unique_msg(typed_message_ptr);
+          uint64_t message_seq =
+            ipm->store_intra_process_message<MessageT, Alloc>(publisher_id, unique_msg);
+          return message_seq;
+        };
+
+      return shared_publish_callback;
+    };
+
+  // return the factory now that it is populated
+  return factory;
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__PUBLISHER_FACTORY_HPP_

rclcpp/include/rclcpp/rate.hpp

@@ -31,7 +31,7 @@ namespace rate
 class RateBase
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(RateBase);
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(RateBase)
 
   virtual bool sleep() = 0;
   virtual bool is_steady() const = 0;
@@ -46,7 +46,7 @@ template<class Clock = std::chrono::high_resolution_clock>
 class GenericRate : public RateBase
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(GenericRate);
+  RCLCPP_SMART_PTR_DEFINITIONS(GenericRate)
 
   explicit GenericRate(double rate)
   : GenericRate<Clock>(
@@ -106,7 +106,7 @@ public:
   }
 
 private:
-  RCLCPP_DISABLE_COPY(GenericRate);
+  RCLCPP_DISABLE_COPY(GenericRate)
 
   std::chrono::nanoseconds period_;
   using ClockDurationNano = std::chrono::duration<typename Clock::rep, std::nano>;

rclcpp/include/rclcpp/rclcpp.hpp

@@ -12,6 +12,113 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+/** \mainpage rclcpp: ROS Client Library for C++
+ *
+ * `rclcpp` provides the canonical C++ API for interacting with ROS.
+ * It consists of these main components:
+ *
+ * - Nodes
+ *   - rclcpp::node::Node
+ *   - rclcpp/node.hpp
+ * - Publisher
+ *   - rclcpp::node::Node::create_publisher()
+ *   - rclcpp::publisher::Publisher
+ *   - rclcpp::publisher::Publisher::publish()
+ *   - rclcpp/publisher.hpp
+ * - Subscription
+ *   - rclcpp::node::Node::create_subscription()
+ *   - rclcpp::subscription::Subscription
+ *   - rclcpp/subscription.hpp
+ * - Service Client
+ *   - rclcpp::node::Node::create_client()
+ *   - rclcpp::client::Client
+ *   - rclcpp/client.hpp
+ * - Service Server
+ *   - rclcpp::node::Node::create_service()
+ *   - rclcpp::service::Service
+ *   - rclcpp/service.hpp
+ * - Timer
+ *   - rclcpp::node::Node::create_wall_timer()
+ *   - rclcpp::timer::WallTimer
+ *   - rclcpp::timer::TimerBase
+ *   - rclcpp/timer.hpp
+ * - Parameters:
+ *   - rclcpp::node::Node::set_parameters()
+ *   - rclcpp::node::Node::get_parameters()
+ *   - rclcpp::node::Node::get_parameter()
+ *   - rclcpp::node::Node::describe_parameters()
+ *   - rclcpp::node::Node::list_parameters()
+ *   - rclcpp::node::Node::register_param_change_callback()
+ *   - rclcpp::parameter::ParameterVariant
+ *   - rclcpp::parameter_client::AsyncParametersClient
+ *   - rclcpp::parameter_client::SyncParametersClient
+ *   - rclcpp/parameter.hpp
+ *   - rclcpp/parameter_client.hpp
+ *   - rclcpp/parameter_service.hpp
+ * - Rate:
+ *   - rclcpp::rate::Rate
+ *   - rclcpp::rate::WallRate
+ *   - rclcpp/rate.hpp
+ *
+ * There are also some components which help control the execution of callbacks:
+ *
+ * - Executors (responsible for execution of callbacks through a blocking spin):
+ *   - rclcpp::spin()
+ *   - rclcpp::spin_some()
+ *   - rclcpp::spin_until_future_complete()
+ *   - rclcpp::executors::single_threaded_executor::SingleThreadedExecutor
+ *   - rclcpp::executors::single_threaded_executor::SingleThreadedExecutor::add_node()
+ *   - rclcpp::executors::single_threaded_executor::SingleThreadedExecutor::spin()
+ *   - rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor
+ *   - rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor::add_node()
+ *   - rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor::spin()
+ *   - rclcpp/executor.hpp
+ *   - rclcpp/executors.hpp
+ *   - rclcpp/executors/single_threaded_executor.hpp
+ *   - rclcpp/executors/multi_threaded_executor.hpp
+ * - CallbackGroups (mechanism for enforcing concurrency rules for callbacks):
+ *   - rclcpp::node::Node::create_callback_group()
+ *   - rclcpp::callback_group::CallbackGroup
+ *   - rclcpp/callback_group.hpp
+ *
+ * Additionally, there are some methods for introspecting the ROS graph:
+ *
+ * - Graph Events (a waitable event object that wakes up when the graph changes):
+ *   - rclcpp::node::Node::get_graph_event()
+ *   - rclcpp::node::Node::wait_for_graph_change()
+ *   - rclcpp::event::Event
+ * - List topic names and types:
+ *   - rclcpp::node::Node::get_topic_names_and_types()
+ * - Get the number of publishers or subscribers on a topic:
+ *   - rclcpp::node::Node::count_publishers()
+ *   - rclcpp::node::Node::count_subscribers()
+ *
+ * Finally, there are many internal API's and utilities:
+ *
+ * - Exceptions:
+ *   - rclcpp/exceptions.hpp
+ * - Allocator related items:
+ *   - rclcpp/allocator/allocator_common.hpp
+ *   - rclcpp/allocator/allocator_deleter.hpp
+ * - Memory management tools:
+ *   - rclcpp/memory_strategies.hpp
+ *   - rclcpp/memory_strategy.hpp
+ *   - rclcpp/message_memory_strategy.hpp
+ *   - rclcpp/strategies/allocator_memory_strategy.hpp
+ *   - rclcpp/strategies/message_pool_memory_strategy.hpp
+ * - Context object which is shared amongst multiple Nodes:
+ *   - rclcpp::context::Context
+ *   - rclcpp/context.hpp
+ *   - rclcpp/contexts/default_context.hpp
+ * - Various utilities:
+ *   - rclcpp/function_traits.hpp
+ *   - rclcpp/macros.hpp
+ *   - rclcpp/scope_exit.hpp
+ *   - rclcpp/time.hpp
+ *   - rclcpp/utilities.hpp
+ *   - rclcpp/visibility_control.hpp
+ */
+
 #ifndef RCLCPP__RCLCPP_HPP_
 #define RCLCPP__RCLCPP_HPP_
 
@@ -24,42 +131,10 @@
 #include "rclcpp/parameter_client.hpp"
 #include "rclcpp/parameter_service.hpp"
 #include "rclcpp/rate.hpp"
+#include "rclcpp/time.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
 
-// NOLINTNEXTLINE(runtime/int)
-inline const std::chrono::seconds operator"" _s(unsigned long long s)
-{
-  return std::chrono::seconds(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));
-}
-
-// NOLINTNEXTLINE(runtime/int)
-inline const std::chrono::nanoseconds operator"" _ms(unsigned long long ms)
-{
-  return std::chrono::milliseconds(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));
-}
-
-// NOLINTNEXTLINE(runtime/int)
-inline const std::chrono::nanoseconds operator"" _ns(unsigned long long ns)
-{
-  return std::chrono::nanoseconds(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
 {
 

rclcpp/include/rclcpp/service.hpp

@@ -21,8 +21,14 @@
 #include <sstream>
 #include <string>
 
+#include "rcl/error_handling.h"
+#include "rcl/service.h"
+
 #include "rclcpp/any_service_callback.hpp"
+#include "rclcpp/exceptions.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
@@ -35,13 +41,16 @@ namespace service
 class ServiceBase
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase);
+  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);
+    std::shared_ptr<rcl_node_t> node_handle,
+    const std::string & service_name);
+
+  RCLCPP_PUBLIC
+  explicit ServiceBase(
+    std::shared_ptr<rcl_node_t> node_handle);
 
   RCLCPP_PUBLIC
   virtual ~ServiceBase();
@@ -51,22 +60,35 @@ public:
   get_service_name();
 
   RCLCPP_PUBLIC
-  const rmw_service_t *
+  rcl_service_t *
   get_service_handle();
 
+  RCLCPP_PUBLIC
+  const rcl_service_t *
+  get_service_handle() const;
+
   virtual std::shared_ptr<void> create_request() = 0;
   virtual std::shared_ptr<rmw_request_id_t> create_request_header() = 0;
   virtual void handle_request(
     std::shared_ptr<rmw_request_id_t> request_header,
     std::shared_ptr<void> request) = 0;
 
-private:
-  RCLCPP_DISABLE_COPY(ServiceBase);
+protected:
+  RCLCPP_DISABLE_COPY(ServiceBase)
 
-  std::shared_ptr<rmw_node_t> node_handle_;
+  RCLCPP_PUBLIC
+  rcl_node_t *
+  get_rcl_node_handle();
 
-  rmw_service_t * service_handle_;
+  RCLCPP_PUBLIC
+  const rcl_node_t *
+  get_rcl_node_handle() const;
+
+  std::shared_ptr<rcl_node_t> node_handle_;
+
+  rcl_service_t * service_handle_ = nullptr;
   std::string service_name_;
+  bool owns_rcl_handle_ = true;
 };
 
 using any_service_callback::AnyServiceCallback;
@@ -85,18 +107,83 @@ public:
         const std::shared_ptr<rmw_request_id_t>,
         const std::shared_ptr<typename ServiceT::Request>,
         std::shared_ptr<typename ServiceT::Response>)>;
-  RCLCPP_SMART_PTR_DEFINITIONS(Service);
+  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,
+    rcl_service_options_t & service_options)
+  : ServiceBase(node_handle, service_name), any_callback_(any_callback)
+  {
+    using rosidl_typesupport_cpp::get_service_type_support_handle;
+    auto service_type_support_handle = get_service_type_support_handle<ServiceT>();
+
+    // rcl does the static memory allocation here
+    service_handle_ = new rcl_service_t;
+    *service_handle_ = rcl_get_zero_initialized_service();
+
+    rcl_ret_t ret = rcl_service_init(
+      service_handle_,
+      node_handle.get(),
+      service_type_support_handle,
+      service_name.c_str(),
+      &service_options);
+    if (ret != RCL_RET_OK) {
+      if (ret == RCL_RET_SERVICE_NAME_INVALID) {
+        auto rcl_node_handle = get_rcl_node_handle();
+        // this will throw on any validation problem
+        rcl_reset_error();
+        expand_topic_or_service_name(
+          service_name,
+          rcl_node_get_name(rcl_node_handle),
+          rcl_node_get_namespace(rcl_node_handle),
+          true);
+      }
+
+      rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service");
+    }
+  }
+
+  Service(
+    std::shared_ptr<rcl_node_t> node_handle,
+    rcl_service_t * service_handle,
     AnyServiceCallback<ServiceT> any_callback)
-  : ServiceBase(node_handle, service_handle, service_name), any_callback_(any_callback)
-  {}
+  : ServiceBase(node_handle),
+    any_callback_(any_callback)
+  {
+    // check if service handle was initialized
+    // TODO(karsten1987): Take this verification
+    // directly in rcl_*_t
+    // see: https://github.com/ros2/rcl/issues/81
+    if (!service_handle->impl) {
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
+      throw std::runtime_error(
+        std::string("rcl_service_t in constructor argument must be initialized beforehand."));
+      // *INDENT-ON*
+    }
+    service_handle_ = service_handle;
+    service_name_ = std::string(rcl_service_get_service_name(service_handle));
+    owns_rcl_handle_ = false;
+  }
 
   Service() = delete;
 
+  virtual ~Service()
+  {
+    // check if you have ownership of the handle
+    if (owns_rcl_handle_) {
+      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();
+        rcl_reset_error();
+      }
+      delete service_handle_;
+    }
+  }
+
   std::shared_ptr<void> create_request()
   {
     return std::shared_ptr<void>(new typename ServiceT::Request());
@@ -122,17 +209,15 @@ public:
     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)
-      throw std::runtime_error(
-        std::string("failed to send response: ") + rmw_get_error_string_safe());
-      // *INDENT-ON*
+    rcl_ret_t status = rcl_send_response(get_service_handle(), req_id.get(), response.get());
+
+    if (status != RCL_RET_OK) {
+      rclcpp::exceptions::throw_from_rcl_error(status, "failed to send response");
     }
   }
 
 private:
-  RCLCPP_DISABLE_COPY(Service);
+  RCLCPP_DISABLE_COPY(Service)
 
   AnyServiceCallback<ServiceT> any_callback_;
 };

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -18,6 +18,8 @@
 #include <memory>
 #include <vector>
 
+#include "rcl/allocator.h"
+
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/memory_strategy.hpp"
 #include "rclcpp/node.hpp"
@@ -42,7 +44,7 @@ template<typename Alloc = std::allocator<void>>
 class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(AllocatorMemoryStrategy<Alloc>);
+  RCLCPP_SMART_PTR_DEFINITIONS(AllocatorMemoryStrategy<Alloc>)
 
   using ExecAllocTraits = allocator::AllocRebind<executor::AnyExecutable, Alloc>;
   using ExecAlloc = typename ExecAllocTraits::allocator_type;
@@ -50,8 +52,6 @@ public:
   using VoidAllocTraits = typename allocator::AllocRebind<void *, Alloc>;
   using VoidAlloc = typename VoidAllocTraits::allocator_type;
 
-  using WeakNodeVector = std::vector<std::weak_ptr<node::Node>>;
-
   explicit AllocatorMemoryStrategy(std::shared_ptr<Alloc> allocator)
   {
     executable_allocator_ = std::make_shared<ExecAlloc>(*allocator.get());
@@ -64,7 +64,7 @@ public:
     allocator_ = std::make_shared<VoidAlloc>();
   }
 
-  void add_guard_condition(const rmw_guard_condition_t * guard_condition)
+  void add_guard_condition(const rcl_guard_condition_t * guard_condition)
   {
     for (const auto & existing_guard_condition : guard_conditions_) {
       if (existing_guard_condition == guard_condition) {
@@ -74,7 +74,7 @@ public:
     guard_conditions_.push_back(guard_condition);
   }
 
-  void remove_guard_condition(const rmw_guard_condition_t * guard_condition)
+  void remove_guard_condition(const rcl_guard_condition_t * guard_condition)
   {
     for (auto it = guard_conditions_.begin(); it != guard_conditions_.end(); ++it) {
       if (*it == guard_condition) {
@@ -84,69 +84,40 @@ public:
     }
   }
 
-  size_t fill_subscriber_handles(void ** & ptr)
-  {
-    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);
-      }
-    }
-    ptr = subscriber_handles_.data();
-    return subscriber_handles_.size();
-  }
-
-  // return the new number of services
-  size_t fill_service_handles(void ** & ptr)
-  {
-    for (auto & service : services_) {
-      service_handles_.push_back(service->get_service_handle()->data);
-    }
-    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();
-  }
-
-  size_t fill_guard_condition_handles(void ** & ptr)
-  {
-    for (const auto & guard_condition : guard_conditions_) {
-      if (guard_condition) {
-        guard_condition_handles_.push_back(guard_condition->data);
-      }
-    }
-    ptr = guard_condition_handles_.data();
-    return guard_condition_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();
-    guard_condition_handles_.clear();
+    timer_handles_.clear();
   }
 
-  void remove_null_handles()
+  virtual void remove_null_handles(rcl_wait_set_t * wait_set)
   {
-    subscriber_handles_.erase(
-      std::remove(subscriber_handles_.begin(), subscriber_handles_.end(), nullptr),
-      subscriber_handles_.end()
+    for (size_t i = 0; i < wait_set->size_of_subscriptions; ++i) {
+      if (!wait_set->subscriptions[i]) {
+        subscription_handles_[i] = nullptr;
+      }
+    }
+    for (size_t i = 0; i < wait_set->size_of_services; ++i) {
+      if (!wait_set->services[i]) {
+        service_handles_[i] = nullptr;
+      }
+    }
+    for (size_t i = 0; i < wait_set->size_of_clients; ++i) {
+      if (!wait_set->clients[i]) {
+        client_handles_[i] = nullptr;
+      }
+    }
+    for (size_t i = 0; i < wait_set->size_of_timers; ++i) {
+      if (!wait_set->timers[i]) {
+        timer_handles_[i] = nullptr;
+      }
+    }
+
+    subscription_handles_.erase(
+      std::remove(subscription_handles_.begin(), subscription_handles_.end(), nullptr),
+      subscription_handles_.end()
     );
 
     service_handles_.erase(
@@ -159,9 +130,9 @@ public:
       client_handles_.end()
     );
 
-    guard_condition_handles_.erase(
-      std::remove(guard_condition_handles_.begin(), guard_condition_handles_.end(), nullptr),
-      guard_condition_handles_.end()
+    timer_handles_.erase(
+      std::remove(timer_handles_.begin(), timer_handles_.end(), nullptr),
+      timer_handles_.end()
     );
   }
 
@@ -171,7 +142,7 @@ public:
     for (auto & weak_node : weak_nodes) {
       auto node = weak_node.lock();
       if (!node) {
-        has_invalid_weak_nodes = false;
+        has_invalid_weak_nodes = true;
         continue;
       }
       for (auto & weak_group : node->get_callback_groups()) {
@@ -182,18 +153,29 @@ 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()) {
+        for (auto & weak_service : group->get_service_ptrs()) {
+          auto service = weak_service.lock();
           if (service) {
-            services_.push_back(service);
+            service_handles_.push_back(service->get_service_handle());
           }
         }
         for (auto & weak_client : group->get_client_ptrs()) {
           auto client = weak_client.lock();
           if (client) {
-            clients_.push_back(client);
+            client_handles_.push_back(client->get_client_handle());
+          }
+        }
+        for (auto & weak_timer : group->get_timer_ptrs()) {
+          auto timer = weak_timer.lock();
+          if (timer) {
+            timer_handles_.push_back(timer->get_timer_handle());
           }
         }
       }
@@ -201,6 +183,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.
@@ -213,21 +234,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()) {
@@ -243,12 +264,12 @@ public:
           any_exec->subscription = subscription;
         }
         any_exec->callback_group = group;
-        any_exec->node = get_node_by_group(group, weak_nodes);
-        subscriber_handles_.erase(it);
+        any_exec->node_base = get_node_by_group(group, weak_nodes);
+        subscription_handles_.erase(it);
         return;
       }
       // Else, the subscription is no longer valid, remove it and continue
-      subscriber_handles_.erase(it);
+      subscription_handles_.erase(it);
     }
   }
 
@@ -277,7 +298,7 @@ public:
         // Otherwise it is safe to set and return the any_exec
         any_exec->service = service;
         any_exec->callback_group = group;
-        any_exec->node = get_node_by_group(group, weak_nodes);
+        any_exec->node_base = get_node_by_group(group, weak_nodes);
         service_handles_.erase(it);
         return;
       }
@@ -310,7 +331,7 @@ public:
         // Otherwise it is safe to set and return the any_exec
         any_exec->client = client;
         any_exec->callback_group = group;
-        any_exec->node = get_node_by_group(group, weak_nodes);
+        any_exec->node_base = get_node_by_group(group, weak_nodes);
         client_handles_.erase(it);
         return;
       }
@@ -319,20 +340,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 rmw_guard_condition_t *> guard_conditions_;
+  VectorRebind<const rcl_guard_condition_t *> guard_conditions_;
 
-  VectorRebind<void *> subscriber_handles_;
-  VectorRebind<void *> service_handles_;
-  VectorRebind<void *> client_handles_;
-  VectorRebind<void *> guard_condition_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_;

rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp

@@ -15,6 +15,8 @@
 #ifndef RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_
 #define RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_
 
+#include <memory>
+
 #include "rclcpp/macros.hpp"
 #include "rclcpp/message_memory_strategy.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -44,7 +46,7 @@ class MessagePoolMemoryStrategy
   : public message_memory_strategy::MessageMemoryStrategy<MessageT>
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(MessagePoolMemoryStrategy);
+  RCLCPP_SMART_PTR_DEFINITIONS(MessagePoolMemoryStrategy)
 
   /// Default constructor
   MessagePoolMemoryStrategy()

rclcpp/include/rclcpp/subscription.hpp

@@ -24,20 +24,26 @@
 #include <sstream>
 #include <string>
 
+#include "rcl/error_handling.h"
+#include "rcl/subscription.h"
+
 #include "rcl_interfaces/msg/intra_process_message.hpp"
 
+#include "rclcpp/exceptions.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/expand_topic_or_service_name.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
 
-namespace node
+namespace node_interfaces
 {
-class Node;
-}  // namespace node
+class NodeTopicsInterface;
+}  // namespace node_interfaces
 
 namespace subscription
 {
@@ -47,20 +53,21 @@ namespace subscription
 class SubscriptionBase
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(SubscriptionBase);
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(SubscriptionBase)
 
   /// 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] type_support_handle rosidl type support struct, for the Message type of the topic.
    * \param[in] topic_name Name of the topic to subscribe to.
-   * \param[in] ignore_local_publications True to ignore local publications (unused).
+   * \param[in] subscription_options options for the subscription.
    */
   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 rosidl_message_type_support_t & type_support_handle,
     const std::string & topic_name,
-    bool ignore_local_publications);
+    const rcl_subscription_options_t & subscription_options);
 
   /// Default destructor.
   RCLCPP_PUBLIC
@@ -68,19 +75,23 @@ public:
 
   /// Get the topic that this subscription is subscribed on.
   RCLCPP_PUBLIC
-  const std::string &
+  const char *
   get_topic_name() const;
 
   RCLCPP_PUBLIC
-  const rmw_subscription_t *
+  rcl_subscription_t *
+  get_subscription_handle();
+
+  RCLCPP_PUBLIC
+  const rcl_subscription_t *
   get_subscription_handle() const;
 
   RCLCPP_PUBLIC
-  const rmw_subscription_t *
+  virtual const rcl_subscription_t *
   get_intra_process_subscription_handle() const;
 
   /// Borrow a new message.
-  // \return Shared pointer to the fresh message.
+  /** \return Shared pointer to the fresh message. */
   virtual std::shared_ptr<void>
   create_message() = 0;
   /// Check if we need to handle the message, and execute the callback if we do.
@@ -92,26 +103,22 @@ public:
   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.
+  /** \param[in] message Shared pointer to the returned 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_;
+  RCLCPP_DISABLE_COPY(SubscriptionBase)
 };
 
 using any_subscription_callback::AnySubscriptionCallback;
@@ -120,7 +127,7 @@ using any_subscription_callback::AnySubscriptionCallback;
 template<typename MessageT, typename Alloc = std::allocator<void>>
 class Subscription : public SubscriptionBase
 {
-  friend class rclcpp::node::Node;
+  friend class rclcpp::node_interfaces::NodeTopicsInterface;
 
 public:
   using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
@@ -128,34 +135,36 @@ public:
   using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
   using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
 
-  RCLCPP_SMART_PTR_DEFINITIONS(Subscription);
+  RCLCPP_SMART_PTR_DEFINITIONS(Subscription)
 
   /// Default constructor.
   /**
    * 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] subscription_options options for the subscription.
+   * \param[in] callback User defined callback to call when a message is received.
    * \param[in] memory_strategy The memory strategy to be used for managing message memory.
    */
   Subscription(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_subscription_t * subscription_handle,
+    std::shared_ptr<rcl_node_t> node_handle,
     const std::string & topic_name,
-    bool ignore_local_publications,
+    const rcl_subscription_options_t & subscription_options,
     AnySubscriptionCallback<MessageT, Alloc> callback,
     typename message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
     memory_strategy = message_memory_strategy::MessageMemoryStrategy<MessageT,
     Alloc>::create_default())
-  : SubscriptionBase(node_handle, subscription_handle, topic_name, ignore_local_publications),
+  : SubscriptionBase(
+      node_handle,
+      *rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>(),
+      topic_name,
+      subscription_options),
     any_callback_(callback),
     message_memory_strategy_(memory_strategy),
     get_intra_process_message_callback_(nullptr),
     matches_any_intra_process_publishers_(nullptr)
-  {
-  }
+  {}
 
   /// Support dynamically setting the message memory strategy.
   /**
@@ -190,6 +199,8 @@ public:
     any_callback_.dispatch(typed_message, message_info);
   }
 
+  /// Return the loaned message.
+  /** \param message message to be returned */
   void return_message(std::shared_ptr<void> & message)
   {
     auto typed_message = std::static_pointer_cast<MessageT>(message);
@@ -222,26 +233,55 @@ public:
     any_callback_.dispatch_intra_process(msg, message_info);
   }
 
-private:
-  typedef
-    std::function<
-      void (uint64_t, uint64_t, uint64_t, MessageUniquePtr &)
-    > GetMessageCallbackType;
-  typedef std::function<bool (const rmw_gid_t *)> MatchesAnyPublishersCallbackType;
+  using GetMessageCallbackType =
+      std::function<void(uint64_t, uint64_t, uint64_t, MessageUniquePtr &)>;
+  using MatchesAnyPublishersCallbackType = std::function<bool(const rmw_gid_t *)>;
 
+  /// Implemenation detail.
   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)
   {
+    std::string intra_process_topic_name = std::string(get_topic_name()) + "/_intra";
+    rcl_ret_t ret = rcl_subscription_init(
+      &intra_process_subscription_handle_,
+      node_handle_.get(),
+      rclcpp::type_support::get_intra_process_message_msg_type_support(),
+      intra_process_topic_name.c_str(),
+      &intra_process_options);
+    if (ret != RCL_RET_OK) {
+      if (ret == RCL_RET_TOPIC_NAME_INVALID) {
+        auto rcl_node_handle = node_handle_.get();
+        // this will throw on any validation problem
+        rcl_reset_error();
+        expand_topic_or_service_name(
+          intra_process_topic_name,
+          rcl_node_get_name(rcl_node_handle),
+          rcl_node_get_namespace(rcl_node_handle));
+      }
+
+      rclcpp::exceptions::throw_from_rcl_error(ret, "could not create intra process subscription");
+    }
+
     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;
   }
 
-  RCLCPP_DISABLE_COPY(Subscription);
+  /// Implemenation detail.
+  const rcl_subscription_t *
+  get_intra_process_subscription_handle() const
+  {
+    if (!get_intra_process_message_callback_) {
+      return nullptr;
+    }
+    return &intra_process_subscription_handle_;
+  }
+
+private:
+  RCLCPP_DISABLE_COPY(Subscription)
 
   AnySubscriptionCallback<MessageT, Alloc> any_callback_;
   typename message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr

rclcpp/include/rclcpp/subscription_factory.hpp

@@ -0,0 +1,171 @@
+// 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__SUBSCRIPTION_FACTORY_HPP_
+#define RCLCPP__SUBSCRIPTION_FACTORY_HPP_
+
+#include <functional>
+#include <memory>
+#include <string>
+#include <utility>
+
+#include "rcl/subscription.h"
+
+#include "rosidl_typesupport_cpp/message_type_support.hpp"
+
+#include "rclcpp/subscription.hpp"
+#include "rclcpp/intra_process_manager.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Factory with functions used to create a Subscription<MessageT>.
+/**
+ * This factory class is used to encapsulate the template generated functions
+ * which are used during the creation of a Message type specific subscription
+ * within a non-templated class.
+ *
+ * It is created using the create_subscription_factory function, which is
+ * usually called from a templated "create_subscription" method of the Node
+ * class, and is passed to the non-templated "create_subscription" method of
+ * the NodeTopics class where it is used to create and setup the Subscription.
+ */
+struct SubscriptionFactory
+{
+  // Creates a Subscription<MessageT> object and returns it as a SubscriptionBase.
+  using SubscriptionFactoryFunction = std::function<
+      rclcpp::subscription::SubscriptionBase::SharedPtr(
+        rclcpp::node_interfaces::NodeBaseInterface * node_base,
+        const std::string & topic_name,
+        rcl_subscription_options_t & subscription_options)>;
+
+  SubscriptionFactoryFunction create_typed_subscription;
+
+  // Function that takes a MessageT from the intra process manager
+  using SetupIntraProcessFunction = std::function<void(
+        rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm,
+        rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
+        const rcl_subscription_options_t & subscription_options)>;
+
+  SetupIntraProcessFunction setup_intra_process;
+};
+
+/// Return a SubscriptionFactory with functions for creating a SubscriptionT<MessageT, Alloc>.
+template<typename MessageT, typename CallbackT, typename Alloc, typename SubscriptionT>
+SubscriptionFactory
+create_subscription_factory(
+  CallbackT && callback,
+  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
+  msg_mem_strat,
+  std::shared_ptr<Alloc> allocator)
+{
+  SubscriptionFactory factory;
+
+  using rclcpp::subscription::AnySubscriptionCallback;
+  AnySubscriptionCallback<MessageT, Alloc> any_subscription_callback(allocator);
+  any_subscription_callback.set(std::forward<CallbackT>(callback));
+
+  auto message_alloc =
+    std::make_shared<typename subscription::Subscription<MessageT, Alloc>::MessageAlloc>();
+
+  // factory function that creates a MessageT specific SubscriptionT
+  factory.create_typed_subscription =
+    [allocator, msg_mem_strat, any_subscription_callback, message_alloc](
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    const std::string & topic_name,
+    rcl_subscription_options_t & subscription_options
+    ) -> rclcpp::subscription::SubscriptionBase::SharedPtr
+    {
+      subscription_options.allocator =
+        rclcpp::allocator::get_rcl_allocator<MessageT>(*message_alloc.get());
+
+      using rclcpp::subscription::Subscription;
+      using rclcpp::subscription::SubscriptionBase;
+
+      auto sub = Subscription<MessageT, Alloc>::make_shared(
+        node_base->get_shared_rcl_node_handle(),
+        topic_name,
+        subscription_options,
+        any_subscription_callback,
+        msg_mem_strat);
+      auto sub_base_ptr = std::dynamic_pointer_cast<SubscriptionBase>(sub);
+      return sub_base_ptr;
+    };
+
+  // function that will setup intra process communications for the subscription
+  factory.setup_intra_process =
+    [message_alloc](
+    rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm,
+    rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
+    const rcl_subscription_options_t & subscription_options)
+    {
+      rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = ipm;
+      uint64_t intra_process_subscription_id = ipm->add_subscription(subscription);
+
+      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 = subscription_options.qos;
+      intra_process_options.ignore_local_publications = false;
+
+      // function that will be called to take a MessageT from the intra process manager
+      auto take_intra_process_message_func =
+        [weak_ipm](
+        uint64_t publisher_id,
+        uint64_t message_sequence,
+        uint64_t subscription_id,
+        typename rclcpp::subscription::Subscription<MessageT, Alloc>::MessageUniquePtr & message)
+        {
+          auto ipm = weak_ipm.lock();
+          if (!ipm) {
+            // TODO(wjwwood): should this just return silently? Or return with a logged warning?
+            throw std::runtime_error(
+                    "intra process take called after destruction of intra process manager");
+          }
+          ipm->take_intra_process_message<MessageT, Alloc>(
+            publisher_id, message_sequence, subscription_id, message);
+        };
+
+      // function that is called to see if the publisher id matches any local publishers
+      auto matches_any_publisher_func =
+        [weak_ipm](const rmw_gid_t * sender_gid) -> bool
+        {
+          auto ipm = weak_ipm.lock();
+          if (!ipm) {
+            throw std::runtime_error(
+                    "intra process publisher check called "
+                    "after destruction of intra process manager");
+          }
+          return ipm->matches_any_publishers(sender_gid);
+        };
+
+      auto typed_sub_ptr = std::dynamic_pointer_cast<SubscriptionT>(subscription);
+      typed_sub_ptr->setup_intra_process(
+        intra_process_subscription_id,
+        take_intra_process_message_func,
+        matches_any_publisher_func,
+        intra_process_options
+      );
+    };
+  // end definition of factory function to setup intra process
+
+  // return the factory now that it is populated
+  return factory;
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__SUBSCRIPTION_FACTORY_HPP_

rclcpp/include/rclcpp/time.hpp

@@ -0,0 +1,100 @@
+// Copyright 2017 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__TIME_HPP_
+#define RCLCPP__TIME_HPP_
+
+#include "builtin_interfaces/msg/time.hpp"
+
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcl/time.h"
+
+namespace rclcpp
+{
+
+class Time
+{
+public:
+  RCLCPP_PUBLIC
+  static
+  Time
+  now(rcl_time_source_type_t clock = RCL_SYSTEM_TIME);
+
+  RCLCPP_PUBLIC
+  Time(int32_t seconds, uint32_t nanoseconds, rcl_time_source_type_t clock = RCL_SYSTEM_TIME);
+
+  RCLCPP_PUBLIC
+  explicit Time(uint64_t nanoseconds, rcl_time_source_type_t clock = RCL_SYSTEM_TIME);
+
+  RCLCPP_PUBLIC
+  Time(const Time & rhs);
+
+  RCLCPP_PUBLIC
+  Time(const builtin_interfaces::msg::Time & time_msg);  // NOLINT
+
+  RCLCPP_PUBLIC
+  virtual ~Time();
+
+  RCLCPP_PUBLIC
+  operator builtin_interfaces::msg::Time() const;
+
+  RCLCPP_PUBLIC
+  void
+  operator=(const Time & rhs);
+
+  RCLCPP_PUBLIC
+  void
+  operator=(const builtin_interfaces::msg::Time & time_msg);
+
+  RCLCPP_PUBLIC
+  bool
+  operator==(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator<(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator<=(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator>=(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator>(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  Time
+  operator+(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  Time
+  operator-(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  uint64_t
+  nanoseconds() const;
+
+private:
+  rcl_time_source_t rcl_time_source_;
+  rcl_time_point_t rcl_time_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__TIME_HPP_

rclcpp/include/rclcpp/timer.hpp

@@ -21,12 +21,17 @@
 #include <sstream>
 #include <thread>
 #include <type_traits>
+#include <utility>
 
 #include "rclcpp/function_traits.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/rate.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
+
+#include "rcl/error_handling.h"
+#include "rcl/timer.h"
+
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 
@@ -38,43 +43,51 @@ namespace timer
 class TimerBase
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase);
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase)
 
   RCLCPP_PUBLIC
   explicit TimerBase(std::chrono::nanoseconds period);
 
   RCLCPP_PUBLIC
-  virtual ~TimerBase();
+  ~TimerBase();
 
   RCLCPP_PUBLIC
   void
   cancel();
 
+  RCLCPP_PUBLIC
+  void
+  reset();
+
   RCLCPP_PUBLIC
   virtual void
   execute_callback() = 0;
 
+  RCLCPP_PUBLIC
+  const rcl_timer_t *
+  get_timer_handle();
+
   /// Check how long the timer has until its next scheduled callback.
-  // \return A std::chrono::duration representing the relative time until the next callback.
-  virtual std::chrono::nanoseconds
-  time_until_trigger() = 0;
+  /** \return A std::chrono::duration representing the relative time until the next callback. */
+  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.
+  /** \return True if the clock used by this timer is steady. */
   virtual bool is_steady() = 0;
 
-  /// Check if the timer needs to trigger the callback.
+  /// Check if the timer is ready to trigger the callback.
   /**
    * This function expects its caller to immediately trigger the callback after this function,
    * since it maintains the last time the callback was triggered.
    * \return True if the timer needs to trigger.
    */
-  virtual bool check_and_trigger() = 0;
+  RCLCPP_PUBLIC
+  bool is_ready();
 
 protected:
-  std::chrono::nanoseconds period_;
-
-  bool canceled_;
+  rcl_timer_t timer_handle_ = rcl_get_zero_initialized_timer();
 };
 
 
@@ -94,7 +107,7 @@ template<
 class GenericTimer : public TimerBase
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(GenericTimer);
+  RCLCPP_SMART_PTR_DEFINITIONS(GenericTimer)
 
   /// Default constructor.
   /**
@@ -102,10 +115,8 @@ public:
    * \param[in] callback User-specified callback function.
    */
   GenericTimer(std::chrono::nanoseconds period, FunctorT && callback)
-  : TimerBase(period), callback_(std::forward<FunctorT>(callback)), loop_rate_(period)
+  : TimerBase(period), callback_(std::forward<FunctorT>(callback))
   {
-    /* Set last_triggered_time_ so that the timer fires at least one period after being created. */
-    last_triggered_time_ = Clock::now();
   }
 
   /// Default destructor.
@@ -113,11 +124,21 @@ public:
   {
     // Stop the timer from running.
     cancel();
+    if (rcl_timer_fini(&timer_handle_) != RCL_RET_OK) {
+      fprintf(stderr, "Failed to clean up rcl timer handle: %s\n", rcl_get_error_string_safe());
+    }
   }
 
   void
   execute_callback()
   {
+    rcl_ret_t ret = rcl_timer_call(&timer_handle_);
+    if (ret == RCL_RET_TIMER_CANCELED) {
+      return;
+    }
+    if (ret != RCL_RET_OK) {
+      throw std::runtime_error("Failed to notify timer that callback occurred");
+    }
     execute_callback_delegate<>();
   }
 
@@ -146,39 +167,6 @@ public:
     callback_(*this);
   }
 
-  bool
-  check_and_trigger()
-  {
-    if (canceled_) {
-      return false;
-    }
-    if (Clock::now() < last_triggered_time_) {
-      return false;
-    }
-    if (std::chrono::duration_cast<std::chrono::nanoseconds>(Clock::now() - last_triggered_time_) >=
-      loop_rate_.period())
-    {
-      last_triggered_time_ = Clock::now();
-      return true;
-    }
-    return false;
-  }
-
-  std::chrono::nanoseconds
-  time_until_trigger()
-  {
-    std::chrono::nanoseconds time_until_trigger;
-    // Calculate the time between the next trigger and the current time
-    if (last_triggered_time_ + loop_rate_.period() < Clock::now()) {
-      // time is overdue, need to trigger immediately
-      time_until_trigger = std::chrono::nanoseconds::zero();
-    } else {
-      time_until_trigger = std::chrono::duration_cast<std::chrono::nanoseconds>(
-        last_triggered_time_ - Clock::now()) + loop_rate_.period();
-    }
-    return time_until_trigger;
-  }
-
   virtual bool
   is_steady()
   {
@@ -186,11 +174,9 @@ public:
   }
 
 protected:
-  RCLCPP_DISABLE_COPY(GenericTimer);
+  RCLCPP_DISABLE_COPY(GenericTimer)
 
   FunctorT callback_;
-  rclcpp::rate::GenericRate<Clock> loop_rate_;
-  std::chrono::time_point<Clock> last_triggered_time_;
 };
 
 template<typename CallbackType>

rclcpp/include/rclcpp/type_support_decl.hpp

@@ -18,8 +18,8 @@
 #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 "rosidl_typesupport_cpp/message_type_support.hpp"
+#include "rosidl_typesupport_cpp/service_type_support.hpp"
 
 #include "rclcpp/visibility_control.hpp"
 

rclcpp/include/rclcpp/utilities.hpp

@@ -16,11 +16,32 @@
 #define RCLCPP__UTILITIES_HPP_
 
 #include <chrono>
+#include <functional>
 
 #include "rclcpp/visibility_control.hpp"
+
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
+
 #include "rmw/macros.h"
 #include "rmw/rmw.h"
 
+#ifdef ANDROID
+#include <string>
+#include <sstream>
+
+namespace std
+{
+template<typename T>
+std::string to_string(T value)
+{
+  std::ostringstream os;
+  os << value;
+  return os.str();
+}
+}
+#endif
+
 namespace rclcpp
 {
 namespace utilities
@@ -36,7 +57,7 @@ void
 init(int argc, char * argv[]);
 
 /// Check rclcpp's status.
-// \return True if SIGINT hasn't fired yet, false otherwise.
+/** \return True if SIGINT hasn't fired yet, false otherwise. */
 RCLCPP_PUBLIC
 bool
 ok();
@@ -46,10 +67,40 @@ RCLCPP_PUBLIC
 void
 shutdown();
 
-/// Get a handle to the rmw guard condition that manages the signal handler.
+/// Register a function to be called when shutdown is called.
+/** Calling the callbacks is the last thing shutdown() does. */
 RCLCPP_PUBLIC
-rmw_guard_condition_t *
-get_global_sigint_guard_condition();
+void
+on_shutdown(std::function<void(void)> callback);
+
+/// Get a handle to the rmw guard condition that manages the signal handler.
+/**
+ * The first time that this function is called for a given waitset a new guard
+ * condition will be created and returned; thereafter the same guard condition
+ * will be returned for the same waitset. This mechanism is designed to ensure
+ * that the same guard condition is not reused across waitsets (e.g., when
+ * using multiple executors in the same process). Will throw an exception if
+ * initialization of the guard condition fails.
+ * \param waitset 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 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.
 /**

rclcpp/include/rclcpp/visibility_control.hpp

@@ -22,8 +22,6 @@
 #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
 

rclcpp/package.xml

@@ -1,7 +1,8 @@
 <?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>
+  <version>0.0.3</version>
   <description>The ROS client library in C++.</description>
   <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
   <license>Apache License 2.0</license>
@@ -10,12 +11,18 @@
 
   <build_export_depend>rmw</build_export_depend>
 
+  <build_depend>builtin_interfaces</build_depend>
   <build_depend>rcl_interfaces</build_depend>
-  <build_depend>rmw_implementation_cmake</build_depend>
   <build_depend>rosidl_generator_cpp</build_depend>
+  <build_depend>rosidl_typesupport_c</build_depend>
+  <build_depend>rosidl_typesupport_cpp</build_depend>
+  <build_export_depend>builtin_interfaces</build_export_depend>
   <build_export_depend>rcl_interfaces</build_export_depend>
   <build_export_depend>rosidl_generator_cpp</build_export_depend>
+  <build_export_depend>rosidl_typesupport_c</build_export_depend>
+  <build_export_depend>rosidl_typesupport_cpp</build_export_depend>
 
+  <depend>rcl</depend>
   <depend>rmw_implementation</depend>
 
   <exec_depend>ament_cmake</exec_depend>
@@ -24,6 +31,7 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>rmw</test_depend>
+  <test_depend>rmw_implementation_cmake</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

rclcpp/rclcpp-extras.cmake

@@ -14,16 +14,16 @@
 
 # copied from rclcpp/rclcpp-extras.cmake
 
-include("${rclcpp_DIR}/get_rclcpp_information.cmake")
+# register ament_package() hook for node plugins once
+macro(_rclcpp_register_package_hook)
+  if(NOT DEFINED _RCLCPP_PACKAGE_HOOK_REGISTERED)
+    set(_RCLCPP_PACKAGE_HOOK_REGISTERED TRUE)
 
-set(rclcpp_node_main_SRC "${rclcpp_DIR}/../../../src/rclcpp/node_main.cpp")
-
-function(rclcpp_create_node_main node_library_target)
-  if(NOT TARGET ${node_library_target})
-    message(FATAL_ERROR "rclcpp_create_node_main() the first argument must be a valid target name")
+    find_package(ament_cmake_core QUIET REQUIRED)
+    ament_register_extension("ament_package" "rclcpp"
+      "rclcpp_package_hook.cmake")
   endif()
-  set(executable_name_ ${node_library_target}_node)
-  add_executable(${executable_name_} ${rclcpp_node_main_SRC})
-  target_link_libraries(${executable_name_} ${node_library_target})
-  install(TARGETS ${executable_name_} DESTINATION bin)
-endfunction()
+endmacro()
+
+include("${rclcpp_DIR}/rclcpp_create_node_main.cmake")
+include("${rclcpp_DIR}/rclcpp_register_node_plugins.cmake")

rclcpp/src/rclcpp/any_executable.cpp

@@ -23,7 +23,7 @@ AnyExecutable::AnyExecutable()
   service(nullptr),
   client(nullptr),
   callback_group(nullptr),
-  node(nullptr)
+  node_base(nullptr)
 {}
 
 AnyExecutable::~AnyExecutable()

rclcpp/src/rclcpp/callback_group.cpp

@@ -26,24 +26,28 @@ CallbackGroup::CallbackGroup(CallbackGroupType group_type)
 const std::vector<rclcpp::subscription::SubscriptionBase::WeakPtr> &
 CallbackGroup::get_subscription_ptrs() const
 {
+  std::lock_guard<std::mutex> lock(mutex_);
   return subscription_ptrs_;
 }
 
 const std::vector<rclcpp::timer::TimerBase::WeakPtr> &
 CallbackGroup::get_timer_ptrs() const
 {
+  std::lock_guard<std::mutex> lock(mutex_);
   return timer_ptrs_;
 }
 
-const std::vector<rclcpp::service::ServiceBase::SharedPtr> &
+const std::vector<rclcpp::service::ServiceBase::WeakPtr> &
 CallbackGroup::get_service_ptrs() const
 {
+  std::lock_guard<std::mutex> lock(mutex_);
   return service_ptrs_;
 }
 
 const std::vector<rclcpp::client::ClientBase::WeakPtr> &
 CallbackGroup::get_client_ptrs() const
 {
+  std::lock_guard<std::mutex> lock(mutex_);
   return client_ptrs_;
 }
 
@@ -63,23 +67,27 @@ void
 CallbackGroup::add_subscription(
   const rclcpp::subscription::SubscriptionBase::SharedPtr subscription_ptr)
 {
+  std::lock_guard<std::mutex> lock(mutex_);
   subscription_ptrs_.push_back(subscription_ptr);
 }
 
 void
 CallbackGroup::add_timer(const rclcpp::timer::TimerBase::SharedPtr timer_ptr)
 {
+  std::lock_guard<std::mutex> lock(mutex_);
   timer_ptrs_.push_back(timer_ptr);
 }
 
 void
 CallbackGroup::add_service(const rclcpp::service::ServiceBase::SharedPtr service_ptr)
 {
+  std::lock_guard<std::mutex> lock(mutex_);
   service_ptrs_.push_back(service_ptr);
 }
 
 void
 CallbackGroup::add_client(const rclcpp::client::ClientBase::SharedPtr client_ptr)
 {
+  std::lock_guard<std::mutex> lock(mutex_);
   client_ptrs_.push_back(client_ptr);
 }

rclcpp/src/rclcpp/client.cpp

@@ -14,29 +14,33 @@
 
 #include "rclcpp/client.hpp"
 
+#include <chrono>
 #include <cstdio>
+#include <memory>
 #include <string>
 
-#include "rmw/rmw.h"
+#include "rcl/graph.h"
+#include "rcl/node.h"
+#include "rcl/wait.h"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_graph_interface.hpp"
+#include "rclcpp/utilities.hpp"
 
 using rclcpp::client::ClientBase;
+using rclcpp::exceptions::InvalidNodeError;
+using rclcpp::exceptions::throw_from_rcl_error;
 
 ClientBase::ClientBase(
-  std::shared_ptr<rmw_node_t> node_handle,
-  rmw_client_t * client_handle,
+  rclcpp::node_interfaces::NodeBaseInterface * node_base,
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
   const std::string & service_name)
-: node_handle_(node_handle), client_handle_(client_handle), service_name_(service_name)
+: node_graph_(node_graph),
+  node_handle_(node_base->get_shared_rcl_node_handle()),
+  service_name_(service_name)
 {}
 
-ClientBase::~ClientBase()
-{
-  if (client_handle_) {
-    if (rmw_destroy_client(client_handle_) != RMW_RET_OK) {
-      fprintf(stderr,
-        "Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe());
-    }
-  }
-}
+ClientBase::~ClientBase() {}
 
 const std::string &
 ClientBase::get_service_name() const
@@ -44,8 +48,86 @@ ClientBase::get_service_name() const
   return this->service_name_;
 }
 
-const rmw_client_t *
+rcl_client_t *
+ClientBase::get_client_handle()
+{
+  return &client_handle_;
+}
+
+const rcl_client_t *
 ClientBase::get_client_handle() const
 {
-  return this->client_handle_;
+  return &client_handle_;
+}
+
+bool
+ClientBase::service_is_ready() const
+{
+  bool is_ready;
+  rcl_ret_t ret =
+    rcl_service_server_is_available(this->get_rcl_node_handle(), &client_handle_, &is_ready);
+  if (ret != RCL_RET_OK) {
+    throw_from_rcl_error(ret, "rcl_service_server_is_available failed");
+  }
+  return is_ready;
+}
+
+bool
+ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
+{
+  auto start = std::chrono::steady_clock::now();
+  // make an event to reuse, rather than create a new one each time
+  auto node_ptr = node_graph_.lock();
+  if (!node_ptr) {
+    throw InvalidNodeError();
+  }
+  auto event = node_ptr->get_graph_event();
+  // 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;
+  }
+  // update the time even on the first loop to account for time spent in the first call
+  // to this->server_is_ready()
+  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);
+    event->check_and_clear();  // reset the event
+
+    // always check if the service is ready, even if the graph event wasn't triggered
+    // this is needed to avoid a race condition that is specific to the Connext RMW implementation
+    // (see https://github.com/ros2/rmw_connext/issues/201)
+    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()
+{
+  return node_handle_.get();
+}
+
+const rcl_node_t *
+ClientBase::get_rcl_node_handle() const
+{
+  return node_handle_.get();
 }

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,116 @@
+// 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>
+
+using namespace std::string_literals;
+
+namespace rclcpp
+{
+namespace exceptions
+{
+
+std::string
+NameValidationError::format_error(
+  const char * name_type,
+  const char * name,
+  const char * error_msg,
+  size_t invalid_index)
+{
+  std::string msg = "";
+  msg += "Invalid "s + name_type + ": " + error_msg + ":\n";
+  msg += "  '"s + name + "'\n";
+  msg += "   "s + std::string(invalid_index, ' ') + "^\n";
+  return msg;
+}
+
+void
+throw_from_rcl_error(
+  rcl_ret_t ret,
+  const std::string & prefix,
+  const rcl_error_state_t * error_state,
+  void (* reset_error)())
+{
+  if (RCL_RET_OK == ret) {
+    throw std::invalid_argument("ret is RCL_RET_OK");
+  }
+  if (!error_state) {
+    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) {
+    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

@@ -13,11 +13,17 @@
 // limitations under the License.
 
 #include <algorithm>
+#include <memory>
 #include <string>
 #include <type_traits>
 
+#include "rcl/allocator.h"
+#include "rcl/error_handling.h"
+
 #include "rclcpp/executor.hpp"
+#include "rclcpp/node.hpp"
 #include "rclcpp/scope_exit.hpp"
+#include "rclcpp/utilities.hpp"
 
 #include "rcl_interfaces/msg/intra_process_message.hpp"
 
@@ -30,29 +36,33 @@ Executor::Executor(const ExecutorArgs & args)
 : spinning(false),
   memory_strategy_(args.memory_strategy)
 {
-  interrupt_guard_condition_ = rmw_create_guard_condition();
-  if (!interrupt_guard_condition_) {
-    throw std::runtime_error("Failed to create interrupt guard condition in Executor constructor");
+  rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options();
+  if (rcl_guard_condition_init(
+      &interrupt_guard_condition_, guard_condition_options) != RCL_RET_OK)
+  {
+    throw std::runtime_error(
+            std::string("Failed to create interrupt guard condition in Executor constructor: ") +
+            rcl_get_error_string_safe());
   }
 
   // The number of guard conditions is always at least 2: 1 for the ctrl-c guard cond,
   // and one for the executor's guard cond (interrupt_guard_condition_)
-  // The size of guard conditions is variable because the number of nodes can vary
 
   // Put the global ctrl-c guard condition in
-  memory_strategy_->add_guard_condition(rclcpp::utilities::get_global_sigint_guard_condition());
+  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_);
+  memory_strategy_->add_guard_condition(&interrupt_guard_condition_);
+  rcl_allocator_t allocator = memory_strategy_->get_allocator();
 
-  waitset_ = rmw_create_waitset(args.max_conditions);
-
-  if (!waitset_) {
+  if (rcl_wait_set_init(
+      &waitset_, 0, 2, 0, 0, 0, allocator) != RCL_RET_OK)
+  {
     fprintf(stderr,
-      "[rclcpp::error] failed to create waitset: %s\n", rmw_get_error_string_safe());
-    rmw_ret_t status = rmw_destroy_guard_condition(interrupt_guard_condition_);
-    if (status != RMW_RET_OK) {
+      "[rclcpp::error] failed to create waitset: %s\n", rcl_get_error_string_safe());
+    if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
       fprintf(stderr,
-        "[rclcpp::error] failed to destroy guard condition: %s\n", rmw_get_error_string_safe());
+        "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
     }
     throw std::runtime_error("Failed to create waitset in Executor constructor");
   }
@@ -60,29 +70,28 @@ Executor::Executor(const ExecutorArgs & args)
 
 Executor::~Executor()
 {
-  // Try to deallocate the waitset.
-  if (waitset_) {
-    rmw_ret_t status = rmw_destroy_waitset(waitset_);
-    if (status != RMW_RET_OK) {
-      fprintf(stderr,
-        "[rclcpp::error] failed to destroy waitset: %s\n", rmw_get_error_string_safe());
-    }
+  // Finalize the waitset.
+  if (rcl_wait_set_fini(&waitset_) != RCL_RET_OK) {
+    fprintf(stderr,
+      "[rclcpp::error] failed to destroy waitset: %s\n", rcl_get_error_string_safe());
   }
-  // Try to deallocate the interrupt guard condition.
-  if (interrupt_guard_condition_ != nullptr) {
-    rmw_ret_t status = rmw_destroy_guard_condition(interrupt_guard_condition_);
-    if (status != RMW_RET_OK) {
-      fprintf(stderr,
-        "[rclcpp::error] failed to destroy guard condition: %s\n", rmw_get_error_string_safe());
-    }
+  // Finalize the interrupt guard condition.
+  if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
+    fprintf(stderr,
+      "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
   }
+  // Remove and release the sigint guard condition
+  memory_strategy_->remove_guard_condition(
+    rclcpp::utilities::get_sigint_guard_condition(&waitset_));
+  rclcpp::utilities::release_sigint_guard_condition(&waitset_);
 }
 
 void
-Executor::add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify)
+Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
 {
   // If the node already has an executor
-  if (node_ptr->has_executor.exchange(true)) {
+  std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
+  if (has_executor.exchange(true)) {
     throw std::runtime_error("Node has already been added to an executor.");
   }
   // Check to ensure node not already added
@@ -96,9 +105,8 @@ Executor::add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify)
   weak_nodes_.push_back(node_ptr);
   if (notify) {
     // Interrupt waiting to handle new node
-    rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
-    if (status != RMW_RET_OK) {
-      throw std::runtime_error(rmw_get_error_string_safe());
+    if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+      throw std::runtime_error(rcl_get_error_string_safe());
     }
   }
   // Add the node's notify condition to the guard condition handles
@@ -106,14 +114,20 @@ Executor::add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify)
 }
 
 void
-Executor::remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify)
+Executor::add_node(std::shared_ptr<rclcpp::node::Node> node_ptr, bool notify)
+{
+  this->add_node(node_ptr->get_node_base_interface(), notify);
+}
+
+void
+Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::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 unecessary indents here)
-      [&](std::weak_ptr<rclcpp::node::Node> & i)
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
+      [&](rclcpp::node_interfaces::NodeBaseInterface::WeakPtr & i)
       {
         bool matched = (i.lock() == node_ptr);
         node_removed |= matched;
@@ -122,22 +136,28 @@ Executor::remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify)
       // *INDENT-ON*
     )
   );
-  node_ptr->has_executor.store(false);
+  std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
+  has_executor.store(false);
   if (notify) {
     // If the node was matched and removed, interrupt waiting
     if (node_removed) {
-      rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
-      if (status != RMW_RET_OK) {
-        throw std::runtime_error(rmw_get_error_string_safe());
+      if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+        throw std::runtime_error(rcl_get_error_string_safe());
       }
     }
   }
   memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition());
 }
 
+void
+Executor::remove_node(std::shared_ptr<rclcpp::node::Node> node_ptr, bool notify)
+{
+  this->remove_node(node_ptr->get_node_base_interface(), notify);
+}
+
 void
 Executor::spin_node_once_nanoseconds(
-  rclcpp::node::Node::SharedPtr node,
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
   std::chrono::nanoseconds timeout)
 {
   this->add_node(node, false);
@@ -147,13 +167,19 @@ Executor::spin_node_once_nanoseconds(
 }
 
 void
-Executor::spin_node_some(rclcpp::node::Node::SharedPtr node)
+Executor::spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node)
 {
   this->add_node(node, false);
   spin_some();
   this->remove_node(node, false);
 }
 
+void
+Executor::spin_node_some(std::shared_ptr<rclcpp::node::Node> node)
+{
+  this->spin_node_some(node->get_node_base_interface());
+}
+
 void
 Executor::spin_some()
 {
@@ -184,9 +210,8 @@ void
 Executor::cancel()
 {
   spinning.store(false);
-  rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
-  if (status != RMW_RET_OK) {
-    throw std::runtime_error(rmw_get_error_string_safe());
+  if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+    throw std::runtime_error(rcl_get_error_string_safe());
   }
 }
 
@@ -224,9 +249,8 @@ Executor::execute_any_executable(AnyExecutable::SharedPtr any_exec)
   any_exec->callback_group->can_be_taken_from().store(true);
   // Wake the wait, because it may need to be recalculated or work that
   // was previously blocked is now available.
-  rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
-  if (status != RMW_RET_OK) {
-    throw std::runtime_error(rmw_get_error_string_safe());
+  if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
+    throw std::runtime_error(rcl_get_error_string_safe());
   }
 }
 
@@ -235,20 +259,17 @@ Executor::execute_subscription(
   rclcpp::subscription::SubscriptionBase::SharedPtr subscription)
 {
   std::shared_ptr<void> message = subscription->create_message();
-  bool taken = false;
   rmw_message_info_t message_info;
-  auto ret =
-    rmw_take_with_info(subscription->get_subscription_handle(),
-      message.get(), &taken, &message_info);
-  if (ret == RMW_RET_OK) {
-    if (taken) {
-      message_info.from_intra_process = false;
-      subscription->handle_message(message, message_info);
-    }
-  } else {
+
+  auto ret = rcl_take(subscription->get_subscription_handle(),
+      message.get(), &message_info);
+  if (ret == RCL_RET_OK) {
+    message_info.from_intra_process = false;
+    subscription->handle_message(message, message_info);
+  } else if (ret != RCL_RET_SUBSCRIPTION_TAKE_FAILED) {
     fprintf(stderr,
       "[rclcpp::error] take failed for subscription on topic '%s': %s\n",
-      subscription->get_topic_name().c_str(), rmw_get_error_string_safe());
+      subscription->get_topic_name(), rcl_get_error_string_safe());
   }
   subscription->return_message(message);
 }
@@ -258,22 +279,19 @@ Executor::execute_intra_process_subscription(
   rclcpp::subscription::SubscriptionBase::SharedPtr subscription)
 {
   rcl_interfaces::msg::IntraProcessMessage ipm;
-  bool taken = false;
   rmw_message_info_t message_info;
-  rmw_ret_t status = rmw_take_with_info(
+  rcl_ret_t status = rcl_take(
     subscription->get_intra_process_subscription_handle(),
     &ipm,
-    &taken,
     &message_info);
-  if (status == RMW_RET_OK) {
-    if (taken) {
-      message_info.from_intra_process = true;
-      subscription->handle_intra_process_message(ipm, message_info);
-    }
-  } else {
+
+  if (status == RCL_RET_OK) {
+    message_info.from_intra_process = true;
+    subscription->handle_intra_process_message(ipm, message_info);
+  } else if (status != RCL_RET_SUBSCRIPTION_TAKE_FAILED) {
     fprintf(stderr,
       "[rclcpp::error] take failed for intra process subscription on topic '%s': %s\n",
-      subscription->get_topic_name().c_str(), rmw_get_error_string_safe());
+      subscription->get_topic_name(), rcl_get_error_string_safe());
   }
 }
 
@@ -290,20 +308,16 @@ Executor::execute_service(
 {
   auto request_header = service->create_request_header();
   std::shared_ptr<void> request = service->create_request();
-  bool taken = false;
-  rmw_ret_t status = rmw_take_request(
+  rcl_ret_t status = rcl_take_request(
     service->get_service_handle(),
     request_header.get(),
-    request.get(),
-    &taken);
-  if (status == RMW_RET_OK) {
-    if (taken) {
-      service->handle_request(request_header, request);
-    }
-  } else {
+    request.get());
+  if (status == RCL_RET_OK) {
+    service->handle_request(request_header, request);
+  } else if (status != RCL_RET_SERVICE_TAKE_FAILED) {
     fprintf(stderr,
       "[rclcpp::error] take request failed for server of service '%s': %s\n",
-      service->get_service_name().c_str(), rmw_get_error_string_safe());
+      service->get_service_name().c_str(), rcl_get_error_string_safe());
   }
 }
 
@@ -313,29 +327,24 @@ Executor::execute_client(
 {
   auto request_header = client->create_request_header();
   std::shared_ptr<void> response = client->create_response();
-  bool taken = false;
-  rmw_ret_t status = rmw_take_response(
+  rcl_ret_t status = rcl_take_response(
     client->get_client_handle(),
     request_header.get(),
-    response.get(),
-    &taken);
-  if (status == RMW_RET_OK) {
-    if (taken) {
-      client->handle_response(request_header, response);
-    }
-  } else {
+    response.get());
+  if (status == RCL_RET_OK) {
+    client->handle_response(request_header, response);
+  } else if (status != RCL_RET_CLIENT_TAKE_FAILED) {
     fprintf(stderr,
       "[rclcpp::error] take response failed for client of service '%s': %s\n",
-      client->get_service_name().c_str(), rmw_get_error_string_safe());
+      client->get_service_name().c_str(), rcl_get_error_string_safe());
   }
 }
 
 void
 Executor::wait_for_work(std::chrono::nanoseconds timeout)
 {
-  memory_strategy_->clear_active_entities();
-
   // Collect the subscriptions and timers to be waited on
+  memory_strategy_->clear_handles();
   bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_);
 
   // Clean up any invalid nodes, if they were detected
@@ -343,8 +352,8 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
     weak_nodes_.erase(
       remove_if(
         weak_nodes_.begin(), weak_nodes_.end(),
-        // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-        [](std::weak_ptr<rclcpp::node::Node> i)
+        // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
+        [](rclcpp::node_interfaces::NodeBaseInterface::WeakPtr i)
         {
           return i.expired();
         }
@@ -353,68 +362,82 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
     );
   }
 
-  memory_strategy_->clear_handles();
-  // 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);
-
-  // construct a guard conditions struct
-  rmw_guard_conditions_t guard_conditions;
-  guard_conditions.guard_condition_count =
-    memory_strategy_->fill_guard_condition_handles(guard_conditions.guard_conditions);
-
-  rmw_time_t * wait_timeout = NULL;
-  rmw_time_t rmw_timeout;
-
-  auto next_timer_duration = get_earliest_timer();
-  // If the next timer timeout must preempt the requested timeout
-  // or if the requested timeout blocks forever, and there exists a valid timer,
-  // replace the requested timeout with the next timeout.
-  bool has_valid_timer = next_timer_duration >= std::chrono::nanoseconds::zero();
-  if ((next_timer_duration < timeout ||
-    timeout < std::chrono::nanoseconds::zero()) && has_valid_timer)
+  if (rcl_wait_set_resize_subscriptions(
+      &waitset_, memory_strategy_->number_of_ready_subscriptions()) != RCL_RET_OK)
   {
-    rmw_timeout.sec =
-      std::chrono::duration_cast<std::chrono::seconds>(next_timer_duration).count();
-    rmw_timeout.nsec = next_timer_duration.count() % (1000 * 1000 * 1000);
-    wait_timeout = &rmw_timeout;
-  } else if (timeout >= std::chrono::nanoseconds::zero()) {
-    // Convert timeout representation to rmw_time
-    rmw_timeout.sec = std::chrono::duration_cast<std::chrono::seconds>(timeout).count();
-    rmw_timeout.nsec = std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count() %
-      (1000 * 1000 * 1000);
-    wait_timeout = &rmw_timeout;
+    throw std::runtime_error(
+            std::string("Couldn't resize the number of subscriptions in waitset : ") +
+            rcl_get_error_string_safe());
   }
 
-  // Now wait on the waitable subscriptions and timers
-  rmw_ret_t status = rmw_wait(
-    &subscriber_handles,
-    &guard_conditions,
-    &service_handles,
-    &client_handles,
-    waitset_,
-    wait_timeout);
-  if (status != RMW_RET_OK && status != RMW_RET_TIMEOUT) {
-    throw std::runtime_error(rmw_get_error_string_safe());
+  if (rcl_wait_set_resize_services(
+      &waitset_, memory_strategy_->number_of_ready_services()) != RCL_RET_OK)
+  {
+    throw std::runtime_error(
+            std::string("Couldn't resize the number of services in waitset : ") +
+            rcl_get_error_string_safe());
   }
 
-  memory_strategy_->remove_null_handles();
+  if (rcl_wait_set_resize_clients(
+      &waitset_, memory_strategy_->number_of_ready_clients()) != RCL_RET_OK)
+  {
+    throw std::runtime_error(
+            std::string("Couldn't resize the number of clients in waitset : ") +
+            rcl_get_error_string_safe());
+  }
+
+  if (rcl_wait_set_resize_guard_conditions(
+      &waitset_, memory_strategy_->number_of_guard_conditions()) != RCL_RET_OK)
+  {
+    throw std::runtime_error(
+            std::string("Couldn't resize the number of guard_conditions in waitset : ") +
+            rcl_get_error_string_safe());
+  }
+
+  if (rcl_wait_set_resize_timers(
+      &waitset_, memory_strategy_->number_of_ready_timers()) != RCL_RET_OK)
+  {
+    throw std::runtime_error(
+            std::string("Couldn't resize the number of timers in waitset : ") +
+            rcl_get_error_string_safe());
+  }
+
+  if (!memory_strategy_->add_handles_to_waitset(&waitset_)) {
+    throw std::runtime_error("Couldn't fill waitset");
+  }
+  rcl_ret_t status =
+    rcl_wait(&waitset_, std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count());
+  if (status == RCL_RET_WAIT_SET_EMPTY) {
+    fprintf(stderr, "Warning: empty waitset received in rcl_wait(). This should never happen.\n");
+  } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) {
+    throw std::runtime_error(std::string("rcl_wait() failed: ") + rcl_get_error_string_safe());
+  }
+
+  // check the null handles in the waitset and remove them from the handles in memory strategy
+  // for callback-based entities
+  memory_strategy_->remove_null_handles(&waitset_);
+  if (rcl_wait_set_clear_subscriptions(&waitset_) != RCL_RET_OK) {
+    throw std::runtime_error("Couldn't clear subscriptions from waitset");
+  }
+  if (rcl_wait_set_clear_services(&waitset_) != RCL_RET_OK) {
+    throw std::runtime_error("Couldn't clear servicess from waitset");
+  }
+  if (rcl_wait_set_clear_clients(&waitset_) != RCL_RET_OK) {
+    throw std::runtime_error("Couldn't clear clients from waitset");
+  }
+  if (rcl_wait_set_clear_guard_conditions(&waitset_) != RCL_RET_OK) {
+    throw std::runtime_error("Couldn't clear guard conditions from waitset");
+  }
+  if (rcl_wait_set_clear_timers(&waitset_) != RCL_RET_OK) {
+    throw std::runtime_error("Couldn't clear timers from waitset");
+  }
 }
 
-rclcpp::node::Node::SharedPtr
+rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
 Executor::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
   if (!group) {
-    return rclcpp::node::Node::SharedPtr();
+    return nullptr;
   }
   for (auto & weak_node : weak_nodes_) {
     auto node = weak_node.lock();
@@ -428,7 +451,7 @@ Executor::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr gro
       }
     }
   }
-  return rclcpp::node::Node::SharedPtr();
+  return nullptr;
 }
 
 rclcpp::callback_group::CallbackGroup::SharedPtr
@@ -470,7 +493,7 @@ Executor::get_next_timer(AnyExecutable::SharedPtr any_exec)
       }
       for (auto & timer_ref : group->get_timer_ptrs()) {
         auto timer = timer_ref.lock();
-        if (timer && timer->check_and_trigger()) {
+        if (timer && timer->is_ready()) {
           any_exec->timer = timer;
           any_exec->callback_group = group;
           node = get_node_by_group(group);
@@ -481,37 +504,6 @@ Executor::get_next_timer(AnyExecutable::SharedPtr any_exec)
   }
 }
 
-std::chrono::nanoseconds
-Executor::get_earliest_timer()
-{
-  std::chrono::nanoseconds latest = std::chrono::nanoseconds::max();
-  bool timers_empty = true;
-  for (auto & weak_node : weak_nodes_) {
-    auto node = weak_node.lock();
-    if (!node) {
-      continue;
-    }
-    for (auto & weak_group : node->get_callback_groups()) {
-      auto group = weak_group.lock();
-      if (!group || !group->can_be_taken_from().load()) {
-        continue;
-      }
-      for (auto & timer_ref : group->get_timer_ptrs()) {
-        timers_empty = false;
-        // Check the expected trigger time
-        auto timer = timer_ref.lock();
-        if (timer && timer->time_until_trigger() < latest) {
-          latest = timer->time_until_trigger();
-        }
-      }
-    }
-  }
-  if (timers_empty) {
-    return std::chrono::nanoseconds(-1);
-  }
-  return latest;
-}
-
 AnyExecutable::SharedPtr
 Executor::get_next_ready_executable()
 {

rclcpp/src/rclcpp/executors.cpp

@@ -15,17 +15,29 @@
 #include "rclcpp/executors.hpp"
 
 void
-rclcpp::spin_some(node::Node::SharedPtr node_ptr)
+rclcpp::spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
 {
   rclcpp::executors::SingleThreadedExecutor exec;
   exec.spin_node_some(node_ptr);
 }
 
 void
-rclcpp::spin(node::Node::SharedPtr node_ptr)
+rclcpp::spin_some(rclcpp::node::Node::SharedPtr node_ptr)
+{
+  rclcpp::spin_some(node_ptr->get_node_base_interface());
+}
+
+void
+rclcpp::spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
 {
   rclcpp::executors::SingleThreadedExecutor exec;
   exec.add_node(node_ptr);
   exec.spin();
   exec.remove_node(node_ptr);
 }
+
+void
+rclcpp::spin(rclcpp::node::Node::SharedPtr node_ptr)
+{
+  rclcpp::spin(node_ptr->get_node_base_interface());
+}

rclcpp/src/rclcpp/executors/single_threaded_executor.cpp

@@ -26,7 +26,7 @@ void
 SingleThreadedExecutor::spin()
 {
   if (spinning.exchange(true)) {
-    throw std::runtime_error("spin_some() called while already spinning");
+    throw std::runtime_error("spin() called while already spinning");
   }
   RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
   while (rclcpp::utilities::ok() && spinning.load()) {

rclcpp/src/rclcpp/expand_topic_or_service_name.cpp

@@ -0,0 +1,196 @@
+// Copyright 2017 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/expand_topic_or_service_name.hpp"
+
+#include <string>
+
+#include "rcl/expand_topic_name.h"
+#include "rcl/validate_topic_name.h"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/scope_exit.hpp"
+#include "rcutils/types/string_map.h"
+#include "rmw/error_handling.h"
+#include "rmw/validate_namespace.h"
+#include "rmw/validate_node_name.h"
+#include "rmw/validate_full_topic_name.h"
+
+using rclcpp::exceptions::throw_from_rcl_error;
+
+std::string
+rclcpp::expand_topic_or_service_name(
+  const std::string & name,
+  const std::string & node_name,
+  const std::string & namespace_,
+  bool is_service)
+{
+  char * expanded_topic = nullptr;
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  rcutils_allocator_t rcutils_allocator = rcutils_get_default_allocator();
+  rcutils_string_map_t substitutions_map = rcutils_get_zero_initialized_string_map();
+
+  rcutils_ret_t rcutils_ret = rcutils_string_map_init(&substitutions_map, 0, rcutils_allocator);
+  if (rcutils_ret != RCUTILS_RET_OK) {
+    if (rcutils_ret == RCUTILS_RET_BAD_ALLOC) {
+      throw_from_rcl_error(RCL_RET_BAD_ALLOC, "", rcutils_get_error_state(), rcutils_reset_error);
+    } else {
+      throw_from_rcl_error(RCL_RET_ERROR, "", rcutils_get_error_state(), rcutils_reset_error);
+    }
+  }
+  rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&substitutions_map);
+  if (ret != RCL_RET_OK) {
+    rcutils_error_state_t error_state;
+    if (rcutils_error_state_copy(rcl_get_error_state(), &error_state) != RCUTILS_RET_OK) {
+      throw std::bad_alloc();
+    }
+    auto error_state_scope_exit = rclcpp::make_scope_exit([&error_state]() {
+      rcutils_error_state_fini(&error_state);
+    });
+    // finalize the string map before throwing
+    rcutils_ret = rcutils_string_map_fini(&substitutions_map);
+    if (rcutils_ret != RCUTILS_RET_OK) {
+      fprintf(stderr,
+        "[rclcpp|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: "
+        "failed to fini string_map (%d) during error handling: %s\n",
+        rcutils_ret,
+        rcutils_get_error_string_safe());
+      rcutils_reset_error();
+    }
+    throw_from_rcl_error(ret, "", &error_state);
+  }
+
+  ret = rcl_expand_topic_name(
+    name.c_str(),
+    node_name.c_str(),
+    namespace_.c_str(),
+    &substitutions_map,
+    allocator,
+    &expanded_topic);
+
+  std::string result;
+  if (ret == RCL_RET_OK) {
+    result = expanded_topic;
+    allocator.deallocate(expanded_topic, allocator.state);
+  }
+
+  rcutils_ret = rcutils_string_map_fini(&substitutions_map);
+  if (rcutils_ret != RCUTILS_RET_OK) {
+    throw_from_rcl_error(RCL_RET_ERROR, "", rcutils_get_error_state(), rcutils_reset_error);
+  }
+
+  // expansion failed
+  if (ret != RCL_RET_OK) {
+    // if invalid topic or unknown substitution
+    if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) {
+      rcl_reset_error();  // explicitly discard error from rcl_expand_topic_name()
+      int validation_result;
+      size_t invalid_index;
+      rcl_ret_t ret = rcl_validate_topic_name(name.c_str(), &validation_result, &invalid_index);
+      if (ret != RCL_RET_OK) {
+        throw_from_rcl_error(ret);
+      }
+
+      if (validation_result == RCL_TOPIC_NAME_VALID) {
+        throw std::runtime_error("topic name unexpectedly valid");
+      }
+      const char * validation_message = rcl_topic_name_validation_result_string(validation_result);
+      if (!validation_message) {
+        throw std::runtime_error("unable to get validation error message");
+      }
+      if (is_service) {
+        using rclcpp::exceptions::InvalidServiceNameError;
+        throw InvalidServiceNameError(name.c_str(), validation_message, invalid_index);
+      } else {
+        using rclcpp::exceptions::InvalidTopicNameError;
+        throw InvalidTopicNameError(name.c_str(), validation_message, invalid_index);
+      }
+      // if invalid node name
+    } else if (ret == RCL_RET_NODE_INVALID_NAME) {
+      rcl_reset_error();  // explicitly discard error from rcl_expand_topic_name()
+      int validation_result;
+      size_t invalid_index;
+      rmw_ret_t rmw_ret =
+        rmw_validate_node_name(node_name.c_str(), &validation_result, &invalid_index);
+      if (rmw_ret != RMW_RET_OK) {
+        if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
+          throw_from_rcl_error(
+            RCL_RET_INVALID_ARGUMENT, "failed to validate node name",
+            rmw_get_error_state(), rmw_reset_error);
+        }
+        throw_from_rcl_error(
+          RCL_RET_ERROR, "failed to validate node name",
+          rmw_get_error_state(), rmw_reset_error);
+      }
+      throw rclcpp::exceptions::InvalidNodeNameError(
+              node_name.c_str(),
+              rmw_node_name_validation_result_string(validation_result),
+              invalid_index);
+      // if invalid namespace
+    } else if (ret == RCL_RET_NODE_INVALID_NAMESPACE) {
+      rcl_reset_error();  // explicitly discard error from rcl_expand_topic_name()
+      int validation_result;
+      size_t invalid_index;
+      rmw_ret_t rmw_ret =
+        rmw_validate_namespace(namespace_.c_str(), &validation_result, &invalid_index);
+      if (rmw_ret != RMW_RET_OK) {
+        if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
+          throw_from_rcl_error(
+            RCL_RET_INVALID_ARGUMENT, "failed to validate namespace",
+            rmw_get_error_state(), rmw_reset_error);
+        }
+        throw_from_rcl_error(
+          RCL_RET_ERROR, "failed to validate namespace",
+          rmw_get_error_state(), rmw_reset_error);
+      }
+      throw rclcpp::exceptions::InvalidNamespaceError(
+              namespace_.c_str(),
+              rmw_namespace_validation_result_string(validation_result),
+              invalid_index);
+      // something else happened
+    } else {
+      throw_from_rcl_error(ret);
+    }
+  }
+
+  // expand succeeded, but full name validation may fail still
+  int validation_result;
+  size_t invalid_index;
+  rmw_ret_t rmw_ret =
+    rmw_validate_full_topic_name(result.c_str(), &validation_result, &invalid_index);
+  if (rmw_ret != RMW_RET_OK) {
+    if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
+      throw_from_rcl_error(
+        RCL_RET_INVALID_ARGUMENT, "failed to validate full topic name",
+        rmw_get_error_state(), rmw_reset_error);
+    }
+    throw_from_rcl_error(
+      RCL_RET_ERROR, "failed to validate full topic name",
+      rmw_get_error_state(), rmw_reset_error);
+  }
+  if (validation_result != RMW_TOPIC_VALID) {
+    if (is_service) {
+      throw rclcpp::exceptions::InvalidServiceNameError(
+              result.c_str(),
+              rmw_full_topic_name_validation_result_string(validation_result),
+              invalid_index);
+    } else {
+      throw rclcpp::exceptions::InvalidTopicNameError(
+              result.c_str(),
+              rmw_full_topic_name_validation_result_string(validation_result),
+              invalid_index);
+    }
+  }
+
+  return result;
+}

rclcpp/src/rclcpp/graph_listener.cpp

@@ -0,0 +1,356 @@
+// 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 <memory>
+#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
+      // node_graph_interfaces_mutex_ after waking up rcl_wait.
+      std::lock_guard<std::mutex> nodes_barrier_lock(node_graph_interfaces_barrier_mutex_);
+      // This is ownership is passed to nodes_lock in the next line.
+      node_graph_interfaces_mutex_.lock();
+    }
+    // This lock is released when the loop continues or exits.
+    std::lock_guard<std::mutex> nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock);
+
+    // Resize the wait set if necessary.
+    if (wait_set_.size_of_guard_conditions < (node_graph_interfaces_.size() + 2)) {
+      ret = rcl_wait_set_resize_guard_conditions(&wait_set_, node_graph_interfaces_.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 : node_graph_interfaces_) {
+      // 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 = node_ptr->get_graph_guard_condition();
+      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 : node_graph_interfaces_) {
+      auto graph_gc = node_ptr->get_graph_guard_condition();
+      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 * node_graph_interfaces_barrier_mutex,
+  std::mutex * node_graph_interfaces_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(*node_graph_interfaces_barrier_mutex);
+    // Trigger the interrupt guard condition to wake up rcl_wait.
+    interrupt_(interrupt_guard_condition);
+    node_graph_interfaces_mutex->lock();
+  }
+}
+
+static bool
+has_node_(
+  std::vector<rclcpp::node_interfaces::NodeGraphInterface *> * node_graph_interfaces,
+  rclcpp::node_interfaces::NodeGraphInterface * node_graph)
+{
+  for (const auto node_ptr : (*node_graph_interfaces)) {
+    if (node_graph == node_ptr) {
+      return true;
+    }
+  }
+  return false;
+}
+
+bool
+GraphListener::has_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph)
+{
+  if (!node_graph) {
+    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_(
+    &node_graph_interfaces_barrier_mutex_,
+    &node_graph_interfaces_mutex_,
+    &interrupt_guard_condition_);
+  // Store the now acquired node_graph_interfaces_mutex_ in the scoped lock using adopt_lock.
+  std::lock_guard<std::mutex> nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock);
+  return has_node_(&node_graph_interfaces_, node_graph);
+}
+
+void
+GraphListener::add_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph)
+{
+  if (!node_graph) {
+    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_(
+    &node_graph_interfaces_barrier_mutex_,
+    &node_graph_interfaces_mutex_,
+    &interrupt_guard_condition_);
+  // Store the now acquired node_graph_interfaces_mutex_ in the scoped lock using adopt_lock.
+  std::lock_guard<std::mutex> nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock);
+  if (has_node_(&node_graph_interfaces_, node_graph)) {
+    throw NodeAlreadyAddedError();
+  }
+  node_graph_interfaces_.push_back(node_graph);
+  // The run loop has already been interrupted by acquire_nodes_lock_() and
+  // will evaluate the new node when nodes_lock releases the node_graph_interfaces_mutex_.
+}
+
+static void
+remove_node_(
+  std::vector<rclcpp::node_interfaces::NodeGraphInterface *> * node_graph_interfaces,
+  rclcpp::node_interfaces::NodeGraphInterface * node_graph)
+{
+  // Remove the node if it is found.
+  for (auto it = node_graph_interfaces->begin(); it != node_graph_interfaces->end(); ++it) {
+    if (node_graph == *it) {
+      // Found the node, remove it.
+      node_graph_interfaces->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_interfaces::NodeGraphInterface * node_graph)
+{
+  if (!node_graph) {
+    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_(&node_graph_interfaces_, node_graph);
+  }
+  // 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_(
+    &node_graph_interfaces_barrier_mutex_,
+    &node_graph_interfaces_mutex_,
+    &interrupt_guard_condition_);
+  // Store the now acquired node_graph_interfaces_mutex_ in the scoped lock using adopt_lock.
+  std::lock_guard<std::mutex> nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock);
+  remove_node_(&node_graph_interfaces_, node_graph);
+}
+
+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

@@ -69,7 +69,7 @@ IntraProcessManager::get_next_unique_id()
     // So around 585 million years. Even at 1 GHz, it would take 585 years.
     // I think it's safe to avoid trying to handle overflow.
     // If we roll over then it's most likely a bug.
-    // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+    // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
     throw std::overflow_error(
       "exhausted the unique id's for publishers and subscribers in this process "
       "(congratulations your computer is either extremely fast or extremely old)");

rclcpp/src/rclcpp/memory_strategies.cpp

@@ -14,6 +14,8 @@
 
 #include "rclcpp/memory_strategies.hpp"
 
+#include <memory>
+
 #include "rclcpp/strategies/allocator_memory_strategy.hpp"
 
 using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy;

rclcpp/src/rclcpp/memory_strategy.cpp

@@ -18,7 +18,7 @@ using rclcpp::memory_strategy::MemoryStrategy;
 
 rclcpp::subscription::SubscriptionBase::SharedPtr
 MemoryStrategy::get_subscription_by_handle(
-  void * subscriber_handle, const WeakNodeVector & weak_nodes)
+  const rcl_subscription_t * subscriber_handle, const WeakNodeVector & weak_nodes)
 {
   for (auto & weak_node : weak_nodes) {
     auto node = weak_node.lock();
@@ -33,12 +33,10 @@ MemoryStrategy::get_subscription_by_handle(
       for (auto & weak_subscription : group->get_subscription_ptrs()) {
         auto subscription = weak_subscription.lock();
         if (subscription) {
-          if (subscription->get_subscription_handle()->data == subscriber_handle) {
+          if (subscription->get_subscription_handle() == subscriber_handle) {
             return subscription;
           }
-          if (subscription->get_intra_process_subscription_handle() &&
-            subscription->get_intra_process_subscription_handle()->data == subscriber_handle)
-          {
+          if (subscription->get_intra_process_subscription_handle() == subscriber_handle) {
             return subscription;
           }
         }
@@ -49,7 +47,8 @@ MemoryStrategy::get_subscription_by_handle(
 }
 
 rclcpp::service::ServiceBase::SharedPtr
-MemoryStrategy::get_service_by_handle(void * service_handle, const WeakNodeVector & weak_nodes)
+MemoryStrategy::get_service_by_handle(const rcl_service_t * service_handle,
+  const WeakNodeVector & weak_nodes)
 {
   for (auto & weak_node : weak_nodes) {
     auto node = weak_node.lock();
@@ -61,8 +60,9 @@ MemoryStrategy::get_service_by_handle(void * service_handle, const WeakNodeVecto
       if (!group) {
         continue;
       }
-      for (auto & service : group->get_service_ptrs()) {
-        if (service->get_service_handle()->data == service_handle) {
+      for (auto & weak_service : group->get_service_ptrs()) {
+        auto service = weak_service.lock();
+        if (service && service->get_service_handle() == service_handle) {
           return service;
         }
       }
@@ -72,7 +72,8 @@ MemoryStrategy::get_service_by_handle(void * service_handle, const WeakNodeVecto
 }
 
 rclcpp::client::ClientBase::SharedPtr
-MemoryStrategy::get_client_by_handle(void * client_handle, const WeakNodeVector & weak_nodes)
+MemoryStrategy::get_client_by_handle(const rcl_client_t * client_handle,
+  const WeakNodeVector & weak_nodes)
 {
   for (auto & weak_node : weak_nodes) {
     auto node = weak_node.lock();
@@ -86,7 +87,7 @@ MemoryStrategy::get_client_by_handle(void * client_handle, const WeakNodeVector
       }
       for (auto & weak_client : group->get_client_ptrs()) {
         auto client = weak_client.lock();
-        if (client && client->get_client_handle()->data == client_handle) {
+        if (client && client->get_client_handle() == client_handle) {
           return client;
         }
       }
@@ -95,7 +96,7 @@ MemoryStrategy::get_client_by_handle(void * client_handle, const WeakNodeVector
   return nullptr;
 }
 
-rclcpp::node::Node::SharedPtr
+rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
 MemoryStrategy::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group,
   const WeakNodeVector & weak_nodes)
 {
@@ -158,8 +159,9 @@ MemoryStrategy::get_group_by_service(
       if (!group) {
         continue;
       }
-      for (auto & serv : group->get_service_ptrs()) {
-        if (serv == service) {
+      for (auto & weak_serv : group->get_service_ptrs()) {
+        auto serv = weak_serv.lock();
+        if (serv && serv == service) {
           return group;
         }
       }

rclcpp/src/rclcpp/node.cpp

@@ -15,348 +15,211 @@
 #include <algorithm>
 #include <limits>
 #include <map>
+#include <memory>
 #include <string>
 #include <utility>
 #include <vector>
 
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/graph_listener.hpp"
 #include "rclcpp/node.hpp"
+#include "rclcpp/node_interfaces/node_base.hpp"
+#include "rclcpp/node_interfaces/node_graph.hpp"
+#include "rclcpp/node_interfaces/node_parameters.hpp"
+#include "rclcpp/node_interfaces/node_services.hpp"
+#include "rclcpp/node_interfaces/node_timers.hpp"
+#include "rclcpp/node_interfaces/node_topics.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(
+  const std::string & node_name,
+  const std::string & namespace_,
+  bool use_intra_process_comms)
 : Node(
     node_name,
+    namespace_,
     rclcpp::contexts::default_context::get_global_default_context(),
     use_intra_process_comms)
 {}
 
 Node::Node(
   const std::string & node_name,
+  const std::string & namespace_,
   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_(rmw_create_guard_condition())
+: node_base_(new rclcpp::node_interfaces::NodeBase(node_name, namespace_, context)),
+  node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())),
+  node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())),
+  node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())),
+  node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())),
+  node_parameters_(new rclcpp::node_interfaces::NodeParameters(
+      node_topics_.get(),
+      use_intra_process_comms
+    )),
+  use_intra_process_comms_(use_intra_process_comms)
 {
-  if (!notify_guard_condition_) {
-    throw std::runtime_error("Failed to create guard condition for node: " +
-            std::string(rmw_get_error_string_safe()));
-  }
-  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)()) {
-      if (rmw_destroy_guard_condition(notify_guard_condition_) != RMW_RET_OK) {
-        fprintf(
-          stderr, "Failed to destroy notify guard condition: %s\n", rmw_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
-  }
-
-  auto node = rmw_create_node(name_.c_str(), domain_id);
-  if (!node) {
-    // *INDENT-OFF*
-    if (rmw_destroy_guard_condition(notify_guard_condition_) != RMW_RET_OK) {
-      fprintf(
-        stderr, "Failed to destroy notify guard condition: %s\n", rmw_get_error_string_safe());
-    }
-    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);
 }
 
 Node::~Node()
-{
-  if (rmw_destroy_guard_condition(notify_guard_condition_) != RMW_RET_OK) {
-    fprintf(stderr, "Warning! Failed to destroy guard condition in Node destructor: %s\n",
-      rmw_get_error_string_safe());
-  }
-}
+{}
 
-const std::string &
+const char *
 Node::get_name() const
 {
-  return name_;
+  return node_base_->get_name();
+}
+
+const char *
+Node::get_namespace() const
+{
+  return node_base_->get_namespace();
 }
 
 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;
+  return node_base_->create_callback_group(group_type);
 }
 
 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;
+  return node_base_->callback_group_in_node(group);
 }
 
-// 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;
+  return node_parameters_->set_parameters(parameters);
 }
 
 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;
+  return node_parameters_->set_parameters_atomically(parameters);
 }
 
 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;
+  return node_parameters_->get_parameters(names);
+}
 
-  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;
+rclcpp::parameter::ParameterVariant
+Node::get_parameter(const std::string & name) const
+{
+  return node_parameters_->get_parameter(name);
+}
+
+bool Node::get_parameter(const std::string & name,
+  rclcpp::parameter::ParameterVariant & parameter) const
+{
+  return node_parameters_->get_parameter(name, parameter);
 }
 
 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;
+  return node_parameters_->describe_parameters(names);
 }
 
 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;
+  return node_parameters_->get_parameter_types(names);
 }
 
 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;
+  return node_parameters_->list_parameters(prefixes, depth);
 }
 
-std::map<std::string, std::string>
+std::map<std::string, std::vector<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;
+  return node_graph_->get_topic_names_and_types();
+}
 
-  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;
+std::map<std::string, std::vector<std::string>>
+Node::get_service_names_and_types() const
+{
+  return node_graph_->get_service_names_and_types();
 }
 
 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;
+  return node_graph_->count_publishers(topic_name);
 }
 
 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;
+  return node_graph_->count_subscribers(topic_name);
 }
 
-
-const Node::CallbackGroupWeakPtrList &
+const std::vector<rclcpp::callback_group::CallbackGroup::WeakPtr> &
 Node::get_callback_groups() const
 {
-  return callback_groups_;
+  return node_base_->get_callback_groups();
 }
 
-rmw_guard_condition_t * Node::get_notify_guard_condition() const
+rclcpp::event::Event::SharedPtr
+Node::get_graph_event()
 {
-  return notify_guard_condition_;
+  return node_graph_->get_graph_event();
+}
+
+void
+Node::wait_for_graph_change(
+  rclcpp::event::Event::SharedPtr event,
+  std::chrono::nanoseconds timeout)
+{
+  node_graph_->wait_for_graph_change(event, timeout);
+}
+
+rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
+Node::get_node_base_interface()
+{
+  return node_base_;
+}
+
+rclcpp::node_interfaces::NodeGraphInterface::SharedPtr
+Node::get_node_graph_interface()
+{
+  return node_graph_;
+}
+
+rclcpp::node_interfaces::NodeTimersInterface::SharedPtr
+Node::get_node_timers_interface()
+{
+  return node_timers_;
+}
+
+rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr
+Node::get_node_topics_interface()
+{
+  return node_topics_;
+}
+
+rclcpp::node_interfaces::NodeServicesInterface::SharedPtr
+Node::get_node_services_interface()
+{
+  return node_services_;
+}
+
+rclcpp::node_interfaces::NodeParametersInterface::SharedPtr
+Node::get_node_parameters_interface()
+{
+  return node_parameters_;
 }

rclcpp/src/rclcpp/node_interfaces/node_base.cpp

@@ -0,0 +1,262 @@
+// 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 <string>
+#include <limits>
+#include <memory>
+#include <vector>
+
+#include "rclcpp/node_interfaces/node_base.hpp"
+
+#include "rclcpp/exceptions.hpp"
+#include "rmw/validate_node_name.h"
+#include "rmw/validate_namespace.h"
+
+using rclcpp::exceptions::throw_from_rcl_error;
+
+using rclcpp::node_interfaces::NodeBase;
+
+NodeBase::NodeBase(
+  const std::string & node_name,
+  const std::string & namespace_,
+  rclcpp::context::Context::SharedPtr context)
+: context_(context),
+  node_handle_(nullptr),
+  default_callback_group_(nullptr),
+  associated_with_executor_(false),
+  notify_guard_condition_is_valid_(false)
+{
+  // Setup the guard condition that is notified when changes occur in the graph.
+  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");
+  }
+
+  // Setup a safe exit lamda to clean up the guard condition in case of an error here.
+  auto finalize_notify_guard_condition = [this]() {
+      // 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());
+      }
+    };
+
+  // Determine the domain id based on the options and the ROS_DOMAIN_ID env variable.
+  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.
+      finalize_notify_guard_condition();
+#ifdef _WIN32
+      // free the ros_domain_id before throwing, if getenv was used on Windows
+      free(ros_domain_id);
+#endif
+
+      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
+  }
+
+  // Create the rcl node and store it in a shared_ptr with a custom destructor.
+  rcl_node_t * rcl_node = new rcl_node_t(rcl_get_zero_initialized_node());
+
+  rcl_node_options_t options = rcl_node_get_default_options();
+  // TODO(wjwwood): pass the Allocator to the options
+  options.domain_id = domain_id;
+  ret = rcl_node_init(rcl_node, node_name.c_str(), namespace_.c_str(), &options);
+  if (ret != RCL_RET_OK) {
+    // Finalize the interrupt guard condition.
+    finalize_notify_guard_condition();
+
+    delete rcl_node;
+
+    if (ret == RCL_RET_NODE_INVALID_NAME) {
+      rcl_reset_error();  // discard rcl_node_init error
+      int validation_result;
+      size_t invalid_index;
+      rmw_ret_t rmw_ret =
+        rmw_validate_node_name(node_name.c_str(), &validation_result, &invalid_index);
+      if (rmw_ret != RMW_RET_OK) {
+        if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
+          throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "failed to validate node name");
+        }
+        throw_from_rcl_error(RCL_RET_ERROR, "failed to validate node name");
+      }
+      throw rclcpp::exceptions::InvalidNodeNameError(
+              node_name.c_str(),
+              rmw_node_name_validation_result_string(validation_result),
+              invalid_index);
+    }
+
+    if (ret == RCL_RET_NODE_INVALID_NAMESPACE) {
+      rcl_reset_error();  // discard rcl_node_init error
+      int validation_result;
+      size_t invalid_index;
+      rmw_ret_t rmw_ret =
+        rmw_validate_namespace(namespace_.c_str(), &validation_result, &invalid_index);
+      if (rmw_ret != RMW_RET_OK) {
+        if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
+          throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "failed to validate namespace");
+        }
+        throw_from_rcl_error(RCL_RET_ERROR, "failed to validate namespace");
+      }
+      throw rclcpp::exceptions::InvalidNamespaceError(
+              namespace_.c_str(),
+              rmw_namespace_validation_result_string(validation_result),
+              invalid_index);
+    }
+
+    throw_from_rcl_error(ret, "failed to initialize rcl node");
+  }
+
+  node_handle_.reset(rcl_node, [](rcl_node_t * node) -> void {
+    if (rcl_node_fini(node) != RCL_RET_OK) {
+      fprintf(
+        stderr, "Error in destruction of rcl node handle: %s\n", rcl_get_error_string_safe());
+    }
+    delete node;
+  });
+
+  // Create the default callback group.
+  using rclcpp::callback_group::CallbackGroupType;
+  default_callback_group_ = create_callback_group(CallbackGroupType::MutuallyExclusive);
+
+  // Indicate the notify_guard_condition is now valid.
+  notify_guard_condition_is_valid_ = true;
+}
+
+NodeBase::~NodeBase()
+{
+  // Finalize the interrupt guard condition after removing self from graph listener.
+  {
+    std::lock_guard<std::recursive_mutex> notify_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 char *
+NodeBase::get_name() const
+{
+  return rcl_node_get_name(node_handle_.get());
+}
+
+const char *
+NodeBase::get_namespace() const
+{
+  return rcl_node_get_namespace(node_handle_.get());
+}
+
+rclcpp::context::Context::SharedPtr
+NodeBase::get_context()
+{
+  return context_;
+}
+
+rcl_node_t *
+NodeBase::get_rcl_node_handle()
+{
+  return node_handle_.get();
+}
+
+const rcl_node_t *
+NodeBase::get_rcl_node_handle() const
+{
+  return node_handle_.get();
+}
+
+std::shared_ptr<rcl_node_t>
+NodeBase::get_shared_rcl_node_handle()
+{
+  return node_handle_;
+}
+
+std::shared_ptr<const rcl_node_t>
+NodeBase::get_shared_rcl_node_handle() const
+{
+  return node_handle_;
+}
+
+rclcpp::callback_group::CallbackGroup::SharedPtr
+NodeBase::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;
+}
+
+rclcpp::callback_group::CallbackGroup::SharedPtr
+NodeBase::get_default_callback_group()
+{
+  return default_callback_group_;
+}
+
+bool
+NodeBase::callback_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;
+}
+
+const std::vector<rclcpp::callback_group::CallbackGroup::WeakPtr> &
+NodeBase::get_callback_groups() const
+{
+  return callback_groups_;
+}
+
+std::atomic_bool &
+NodeBase::get_associated_with_executor_atomic()
+{
+  return associated_with_executor_;
+}
+
+rcl_guard_condition_t *
+NodeBase::get_notify_guard_condition()
+{
+  std::lock_guard<std::recursive_mutex> notify_condition_lock(notify_guard_condition_mutex_);
+  if (!notify_guard_condition_is_valid_) {
+    return nullptr;
+  }
+  return &notify_guard_condition_;
+}
+
+std::unique_lock<std::recursive_mutex>
+NodeBase::acquire_notify_guard_condition_lock() const
+{
+  return std::unique_lock<std::recursive_mutex>(notify_guard_condition_mutex_);
+}

rclcpp/src/rclcpp/node_interfaces/node_graph.cpp

@@ -0,0 +1,312 @@
+// 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/node_interfaces/node_graph.hpp"
+
+#include <map>
+#include <string>
+#include <vector>
+
+#include "rcl/graph.h"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/event.hpp"
+#include "rclcpp/graph_listener.hpp"
+
+using rclcpp::node_interfaces::NodeGraph;
+using rclcpp::exceptions::throw_from_rcl_error;
+using rclcpp::graph_listener::GraphListener;
+
+NodeGraph::NodeGraph(rclcpp::node_interfaces::NodeBaseInterface * node_base)
+: node_base_(node_base),
+  graph_listener_(node_base->get_context()->get_sub_context<GraphListener>()),
+  should_add_to_graph_listener_(true),
+  graph_users_count_(0)
+{}
+
+NodeGraph::~NodeGraph()
+{
+  // 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);
+  }
+}
+
+std::map<std::string, std::vector<std::string>>
+NodeGraph::get_topic_names_and_types(bool no_demangle) const
+{
+  rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types();
+
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto ret = rcl_get_topic_names_and_types(
+    node_base_->get_rcl_node_handle(),
+    &allocator,
+    no_demangle,
+    &topic_names_and_types);
+  if (ret != RCL_RET_OK) {
+    auto error_msg = std::string("failed to get topic names and types: ") +
+      rcl_get_error_string_safe();
+    rcl_reset_error();
+    if (rcl_names_and_types_fini(&topic_names_and_types) != RCL_RET_OK) {
+      error_msg += std::string(", failed also to cleanup topic names and types, leaking memory: ") +
+        rcl_get_error_string_safe();
+    }
+    throw std::runtime_error(error_msg + rcl_get_error_string_safe());
+  }
+
+  std::map<std::string, std::vector<std::string>> topics_and_types;
+  for (size_t i = 0; i < topic_names_and_types.names.size; ++i) {
+    std::string topic_name = topic_names_and_types.names.data[i];
+    for (size_t j = 0; j < topic_names_and_types.types[i].size; ++j) {
+      topics_and_types[topic_name].emplace_back(topic_names_and_types.types[i].data[j]);
+    }
+  }
+
+  ret = rcl_names_and_types_fini(&topic_names_and_types);
+  if (ret != RCL_RET_OK) {
+    // *INDENT-OFF*
+    throw std::runtime_error(
+      std::string("could not destroy topic names and types: ") + rcl_get_error_string_safe());
+    // *INDENT-ON*
+  }
+
+  return topics_and_types;
+}
+
+std::map<std::string, std::vector<std::string>>
+NodeGraph::get_service_names_and_types() const
+{
+  rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types();
+
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto ret = rcl_get_service_names_and_types(
+    node_base_->get_rcl_node_handle(),
+    &allocator,
+    &service_names_and_types);
+  if (ret != RCL_RET_OK) {
+    auto error_msg = std::string("failed to get service names and types: ") +
+      rcl_get_error_string_safe();
+    rcl_reset_error();
+    if (rcl_names_and_types_fini(&service_names_and_types) != RCL_RET_OK) {
+      error_msg +=
+        std::string(", failed also to cleanup service names and types, leaking memory: ") +
+        rcl_get_error_string_safe();
+    }
+    throw std::runtime_error(error_msg + rcl_get_error_string_safe());
+  }
+
+  std::map<std::string, std::vector<std::string>> services_and_types;
+  for (size_t i = 0; i < service_names_and_types.names.size; ++i) {
+    std::string service_name = service_names_and_types.names.data[i];
+    for (size_t j = 0; j < service_names_and_types.types[i].size; ++j) {
+      services_and_types[service_name].emplace_back(service_names_and_types.types[i].data[j]);
+    }
+  }
+
+  ret = rcl_names_and_types_fini(&service_names_and_types);
+  if (ret != RCL_RET_OK) {
+    // *INDENT-OFF*
+    throw std::runtime_error(
+      std::string("could not destroy service names and types: ") + rcl_get_error_string_safe());
+    // *INDENT-ON*
+  }
+
+  return services_and_types;
+}
+
+std::vector<std::string>
+NodeGraph::get_node_names() const
+{
+  rcutils_string_array_t node_names_c =
+    rcutils_get_zero_initialized_string_array();
+
+  auto allocator = rcl_get_default_allocator();
+  auto ret = rcl_get_node_names(
+    node_base_->get_rcl_node_handle(),
+    allocator,
+    &node_names_c);
+  if (ret != RCL_RET_OK) {
+    auto error_msg = std::string("failed to get node names: ") + rcl_get_error_string_safe();
+    rcl_reset_error();
+    if (rcutils_string_array_fini(&node_names_c) != RCUTILS_RET_OK) {
+      error_msg += std::string(", failed also to cleanup node names, leaking memory: ") +
+        rcl_get_error_string_safe();
+    }
+    // TODO(karsten1987): Append rcutils_error_message once it's in master
+    throw std::runtime_error(error_msg);
+  }
+
+  std::vector<std::string> node_names(&node_names_c.data[0],
+    &node_names_c.data[0 + node_names_c.size]);
+  ret = rcutils_string_array_fini(&node_names_c);
+  if (ret != RCUTILS_RET_OK) {
+    // *INDENT-OFF*
+    // TODO(karsten1987): Append rcutils_error_message once it's in master
+    throw std::runtime_error(
+      std::string("could not destroy node names: "));
+    // *INDENT-ON*
+  }
+
+  return node_names;
+}
+
+size_t
+NodeGraph::count_publishers(const std::string & topic_name) const
+{
+  auto rmw_node_handle = rcl_node_get_rmw_handle(node_base_->get_rcl_node_handle());
+  auto fqdn = rclcpp::expand_topic_or_service_name(
+    topic_name,
+    rmw_node_handle->name,
+    rmw_node_handle->namespace_,
+    false);    // false = not a service
+
+  size_t count;
+  // TODO(wjwwood): use the rcl equivalent methods
+  auto ret = rmw_count_publishers(rmw_node_handle, fqdn.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
+NodeGraph::count_subscribers(const std::string & topic_name) const
+{
+  auto rmw_node_handle = rcl_node_get_rmw_handle(node_base_->get_rcl_node_handle());
+  auto fqdn = rclcpp::expand_topic_or_service_name(
+    topic_name,
+    rmw_node_handle->name,
+    rmw_node_handle->namespace_,
+    false);    // false = not a service
+
+  size_t count;
+  // TODO(wjwwood): use the rcl equivalent methods
+  auto ret = rmw_count_subscribers(rmw_node_handle, fqdn.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 rcl_guard_condition_t *
+NodeGraph::get_graph_guard_condition() const
+{
+  return rcl_node_get_graph_guard_condition(node_base_->get_rcl_node_handle());
+}
+
+void
+NodeGraph::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();
+  {
+    auto notify_condition_lock = node_base_->acquire_notify_guard_condition_lock();
+    rcl_ret_t ret = rcl_trigger_guard_condition(node_base_->get_notify_guard_condition());
+    if (RCL_RET_OK != ret) {
+      throw_from_rcl_error(ret, "failed to trigger notify guard condition");
+    }
+  }
+}
+
+void
+NodeGraph::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
+NodeGraph::get_graph_event()
+{
+  auto event = rclcpp::event::Event::make_shared();
+  std::lock_guard<std::mutex> graph_changed_lock(graph_mutex_);
+  graph_events_.push_back(event);
+  graph_users_count_++;
+  // 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();
+  }
+  return event;
+}
+
+void
+NodeGraph::wait_for_graph_change(
+  rclcpp::event::Event::SharedPtr event,
+  std::chrono::nanoseconds timeout)
+{
+  using rclcpp::exceptions::InvalidEventError;
+  using rclcpp::exceptions::EventNotRegisteredError;
+  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();
+    }
+  }
+  auto pred = [&event]() {
+      return event->check() || !rclcpp::utilities::ok();
+    };
+  std::unique_lock<std::mutex> graph_lock(graph_mutex_);
+  if (!pred()) {
+    graph_cv_.wait_for(graph_lock, timeout, pred);
+  }
+}
+
+size_t
+NodeGraph::count_graph_users()
+{
+  return graph_users_count_.load();
+}

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -0,0 +1,245 @@
+// 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/node_interfaces/node_parameters.hpp"
+
+#include <map>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "rcl_interfaces/srv/list_parameters.hpp"
+#include "rclcpp/create_publisher.hpp"
+#include "rcutils/logging_macros.h"
+#include "rmw/qos_profiles.h"
+
+using rclcpp::node_interfaces::NodeParameters;
+
+NodeParameters::NodeParameters(
+  rclcpp::node_interfaces::NodeTopicsInterface * node_topics,
+  bool use_intra_process)
+: node_topics_(node_topics)
+{
+  using MessageT = rcl_interfaces::msg::ParameterEvent;
+  using PublisherT = rclcpp::publisher::Publisher<MessageT>;
+  using AllocatorT = std::allocator<void>;
+  // TODO(wjwwood): expose this allocator through the Parameter interface.
+  auto allocator = std::make_shared<AllocatorT>();
+
+  events_publisher_ = rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
+    node_topics_,
+    "parameter_events",
+    rmw_qos_profile_parameter_events,
+    use_intra_process,
+    allocator);
+}
+
+NodeParameters::~NodeParameters()
+{}
+
+std::vector<rcl_interfaces::msg::SetParametersResult>
+NodeParameters::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
+NodeParameters::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>();
+
+  // TODO(jacquelinekay): handle parameter constraints
+  rcl_interfaces::msg::SetParametersResult result;
+  if (parameters_callback_) {
+    result = parameters_callback_(parameters);
+  } else {
+    result.successful = true;
+  }
+
+  if (!result.successful) {
+    return result;
+  }
+
+  for (auto p : parameters) {
+    if (parameters_.find(p.get_name()) == parameters_.end()) {
+      if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
+        // case: parameter not set before, and input is something other than "NOT_SET"
+        parameter_event->new_parameters.push_back(p.to_parameter());
+      }
+    } else if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
+      // case: parameter was set before, and input is something other than "NOT_SET"
+      parameter_event->changed_parameters.push_back(p.to_parameter());
+    } else {
+      // case: parameter was set before, and input is "NOT_SET"
+      // therefore we will "unset" the previously set parameter
+      // it is not necessary to erase the parameter from parameters_
+      // because the new value for this key (p.get_name()) will be a
+      // ParameterVariant with type "NOT_SET"
+      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_);
+
+  events_publisher_->publish(parameter_event);
+
+  return result;
+}
+
+std::vector<rclcpp::parameter::ParameterVariant>
+NodeParameters::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;
+}
+
+rclcpp::parameter::ParameterVariant
+NodeParameters::get_parameter(const std::string & name) const
+{
+  rclcpp::parameter::ParameterVariant parameter;
+
+  if (get_parameter(name, parameter)) {
+    return parameter;
+  } else {
+    throw std::out_of_range("Parameter '" + name + "' not set");
+  }
+}
+
+bool
+NodeParameters::get_parameter(
+  const std::string & name,
+  rclcpp::parameter::ParameterVariant & parameter) const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  if (parameters_.count(name)) {
+    parameter = parameters_.at(name);
+    return true;
+  } else {
+    return false;
+  }
+}
+
+std::vector<rcl_interfaces::msg::ParameterDescriptor>
+NodeParameters::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>
+NodeParameters::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
+NodeParameters::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(mikaelarguedas) define parameter separator different from "/" to avoid ambiguity
+  // using "." for now
+  const char separator = '.';
+  for (auto & kv : parameters_) {
+    bool get_all = (prefixes.size() == 0) &&
+      ((depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) ||
+      (static_cast<uint64_t>(std::count(kv.first.begin(), kv.first.end(), separator)) < depth));
+    bool prefix_matches = std::any_of(prefixes.cbegin(), prefixes.cend(),
+        [&kv, &depth, &separator](const std::string & prefix) {
+      if (kv.first == prefix) {
+        return true;
+      } else if (kv.first.find(prefix + separator) == 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(), separator)) < depth);
+      }
+      return false;
+    });
+    if (get_all || prefix_matches) {
+      result.names.push_back(kv.first);
+      size_t last_separator = kv.first.find_last_of(separator);
+      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;
+}
+
+void
+NodeParameters::register_param_change_callback(ParametersCallbackFunction callback)
+{
+  if (parameters_callback_) {
+    RCUTILS_LOG_WARN("param_change_callback already registered, "
+      "overwriting previous callback")
+  }
+  parameters_callback_ = callback;
+}

rclcpp/src/rclcpp/node_interfaces/node_services.cpp

@@ -0,0 +1,78 @@
+// 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/node_interfaces/node_services.hpp"
+
+#include <string>
+
+using rclcpp::node_interfaces::NodeServices;
+
+NodeServices::NodeServices(rclcpp::node_interfaces::NodeBaseInterface * node_base)
+: node_base_(node_base)
+{}
+
+NodeServices::~NodeServices()
+{}
+
+void
+NodeServices::add_service(
+  rclcpp::service::ServiceBase::SharedPtr service_base_ptr,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group)
+{
+  if (group) {
+    if (!node_base_->callback_group_in_node(group)) {
+      // TODO(jacquelinekay): use custom exception
+      throw std::runtime_error("Cannot create service, group not in node.");
+    }
+    group->add_service(service_base_ptr);
+  } else {
+    node_base_->get_default_callback_group()->add_service(service_base_ptr);
+  }
+
+  // Notify the executor that a new service was created using the parent Node.
+  {
+    auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
+    if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
+      throw std::runtime_error(
+              std::string("Failed to notify waitset on service creation: ") + rmw_get_error_string()
+      );
+    }
+  }
+}
+
+void
+NodeServices::add_client(
+  rclcpp::client::ClientBase::SharedPtr client_base_ptr,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group)
+{
+  if (group) {
+    if (!node_base_->callback_group_in_node(group)) {
+      // TODO(jacquelinekay): use custom exception
+      throw std::runtime_error("Cannot create client, group not in node.");
+    }
+    group->add_client(client_base_ptr);
+  } else {
+    node_base_->get_default_callback_group()->add_client(client_base_ptr);
+  }
+
+  // Notify the executor that a new client was created using the parent Node.
+  {
+    auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
+    if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
+      throw std::runtime_error(
+              std::string("Failed to notify waitset on client creation: ") + rmw_get_error_string()
+      );
+    }
+  }
+}

rclcpp/src/rclcpp/node_interfaces/node_timers.cpp

@@ -0,0 +1,47 @@
+// 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/node_interfaces/node_timers.hpp"
+
+#include <string>
+
+using rclcpp::node_interfaces::NodeTimers;
+
+NodeTimers::NodeTimers(rclcpp::node_interfaces::NodeBaseInterface * node_base)
+: node_base_(node_base)
+{}
+
+NodeTimers::~NodeTimers()
+{}
+
+void
+NodeTimers::add_timer(
+  rclcpp::timer::TimerBase::SharedPtr timer,
+  rclcpp::callback_group::CallbackGroup::SharedPtr callback_group)
+{
+  if (callback_group) {
+    if (!node_base_->callback_group_in_node(callback_group)) {
+      // TODO(jacquelinekay): use custom exception
+      throw std::runtime_error("Cannot create timer, group not in node.");
+    }
+    callback_group->add_timer(timer);
+  } else {
+    node_base_->get_default_callback_group()->add_timer(timer);
+  }
+  if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
+    throw std::runtime_error(
+            std::string(
+              "Failed to notify waitset on timer creation: ") + rmw_get_error_string());
+  }
+}

rclcpp/src/rclcpp/node_interfaces/node_topics.cpp

@@ -0,0 +1,131 @@
+// 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/node_interfaces/node_topics.hpp"
+
+#include <string>
+
+#include "rclcpp/intra_process_manager.hpp"
+#include "rclcpp/exceptions.hpp"
+
+using rclcpp::exceptions::throw_from_rcl_error;
+
+using rclcpp::node_interfaces::NodeTopics;
+
+NodeTopics::NodeTopics(rclcpp::node_interfaces::NodeBaseInterface * node_base)
+: node_base_(node_base)
+{}
+
+NodeTopics::~NodeTopics()
+{}
+
+rclcpp::publisher::PublisherBase::SharedPtr
+NodeTopics::create_publisher(
+  const std::string & topic_name,
+  const rclcpp::PublisherFactory & publisher_factory,
+  rcl_publisher_options_t & publisher_options,
+  bool use_intra_process)
+{
+  // Create the MessageT specific Publisher using the factory, but store it as PublisherBase.
+  auto publisher = publisher_factory.create_typed_publisher(
+    node_base_, topic_name, publisher_options);
+
+  // Setup intra process publishing if requested.
+  if (use_intra_process) {
+    auto context = node_base_->get_context();
+    // Get the intra process manager instance for this context.
+    auto ipm = context->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
+    // Register the publisher with the intra process manager.
+    uint64_t intra_process_publisher_id =
+      publisher_factory.add_publisher_to_intra_process_manager(ipm.get(), publisher);
+    // Create a function to be called when publisher to do the intra process publish.
+    auto shared_publish_callback = publisher_factory.create_shared_publish_callback(ipm);
+    publisher->setup_intra_process(
+      intra_process_publisher_id,
+      shared_publish_callback,
+      publisher_options);
+  }
+
+  // Return the completed publisher.
+  return publisher;
+}
+
+void
+NodeTopics::add_publisher(
+  rclcpp::publisher::PublisherBase::SharedPtr publisher)
+{
+  // The publisher is not added to a callback group or anthing like that for now.
+  // It may be stored within the NodeTopics class or the NodeBase class in the future.
+  (void)publisher;
+  // Notify the executor that a new publisher was created using the parent Node.
+  {
+    auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
+    if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
+      throw std::runtime_error(
+              std::string(
+                "Failed to notify waitset on publisher creation: ") + rmw_get_error_string());
+    }
+  }
+}
+
+rclcpp::subscription::SubscriptionBase::SharedPtr
+NodeTopics::create_subscription(
+  const std::string & topic_name,
+  const rclcpp::SubscriptionFactory & subscription_factory,
+  rcl_subscription_options_t & subscription_options,
+  bool use_intra_process)
+{
+  auto subscription = subscription_factory.create_typed_subscription(
+    node_base_, topic_name, subscription_options);
+
+  // Setup intra process publishing if requested.
+  if (use_intra_process) {
+    auto context = node_base_->get_context();
+    auto intra_process_manager =
+      context->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
+    subscription_factory.setup_intra_process(
+      intra_process_manager, subscription, subscription_options);
+  }
+
+  // Return the completed subscription.
+  return subscription;
+}
+
+void
+NodeTopics::add_subscription(
+  rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
+  rclcpp::callback_group::CallbackGroup::SharedPtr callback_group)
+{
+  // Assign to a group.
+  if (callback_group) {
+    if (!node_base_->callback_group_in_node(callback_group)) {
+      // TODO(jacquelinekay): use custom exception
+      throw std::runtime_error("Cannot create subscription, callback group not in node.");
+    }
+    callback_group->add_subscription(subscription);
+  } else {
+    node_base_->get_default_callback_group()->add_subscription(subscription);
+  }
+
+  // Notify the executor that a new subscription was created using the parent Node.
+  {
+    auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
+    if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
+      throw std::runtime_error(
+              std::string(
+                "Failed to notify waitset on subscription creation: ") + rmw_get_error_string()
+      );
+    }
+  }
+}

rclcpp/src/rclcpp/parameter.cpp

@@ -12,13 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "rclcpp/parameter.hpp"
-
 #include <ostream>
 #include <sstream>
 #include <string>
 #include <vector>
 
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/utilities.hpp"
+
 using rclcpp::parameter::ParameterType;
 using rclcpp::parameter::ParameterVariant;
 
@@ -105,7 +106,7 @@ ParameterVariant::get_type_name() const
     case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET:
       return "not set";
     default:
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
       throw std::runtime_error(
         "Unexpected type from ParameterVariant: " + std::to_string(get_type()));
       // *INDENT-ON*
@@ -172,7 +173,7 @@ ParameterVariant::from_parameter(const rcl_interfaces::msg::Parameter & paramete
       throw std::runtime_error("Type from ParameterValue is not set");
     default:
       // TODO(wjwwood): use custom exception
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
       throw std::runtime_error(
         "Unexpected type from ParameterVariant: " + std::to_string(parameter.value.type));
       // *INDENT-ON*
@@ -218,7 +219,7 @@ ParameterVariant::value_to_string() const
     case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET:
       return "not set";
     default:
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
       throw std::runtime_error(
         "Unexpected type from ParameterVariant: " + std::to_string(get_type()));
       // *INDENT-ON*

rclcpp/src/rclcpp/parameter_client.cpp

@@ -15,34 +15,92 @@
 #include "rclcpp/parameter_client.hpp"
 
 #include <algorithm>
+#include <memory>
 #include <string>
 #include <vector>
 
+#include "./parameter_service_names.hpp"
+
 using rclcpp::parameter_client::AsyncParametersClient;
 using rclcpp::parameter_client::SyncParametersClient;
 
 AsyncParametersClient::AsyncParametersClient(
-  const rclcpp::node::Node::SharedPtr node,
-  const std::string & remote_node_name)
-: node_(node)
+  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+  const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
+  const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
+  const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
+  const std::string & remote_node_name,
+  const rmw_qos_profile_t & qos_profile)
+: node_topics_interface_(node_topics_interface)
 {
   if (remote_node_name != "") {
     remote_node_name_ = remote_node_name;
   } else {
-    remote_node_name_ = node_->get_name();
+    remote_node_name_ = node_base_interface->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");
+
+  rcl_client_options_t options = rcl_client_get_default_options();
+  options.qos = qos_profile;
+
+  using rclcpp::client::Client;
+  using rclcpp::client::ClientBase;
+
+  get_parameters_client_ = Client<rcl_interfaces::srv::GetParameters>::make_shared(
+    node_base_interface.get(),
+    node_graph_interface,
+    remote_node_name_ + "/" + parameter_service_names::get_parameters,
+    options);
+  auto get_parameters_base = std::dynamic_pointer_cast<ClientBase>(get_parameters_client_);
+  node_services_interface->add_client(get_parameters_base, nullptr);
+
+  get_parameter_types_client_ = Client<rcl_interfaces::srv::GetParameterTypes>::make_shared(
+    node_base_interface.get(),
+    node_graph_interface,
+    remote_node_name_ + "/" + parameter_service_names::get_parameter_types,
+    options);
+  auto get_parameter_types_base =
+    std::dynamic_pointer_cast<ClientBase>(get_parameter_types_client_);
+  node_services_interface->add_client(get_parameter_types_base, nullptr);
+
+  set_parameters_client_ = Client<rcl_interfaces::srv::SetParameters>::make_shared(
+    node_base_interface.get(),
+    node_graph_interface,
+    remote_node_name_ + "/" + parameter_service_names::set_parameters,
+    options);
+  auto set_parameters_base = std::dynamic_pointer_cast<ClientBase>(set_parameters_client_);
+  node_services_interface->add_client(set_parameters_base, nullptr);
+
+  list_parameters_client_ = Client<rcl_interfaces::srv::ListParameters>::make_shared(
+    node_base_interface.get(),
+    node_graph_interface,
+    remote_node_name_ + "/" + parameter_service_names::list_parameters,
+    options);
+  auto list_parameters_base = std::dynamic_pointer_cast<ClientBase>(list_parameters_client_);
+  node_services_interface->add_client(list_parameters_base, nullptr);
+
+  describe_parameters_client_ = Client<rcl_interfaces::srv::DescribeParameters>::make_shared(
+    node_base_interface.get(),
+    node_graph_interface,
+    remote_node_name_ + "/" + parameter_service_names::describe_parameters,
+    options);
+  auto describe_parameters_base =
+    std::dynamic_pointer_cast<ClientBase>(describe_parameters_client_);
+  node_services_interface->add_client(describe_parameters_base, nullptr);
 }
 
+AsyncParametersClient::AsyncParametersClient(
+  const rclcpp::node::Node::SharedPtr node,
+  const std::string & remote_node_name,
+  const rmw_qos_profile_t & qos_profile)
+: AsyncParametersClient(
+    node->get_node_base_interface(),
+    node->get_node_topics_interface(),
+    node->get_node_graph_interface(),
+    node->get_node_services_interface(),
+    remote_node_name,
+    qos_profile)
+{}
+
 std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>
 AsyncParametersClient::get_parameters(
   const std::vector<std::string> & names,
@@ -57,7 +115,7 @@ AsyncParametersClient::get_parameters(
   auto request = std::make_shared<rcl_interfaces::srv::GetParameters::Request>();
   request->names = names;
 
-  // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+  // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
   get_parameters_client_->async_send_request(
     request,
     [request, promise_result, future_result, &callback](
@@ -100,7 +158,7 @@ AsyncParametersClient::get_parameter_types(
   auto request = std::make_shared<rcl_interfaces::srv::GetParameterTypes::Request>();
   request->names = names;
 
-  // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+  // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
   get_parameter_types_client_->async_send_request(
     request,
     [promise_result, future_result, &callback](
@@ -135,7 +193,7 @@ AsyncParametersClient::set_parameters(
 
   auto request = std::make_shared<rcl_interfaces::srv::SetParameters::Request>();
 
-  // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+  // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
   std::transform(parameters.begin(), parameters.end(), std::back_inserter(request->parameters),
     [](rclcpp::parameter::ParameterVariant p) {
       return p.to_parameter();
@@ -171,7 +229,7 @@ AsyncParametersClient::set_parameters_atomically(
 
   auto request = std::make_shared<rcl_interfaces::srv::SetParametersAtomically::Request>();
 
-  // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+  // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
   std::transform(parameters.begin(), parameters.end(), std::back_inserter(request->parameters),
     [](rclcpp::parameter::ParameterVariant p) {
       return p.to_parameter();
@@ -210,7 +268,7 @@ AsyncParametersClient::list_parameters(
   request->prefixes = prefixes;
   request->depth = depth;
 
-  // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+  // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
   list_parameters_client_->async_send_request(
     request,
     [promise_result, future_result, &callback](
@@ -227,27 +285,67 @@ AsyncParametersClient::list_parameters(
   return future_result;
 }
 
+bool
+AsyncParametersClient::service_is_ready() const
+{
+  return
+    get_parameters_client_->service_is_ready() &&
+    get_parameter_types_client_->service_is_ready() &&
+    set_parameters_client_->service_is_ready() &&
+    list_parameters_client_->service_is_ready() &&
+    describe_parameters_client_->service_is_ready();
+}
+
+bool
+AsyncParametersClient::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
+{
+  const std::vector<std::shared_ptr<rclcpp::client::ClientBase>> clients = {
+    get_parameters_client_,
+    get_parameter_types_client_,
+    set_parameters_client_,
+    list_parameters_client_,
+    describe_parameters_client_
+  };
+  for (auto & client : clients) {
+    auto stamp = std::chrono::steady_clock::now();
+    if (!client->wait_for_service(timeout)) {
+      return false;
+    }
+    if (timeout > std::chrono::nanoseconds::zero()) {
+      timeout -= std::chrono::duration_cast<std::chrono::nanoseconds>(
+        std::chrono::steady_clock::now() - stamp);
+      if (timeout < std::chrono::nanoseconds::zero()) {
+        timeout = std::chrono::nanoseconds::zero();
+      }
+    }
+  }
+  return true;
+}
+
 SyncParametersClient::SyncParametersClient(
-  rclcpp::node::Node::SharedPtr node)
+  rclcpp::node::Node::SharedPtr node,
+  const rmw_qos_profile_t & qos_profile)
 : node_(node)
 {
   executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
-  async_parameters_client_ = std::make_shared<AsyncParametersClient>(node);
+  async_parameters_client_ = std::make_shared<AsyncParametersClient>(node, "", qos_profile);
 }
 
 SyncParametersClient::SyncParametersClient(
   rclcpp::executor::Executor::SharedPtr executor,
-  rclcpp::node::Node::SharedPtr node)
+  rclcpp::node::Node::SharedPtr node,
+  const rmw_qos_profile_t & qos_profile)
 : executor_(executor), node_(node)
 {
-  async_parameters_client_ = std::make_shared<AsyncParametersClient>(node);
+  async_parameters_client_ = std::make_shared<AsyncParametersClient>(node, "", qos_profile);
 }
 
 std::vector<rclcpp::parameter::ParameterVariant>
 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) ==
+  using rclcpp::executors::spin_node_until_future_complete;
+  if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) ==
     rclcpp::executor::FutureReturnCode::SUCCESS)
   {
     return f.get();
@@ -256,12 +354,22 @@ SyncParametersClient::get_parameters(const std::vector<std::string> & parameter_
   return std::vector<rclcpp::parameter::ParameterVariant>();
 }
 
+bool
+SyncParametersClient::has_parameter(const std::string & parameter_name)
+{
+  std::vector<std::string> names;
+  names.push_back(parameter_name);
+  auto vars = list_parameters(names, 1);
+  return vars.names.size() > 0;
+}
+
 std::vector<rclcpp::parameter::ParameterType>
 SyncParametersClient::get_parameter_types(const std::vector<std::string> & parameter_names)
 {
   auto f = async_parameters_client_->get_parameter_types(parameter_names);
 
-  if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
+  using rclcpp::executors::spin_node_until_future_complete;
+  if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) ==
     rclcpp::executor::FutureReturnCode::SUCCESS)
   {
     return f.get();
@@ -275,7 +383,8 @@ SyncParametersClient::set_parameters(
 {
   auto f = async_parameters_client_->set_parameters(parameters);
 
-  if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
+  using rclcpp::executors::spin_node_until_future_complete;
+  if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) ==
     rclcpp::executor::FutureReturnCode::SUCCESS)
   {
     return f.get();
@@ -289,7 +398,8 @@ SyncParametersClient::set_parameters_atomically(
 {
   auto f = async_parameters_client_->set_parameters_atomically(parameters);
 
-  if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
+  using rclcpp::executors::spin_node_until_future_complete;
+  if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) ==
     rclcpp::executor::FutureReturnCode::SUCCESS)
   {
     return f.get();
@@ -305,7 +415,8 @@ SyncParametersClient::list_parameters(
 {
   auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth);
 
-  if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
+  using rclcpp::executors::spin_node_until_future_complete;
+  if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) ==
     rclcpp::executor::FutureReturnCode::SUCCESS)
   {
     return f.get();

rclcpp/src/rclcpp/parameter_service.cpp

@@ -15,17 +15,23 @@
 #include "rclcpp/parameter_service.hpp"
 
 #include <algorithm>
+#include <memory>
+#include <string>
 #include <vector>
 
+#include "./parameter_service_names.hpp"
+
 using rclcpp::parameter_service::ParameterService;
 
-ParameterService::ParameterService(const rclcpp::node::Node::SharedPtr node)
+ParameterService::ParameterService(
+  const rclcpp::node::Node::SharedPtr node,
+  const rmw_qos_profile_t & qos_profile)
 : node_(node)
 {
   std::weak_ptr<rclcpp::node::Node> captured_node = node_;
-  // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+  // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
   get_parameters_service_ = node_->create_service<rcl_interfaces::srv::GetParameters>(
-    node_->get_name() + "__get_parameters",
+    std::string(node_->get_name()) + "/" + parameter_service_names::get_parameters,
     [captured_node](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::GetParameters::Request> request,
@@ -39,11 +45,12 @@ ParameterService::ParameterService(const rclcpp::node::Node::SharedPtr node)
       for (auto & pvariant : values) {
         response->values.push_back(pvariant.get_parameter_value());
       }
-    }
+    },
+    qos_profile
   );
 
   get_parameter_types_service_ = node_->create_service<rcl_interfaces::srv::GetParameterTypes>(
-    node_->get_name() + "__get_parameter_types",
+    std::string(node_->get_name()) + "/" + parameter_service_names::get_parameter_types,
     [captured_node](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::GetParameterTypes::Request> request,
@@ -58,11 +65,12 @@ ParameterService::ParameterService(const rclcpp::node::Node::SharedPtr node)
       std::back_inserter(response->types), [](const uint8_t & type) {
         return static_cast<rclcpp::parameter::ParameterType>(type);
       });
-    }
+    },
+    qos_profile
   );
 
   set_parameters_service_ = node_->create_service<rcl_interfaces::srv::SetParameters>(
-    node_->get_name() + "__set_parameters",
+    std::string(node_->get_name()) + "/" + parameter_service_names::set_parameters,
     [captured_node](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::SetParameters::Request> request,
@@ -78,12 +86,13 @@ ParameterService::ParameterService(const rclcpp::node::Node::SharedPtr node)
       }
       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",
+    std::string(node_->get_name()) + "/" + parameter_service_names::set_parameters_atomically,
     [captured_node](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::SetParametersAtomically::Request> request,
@@ -102,11 +111,12 @@ ParameterService::ParameterService(const rclcpp::node::Node::SharedPtr node)
       });
       auto result = node->set_parameters_atomically(pvariants);
       response->result = result;
-    }
+    },
+    qos_profile
   );
 
   describe_parameters_service_ = node_->create_service<rcl_interfaces::srv::DescribeParameters>(
-    node_->get_name() + "__describe_parameters",
+    std::string(node_->get_name()) + "/" + parameter_service_names::describe_parameters,
     [captured_node](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::DescribeParameters::Request> request,
@@ -118,11 +128,12 @@ ParameterService::ParameterService(const rclcpp::node::Node::SharedPtr node)
       }
       auto descriptors = node->describe_parameters(request->names);
       response->descriptors = descriptors;
-    }
+    },
+    qos_profile
   );
 
   list_parameters_service_ = node_->create_service<rcl_interfaces::srv::ListParameters>(
-    node_->get_name() + "__list_parameters",
+    std::string(node_->get_name()) + "/" + parameter_service_names::list_parameters,
     [captured_node](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::ListParameters::Request> request,
@@ -134,7 +145,8 @@ ParameterService::ParameterService(const rclcpp::node::Node::SharedPtr node)
       }
       auto result = node->list_parameters(request->prefixes, request->depth);
       response->result = result;
-    }
+    },
+    qos_profile
   );
   // *INDENT-ON*
 }

rclcpp/src/rclcpp/parameter_service_names.hpp

@@ -0,0 +1,33 @@
+// Copyright 2017 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__PARAMETER_SERVICE_NAMES_HPP_
+#define RCLCPP__PARAMETER_SERVICE_NAMES_HPP_
+
+namespace rclcpp
+{
+namespace parameter_service_names
+{
+
+static constexpr const char * get_parameters = "get_parameters";
+static constexpr const char * get_parameter_types = "get_parameter_types";
+static constexpr const char * set_parameters = "set_parameters";
+static constexpr const char * set_parameters_atomically = "set_parameters_atomically";
+static constexpr const char * describe_parameters = "describe_parameters";
+static constexpr const char * list_parameters = "list_parameters";
+
+}  // namespace parameter_service_names
+}  // namespace rclcpp
+
+#endif  // RCLCPP__PARAMETER_SERVICE_NAMES_HPP_

rclcpp/src/rclcpp/publisher.cpp

@@ -28,59 +28,89 @@
 
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/allocator/allocator_deleter.hpp"
+#include "rclcpp/exceptions.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
 
 using rclcpp::publisher::PublisherBase;
 
 PublisherBase::PublisherBase(
-  std::shared_ptr<rmw_node_t> node_handle,
-  rmw_publisher_t * publisher_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),
+  rclcpp::node_interfaces::NodeBaseInterface * node_base,
+  const std::string & topic,
+  const rosidl_message_type_support_t & type_support,
+  const rcl_publisher_options_t & publisher_options)
+: rcl_node_handle_(node_base->get_shared_rcl_node_handle()),
   intra_process_publisher_id_(0), store_intra_process_message_(nullptr)
 {
+  rcl_ret_t ret = rcl_publisher_init(
+    &publisher_handle_,
+    rcl_node_handle_.get(),
+    &type_support,
+    topic.c_str(),
+    &publisher_options);
+  if (ret != RCL_RET_OK) {
+    if (ret == RCL_RET_TOPIC_NAME_INVALID) {
+      auto rcl_node_handle = rcl_node_handle_.get();
+      // this will throw on any validation problem
+      rcl_reset_error();
+      expand_topic_or_service_name(
+        topic,
+        rcl_node_get_name(rcl_node_handle),
+        rcl_node_get_namespace(rcl_node_handle));
+    }
+
+    rclcpp::exceptions::throw_from_rcl_error(ret, "could not create publisher");
+  }
   // 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*
+  rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(&publisher_handle_);
+  if (!publisher_rmw_handle) {
+    auto msg = std::string("failed to get rmw handle: ") + rcl_get_error_string_safe();
+    rcl_reset_error();
+    throw std::runtime_error(msg);
+  }
+  if (rmw_get_gid_for_publisher(publisher_rmw_handle, &rmw_gid_) != RMW_RET_OK) {
+    auto msg = std::string("failed to get publisher gid: ") + rmw_get_error_string_safe();
+    rmw_reset_error();
+    throw std::runtime_error(msg);
   }
 }
 
 PublisherBase::~PublisherBase()
 {
-  if (intra_process_publisher_handle_) {
-    if (rmw_destroy_publisher(node_handle_.get(), intra_process_publisher_handle_)) {
-      fprintf(
-        stderr,
-        "Error in destruction of intra process rmw publisher handle: %s\n",
-        rmw_get_error_string_safe());
-    }
+  if (rcl_publisher_fini(&intra_process_publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) {
+    fprintf(
+      stderr,
+      "Error in destruction of intra process rcl publisher handle: %s\n",
+      rcl_get_error_string_safe());
+    rcl_reset_error();
   }
-  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());
-    }
+
+  if (rcl_publisher_fini(&publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) {
+    fprintf(
+      stderr,
+      "Error in destruction of rcl publisher handle: %s\n",
+      rcl_get_error_string_safe());
+    rcl_reset_error();
   }
 }
 
-const std::string &
+const char *
 PublisherBase::get_topic_name() const
 {
-  return topic_;
+  return rcl_publisher_get_topic_name(&publisher_handle_);
 }
 
 size_t
 PublisherBase::get_queue_size() const
 {
-  return queue_size_;
+  const rcl_publisher_options_t * publisher_options = rcl_publisher_get_options(&publisher_handle_);
+  if (!publisher_options) {
+    auto msg = std::string("failed to get publisher options: ") + rcl_get_error_string_safe();
+    rcl_reset_error();
+    throw std::runtime_error(msg);
+  }
+  return publisher_options->qos.depth;
 }
 
 const rmw_gid_t &
@@ -95,6 +125,18 @@ PublisherBase::get_intra_process_gid() const
   return intra_process_rmw_gid_;
 }
 
+rcl_publisher_t *
+PublisherBase::get_publisher_handle()
+{
+  return &publisher_handle_;
+}
+
+const rcl_publisher_t *
+PublisherBase::get_publisher_handle() const
+{
+  return &publisher_handle_;
+}
+
 bool
 PublisherBase::operator==(const rmw_gid_t & gid) const
 {
@@ -107,14 +149,16 @@ 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());
+    auto msg = std::string("failed to compare gids: ") + rmw_get_error_string_safe();
+    rmw_reset_error();
+    throw std::runtime_error(msg);
   }
   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());
+      auto msg = std::string("failed to compare gids: ") + rmw_get_error_string_safe();
+      rmw_reset_error();
+      throw std::runtime_error(msg);
     }
   }
   return result;
@@ -124,18 +168,45 @@ void
 PublisherBase::setup_intra_process(
   uint64_t intra_process_publisher_id,
   StoreMessageCallbackT callback,
-  rmw_publisher_t * intra_process_publisher_handle)
+  const rcl_publisher_options_t & intra_process_options)
 {
+  auto intra_process_topic_name = std::string(this->get_topic_name()) + "/_intra";
+  rcl_ret_t ret = rcl_publisher_init(
+    &intra_process_publisher_handle_,
+    rcl_node_handle_.get(),
+    rclcpp::type_support::get_intra_process_message_msg_type_support(),
+    intra_process_topic_name.c_str(),
+    &intra_process_options);
+  if (ret != RCL_RET_OK) {
+    if (ret == RCL_RET_TOPIC_NAME_INVALID) {
+      auto rcl_node_handle = rcl_node_handle_.get();
+      // this will throw on any validation problem
+      rcl_reset_error();
+      expand_topic_or_service_name(
+        intra_process_topic_name,
+        rcl_node_get_name(rcl_node_handle),
+        rcl_node_get_namespace(rcl_node_handle));
+    }
+
+    rclcpp::exceptions::throw_from_rcl_error(ret, "could not create intra process publisher");
+  }
+
   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*
+  rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(
+    &intra_process_publisher_handle_);
+  if (publisher_rmw_handle == nullptr) {
+    auto msg = std::string("Failed to get rmw publisher handle") + rcl_get_error_string_safe();
+    rcl_reset_error();
+    throw std::runtime_error(msg);
+  }
+  auto rmw_ret = rmw_get_gid_for_publisher(
+    publisher_rmw_handle, &intra_process_rmw_gid_);
+  if (rmw_ret != RMW_RET_OK) {
+    auto msg =
+      std::string("failed to create intra process publisher gid: ") + rmw_get_error_string_safe();
+    rmw_reset_error();
+    throw std::runtime_error(msg);
   }
 }

rclcpp/src/rclcpp/service.cpp

@@ -28,23 +28,17 @@
 using rclcpp::service::ServiceBase;
 
 ServiceBase::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)
+: node_handle_(node_handle), service_name_(service_name)
+{}
+
+ServiceBase::ServiceBase(std::shared_ptr<rcl_node_t> node_handle)
+: node_handle_(node_handle)
 {}
 
 ServiceBase::~ServiceBase()
-{
-  if (service_handle_) {
-    if (rmw_destroy_service(service_handle_) != RMW_RET_OK) {
-      std::stringstream ss;
-      ss << "Error in destruction of rmw service_handle_ handle: " <<
-        rmw_get_error_string_safe() << '\n';
-      (std::cerr << ss.str()).flush();
-    }
-  }
-}
+{}
 
 std::string
 ServiceBase::get_service_name()
@@ -52,8 +46,26 @@ ServiceBase::get_service_name()
   return this->service_name_;
 }
 
-const rmw_service_t *
+rcl_service_t *
 ServiceBase::get_service_handle()
 {
-  return this->service_handle_;
+  return service_handle_;
+}
+
+const rcl_service_t *
+ServiceBase::get_service_handle() const
+{
+  return service_handle_;
+}
+
+rcl_node_t *
+ServiceBase::get_rcl_node_handle()
+{
+  return node_handle_.get();
+}
+
+const rcl_node_t *
+ServiceBase::get_rcl_node_handle() const
+{
+  return node_handle_.get();
 }

rclcpp/src/rclcpp/subscription.cpp

@@ -15,8 +15,12 @@
 #include "rclcpp/subscription.hpp"
 
 #include <cstdio>
+#include <memory>
 #include <string>
 
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
+
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 
@@ -24,55 +28,71 @@
 using rclcpp::subscription::SubscriptionBase;
 
 SubscriptionBase::SubscriptionBase(
-  std::shared_ptr<rmw_node_t> node_handle,
-  rmw_subscription_t * subscription_handle,
+  std::shared_ptr<rcl_node_t> node_handle,
+  const rosidl_message_type_support_t & type_support_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)
+  const rcl_subscription_options_t & subscription_options)
+: node_handle_(node_handle)
 {
-  // To avoid unused private member warnings.
-  (void)ignore_local_publications_;
+  rcl_ret_t ret = rcl_subscription_init(
+    &subscription_handle_,
+    node_handle_.get(),
+    &type_support_handle,
+    topic_name.c_str(),
+    &subscription_options);
+  if (ret != RCL_RET_OK) {
+    if (ret == RCL_RET_TOPIC_NAME_INVALID) {
+      auto rcl_node_handle = node_handle_.get();
+      // this will throw on any validation problem
+      rcl_reset_error();
+      expand_topic_or_service_name(
+        topic_name,
+        rcl_node_get_name(rcl_node_handle),
+        rcl_node_get_namespace(rcl_node_handle));
+    }
+
+    rclcpp::exceptions::throw_from_rcl_error(ret, "could not create subscription");
+  }
 }
 
 SubscriptionBase::~SubscriptionBase()
 {
-  if (subscription_handle_) {
-    if (rmw_destroy_subscription(node_handle_.get(), subscription_handle_) != RMW_RET_OK) {
-      std::stringstream ss;
-      ss << "Error in destruction of rmw subscription handle: " <<
-        rmw_get_error_string_safe() << '\n';
-      (std::cerr << ss.str()).flush();
-    }
+  if (rcl_subscription_fini(&subscription_handle_, node_handle_.get()) != RCL_RET_OK) {
+    std::stringstream ss;
+    ss << "Error in destruction of rcl subscription handle: " <<
+      rcl_get_error_string_safe() << '\n';
+    (std::cerr << ss.str()).flush();
   }
-  if (intra_process_subscription_handle_) {
-    auto ret = rmw_destroy_subscription(node_handle_.get(), intra_process_subscription_handle_);
-    if (ret != RMW_RET_OK) {
-      std::stringstream ss;
-      ss << "Error in destruction of rmw intra process subscription handle: " <<
-        rmw_get_error_string_safe() << '\n';
-      (std::cerr << ss.str()).flush();
-    }
+  if (rcl_subscription_fini(
+      &intra_process_subscription_handle_, node_handle_.get()) != RCL_RET_OK)
+  {
+    std::stringstream ss;
+    ss << "Error in destruction of rmw intra process subscription handle: " <<
+      rcl_get_error_string_safe() << '\n';
+    (std::cerr << ss.str()).flush();
   }
 }
 
-const std::string &
+const char *
 SubscriptionBase::get_topic_name() const
 {
-  return this->topic_name_;
+  return rcl_subscription_get_topic_name(&subscription_handle_);
 }
 
-const rmw_subscription_t *
+rcl_subscription_t *
+SubscriptionBase::get_subscription_handle()
+{
+  return &subscription_handle_;
+}
+
+const rcl_subscription_t *
 SubscriptionBase::get_subscription_handle() const
 {
-  return subscription_handle_;
+  return &subscription_handle_;
 }
 
-const rmw_subscription_t *
+const rcl_subscription_t *
 SubscriptionBase::get_intra_process_subscription_handle() const
 {
-  return intra_process_subscription_handle_;
+  return &intra_process_subscription_handle_;
 }

rclcpp/src/rclcpp/time.cpp

@@ -0,0 +1,250 @@
+// Copyright 2017 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/time.hpp"
+
+#include <utility>
+
+#include "builtin_interfaces/msg/time.hpp"
+
+#include "rcl/time.h"
+
+#include "rclcpp/exceptions.hpp"
+
+#include "rcutils/logging_macros.h"
+
+namespace
+{
+
+rcl_time_source_t
+init_time_source(rcl_time_source_type_t clock = RCL_SYSTEM_TIME)
+{
+  rcl_time_source_t time_source;
+  auto ret = rcl_time_source_init(clock, &time_source);
+
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "could not initialize time source");
+  }
+
+  return time_source;
+}
+
+rcl_time_point_t
+init_time_point(rcl_time_source_t & time_source)
+{
+  rcl_time_point_t time_point;
+  auto ret = rcl_time_point_init(&time_point, &time_source);
+
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "could not initialize time point");
+  }
+
+  return time_point;
+}
+
+}  // namespace
+
+namespace rclcpp
+{
+
+Time
+Time::now(rcl_time_source_type_t clock)
+{
+  // TODO(karsten1987): This impl throws explicitely on RCL_ROS_TIME
+  // we have to do this, because rcl_time_source_init returns RCL_RET_OK
+  // for RCL_ROS_TIME, however defaults to system time under the hood.
+  // ref: https://github.com/ros2/rcl/blob/master/rcl/src/rcl/time.c#L66-L77
+  // This section can be removed when rcl supports ROS_TIME correctly.
+  if (clock == RCL_ROS_TIME) {
+    throw std::runtime_error("RCL_ROS_TIME is currently not implemented.");
+  }
+
+  Time now(0, 0, clock);
+
+  auto ret = rcl_time_point_get_now(&now.rcl_time_);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "could not get current time stamp");
+  }
+
+  return now;
+}
+
+Time::Time(int32_t seconds, uint32_t nanoseconds, rcl_time_source_type_t clock)
+: rcl_time_source_(init_time_source(clock)),
+  rcl_time_(init_time_point(rcl_time_source_))
+{
+  if (seconds < 0) {
+    throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
+  }
+
+  rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast<uint64_t>(seconds));
+  rcl_time_.nanoseconds += nanoseconds;
+}
+
+Time::Time(uint64_t nanoseconds, rcl_time_source_type_t clock)
+: rcl_time_source_(init_time_source(clock)),
+  rcl_time_(init_time_point(rcl_time_source_))
+{
+  rcl_time_.nanoseconds = nanoseconds;
+}
+
+Time::Time(const Time & rhs)
+: rcl_time_source_(init_time_source(rhs.rcl_time_source_.type)),
+  rcl_time_(init_time_point(rcl_time_source_))
+{
+  rcl_time_.nanoseconds = rhs.rcl_time_.nanoseconds;
+}
+
+Time::Time(const builtin_interfaces::msg::Time & time_msg)  // NOLINT
+: rcl_time_source_(init_time_source(RCL_SYSTEM_TIME)),
+  rcl_time_(init_time_point(rcl_time_source_))
+{
+  if (time_msg.sec < 0) {
+    throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
+  }
+
+  rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast<uint64_t>(time_msg.sec));
+  rcl_time_.nanoseconds += time_msg.nanosec;
+}
+
+Time::~Time()
+{
+  if (rcl_time_source_fini(&rcl_time_source_) != RCL_RET_OK) {
+    RCUTILS_LOG_FATAL("failed to reclaim rcl_time_source_t in destructor of rclcpp::Time")
+  }
+  if (rcl_time_point_fini(&rcl_time_) != RCL_RET_OK) {
+    RCUTILS_LOG_FATAL("failed to reclaim rcl_time_point_t in destructor of rclcpp::Time")
+  }
+}
+
+Time::operator builtin_interfaces::msg::Time() const
+{
+  builtin_interfaces::msg::Time msg_time;
+  msg_time.sec = static_cast<std::int32_t>(RCL_NS_TO_S(rcl_time_.nanoseconds));
+  msg_time.nanosec = static_cast<std::uint32_t>(rcl_time_.nanoseconds % (1000 * 1000 * 1000));
+  return msg_time;
+}
+
+void
+Time::operator=(const Time & rhs)
+{
+  rcl_time_source_ = init_time_source(rhs.rcl_time_source_.type);
+  rcl_time_ = init_time_point(rcl_time_source_);
+  rcl_time_.nanoseconds = rhs.rcl_time_.nanoseconds;
+}
+
+void
+Time::operator=(const builtin_interfaces::msg::Time & time_msg)
+{
+  if (time_msg.sec < 0) {
+    throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
+  }
+
+  this->rcl_time_source_ = init_time_source();
+  this->rcl_time_ = init_time_point(this->rcl_time_source_);
+
+  rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast<uint64_t>(time_msg.sec));
+  rcl_time_.nanoseconds += time_msg.nanosec;
+}
+
+bool
+Time::operator==(const rclcpp::Time & rhs) const
+{
+  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
+    throw std::runtime_error("can't compare times with different time sources");
+  }
+
+  return rcl_time_.nanoseconds == rhs.rcl_time_.nanoseconds;
+}
+
+bool
+Time::operator<(const rclcpp::Time & rhs) const
+{
+  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
+    throw std::runtime_error("can't compare times with different time sources");
+  }
+
+  return rcl_time_.nanoseconds < rhs.rcl_time_.nanoseconds;
+}
+
+bool
+Time::operator<=(const rclcpp::Time & rhs) const
+{
+  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
+    throw std::runtime_error("can't compare times with different time sources");
+  }
+
+  return rcl_time_.nanoseconds <= rhs.rcl_time_.nanoseconds;
+}
+
+bool
+Time::operator>=(const rclcpp::Time & rhs) const
+{
+  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
+    throw std::runtime_error("can't compare times with different time sources");
+  }
+
+  return rcl_time_.nanoseconds >= rhs.rcl_time_.nanoseconds;
+}
+
+bool
+Time::operator>(const rclcpp::Time & rhs) const
+{
+  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
+    throw std::runtime_error("can't compare times with different time sources");
+  }
+
+  return rcl_time_.nanoseconds > rhs.rcl_time_.nanoseconds;
+}
+
+Time
+Time::operator+(const rclcpp::Time & rhs) const
+{
+  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
+    throw std::runtime_error("can't add times with different time sources");
+  }
+
+  auto ns = rcl_time_.nanoseconds + rhs.rcl_time_.nanoseconds;
+  if (ns < rcl_time_.nanoseconds) {
+    throw std::overflow_error("addition leads to uint64_t overflow");
+  }
+
+  return Time(rcl_time_.nanoseconds + rhs.rcl_time_.nanoseconds);
+}
+
+Time
+Time::operator-(const rclcpp::Time & rhs) const
+{
+  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
+    throw std::runtime_error("can't add times with different time sources");
+  }
+
+  auto ns = rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds;
+  if (ns > rcl_time_.nanoseconds) {
+    throw std::underflow_error("subtraction leads to uint64_t underflow");
+  }
+
+  return Time(rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds);
+}
+
+uint64_t
+Time::nanoseconds() const
+{
+  return rcl_time_.nanoseconds;
+}
+
+}  // namespace rclcpp

rclcpp/src/rclcpp/timer.cpp

@@ -15,13 +15,19 @@
 #include "rclcpp/timer.hpp"
 
 #include <chrono>
+#include <string>
 
 using rclcpp::timer::TimerBase;
 
 TimerBase::TimerBase(std::chrono::nanoseconds period)
-: period_(period),
-  canceled_(false)
-{}
+{
+  if (rcl_timer_init(
+      &timer_handle_, period.count(), nullptr,
+      rcl_get_default_allocator()) != RCL_RET_OK)
+  {
+    fprintf(stderr, "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string_safe());
+  }
+}
 
 TimerBase::~TimerBase()
 {}
@@ -29,5 +35,43 @@ TimerBase::~TimerBase()
 void
 TimerBase::cancel()
 {
-  this->canceled_ = true;
+  if (rcl_timer_cancel(&timer_handle_) != RCL_RET_OK) {
+    throw std::runtime_error(std::string("Couldn't cancel timer: ") + rcl_get_error_string_safe());
+  }
+}
+
+void
+TimerBase::reset()
+{
+  if (rcl_timer_reset(&timer_handle_) != RCL_RET_OK) {
+    throw std::runtime_error(std::string("Couldn't reset 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

@@ -30,7 +30,7 @@
 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<
+  return rosidl_typesupport_cpp::get_message_type_support_handle<
     rcl_interfaces::msg::IntraProcessMessage
   >();
 }
@@ -38,7 +38,7 @@ rclcpp::type_support::get_intra_process_message_msg_type_support()
 const rosidl_message_type_support_t *
 rclcpp::type_support::get_parameter_event_msg_type_support()
 {
-  return rosidl_generator_cpp::get_message_type_support_handle<
+  return rosidl_typesupport_cpp::get_message_type_support_handle<
     rcl_interfaces::msg::ParameterEvent
   >();
 }
@@ -46,7 +46,7 @@ rclcpp::type_support::get_parameter_event_msg_type_support()
 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<
+  return rosidl_typesupport_cpp::get_message_type_support_handle<
     rcl_interfaces::msg::SetParametersResult
   >();
 }
@@ -54,7 +54,7 @@ rclcpp::type_support::get_set_parameters_result_msg_type_support()
 const rosidl_message_type_support_t *
 rclcpp::type_support::get_parameter_descriptor_msg_type_support()
 {
-  return rosidl_generator_cpp::get_message_type_support_handle<
+  return rosidl_typesupport_cpp::get_message_type_support_handle<
     rcl_interfaces::msg::ParameterDescriptor
   >();
 }
@@ -62,7 +62,7 @@ rclcpp::type_support::get_parameter_descriptor_msg_type_support()
 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<
+  return rosidl_typesupport_cpp::get_message_type_support_handle<
     rcl_interfaces::msg::ListParametersResult
   >();
 }
@@ -70,7 +70,7 @@ rclcpp::type_support::get_list_parameters_result_msg_type_support()
 const rosidl_service_type_support_t *
 rclcpp::type_support::get_get_parameters_srv_type_support()
 {
-  return rosidl_generator_cpp::get_service_type_support_handle<
+  return rosidl_typesupport_cpp::get_service_type_support_handle<
     rcl_interfaces::srv::GetParameters
   >();
 }
@@ -78,7 +78,7 @@ rclcpp::type_support::get_get_parameters_srv_type_support()
 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<
+  return rosidl_typesupport_cpp::get_service_type_support_handle<
     rcl_interfaces::srv::GetParameterTypes
   >();
 }
@@ -86,7 +86,7 @@ rclcpp::type_support::get_get_parameter_types_srv_type_support()
 const rosidl_service_type_support_t *
 rclcpp::type_support::get_set_parameters_srv_type_support()
 {
-  return rosidl_generator_cpp::get_service_type_support_handle<
+  return rosidl_typesupport_cpp::get_service_type_support_handle<
     rcl_interfaces::srv::SetParameters
   >();
 }
@@ -94,7 +94,7 @@ rclcpp::type_support::get_set_parameters_srv_type_support()
 const rosidl_service_type_support_t *
 rclcpp::type_support::get_list_parameters_srv_type_support()
 {
-  return rosidl_generator_cpp::get_service_type_support_handle<
+  return rosidl_typesupport_cpp::get_service_type_support_handle<
     rcl_interfaces::srv::ListParameters
   >();
 }
@@ -102,7 +102,7 @@ rclcpp::type_support::get_list_parameters_srv_type_support()
 const rosidl_service_type_support_t *
 rclcpp::type_support::get_describe_parameters_srv_type_support()
 {
-  return rosidl_generator_cpp::get_service_type_support_handle<
+  return rosidl_typesupport_cpp::get_service_type_support_handle<
     rcl_interfaces::srv::DescribeParameters
   >();
 }
@@ -110,7 +110,7 @@ rclcpp::type_support::get_describe_parameters_srv_type_support()
 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<
+  return rosidl_typesupport_cpp::get_service_type_support_handle<
     rcl_interfaces::srv::SetParametersAtomically
   >();
 }

rclcpp/src/rclcpp/utilities.cpp

@@ -19,8 +19,13 @@
 #include <csignal>
 #include <cstdio>
 #include <cstring>
+#include <map>
 #include <mutex>
 #include <string>
+#include <vector>
+
+#include "rcl/error_handling.h"
+#include "rcl/rcl.h"
 
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
@@ -32,8 +37,10 @@
 
 /// Represent the status of the global interrupt signal.
 static volatile sig_atomic_t g_signal_status = 0;
-/// Guard condition for interrupting the rmw implementation when the global interrupt signal fired.
-static rmw_guard_condition_t * g_sigint_guard_cond_handle = rmw_create_guard_condition();
+/// Guard conditions for interrupting the rmw implementation when the global interrupt signal fired.
+static std::map<rcl_wait_set_t *, rcl_guard_condition_t> g_sigint_guard_cond_handles;
+/// Mutex to protect g_sigint_guard_cond_handles
+static std::mutex g_sigint_guard_cond_handles_mutex;
 /// Condition variable for timed sleep (see sleep_for).
 static std::condition_variable g_interrupt_condition_variable;
 static std::atomic<bool> g_is_interrupted(false);
@@ -43,9 +50,79 @@ static std::mutex g_interrupt_mutex;
 #ifdef HAS_SIGACTION
 static struct sigaction old_action;
 #else
-static void (* old_signal_handler)(int) = 0;
+typedef void (* signal_handler_t)(int);
+static signal_handler_t old_signal_handler = 0;
 #endif
 
+#ifdef HAS_SIGACTION
+struct sigaction
+set_sigaction(int signal_value, const struct sigaction & action)
+#else
+signal_handler_t
+set_signal_handler(int signal_value, signal_handler_t signal_handler)
+#endif
+{
+#ifdef HAS_SIGACTION
+  struct sigaction old_action;
+  ssize_t ret = sigaction(signal_value, &action, &old_action);
+  if (ret == -1)
+#else
+  signal_handler_t old_signal_handler = std::signal(signal_value, 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*
+  }
+
+#ifdef HAS_SIGACTION
+  return old_action;
+#else
+  return old_signal_handler;
+#endif
+}
+
+void
+trigger_interrupt_guard_condition(int signal_value)
+{
+  g_signal_status = signal_value;
+  {
+    std::lock_guard<std::mutex> lock(g_sigint_guard_cond_handles_mutex);
+    for (auto & 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
 #ifdef HAS_SIGACTION
 signal_handler(int signal_value, siginfo_t * siginfo, void * context)
@@ -55,6 +132,7 @@ signal_handler(int signal_value)
 {
   // 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) {
@@ -76,28 +154,19 @@ signal_handler(int signal_value)
     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();
+
+  trigger_interrupt_guard_condition(signal_value);
 }
 
 void
 rclcpp::utilities::init(int argc, char * argv[])
 {
-  (void)argc;
-  (void)argv;
   g_is_interrupted.store(false);
-  rmw_ret_t status = rmw_init();
-  if (status != RMW_RET_OK) {
-    // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-    throw std::runtime_error(
-      std::string("failed to initialize rmw implementation: ") + rmw_get_error_string_safe());
-    // *INDENT-ON*
+  if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) {
+    std::string msg = "failed to initialize rmw implementation: ";
+    msg += rcl_get_error_string_safe();
+    rcl_reset_error();
+    throw std::runtime_error(msg);
   }
 #ifdef HAS_SIGACTION
   struct sigaction action;
@@ -105,39 +174,18 @@ rclcpp::utilities::init(int argc, char * argv[])
   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)
+  ::old_action = set_sigaction(SIGINT, action);
+  // Register an on_shutdown hook to restore the old action.
+  rclcpp::utilities::on_shutdown([]() {
+    set_sigaction(SIGINT, ::old_action);
+  });
 #else
-  ::old_signal_handler = std::signal(SIGINT, ::signal_handler);
-  // NOLINTNEXTLINE(readability/braces)
-  if (::old_signal_handler == SIG_ERR)
+  ::old_signal_handler = set_signal_handler(SIGINT, ::signal_handler);
+  // Register an on_shutdown hook to restore the old signal handler.
+  rclcpp::utilities::on_shutdown([]() {
+    set_signal_handler(SIGINT, ::old_signal_handler);
+  });
 #endif
-  {
-    const size_t error_length = 1024;
-    // NOLINTNEXTLINE(runtime/arrays)
-    char error_string[error_length];
-#ifndef _WIN32
-#ifdef _GNU_SOURCE
-    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*
-    throw std::runtime_error(
-      std::string("Failed to set SIGINT signal handler: (" + std::to_string(errno) + ")") +
-      error_string);
-    // *INDENT-ON*
-  }
 }
 
 bool
@@ -146,23 +194,71 @@ rclcpp::utilities::ok()
   return ::g_signal_status == 0;
 }
 
+static std::mutex on_shutdown_mutex_;
+static std::vector<std::function<void(void)>> on_shutdown_callbacks_;
+
 void
 rclcpp::utilities::shutdown()
 {
-  g_signal_status = SIGINT;
-  rmw_ret_t status = rmw_trigger_guard_condition(g_sigint_guard_cond_handle);
-  if (status != RMW_RET_OK) {
-    fprintf(stderr,
-      "[rclcpp::error] failed to trigger guard condition: %s\n", rmw_get_error_string_safe());
+  trigger_interrupt_guard_condition(SIGINT);
+
+  {
+    std::lock_guard<std::mutex> lock(on_shutdown_mutex_);
+    for (auto & on_shutdown_callback : on_shutdown_callbacks_) {
+      on_shutdown_callback();
+    }
   }
-  g_is_interrupted.store(true);
-  g_interrupt_condition_variable.notify_all();
 }
 
-rmw_guard_condition_t *
-rclcpp::utilities::get_global_sigint_guard_condition()
+void
+rclcpp::utilities::on_shutdown(std::function<void(void)> callback)
 {
-  return ::g_sigint_guard_cond_handle;
+  std::lock_guard<std::mutex> lock(on_shutdown_mutex_);
+  on_shutdown_callbacks_.push_back(callback);
+}
+
+rcl_guard_condition_t *
+rclcpp::utilities::get_sigint_guard_condition(rcl_wait_set_t * waitset)
+{
+  std::lock_guard<std::mutex> lock(g_sigint_guard_cond_handles_mutex);
+  auto kv = g_sigint_guard_cond_handles.find(waitset);
+  if (kv != g_sigint_guard_cond_handles.end()) {
+    return &kv->second;
+  } else {
+    rcl_guard_condition_t handle =
+      rcl_get_zero_initialized_guard_condition();
+    rcl_guard_condition_options_t options = rcl_guard_condition_get_default_options();
+    if (rcl_guard_condition_init(&handle, options) != RCL_RET_OK) {
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
+      throw std::runtime_error(std::string(
+        "Couldn't initialize guard condition: ") + rcl_get_error_string_safe());
+      // *INDENT-ON*
+    }
+    g_sigint_guard_cond_handles[waitset] = handle;
+    return &g_sigint_guard_cond_handles[waitset];
+  }
+}
+
+void
+rclcpp::utilities::release_sigint_guard_condition(rcl_wait_set_t * waitset)
+{
+  std::lock_guard<std::mutex> lock(g_sigint_guard_cond_handles_mutex);
+  auto kv = g_sigint_guard_cond_handles.find(waitset);
+  if (kv != g_sigint_guard_cond_handles.end()) {
+    if (rcl_guard_condition_fini(&kv->second) != RCL_RET_OK) {
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
+      throw std::runtime_error(std::string(
+        "Failed to destroy sigint guard condition: ") +
+        rcl_get_error_string_safe());
+      // *INDENT-ON*
+    }
+    g_sigint_guard_cond_handles.erase(kv);
+  } else {
+    // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
+    throw std::runtime_error(std::string(
+      "Tried to release sigint guard condition for nonexistent waitset"));
+    // *INDENT-ON*
+  }
 }
 
 bool

rclcpp/test/mock_msgs/srv/Mock.srv

@@ -0,0 +1,3 @@
+bool request
+---
+bool response

rclcpp/test/test_client.cpp

@@ -0,0 +1,60 @@
+// Copyright 2017 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 <memory>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "rcl_interfaces/srv/list_parameters.hpp"
+
+class TestClient : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::node::Node>("my_node", "/ns");
+  }
+
+  void TearDown()
+  {
+    node.reset();
+  }
+
+  rclcpp::node::Node::SharedPtr node;
+};
+
+/*
+   Testing client construction and destruction.
+ */
+TEST_F(TestClient, construction_and_destruction) {
+  using rcl_interfaces::srv::ListParameters;
+  {
+    auto client = node->create_client<ListParameters>("service");
+  }
+
+  {
+    ASSERT_THROW({
+      auto client = node->create_client<ListParameters>("invalid_service?");
+    }, rclcpp::exceptions::InvalidServiceNameError);
+  }
+}

rclcpp/test/test_expand_topic_or_service_name.cpp

@@ -0,0 +1,74 @@
+// Copyright 2017 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 "rclcpp/exceptions.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
+
+/*
+   Testing expand_topic_or_service_name.
+ */
+TEST(TestExpandTopicOrServiceName, normal) {
+  using rclcpp::expand_topic_or_service_name;
+  {
+    ASSERT_EQ("/ns/chatter", expand_topic_or_service_name("chatter", "node", "/ns"));
+  }
+}
+
+/*
+   Testing exceptions of expand_topic_or_service_name.
+ */
+TEST(TestExpandTopicOrServiceName, exceptions) {
+  using rclcpp::expand_topic_or_service_name;
+  {
+    ASSERT_THROW({
+      expand_topic_or_service_name("chatter", "invalid_node?", "/ns");
+    }, rclcpp::exceptions::InvalidNodeNameError);
+  }
+
+  {
+    ASSERT_THROW({
+      expand_topic_or_service_name("chatter", "node", "/invalid_ns?");
+    }, rclcpp::exceptions::InvalidNamespaceError);
+  }
+
+  {
+    ASSERT_THROW({
+      expand_topic_or_service_name("chatter/42invalid", "node", "/ns");
+    }, rclcpp::exceptions::InvalidTopicNameError);
+  }
+
+  {
+    ASSERT_THROW({
+      // this one will only fail on the "full" topic name validation check
+      expand_topic_or_service_name("chatter/{ns}/invalid", "node", "/ns");
+    }, rclcpp::exceptions::InvalidTopicNameError);
+  }
+
+  {
+    ASSERT_THROW({
+      // is_service = true
+      expand_topic_or_service_name("chatter/42invalid", "node", "/ns", true);
+    }, rclcpp::exceptions::InvalidServiceNameError);
+  }
+
+  {
+    ASSERT_THROW({
+      // is_service = true
+      // this one will only fail on the "full" topic name validation check
+      expand_topic_or_service_name("chatter/{ns}/invalid", "node", "/ns", true);
+    }, rclcpp::exceptions::InvalidServiceNameError);
+  }
+}

rclcpp/test/test_externally_defined_services.cpp

@@ -0,0 +1,142 @@
+// 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 <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+
+#include "rclcpp/node.hpp"
+#include "rclcpp/any_service_callback.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "rcl/service.h"
+
+#include "rclcpp/srv/mock.hpp"
+#include "rclcpp/srv/mock.h"
+
+class TestExternallyDefinedServices : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+};
+
+void
+callback(const std::shared_ptr<rclcpp::srv::Mock::Request>/*req*/,
+  std::shared_ptr<rclcpp::srv::Mock::Response>/*resp*/)
+{}
+
+TEST_F(TestExternallyDefinedServices, default_behavior) {
+  auto node_handle = rclcpp::node::Node::make_shared("base_node");
+
+  try {
+    auto srv = node_handle->create_service<rclcpp::srv::Mock>("test",
+        callback);
+  } catch (const std::exception &) {
+    FAIL();
+    return;
+  }
+  SUCCEED();
+}
+
+
+TEST_F(TestExternallyDefinedServices, extern_defined_uninitialized) {
+  auto node_handle = rclcpp::node::Node::make_shared("base_node");
+
+  // mock for externally defined service
+  rcl_service_t service_handle = rcl_get_zero_initialized_service();
+
+  rclcpp::any_service_callback::AnyServiceCallback<rclcpp::srv::Mock> cb;
+
+  // don't initialize the service
+  // expect fail
+  try {
+    rclcpp::service::Service<rclcpp::srv::Mock>(
+      node_handle->get_node_base_interface()->get_shared_rcl_node_handle(),
+      &service_handle, cb);
+  } catch (const std::runtime_error &) {
+    SUCCEED();
+    return;
+  }
+
+  FAIL();
+}
+
+TEST_F(TestExternallyDefinedServices, extern_defined_initialized) {
+  auto node_handle = rclcpp::node::Node::make_shared("base_node");
+
+  // mock for externally defined service
+  rcl_service_t service_handle = rcl_get_zero_initialized_service();
+  rcl_service_options_t service_options = rcl_service_get_default_options();
+  const rosidl_service_type_support_t * ts =
+    rosidl_typesupport_cpp::get_service_type_support_handle<rclcpp::srv::Mock>();
+  rcl_ret_t ret = rcl_service_init(
+    &service_handle,
+    node_handle->get_node_base_interface()->get_rcl_node_handle(),
+    ts, "base_node_service", &service_options);
+  if (ret != RCL_RET_OK) {
+    FAIL();
+    return;
+  }
+
+  rclcpp::any_service_callback::AnyServiceCallback<rclcpp::srv::Mock> cb;
+
+  try {
+    rclcpp::service::Service<rclcpp::srv::Mock>(
+      node_handle->get_node_base_interface()->get_shared_rcl_node_handle(),
+      &service_handle, cb);
+  } catch (const std::runtime_error &) {
+    FAIL();
+    return;
+  }
+
+  SUCCEED();
+}
+
+TEST_F(TestExternallyDefinedServices, extern_defined_destructor) {
+  auto node_handle = rclcpp::node::Node::make_shared("base_node");
+
+  // mock for externally defined service
+  rcl_service_t service_handle = rcl_get_zero_initialized_service();
+  rcl_service_options_t service_options = rcl_service_get_default_options();
+  const rosidl_service_type_support_t * ts =
+    rosidl_typesupport_cpp::get_service_type_support_handle<rclcpp::srv::Mock>();
+  rcl_ret_t ret = rcl_service_init(
+    &service_handle,
+    node_handle->get_node_base_interface()->get_rcl_node_handle(),
+    ts, "base_node_service", &service_options);
+  if (ret != RCL_RET_OK) {
+    FAIL();
+    return;
+  }
+  rclcpp::any_service_callback::AnyServiceCallback<rclcpp::srv::Mock> cb;
+
+  {
+    // Call constructor
+    rclcpp::service::Service<rclcpp::srv::Mock> srv_cpp(
+      node_handle->get_node_base_interface()->get_shared_rcl_node_handle(),
+      &service_handle, cb);
+    // Call destructor
+  }
+
+  if (!service_handle.impl) {
+    FAIL();
+    return;
+  }
+  SUCCEED();
+}