another wip

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

.github/ISSUE_TEMPLATE.md

@@ -0,0 +1,45 @@
+<!--
+For general questions, please ask on ROS answers: https://answers.ros.org, make sure to include at least the `ros2` tag and the rosdistro version you are running, e.g. `ardent`.
+For general design discussions, 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 -->

.gitignore

@@ -0,0 +1 @@
+.DS_Store

rclcpp/.gitignore

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

rclcpp/CHANGELOG.rst

@@ -0,0 +1,97 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package rclcpp
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.6.2 (2018-12-13)
+------------------
+* Updated to use signal safe synchronization with platform specific semaphores (`#607 <https://github.com/ros2/rclcpp/issues/607>`_)
+* Resolved startup race condition for sim time (`#608 <https://github.com/ros2/rclcpp/issues/608>`_)
+  Resolves `#595 <https://github.com/ros2/rclcpp/issues/595>`_
+* Contributors: Tully Foote, William Woodall
+
+0.6.1 (2018-12-07)
+------------------
+* Added wait_for_action_server() for action clients (`#598 <https://github.com/ros2/rclcpp/issues/598>`_)
+* Added node path and time stamp to parameter event message (`#584 <https://github.com/ros2/rclcpp/issues/584>`_)
+* Updated to allow removing a waitable (`#597 <https://github.com/ros2/rclcpp/issues/597>`_)
+* Refactored init to allow for non-global init (`#587 <https://github.com/ros2/rclcpp/issues/587>`_)
+* Fixed wrong use of constructor and hanging test (`#596 <https://github.com/ros2/rclcpp/issues/596>`_)
+* Added class Waitable (`#589 <https://github.com/ros2/rclcpp/issues/589>`_)
+* Updated rcl_wait_set_add\_* calls (`#586 <https://github.com/ros2/rclcpp/issues/586>`_)
+* Contributors: Dirk Thomas, Jacob Perron, Shane Loretz, William Woodall, bpwilcox
+
+0.6.0 (2018-11-19)
+------------------
+* Updated to use new error handling API from rcutils (`#577 <https://github.com/ros2/rclcpp/issues/577>`_)
+* Added a warning when publishing if publisher is not active (`#574 <https://github.com/ros2/rclcpp/issues/574>`_)
+* Added logging macro signature that accepts std::string (`#573 <https://github.com/ros2/rclcpp/issues/573>`_)
+* Added virtual destructors to classes with virtual functions. (`#566 <https://github.com/ros2/rclcpp/issues/566>`_)
+* Added semicolons to all RCLCPP and RCUTILS macros. (`#565 <https://github.com/ros2/rclcpp/issues/565>`_)
+* Removed std::binary_function usage (`#561 <https://github.com/ros2/rclcpp/issues/561>`_)
+* Updated to avoid auto-activating ROS time if clock topic is being published (`#559 <https://github.com/ros2/rclcpp/issues/559>`_)
+* Fixed cpplint on xenial (`#556 <https://github.com/ros2/rclcpp/issues/556>`_)
+* Added get_parameter_or_set_default. (`#551 <https://github.com/ros2/rclcpp/issues/551>`_)
+* Added max_duration to spin_some() (`#558 <https://github.com/ros2/rclcpp/issues/558>`_)
+* Updated to output rcl error message when yaml parsing fails (`#557 <https://github.com/ros2/rclcpp/issues/557>`_)
+* Updated to make sure timer is fini'd before clock (`#553 <https://github.com/ros2/rclcpp/issues/553>`_)
+* Get node names and namespaces (`#545 <https://github.com/ros2/rclcpp/issues/545>`_)
+* Fixed and improved documentation  (`#546 <https://github.com/ros2/rclcpp/issues/546>`_)
+* Updated to use rcl_clock_t jump callbacks (`#543 <https://github.com/ros2/rclcpp/issues/543>`_)
+* Updated to use rcl consolidated wait set functions (`#540 <https://github.com/ros2/rclcpp/issues/540>`_)
+* Addeed TIME_MAX and DURATION_MAX functions (`#538 <https://github.com/ros2/rclcpp/issues/538>`_)
+* Updated to publish shared_ptr of rcl_serialized_message (`#541 <https://github.com/ros2/rclcpp/issues/541>`_)
+* Added Time::is_zero and Duration::seconds (`#536 <https://github.com/ros2/rclcpp/issues/536>`_)
+* Changed to log an error message instead of throwing exception in destructor (`#535 <https://github.com/ros2/rclcpp/issues/535>`_)
+* Updated to relax tolerance of now test because timing affected by OS scheduling (`#533 <https://github.com/ros2/rclcpp/issues/533>`_)
+* Removed incorrect exception on sec < 0 (`#527 <https://github.com/ros2/rclcpp/issues/527>`_)
+* Added rclcpp::Time::seconds() (`#526 <https://github.com/ros2/rclcpp/issues/526>`_)
+* Updated Timer API to construct TimerBase/GenericTimer with Clock (`#523 <https://github.com/ros2/rclcpp/issues/523>`_)
+* Added rclcpp::is_initialized() (`#522 <https://github.com/ros2/rclcpp/issues/522>`_)
+* Added support for jump handlers with only pre- or post-jump callback (`#517 <https://github.com/ros2/rclcpp/issues/517>`_)
+* Removed use of uninitialized CMake var (`#512 <https://github.com/ros2/rclcpp/issues/512>`_)
+* Updated for Uncrustify 0.67 (`#510 <https://github.com/ros2/rclcpp/issues/510>`_)
+* Added get_node_names API from node. (`#508 <https://github.com/ros2/rclcpp/issues/508>`_)
+* Contributors: Anis Ladram, Chris Lalancette, Dirk Thomas, Francisco Martín Rico, Karsten Knese, Michael Carroll, Mikael Arguedas, Sagnik Basu, Shane Loretz, Sriram Raghunathan, William Woodall, chapulina, dhood
+
+0.5.0 (2018-06-25)
+------------------
+* Fixed a bug in the multi-threaded executor which could cause it to take a timer (potentially other types of wait-able items) more than once to be worked one. (`#383 <https://github.com/ros2/rclcpp/issues/383>`_)
+  * Specifically this could result in a timer getting called more often that it should when using the multi-threaded executor.
+* Added functions that allow you to publish serialized messages and received serialized messages in your subscription callback. (`#388 <https://github.com/ros2/rclcpp/issues/388>`_)
+* Changed code to always get the Service name from ``rcl`` to ensure the remapped name is returned. (`#498 <https://github.com/ros2/rclcpp/issues/498>`_)
+* Added previously missing ``set_parameters_atomically()`` method to the Service client interface. (`#494 <https://github.com/ros2/rclcpp/issues/494>`_)
+* Added ability to initialize parameter values in a Node via a YAML file passed on the command line. (`#488 <https://github.com/ros2/rclcpp/issues/488>`_)
+* Fixed the ROS parameter interface which got parameters that aren't set. (`#493 <https://github.com/ros2/rclcpp/issues/493>`_)
+* Added ability to initialize parameter values in a node with an argument to the Node constructor. (`#486 <https://github.com/ros2/rclcpp/issues/486>`_)
+* Added a ``Subscription`` tests which uses ``std::bind`` to a class member callback. (`#480 <https://github.com/ros2/rclcpp/issues/480>`_)
+* Refactored the ``ParameterVariant`` class into the ``Parameter`` and ``ParameterValue`` classes. (`#481 <https://github.com/ros2/rclcpp/issues/481>`_)
+* Relaxed template matching rules for ``std::bind`` and ``GNU C++ >= 7.1``. (`#484 <https://github.com/ros2/rclcpp/issues/484>`_)
+* Changed to use the new ``rosgraph_msgs/Clock`` message type for the ``/clock`` topic. (`#474 <https://github.com/ros2/rclcpp/issues/474>`_)
+* Fixed a flaky ROS time test due to not spinning before getting the time. (`#483 <https://github.com/ros2/rclcpp/issues/483>`_)
+* Nodes now autostart the ROS parameter services which let you get, set, and list parameters in a node. (`#478 <https://github.com/ros2/rclcpp/issues/478>`_)
+* Added support for arrays in Parameters. (`#443 <https://github.com/ros2/rclcpp/issues/443>`_)
+* Changed how executors use ``AnyExecutable`` objects so that they are a reference instead of a shared pointer, in order to avoid memory allocation in the "common case". (`#463 <https://github.com/ros2/rclcpp/issues/463>`_)
+* Added ability to pass command line arguments to the Node constructor. (`#461 <https://github.com/ros2/rclcpp/issues/461>`_)
+* Added an argument to specify the number of threads a multithreaded executor should create. (`#442 <https://github.com/ros2/rclcpp/issues/442>`_)
+* Changed library export order for static linking. (`#446 <https://github.com/ros2/rclcpp/issues/446>`_)
+* Fixed some typos in the time unit tests. (`#453 <https://github.com/ros2/rclcpp/issues/453>`_)
+  Obviously it mean RCL_SYSTEM_TIME but not RCL_ROS_TIME in some test cases
+  * Signed-off-by: jwang <jing.j.wang@intel.com>
+* Added the scale operation to ``rclcpp::Duration``.
+  * Signed-off-by: jwang <jing.j.wang@intel.com>
+* Changed API of the log location parameter to be ``const``. (`#451 <https://github.com/ros2/rclcpp/issues/451>`_)
+* Changed how the subscriber, client, service, and timer handles are stored to resolve shutdown order issues. (`#431 <https://github.com/ros2/rclcpp/issues/431>`_ and `#448 <https://github.com/ros2/rclcpp/issues/448>`_)
+* Updated to get the node's logger name from ``rcl``. (`#433 <https://github.com/ros2/rclcpp/issues/433>`_)
+* Now depends on ``ament_cmake_ros``. (`#444 <https://github.com/ros2/rclcpp/issues/444>`_)
+* Updaed code to use logging macros rather than ``fprintf()``. (`#439 <https://github.com/ros2/rclcpp/issues/439>`_)
+* Fixed a bug that was using an invalid iterator when erasing items using an iterator in a loop. (`#436 <https://github.com/ros2/rclcpp/issues/436>`_)
+* Changed code to support move of ``rcutils_time_point_value_t`` type from ``uint64_t`` to ``int64_t``. (`#429 <https://github.com/ros2/rclcpp/issues/429>`_)
+* Renamed parameter byte type to ``byte_values`` from ``bytes_value``. (`#428 <https://github.com/ros2/rclcpp/issues/428>`_)
+* Changed executor code to clear the wait set before resizing and waiting. (`#427 <https://github.com/ros2/rclcpp/issues/427>`_)
+* Fixed a potential dereference of nullptr in the topic name validation error string. (`#405 <https://github.com/ros2/rclcpp/issues/405>`_)
+  * Signed-off-by: Ethan Gao <ethan.gao@linux.intel.com>
+* Changed to use ``rcl_count_publishers()`` like API's rather than the lower level ``rmw_count_publishers()`` API. (`#425 <https://github.com/ros2/rclcpp/issues/425>`_)
+  * Signed-off-by: Sriram Raghunathan <rsriram7@visteon.com>
+* Fix potential segmentation fault due to ``get_topic_name()`` or ``rcl_service_get_service_name()`` returning nullptr and that not being checked before access in ``rclcpp``. (`#426 <https://github.com/ros2/rclcpp/issues/426>`_)
+  * Signed-off-by: Ethan Gao <ethan.gao@linux.intel.com>
+* Contributors: Denise Eng, Dirk Thomas, Ernesto Corbellini, Esteve Fernandez, Ethan Gao, Guillaume Autran, Karsten Knese, Matthew, Michael Carroll, Mikael Arguedas, Shane Loretz, Sriram Raghunathan, Tom Moore, William Woodall, dhood, jwang, jwang11, serge-nikulin

rclcpp/CMakeLists.txt

@@ -1,34 +1,416 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 
 project(rclcpp)
 
-find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(rcl REQUIRED)
 find_package(rcl_interfaces REQUIRED)
+find_package(rcl_yaml_param_parser REQUIRED)
+find_package(rmw REQUIRED)
+find_package(rmw_implementation REQUIRED)
+find_package(rosgraph_msgs REQUIRED)
+find_package(rosidl_generator_cpp REQUIRED)
+find_package(rosidl_typesupport_c REQUIRED)
+find_package(rosidl_typesupport_cpp REQUIRED)
 
-ament_export_dependencies(rmw)
-ament_export_dependencies(rcl_interfaces)
+# 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")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
 
+include_directories(include)
+
+set(${PROJECT_NAME}_SRCS
+  src/rclcpp/any_executable.cpp
+  src/rclcpp/callback_group.cpp
+  src/rclcpp/client.cpp
+  src/rclcpp/clock.cpp
+  src/rclcpp/context.cpp
+  src/rclcpp/contexts/default_context.cpp
+  src/rclcpp/duration.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/init_options.cpp
+  src/rclcpp/intra_process_manager.cpp
+  src/rclcpp/intra_process_manager_impl.cpp
+  src/rclcpp/logger.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_clock.cpp
+  src/rclcpp/node_interfaces/node_graph.cpp
+  src/rclcpp/node_interfaces/node_logging.cpp
+  src/rclcpp/node_interfaces/node_parameters.cpp
+  src/rclcpp/node_interfaces/node_services.cpp
+  src/rclcpp/node_interfaces/node_time_source.cpp
+  src/rclcpp/node_interfaces/node_timers.cpp
+  src/rclcpp/node_interfaces/node_topics.cpp
+  src/rclcpp/node_interfaces/node_waitables.cpp
+  src/rclcpp/parameter.cpp
+  src/rclcpp/parameter_value.cpp
+  src/rclcpp/parameter_client.cpp
+  src/rclcpp/parameter_events_filter.cpp
+  src/rclcpp/parameter_map.cpp
+  src/rclcpp/parameter_service.cpp
+  src/rclcpp/publisher.cpp
+  src/rclcpp/service.cpp
+  src/rclcpp/signal_handler.cpp
+  src/rclcpp/subscription.cpp
+  src/rclcpp/time.cpp
+  src/rclcpp/time_source.cpp
+  src/rclcpp/timer.cpp
+  src/rclcpp/type_support.cpp
+  src/rclcpp/utilities.cpp
+  src/rclcpp/waitable.cpp
+)
+
+# "watch" template for changes
+configure_file(
+  "resource/logging.hpp.em"
+  "logging.hpp.em.watch"
+  COPYONLY
+)
+# generate header with logging macros
+set(python_code
+  "import em"
+  "em.invoke(['-o', 'include/rclcpp/logging.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/logging.hpp.em'])")
+string(REPLACE ";" "$<SEMICOLON>" python_code "${python_code}")
+add_custom_command(OUTPUT include/rclcpp/logging.hpp
+  COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp"
+  COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_code}"
+  DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/logging.hpp.em.watch"
+  COMMENT "Expanding logging.hpp.em"
+  VERBATIM
+)
+list(APPEND ${PROJECT_NAME}_SRCS
+  include/rclcpp/logging.hpp)
+include_directories("${CMAKE_CURRENT_BINARY_DIR}/include")
+
+add_library(${PROJECT_NAME}
+  ${${PROJECT_NAME}_SRCS})
+# specific order: dependents before dependencies
+ament_target_dependencies(${PROJECT_NAME}
+  "rcl"
+  "rcl_yaml_param_parser"
+  "builtin_interfaces"
+  "rosgraph_msgs"
+  "rosidl_typesupport_cpp"
+  "rosidl_generator_cpp")
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(${PROJECT_NAME}
+  PRIVATE "RCLCPP_BUILDING_LIBRARY")
+
+install(
+  TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+# specific order: dependents before dependencies
 ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
 
-if(AMENT_ENABLE_TESTING)
+ament_export_dependencies(ament_cmake)
+ament_export_dependencies(rcl)
+ament_export_dependencies(builtin_interfaces)
+ament_export_dependencies(rosgraph_msgs)
+ament_export_dependencies(rosidl_typesupport_cpp)
+ament_export_dependencies(rosidl_typesupport_c)
+ament_export_dependencies(rosidl_generator_cpp)
+ament_export_dependencies(rcl_yaml_param_parser)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 
-  find_package(rmw REQUIRED)
+  find_package(rmw_implementation_cmake REQUIRED)
 
-  if(NOT WIN32)
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra")
+  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()
-
-  include_directories(include)
-
   ament_add_gtest(test_function_traits test/test_function_traits.cpp)
+  if(TARGET test_function_traits)
+    target_include_directories(test_function_traits PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${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_interfaces_INCLUDE_DIRS}"
-      "${rmw_INCLUDE_DIRS}")
+      ${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_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_node_global_args test/test_node_global_args.cpp)
+  if(TARGET test_node_global_args)
+    target_include_directories(test_node_global_args PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_node_global_args ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_node_initial_parameters test/test_node_initial_parameters.cpp)
+  if(TARGET test_node_initial_parameters)
+    target_link_libraries(test_node_initial_parameters ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_parameter_events_filter test/test_parameter_events_filter.cpp)
+  if(TARGET test_parameter_events_filter)
+    target_include_directories(test_parameter_events_filter PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_parameter_events_filter ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_parameter test/test_parameter.cpp)
+  if(TARGET test_parameter)
+    target_include_directories(test_parameter PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_parameter ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_parameter_map test/test_parameter_map.cpp)
+  if(TARGET test_parameter_map)
+    target_link_libraries(test_parameter_map ${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)
+  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}
+    )
+  endif()
+  ament_add_gtest(test_serialized_message_allocator test/test_serialized_message_allocator.cpp)
+  if(TARGET test_serialized_message_allocator)
+    target_include_directories(test_serialized_message_allocator PUBLIC
+      ${test_msgs_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_serialized_message_allocator
+      ${PROJECT_NAME}
+      ${test_msgs_LIBRARIES}
+    )
+  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()
+  find_package(test_msgs REQUIRED)
+  ament_add_gtest(test_subscription_traits test/test_subscription_traits.cpp)
+  if(TARGET test_subscription_traits)
+    target_include_directories(test_subscription_traits PUBLIC
+      ${rcl_INCLUDE_DIRS}
+    )
+  ament_target_dependencies(test_subscription_traits
+    "test_msgs"
+  )
+  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_duration test/test_duration.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_duration)
+    ament_target_dependencies(test_duration
+      "rcl")
+    target_link_libraries(test_duration ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_executor test/test_executor.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_executor)
+    ament_target_dependencies(test_executor
+      "rcl")
+    target_link_libraries(test_executor ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_logger test/test_logger.cpp)
+  target_link_libraries(test_logger ${PROJECT_NAME})
+
+  ament_add_gmock(test_logging test/test_logging.cpp)
+  target_link_libraries(test_logging ${PROJECT_NAME})
+
+  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()
+
+  ament_add_gtest(test_time_source test/test_time_source.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_time_source)
+    ament_target_dependencies(test_time_source
+      "rcl")
+    target_link_libraries(test_time_source ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_utilities test/test_utilities.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_utilities)
+    ament_target_dependencies(test_utilities
+      "rcl")
+    target_link_libraries(test_utilities ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_init test/test_init.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_init)
+    ament_target_dependencies(test_init
+      "rcl")
+    target_link_libraries(test_init ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_multi_threaded_executor test/executors/test_multi_threaded_executor.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_multi_threaded_executor)
+    ament_target_dependencies(test_multi_threaded_executor
+      "rcl")
+    target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_local_parameters test/test_local_parameters.cpp)
+  if(TARGET test_local_parameters)
+    target_include_directories(test_local_parameters PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_local_parameters ${PROJECT_NAME})
   endif()
 endif()
 
@@ -37,11 +419,11 @@ ament_package(
 )
 
 install(
-  DIRECTORY include/
-  DESTINATION include
+  DIRECTORY cmake
+  DESTINATION share/${PROJECT_NAME}
 )
 
 install(
-  DIRECTORY src/
-  DESTINATION src/rclcpp
+  DIRECTORY include/ ${CMAKE_CURRENT_BINARY_DIR}/include/
+  DESTINATION include
 )

rclcpp/Doxyfile

@@ -0,0 +1,33 @@
+# All settings not listed here will use the Doxygen default values.
+
+PROJECT_NAME           = "rclcpp"
+PROJECT_NUMBER         = master
+PROJECT_BRIEF          = "C++ ROS Client Library API"
+
+# Use these lines to include the generated logging.hpp (update install path if needed)
+#INPUT                  = ../../../../install_isolated/rclcpp/include
+#STRIP_FROM_PATH        = /Users/william/ros2_ws/install_isolated/rclcpp/include
+# Otherwise just generate for the local (non-generated header files)
+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/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,68 @@ 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/allocator/allocator_deleter.hpp

@@ -16,10 +16,10 @@
 #define RCLCPP__ALLOCATOR__ALLOCATOR_DELETER_HPP_
 
 #include <memory>
+#include <stdexcept>
 
 namespace rclcpp
 {
-
 namespace allocator
 {
 
@@ -94,11 +94,11 @@ void set_allocator_for_deleter(AllocatorDeleter<T> * deleter, Alloc * alloc)
 
 template<typename Alloc, typename T>
 using Deleter = typename std::conditional<
-    std::is_same<typename std::allocator_traits<Alloc>::template rebind_alloc<T>,
-    typename std::allocator<void>::template rebind<T>::other>::value,
-    std::default_delete<T>,
-    AllocatorDeleter<Alloc>
-    >::type;
+  std::is_same<typename std::allocator_traits<Alloc>::template rebind_alloc<T>,
+  typename std::allocator<void>::template rebind<T>::other>::value,
+  std::default_delete<T>,
+  AllocatorDeleter<Alloc>
+  >::type;
 }  // namespace allocator
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/any_executable.hpp

@@ -12,13 +12,20 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_ANY_EXECUTABLE_HPP_
-#define RCLCPP_RCLCPP_ANY_EXECUTABLE_HPP_
+#ifndef RCLCPP__ANY_EXECUTABLE_HPP_
+#define RCLCPP__ANY_EXECUTABLE_HPP_
 
 #include <memory>
 
-#include <rclcpp/macros.hpp>
-#include <rclcpp/node.hpp>
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/client.hpp"
+#include "rclcpp/macros.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"
+#include "rclcpp/waitable.hpp"
 
 namespace rclcpp
 {
@@ -27,22 +34,25 @@ namespace executor
 
 struct AnyExecutable
 {
-  RCLCPP_SMART_PTR_DEFINITIONS(AnyExecutable);
-  AnyExecutable()
-  : subscription(0), timer(0), callback_group(0), node(0)
-  {}
+  RCLCPP_PUBLIC
+  AnyExecutable();
+
+  RCLCPP_PUBLIC
+  virtual ~AnyExecutable();
+
   // Only one of the following pointers will be set.
-  rclcpp::subscription::SubscriptionBase::SharedPtr subscription;
-  rclcpp::subscription::SubscriptionBase::SharedPtr subscription_intra_process;
-  rclcpp::timer::TimerBase::SharedPtr timer;
-  rclcpp::service::ServiceBase::SharedPtr service;
-  rclcpp::client::ClientBase::SharedPtr client;
+  rclcpp::SubscriptionBase::SharedPtr subscription;
+  rclcpp::SubscriptionBase::SharedPtr subscription_intra_process;
+  rclcpp::TimerBase::SharedPtr timer;
+  rclcpp::ServiceBase::SharedPtr service;
+  rclcpp::ClientBase::SharedPtr client;
+  rclcpp::Waitable::SharedPtr waitable;
   // 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;
 };
 
-} /* executor */
-} /* rclcpp */
+}  // namespace executor
+}  // namespace rclcpp
 
-#endif
+#endif  // RCLCPP__ANY_EXECUTABLE_HPP_

rclcpp/include/rclcpp/any_service_callback.hpp

@@ -12,36 +12,36 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_ANY_SERVICE_CALLBACK_HPP_
-#define RCLCPP_RCLCPP_ANY_SERVICE_CALLBACK_HPP_
-
-#include <rclcpp/function_traits.hpp>
+#ifndef RCLCPP__ANY_SERVICE_CALLBACK_HPP_
+#define RCLCPP__ANY_SERVICE_CALLBACK_HPP_
 
 #include <functional>
 #include <memory>
+#include <stdexcept>
 #include <type_traits>
 
-#include <rmw/types.h>
+#include "rclcpp/function_traits.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rmw/types.h"
 
 namespace rclcpp
 {
 
-namespace any_service_callback
-{
-
 template<typename ServiceT>
 class AnyServiceCallback
 {
 private:
-  using SharedPtrCallback = std::function<void(
-        const std::shared_ptr<typename ServiceT::Request>,
-        std::shared_ptr<typename ServiceT::Response>
-      )>;
-  using SharedPtrWithRequestHeaderCallback = std::function<void(
-        const std::shared_ptr<rmw_request_id_t>,
-        const std::shared_ptr<typename ServiceT::Request>,
-        std::shared_ptr<typename ServiceT::Response>
-      )>;
+  using SharedPtrCallback = std::function<
+    void (
+      const std::shared_ptr<typename ServiceT::Request>,
+      std::shared_ptr<typename ServiceT::Response>
+    )>;
+  using SharedPtrWithRequestHeaderCallback = std::function<
+    void (
+      const std::shared_ptr<rmw_request_id_t>,
+      const std::shared_ptr<typename ServiceT::Request>,
+      std::shared_ptr<typename ServiceT::Response>
+    )>;
 
   SharedPtrCallback shared_ptr_callback_;
   SharedPtrWithRequestHeaderCallback shared_ptr_with_request_header_callback_;
@@ -56,7 +56,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         SharedPtrCallback
       >::value
@@ -70,7 +70,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         SharedPtrWithRequestHeaderCallback
       >::value
@@ -97,7 +97,6 @@ public:
   }
 };
 
-} /* namespace any_service_callback */
-} /* namespace rclcpp */
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_ANY_SERVICE_CALLBACK_HPP_ */
+#endif  // RCLCPP__ANY_SERVICE_CALLBACK_HPP_

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -19,17 +19,17 @@
 
 #include <functional>
 #include <memory>
+#include <stdexcept>
 #include <type_traits>
+#include <utility>
 
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/function_traits.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
 
-namespace any_subscription_callback
-{
-
 template<typename MessageT, typename Alloc>
 class AnySubscriptionCallback
 {
@@ -38,15 +38,15 @@ class AnySubscriptionCallback
   using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
   using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
 
-  using SharedPtrCallback = std::function<void(const std::shared_ptr<MessageT>)>;
+  using SharedPtrCallback = std::function<void (const std::shared_ptr<MessageT>)>;
   using SharedPtrWithInfoCallback =
-      std::function<void(const std::shared_ptr<MessageT>, const rmw_message_info_t &)>;
-  using ConstSharedPtrCallback = std::function<void(const std::shared_ptr<const MessageT>)>;
+    std::function<void (const std::shared_ptr<MessageT>, const rmw_message_info_t &)>;
+  using ConstSharedPtrCallback = std::function<void (const std::shared_ptr<const MessageT>)>;
   using ConstSharedPtrWithInfoCallback =
-      std::function<void(const std::shared_ptr<const MessageT>, const rmw_message_info_t &)>;
-  using UniquePtrCallback = std::function<void(MessageUniquePtr)>;
+    std::function<void (const std::shared_ptr<const MessageT>, const rmw_message_info_t &)>;
+  using UniquePtrCallback = std::function<void (MessageUniquePtr)>;
   using UniquePtrWithInfoCallback =
-      std::function<void(MessageUniquePtr, const rmw_message_info_t &)>;
+    std::function<void (MessageUniquePtr, const rmw_message_info_t &)>;
 
   SharedPtrCallback shared_ptr_callback_;
   SharedPtrWithInfoCallback shared_ptr_with_info_callback_;
@@ -70,7 +70,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         SharedPtrCallback
       >::value
@@ -84,7 +84,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         SharedPtrWithInfoCallback
       >::value
@@ -98,7 +98,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         ConstSharedPtrCallback
       >::value
@@ -112,7 +112,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         ConstSharedPtrWithInfoCallback
       >::value
@@ -126,7 +126,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         UniquePtrCallback
       >::value
@@ -140,7 +140,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         UniquePtrWithInfoCallback
       >::value
@@ -181,16 +181,16 @@ public:
   {
     (void)message_info;
     if (shared_ptr_callback_) {
-      typename MessageT::SharedPtr shared_message = std::move(message);
+      typename std::shared_ptr<MessageT> shared_message = std::move(message);
       shared_ptr_callback_(shared_message);
     } else if (shared_ptr_with_info_callback_) {
-      typename MessageT::SharedPtr shared_message = std::move(message);
+      typename std::shared_ptr<MessageT> shared_message = std::move(message);
       shared_ptr_with_info_callback_(shared_message, message_info);
     } else if (const_shared_ptr_callback_) {
-      typename MessageT::ConstSharedPtr const_shared_message = std::move(message);
+      typename std::shared_ptr<MessageT const> const_shared_message = std::move(message);
       const_shared_ptr_callback_(const_shared_message);
     } else if (const_shared_ptr_with_info_callback_) {
-      typename MessageT::ConstSharedPtr const_shared_message = std::move(message);
+      typename std::shared_ptr<MessageT const> const_shared_message = std::move(message);
       const_shared_ptr_with_info_callback_(const_shared_message, message_info);
     } else if (unique_ptr_callback_) {
       unique_ptr_callback_(std::move(message));
@@ -206,7 +206,6 @@ private:
   MessageDeleter message_deleter_;
 };
 
-}  // namespace any_subscription_callback
 }  // namespace rclcpp
 
 #endif  // RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_

rclcpp/include/rclcpp/callback_group.hpp

@@ -12,26 +12,32 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_
-#define RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_
+#ifndef RCLCPP__CALLBACK_GROUP_HPP_
+#define RCLCPP__CALLBACK_GROUP_HPP_
 
 #include <atomic>
+#include <mutex>
 #include <string>
 #include <vector>
 
-#include <rclcpp/subscription.hpp>
-#include <rclcpp/timer.hpp>
-#include <rclcpp/service.hpp>
-#include <rclcpp/client.hpp>
+#include "rclcpp/client.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/subscription.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
 
 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;
+class NodeWaitables;
+}  // namespace node_interfaces
 
 namespace callback_group
 {
@@ -44,87 +50,84 @@ 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;
+  friend class rclcpp::node_interfaces::NodeWaitables;
 
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup);
+  RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup)
 
-  CallbackGroup(CallbackGroupType group_type)
-  : type_(group_type), can_be_taken_from_(true)
-  {}
+  RCLCPP_PUBLIC
+  explicit CallbackGroup(CallbackGroupType group_type);
 
-  const std::vector<subscription::SubscriptionBase::WeakPtr> &
-  get_subscription_ptrs() const
-  {
-    return subscription_ptrs_;
-  }
+  RCLCPP_PUBLIC
+  const std::vector<rclcpp::SubscriptionBase::WeakPtr> &
+  get_subscription_ptrs() const;
 
-  const std::vector<timer::TimerBase::WeakPtr> &
-  get_timer_ptrs() const
-  {
-    return timer_ptrs_;
-  }
+  RCLCPP_PUBLIC
+  const std::vector<rclcpp::TimerBase::WeakPtr> &
+  get_timer_ptrs() const;
 
-  const std::vector<service::ServiceBase::SharedPtr> &
-  get_service_ptrs() const
-  {
-    return service_ptrs_;
-  }
+  RCLCPP_PUBLIC
+  const std::vector<rclcpp::ServiceBase::WeakPtr> &
+  get_service_ptrs() const;
 
-  const std::vector<client::ClientBase::SharedPtr> &
-  get_client_ptrs() const
-  {
-    return client_ptrs_;
-  }
+  RCLCPP_PUBLIC
+  const std::vector<rclcpp::ClientBase::WeakPtr> &
+  get_client_ptrs() const;
 
-  std::atomic_bool & can_be_taken_from()
-  {
-    return can_be_taken_from_;
-  }
+  RCLCPP_PUBLIC
+  const std::vector<rclcpp::Waitable::WeakPtr> &
+  get_waitable_ptrs() const;
 
-  const CallbackGroupType & type() const
-  {
-    return type_;
-  }
+  RCLCPP_PUBLIC
+  std::atomic_bool &
+  can_be_taken_from();
 
-private:
-  RCLCPP_DISABLE_COPY(CallbackGroup);
+  RCLCPP_PUBLIC
+  const CallbackGroupType &
+  type() const;
 
+protected:
+  RCLCPP_DISABLE_COPY(CallbackGroup)
+
+  RCLCPP_PUBLIC
   void
-  add_subscription(
-    const subscription::SubscriptionBase::SharedPtr subscription_ptr)
-  {
-    subscription_ptrs_.push_back(subscription_ptr);
-  }
+  add_subscription(const rclcpp::SubscriptionBase::SharedPtr subscription_ptr);
 
+  RCLCPP_PUBLIC
   void
-  add_timer(const timer::TimerBase::SharedPtr timer_ptr)
-  {
-    timer_ptrs_.push_back(timer_ptr);
-  }
+  add_timer(const rclcpp::TimerBase::SharedPtr timer_ptr);
 
+  RCLCPP_PUBLIC
   void
-  add_service(const service::ServiceBase::SharedPtr service_ptr)
-  {
-    service_ptrs_.push_back(service_ptr);
-  }
+  add_service(const rclcpp::ServiceBase::SharedPtr service_ptr);
 
+  RCLCPP_PUBLIC
   void
-  add_client(const client::ClientBase::SharedPtr client_ptr)
-  {
-    client_ptrs_.push_back(client_ptr);
-  }
+  add_client(const rclcpp::ClientBase::SharedPtr client_ptr);
+
+  RCLCPP_PUBLIC
+  void
+  add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr);
+
+  RCLCPP_PUBLIC
+  void
+  remove_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) noexcept;
 
   CallbackGroupType type_;
-  std::vector<subscription::SubscriptionBase::WeakPtr> subscription_ptrs_;
-  std::vector<timer::TimerBase::WeakPtr> timer_ptrs_;
-  std::vector<service::ServiceBase::SharedPtr> service_ptrs_;
-  std::vector<client::ClientBase::SharedPtr> client_ptrs_;
+  // Mutex to protect the subsequent vectors of pointers.
+  mutable std::mutex mutex_;
+  std::vector<rclcpp::SubscriptionBase::WeakPtr> subscription_ptrs_;
+  std::vector<rclcpp::TimerBase::WeakPtr> timer_ptrs_;
+  std::vector<rclcpp::ServiceBase::WeakPtr> service_ptrs_;
+  std::vector<rclcpp::ClientBase::WeakPtr> client_ptrs_;
+  std::vector<rclcpp::Waitable::WeakPtr> waitable_ptrs_;
   std::atomic_bool can_be_taken_from_;
-
 };
 
-} /* namespace callback_group */
-} /* namespace rclcpp */
+}  // namespace callback_group
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_ */
+#endif  // RCLCPP__CALLBACK_GROUP_HPP_

rclcpp/include/rclcpp/client.hpp

@@ -12,153 +12,271 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_CLIENT_HPP_
-#define RCLCPP_RCLCPP_CLIENT_HPP_
+#ifndef RCLCPP__CLIENT_HPP_
+#define RCLCPP__CLIENT_HPP_
 
 #include <future>
-#include <iostream>
 #include <map>
 #include <memory>
 #include <sstream>
+#include <string>
+#include <tuple>
 #include <utility>
 
-#include <rmw/error_handling.h>
-#include <rmw/rmw.h>
+#include "rcl/client.h"
+#include "rcl/error_handling.h"
+#include "rcl/wait.h"
 
-#include <rclcpp/macros.hpp>
-#include <rclcpp/utilities.hpp>
+#include "rclcpp/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 "rcutils/logging_macros.h"
+
+#include "rmw/error_handling.h"
+#include "rmw/rmw.h"
 
 namespace rclcpp
 {
 
-namespace client
+namespace node_interfaces
 {
+class NodeBaseInterface;
+}  // namespace node_interfaces
 
 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,
-    const std::string & service_name)
-  : node_handle_(node_handle), client_handle_(client_handle), service_name_(service_name)
-  {}
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph);
 
-  virtual ~ClientBase()
-  {
-    if (client_handle_) {
-      if (rmw_destroy_client(client_handle_) != RMW_RET_OK) {
-        fprintf(stderr,
-          "Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe());
-      }
-    }
-  }
+  RCLCPP_PUBLIC
+  virtual ~ClientBase();
 
-  const std::string & get_service_name() const
-  {
-    return this->service_name_;
-  }
+  RCLCPP_PUBLIC
+  const char *
+  get_service_name() const;
 
-  const rmw_client_t * get_client_handle() const
+  RCLCPP_PUBLIC
+  std::shared_ptr<rcl_client_t>
+  get_client_handle();
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<const rcl_client_t>
+  get_client_handle() const;
+
+  RCLCPP_PUBLIC
+  bool
+  service_is_ready() const;
+
+  template<typename RatioT = std::milli>
+  bool
+  wait_for_service(
+    std::chrono::duration<int64_t, RatioT> timeout = std::chrono::duration<int64_t, RatioT>(-1))
   {
-    return this->client_handle_;
+    return wait_for_service_nanoseconds(
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
   }
 
   virtual std::shared_ptr<void> create_response() = 0;
-  virtual std::shared_ptr<void> create_request_header() = 0;
+  virtual std::shared_ptr<rmw_request_id_t> create_request_header() = 0;
   virtual void handle_response(
-    std::shared_ptr<void> & request_header, std::shared_ptr<void> & response) = 0;
+    std::shared_ptr<rmw_request_id_t> request_header, std::shared_ptr<void> response) = 0;
 
-private:
-  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_;
-  std::string service_name_;
+  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_;
+  std::shared_ptr<rclcpp::Context> context_;
+
+  std::shared_ptr<rcl_client_t> client_handle_;
 };
 
 template<typename ServiceT>
 class Client : public ClientBase
 {
 public:
-  using Promise = std::promise<typename ServiceT::Response::SharedPtr>;
+  using SharedRequest = typename ServiceT::Request::SharedPtr;
+  using SharedResponse = typename ServiceT::Response::SharedPtr;
+
+  using Promise = std::promise<SharedResponse>;
+  using PromiseWithRequest = std::promise<std::pair<SharedRequest, SharedResponse>>;
+
   using SharedPromise = std::shared_ptr<Promise>;
-  using SharedFuture = std::shared_future<typename ServiceT::Response::SharedPtr>;
+  using SharedPromiseWithRequest = std::shared_ptr<PromiseWithRequest>;
 
-  using CallbackType = std::function<void(SharedFuture)>;
+  using SharedFuture = std::shared_future<SharedResponse>;
+  using SharedFutureWithRequest = std::shared_future<std::pair<SharedRequest, SharedResponse>>;
 
-  RCLCPP_SMART_PTR_DEFINITIONS(Client);
+  using CallbackType = std::function<void (SharedFuture)>;
+  using CallbackWithRequestType = std::function<void (SharedFutureWithRequest)>;
+
+  RCLCPP_SMART_PTR_DEFINITIONS(Client)
 
   Client(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_client_t * client_handle,
-    const std::string & service_name)
-  : ClientBase(node_handle, client_handle, service_name)
-  {}
+    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)
+  {
+    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(
+      this->get_client_handle().get(),
+      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()
+  {
+  }
+
+  std::shared_ptr<void>
+  create_response() override
   {
     return std::shared_ptr<void>(new typename ServiceT::Response());
   }
 
-  std::shared_ptr<void> create_request_header()
+  std::shared_ptr<rmw_request_id_t>
+  create_request_header() override
   {
     // TODO(wjwwood): This should probably use rmw_request_id's allocator.
     //                (since it is a C type)
-    return std::shared_ptr<void>(new rmw_request_id_t);
+    return std::shared_ptr<rmw_request_id_t>(new rmw_request_id_t);
   }
 
-  void handle_response(std::shared_ptr<void> & request_header, std::shared_ptr<void> & response)
+  void
+  handle_response(
+    std::shared_ptr<rmw_request_id_t> request_header,
+    std::shared_ptr<void> response) override
   {
-    auto typed_request_header = std::static_pointer_cast<rmw_request_id_t>(request_header);
+    std::unique_lock<std::mutex> lock(pending_requests_mutex_);
     auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(response);
-    int64_t sequence_number = typed_request_header->sequence_number;
-    // TODO this must check if the sequence_number is valid otherwise the call_promise will be null
+    int64_t sequence_number = request_header->sequence_number;
+    // TODO(esteve) this should throw instead since it is not expected to happen in the first place
+    if (this->pending_requests_.count(sequence_number) == 0) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "Received invalid sequence number. Ignoring...");
+      return;
+    }
     auto tuple = this->pending_requests_[sequence_number];
     auto call_promise = std::get<0>(tuple);
     auto callback = std::get<1>(tuple);
     auto future = std::get<2>(tuple);
     this->pending_requests_.erase(sequence_number);
+    // Unlock here to allow the service to be called recursively from one of its callbacks.
+    lock.unlock();
+
     call_promise->set_value(typed_response);
     callback(future);
   }
 
-  SharedFuture async_send_request(
-    typename ServiceT::Request::SharedPtr request)
+  SharedFuture
+  async_send_request(SharedRequest request)
   {
     return async_send_request(request, [](SharedFuture) {});
   }
 
-  SharedFuture async_send_request(
-    typename ServiceT::Request::SharedPtr request,
-    CallbackType cb)
+  template<
+    typename CallbackT,
+    typename std::enable_if<
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        CallbackType
+      >::value
+    >::type * = nullptr
+  >
+  SharedFuture
+  async_send_request(SharedRequest request, CallbackT && cb)
   {
+    std::lock_guard<std::mutex> lock(pending_requests_mutex_);
     int64_t sequence_number;
-    if (RMW_RET_OK != rmw_send_request(get_client_handle(), request.get(), &sequence_number)) {
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-      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().get(), 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>();
     SharedFuture f(call_promise->get_future());
-    pending_requests_[sequence_number] = std::make_tuple(call_promise, cb, f);
+    pending_requests_[sequence_number] =
+      std::make_tuple(call_promise, std::forward<CallbackType>(cb), f);
     return f;
   }
 
+  template<
+    typename CallbackT,
+    typename std::enable_if<
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        CallbackWithRequestType
+      >::value
+    >::type * = nullptr
+  >
+  SharedFutureWithRequest
+  async_send_request(SharedRequest request, CallbackT && cb)
+  {
+    SharedPromiseWithRequest promise = std::make_shared<PromiseWithRequest>();
+    SharedFutureWithRequest future_with_request(promise->get_future());
+
+    auto wrapping_cb = [future_with_request, promise, request, &cb](SharedFuture future) {
+        auto response = future.get();
+        promise->set_value(std::make_pair(request, response));
+        cb(future_with_request);
+      };
+
+    async_send_request(request, wrapping_cb);
+
+    return future_with_request;
+  }
+
 private:
-  RCLCPP_DISABLE_COPY(Client);
+  RCLCPP_DISABLE_COPY(Client)
 
   std::map<int64_t, std::tuple<SharedPromise, CallbackType, SharedFuture>> pending_requests_;
+  std::mutex pending_requests_mutex_;
 };
 
-} /* namespace client */
-} /* namespace rclcpp */
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_CLIENT_HPP_ */
+#endif  // RCLCPP__CLIENT_HPP_

rclcpp/include/rclcpp/clock.hpp

@@ -0,0 +1,104 @@
+// 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__CLOCK_HPP_
+#define RCLCPP__CLOCK_HPP_
+
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <vector>
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/time.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcl/time.h"
+
+namespace rclcpp
+{
+
+class TimeSource;
+
+class JumpHandler
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(JumpHandler)
+  JumpHandler(
+    std::function<void()> pre_callback,
+    std::function<void(const rcl_time_jump_t &)> post_callback,
+    const rcl_jump_threshold_t & threshold);
+
+  std::function<void()> pre_callback;
+  std::function<void(const rcl_time_jump_t &)> post_callback;
+  rcl_jump_threshold_t notice_threshold;
+};
+
+class Clock
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(Clock)
+
+  RCLCPP_PUBLIC
+  explicit Clock(rcl_clock_type_t clock_type = RCL_SYSTEM_TIME);
+
+  RCLCPP_PUBLIC
+  ~Clock();
+
+  RCLCPP_PUBLIC
+  Time
+  now();
+
+  RCLCPP_PUBLIC
+  bool
+  ros_time_is_active();
+
+  RCLCPP_PUBLIC
+  rcl_clock_t *
+  get_clock_handle();
+
+  RCLCPP_PUBLIC
+  rcl_clock_type_t
+  get_clock_type();
+
+  // Add a callback to invoke if the jump threshold is exceeded.
+  /**
+   * These callback functions must remain valid as long as the
+   * returned shared pointer is valid.
+   */
+  RCLCPP_PUBLIC
+  JumpHandler::SharedPtr
+  create_jump_callback(
+    std::function<void()> pre_callback,
+    std::function<void(const rcl_time_jump_t &)> post_callback,
+    const rcl_jump_threshold_t & threshold);
+
+private:
+  // Invoke time jump callback
+  RCLCPP_PUBLIC
+  static void
+  on_time_jump(
+    const struct rcl_time_jump_t * time_jump,
+    bool before_jump,
+    void * user_data);
+
+  /// Internal storage backed by rcl
+  rcl_clock_t rcl_clock_;
+  friend TimeSource;  /// Allow TimeSource to access the rcl_clock_ datatype.
+  rcl_allocator_t allocator_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CLOCK_HPP_

rclcpp/include/rclcpp/context.hpp

@@ -12,52 +12,306 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_CONTEXT_HPP_
-#define RCLCPP_RCLCPP_CONTEXT_HPP_
-
-#include <rclcpp/macros.hpp>
-
-#include <iostream>
+#ifndef RCLCPP__CONTEXT_HPP_
+#define RCLCPP__CONTEXT_HPP_
 
+#include <condition_variable>
+#include <functional>
 #include <memory>
 #include <mutex>
-#include <typeinfo>
+#include <string>
 #include <typeindex>
+#include <typeinfo>
 #include <unordered_map>
+#include <utility>
+#include <vector>
 
-#include <rmw/rmw.h>
+#include "rcl/context.h"
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
+#include "rclcpp/init_options.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
 
-namespace context
-{
-
-class Context
+/// Thrown when init is called on an already initialized context.
+class ContextAlreadyInitialized : public std::runtime_error
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(Context);
+  ContextAlreadyInitialized()
+  : std::runtime_error("context is already initialized") {}
+};
 
-  Context() {}
+/// Context which encapsulates shared state between nodes and other similar entities.
+/**
+ * A context also represents the lifecycle between init and shutdown of rclcpp.
+ * It is often used in conjunction with rclcpp::init, or rclcpp::init_local,
+ * and rclcpp::shutdown.
+ */
+class Context : public std::enable_shared_from_this<Context>
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(Context)
 
+  /// Default constructor, after which the Context is still not "initialized".
+  /**
+   * Every context which is constructed is added to a global vector of contexts,
+   * which is used by the signal handler to conditionally shutdown each context
+   * on SIGINT.
+   * See the shutdown_on_sigint option in the InitOptions class.
+   */
+  RCLCPP_PUBLIC
+  Context();
+
+  RCLCPP_PUBLIC
+  virtual
+  ~Context();
+
+  /// Initialize the context, and the underlying elements like the rcl context.
+  /**
+   * This method must be called before passing this context to things like the
+   * constructor of Node.
+   * It must also be called before trying to shutdown the context.
+   *
+   * Note that this function does not setup any signal handlers, so if you want
+   * it to be shutdown by the signal handler, then you need to either install
+   * them manually with rclcpp::install_signal_handers() or use rclcpp::init().
+   * In addition to installing the signal handlers, the shutdown_on_sigint
+   * of the InitOptions needs to be `true` for this context to be shutdown by
+   * the signal handler, otherwise it will be passed over.
+   *
+   * After calling this method, shutdown() can be called to invalidate the
+   * context for derived entities, e.g. nodes, guard conditions, etc.
+   * However, the underlying rcl context is not finalized until this Context's
+   * destructor is called or this function is called again.
+   * Allowing this class to go out of scope and get destructed or calling this
+   * function a second time while derived entities are still using the context
+   * is undefined behavior and should be avoided.
+   * It's a good idea to not reuse context objects and instead create a new one
+   * each time you need to shutdown and init again.
+   * This allows derived entities to hold on to shard pointers to the first
+   * context object until they are done.
+   *
+   * This function is thread-safe.
+   *
+   * \param[in] argc number of arguments
+   * \param[in] argv argument array which may contain arguments intended for ROS
+   * \param[in] init_options initialization options for rclcpp and underlying layers
+   * \throw ContextAlreadyInitialized if called if init is called more than once
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  init(
+    int argc,
+    char const * const argv[],
+    const rclcpp::InitOptions & init_options = rclcpp::InitOptions());
+
+  /// Return true if the context is valid, otherwise false.
+  /**
+   * The context is valid if it has been initialized but not shutdown.
+   *
+   * This function is thread-safe.
+   * This function is lock free so long as pointers and uint64_t atomics are
+   * lock free.
+   *
+   * \return true if valid, otherwise false
+   */
+  RCLCPP_PUBLIC
+  bool
+  is_valid() const;
+
+  /// Return the init options used during init.
+  RCLCPP_PUBLIC
+  const rclcpp::InitOptions &
+  get_init_options() const;
+
+  /// Return a copy of the init options used during init.
+  RCLCPP_PUBLIC
+  rclcpp::InitOptions
+  get_init_options();
+
+  /// Return the shutdown reason, or empty string if not shutdown.
+  /**
+   * This function is thread-safe.
+   */
+  RCLCPP_PUBLIC
+  std::string
+  shutdown_reason();
+
+  /// Shutdown the context, making it uninitialized and therefore invalid for derived entities.
+  /**
+   * Several things happen when the context is shutdown, in this order:
+   *
+   * - acquires a lock to prevent race conditions with init, on_shutdown, etc.
+   * - if the context is not initialized, return false
+   * - rcl_shutdown() is called on the internal rcl_context_t instance
+   * - the shutdown reason is set
+   * - each on_shutdown callback is called, in the order that they were added
+   * - interrupt blocking sleep_for() calls, so they return early due to shutdown
+   * - interrupt blocking executors and wait sets
+   *
+   * The underlying rcl context is not finalized by this function.
+   *
+   * This function is thread-safe.
+   *
+   * \param[in] reason the description of why shutdown happened
+   * \return true if shutdown was successful, false if context was already shutdown
+   * \throw various exceptions derived from RCLErrorBase, if rcl_shutdown fails
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  shutdown(const std::string & reason);
+
+  using OnShutdownCallback = std::function<void ()>;
+
+  /// Add a on_shutdown callback to be called when shutdown is called for this context.
+  /**
+   * These callbacks will be called in the order they are added as the second
+   * to last step in shutdown().
+   *
+   * When shutdown occurs due to the signal handler, these callbacks are run
+   * asynchronoulsy in the dedicated singal handling thread.
+   *
+   * Also, shutdown() may be called from the destructor of this function.
+   * Therefore, it is not safe to throw exceptions from these callbacks.
+   * Instead, log errors or use some other mechanism to indicate an error has
+   * occurred.
+   *
+   * On shutdown callbacks may be registered before init and after shutdown,
+   * and persist on repeated init's.
+   *
+   * \param[in] callback the on shutdown callback to be registered
+   * \return the callback passed, for convenience when storing a passed lambda
+   */
+  RCLCPP_PUBLIC
+  virtual
+  OnShutdownCallback
+  on_shutdown(OnShutdownCallback callback);
+
+  /// Return the shutdown callbacks as const.
+  /**
+   * Using the returned reference is not thread-safe with calls that modify
+   * the list of "on shutdown" callbacks, i.e. on_shutdown().
+   */
+  RCLCPP_PUBLIC
+  const std::vector<OnShutdownCallback> &
+  get_on_shutdown_callbacks() const;
+
+  /// Return the shutdown callbacks.
+  /**
+   * Using the returned reference is not thread-safe with calls that modify
+   * the list of "on shutdown" callbacks, i.e. on_shutdown().
+   */
+  RCLCPP_PUBLIC
+  std::vector<OnShutdownCallback> &
+  get_on_shutdown_callbacks();
+
+  /// Return the internal rcl context.
+  RCLCPP_PUBLIC
+  std::shared_ptr<rcl_context_t>
+  get_rcl_context();
+
+  /// Sleep for a given period of time or until shutdown() is called.
+  /**
+   * This function can be interrupted early if:
+   *
+   *   - this context is shutdown()
+   *   - this context is destructed (resulting in shutdown)
+   *   - this context has shutdown_on_sigint=true and SIGINT occurs (resulting in shutdown)
+   *   - interrupt_all_sleep_for() is called
+   *
+   * \param[in] nanoseconds A std::chrono::duration representing how long to sleep for.
+   * \return true if the condition variable did not timeout, i.e. you were interrupted.
+   */
+  RCLCPP_PUBLIC
+  bool
+  sleep_for(const std::chrono::nanoseconds & nanoseconds);
+
+  /// Interrupt any blocking sleep_for calls, causing them to return immediately and return true.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  interrupt_all_sleep_for();
+
+  /// Get a handle to the guard condition which is triggered when interrupted.
+  /**
+   * This guard condition is triggered any time interrupt_all_wait_sets() is
+   * called, which may be called by the user, or shutdown().
+   * And in turn, shutdown() may be called by the user, the destructor of this
+   * context, or the signal handler if installed and shutdown_on_sigint is true
+   * for this context.
+   *
+   * The first time that this function is called for a given wait set a new guard
+   * condition will be created and returned; thereafter the same guard condition
+   * will be returned for the same wait set.
+   * This mechanism is designed to ensure that the same guard condition is not
+   * reused across wait sets (e.g., when using multiple executors in the same
+   * process).
+   * This method will throw an exception if initialization of the guard
+   * condition fails.
+   *
+   * The returned guard condition needs to be released with the
+   * release_interrupt_guard_condition() method in order to reclaim resources.
+   *
+   * \param[in] wait_set 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_interrupt_guard_condition(rcl_wait_set_t * wait_set);
+
+  /// Release the previously allocated guard condition which is triggered when interrupted.
+  /**
+   * If you previously called get_interrupt_guard_condition() for a given wait
+   * set to get a interrupt guard condition, then you should call
+   * release_interrupt_guard_condition() when you're done, to free that
+   * condition.
+   * Will throw an exception if get_interrupt_guard_condition() wasn't
+   * previously called for the given wait set.
+   *
+   * After calling this, the pointer returned by get_interrupt_guard_condition()
+   * for the given wait_set is invalid.
+   *
+   * \param[in] wait_set Pointer to the rcl_wait_set_t that was using the
+   *   resulting guard condition.
+   */
+  RCLCPP_PUBLIC
+  void
+  release_interrupt_guard_condition(rcl_wait_set_t * wait_set);
+
+  /// Nothrow version of release_interrupt_guard_condition(), logs to RCLCPP_ERROR instead.
+  RCLCPP_PUBLIC
+  void
+  release_interrupt_guard_condition(rcl_wait_set_t * wait_set, const std::nothrow_t &) noexcept;
+
+  /// Interrupt any blocking executors, or wait sets associated with this context.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  interrupt_all_wait_sets();
+
+  /// Return a singleton instance for the SubContext type, constructing one if necessary.
   template<typename SubContext, typename ... Args>
   std::shared_ptr<SubContext>
   get_sub_context(Args && ... args)
   {
-    std::lock_guard<std::mutex> lock(mutex_);
+    std::lock_guard<std::recursive_mutex> lock(sub_contexts_mutex_);
 
     std::type_index type_i(typeid(SubContext));
     std::shared_ptr<SubContext> sub_context;
     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)
       sub_context = std::shared_ptr<SubContext>(
         new SubContext(std::forward<Args>(args) ...),
-        [] (SubContext * sub_context_ptr) {
+        [](SubContext * sub_context_ptr) {
           delete sub_context_ptr;
         });
-      // *INDENT-ON*
       sub_contexts_[type_i] = sub_context;
     } else {
       // It exists, get it out and cast it.
@@ -66,15 +320,51 @@ public:
     return sub_context;
   }
 
+protected:
+  // Called by constructor and destructor to clean up by finalizing the
+  // shutdown rcl context and preparing for a new init cycle.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  clean_up();
+
 private:
-  RCLCPP_DISABLE_COPY(Context);
+  RCLCPP_DISABLE_COPY(Context)
+
+  // This mutex is recursive so that the destructor can ensure atomicity
+  // between is_initialized and shutdown.
+  std::recursive_mutex init_mutex_;
+  std::shared_ptr<rcl_context_t> rcl_context_;
+  rclcpp::InitOptions init_options_;
+  std::string shutdown_reason_;
 
   std::unordered_map<std::type_index, std::shared_ptr<void>> sub_contexts_;
-  std::mutex mutex_;
+  // This mutex is recursive so that the constructor of a sub context may
+  // attempt to acquire another sub context.
+  std::recursive_mutex sub_contexts_mutex_;
 
+  std::vector<OnShutdownCallback> on_shutdown_callbacks_;
+  std::mutex on_shutdown_callbacks_mutex_;
+
+  /// Condition variable for timed sleep (see sleep_for).
+  std::condition_variable interrupt_condition_variable_;
+  /// Mutex for protecting the global condition variable.
+  std::mutex interrupt_mutex_;
+
+  /// Mutex to protect sigint_guard_cond_handles_.
+  std::mutex interrupt_guard_cond_handles_mutex_;
+  /// Guard conditions for interrupting of associated wait sets on interrupt_all_wait_sets().
+  std::unordered_map<rcl_wait_set_t *, rcl_guard_condition_t> interrupt_guard_cond_handles_;
 };
 
-} /* namespace context */
-} /* namespace rclcpp */
+/// Return a copy of the list of context shared pointers.
+/**
+ * This function is thread-safe.
+ */
+RCLCPP_PUBLIC
+std::vector<Context::SharedPtr>
+get_contexts();
 
-#endif /* RCLCPP_RCLCPP_CONTEXT_HPP_ */
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CONTEXT_HPP_

rclcpp/include/rclcpp/contexts/default_context.hpp

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

rclcpp/include/rclcpp/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_service.hpp

@@ -0,0 +1,58 @@
+// Copyright 2018 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_SERVICE_HPP_
+#define RCLCPP__CREATE_SERVICE_HPP_
+
+#include <memory>
+#include <string>
+#include <utility>
+
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rmw/rmw.h"
+
+namespace rclcpp
+{
+
+/// Create a service with a given type.
+/// \internal
+template<typename ServiceT, typename CallbackT>
+typename rclcpp::Service<ServiceT>::SharedPtr
+create_service(
+  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+  std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+  const std::string & service_name,
+  CallbackT && callback,
+  const rmw_qos_profile_t & qos_profile,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group)
+{
+  rclcpp::AnyServiceCallback<ServiceT> any_service_callback;
+  any_service_callback.set(std::forward<CallbackT>(callback));
+
+  rcl_service_options_t service_options = rcl_service_get_default_options();
+  service_options.qos = qos_profile;
+
+  auto serv = Service<ServiceT>::make_shared(
+    node_base->get_shared_rcl_node_handle(),
+    service_name, any_service_callback, service_options);
+  auto serv_base_ptr = std::dynamic_pointer_cast<ServiceBase>(serv);
+  node_services->add_service(serv_base_ptr, group);
+  return serv;
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CREATE_SERVICE_HPP_

rclcpp/include/rclcpp/create_subscription.hpp

@@ -0,0 +1,68 @@
+// 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 CallbackMessageT,
+  typename SubscriptionT = rclcpp::Subscription<CallbackMessageT, AllocatorT>>
+typename std::shared_ptr<SubscriptionT>
+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<
+    CallbackMessageT, 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, CallbackMessageT, 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/duration.hpp

@@ -0,0 +1,117 @@
+// 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__DURATION_HPP_
+#define RCLCPP__DURATION_HPP_
+
+#include <chrono>
+
+#include "builtin_interfaces/msg/duration.hpp"
+#include "rcl/time.h"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+class Duration
+{
+public:
+  RCLCPP_PUBLIC
+  Duration(int32_t seconds, uint32_t nanoseconds);
+
+  RCLCPP_PUBLIC
+  explicit Duration(
+    rcl_duration_value_t nanoseconds);
+
+  RCLCPP_PUBLIC
+  explicit Duration(
+    std::chrono::nanoseconds nanoseconds);
+
+  RCLCPP_PUBLIC
+  Duration(
+    const builtin_interfaces::msg::Duration & duration_msg);
+
+  RCLCPP_PUBLIC
+  explicit Duration(const rcl_duration_t & duration);
+
+  RCLCPP_PUBLIC
+  Duration(const Duration & rhs);
+
+  RCLCPP_PUBLIC
+  virtual ~Duration();
+
+  RCLCPP_PUBLIC
+  operator builtin_interfaces::msg::Duration() const;
+
+  RCLCPP_PUBLIC
+  Duration &
+  operator=(const Duration & rhs);
+
+  RCLCPP_PUBLIC
+  Duration &
+  operator=(const builtin_interfaces::msg::Duration & Duration_msg);
+
+  RCLCPP_PUBLIC
+  bool
+  operator==(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator<(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator<=(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator>=(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator>(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  Duration
+  operator+(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  Duration
+  operator-(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  static Duration
+  max();
+
+  RCLCPP_PUBLIC
+  Duration
+  operator*(double scale) const;
+
+  RCLCPP_PUBLIC
+  rcl_duration_value_t
+  nanoseconds() const;
+
+  /// \return the duration in seconds as a floating point number.
+  /// \warning Depending on sizeof(double) there could be significant precision loss.
+  /// When an exact time is required use nanoseconds() instead.
+  RCLCPP_PUBLIC
+  double
+  seconds() const;
+
+private:
+  rcl_duration_t rcl_duration_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DURATION_HPP_

rclcpp/include/rclcpp/event.hpp

@@ -0,0 +1,55 @@
+// 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
+{
+
+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 rclcpp
+
+#endif  // RCLCPP__EVENT_HPP_

rclcpp/include/rclcpp/exceptions.hpp

@@ -0,0 +1,202 @@
+// 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") {}
+};
+
+/// Thrown if passed parameters are inconsistent or invalid
+class InvalidParametersException : public std::runtime_error
+{
+public:
+  // Inherit constructors from runtime_error;
+  using std::runtime_error::runtime_error;
+};
+
+/// Throwing if passed parameter value is invalid.
+class InvalidParameterValueException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error;
+  using std::runtime_error::runtime_error;
+};
+
+}  // namespace exceptions
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXCEPTIONS_HPP_

rclcpp/include/rclcpp/executor.hpp

@@ -17,26 +17,71 @@
 
 #include <algorithm>
 #include <cassert>
+#include <chrono>
 #include <cstdlib>
 #include <iostream>
 #include <list>
 #include <memory>
+#include <string>
 #include <vector>
 
-#include "rcl_interfaces/msg/intra_process_message.hpp"
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
 
-#include "rclcpp/any_executable.hpp"
-#include "rclcpp/macros.hpp"
+#include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/memory_strategies.hpp"
 #include "rclcpp/memory_strategy.hpp"
-#include "rclcpp/node.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/utilities.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
+
+// Forward declaration is used in convenience method signature.
+class Node;
+
 namespace executor
 {
 
+/// Return codes to be used with spin_until_future_complete.
+/**
+ * SUCCESS: The future is complete and can be accessed with "get" without blocking.
+ * INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error.
+ * TIMEOUT: Spinning timed out.
+ */
+enum class FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT};
+
+RCLCPP_PUBLIC
+std::ostream &
+operator<<(std::ostream & os, const FutureReturnCode & future_return_code);
+
+RCLCPP_PUBLIC
+std::string
+to_string(const FutureReturnCode & future_return_code);
+
+///
+/**
+ * Options to be passed to the executor constructor.
+ */
+struct ExecutorArgs
+{
+  ExecutorArgs()
+  : memory_strategy(memory_strategies::create_default_strategy()),
+    context(rclcpp::contexts::default_context::get_global_default_context()),
+    max_conditions(0)
+  {}
+
+  memory_strategy::MemoryStrategy::SharedPtr memory_strategy;
+  std::shared_ptr<rclcpp::Context> context;
+  size_t max_conditions;
+};
+
+static inline ExecutorArgs create_default_executor_arguments()
+{
+  return ExecutorArgs();
+}
+
 /// Coordinate the order and timing of available communication tasks.
 /**
  * Executor provides spin functions (including spin_node_once and spin_some).
@@ -50,33 +95,21 @@ namespace executor
 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.
-  explicit Executor(memory_strategy::MemoryStrategy::SharedPtr ms =
-    memory_strategies::create_default_strategy())
-  : interrupt_guard_condition_(rmw_create_guard_condition()),
-    memory_strategy_(ms)
-  {
-  }
+  RCLCPP_PUBLIC
+  explicit Executor(const ExecutorArgs & args = ExecutorArgs());
 
   /// Default destructor.
-  virtual ~Executor()
-  {
-    // Try to deallocate the interrupt guard condition.
-    if (interrupt_guard_condition_ != nullptr) {
-      rmw_ret_t status = rmw_destroy_guard_condition(interrupt_guard_condition_);
-      if (status != RMW_RET_OK) {
-        fprintf(stderr,
-          "[rclcpp::error] failed to destroy guard condition: %s\n", rmw_get_error_string_safe());
-      }
-    }
-  }
+  RCLCPP_PUBLIC
+  virtual ~Executor();
 
   /// Do work periodically as it becomes available to us. Blocking call, may block indefinitely.
   // It is up to the implementation of Executor to implement spin.
-  virtual void spin() = 0;
+  virtual void
+  spin() = 0;
 
   /// Add a node to the executor.
   /**
@@ -86,26 +119,14 @@ public:
    * the executor is blocked at the rmw layer while waiting for work and it is notified that a new
    * node was added, it will wake up.
    */
+  RCLCPP_PUBLIC
   virtual void
-  add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true)
-  {
-    // Check to ensure node not already added
-    for (auto & weak_node : weak_nodes_) {
-      auto node = weak_node.lock();
-      if (node == node_ptr) {
-        // TODO(jacquelinekay): Use a different error here?
-        throw std::runtime_error("Cannot add node to executor, node already added.");
-      }
-    }
-    weak_nodes_.push_back(node_ptr);
-    if (notify) {
-      // Interrupt waiting to handle new node
-      rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
-      if (status != RMW_RET_OK) {
-        throw std::runtime_error(rmw_get_error_string_safe());
-      }
-    }
-  }
+  add_node(rclcpp::node_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_ptr, bool notify = true);
 
   /// Remove a node from the executor.
   /**
@@ -114,29 +135,14 @@ public:
    * This is useful if the last node was removed from the executor while the executor was blocked
    * waiting for work in another thread, because otherwise the executor would never be notified.
    */
+  RCLCPP_PUBLIC
   virtual void
-  remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true)
-  {
-    bool node_removed = false;
-    weak_nodes_.erase(
-      std::remove_if(
-        weak_nodes_.begin(), weak_nodes_.end(),
-        [&](std::weak_ptr<rclcpp::node::Node> & i)
-        {
-          bool matched = (i.lock() == node_ptr);
-          node_removed |= matched;
-          return matched;
-        }));
-    if (notify) {
-      // If the node was matched and removed, interrupt waiting
-      if (node_removed) {
-        rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
-        if (status != RMW_RET_OK) {
-          throw std::runtime_error(rmw_get_error_string_safe());
-        }
-      }
-    }
-  }
+  remove_node(rclcpp::node_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_ptr, bool notify = true);
 
   /// Add a node to executor, execute the next available unit of work, and remove the node.
   /**
@@ -146,28 +152,42 @@ public:
    * function to be non-blocking.
    */
   template<typename T = std::milli>
-  void spin_node_once(rclcpp::node::Node::SharedPtr node,
+  void
+  spin_node_once(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
     std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
   {
-    this->add_node(node, false);
-    // non-blocking = true
-    auto any_exec = get_next_executable(timeout);
-    if (any_exec) {
-      execute_any_executable(any_exec);
-    }
-    this->remove_node(node, false);
+    return spin_node_once_nanoseconds(
+      node,
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
+  }
+
+  /// Convenience function which takes Node and forwards NodeBaseInterface.
+  template<typename NodeT = rclcpp::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.
    */
-  void spin_node_some(rclcpp::node::Node::SharedPtr node)
-  {
-    this->add_node(node, false);
-    spin_some();
-    this->remove_node(node, false);
-  }
+  RCLCPP_PUBLIC
+  void
+  spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node);
+
+  /// Convenience function which takes Node and forwards NodeBaseInterface.
+  RCLCPP_PUBLIC
+  void
+  spin_node_some(std::shared_ptr<rclcpp::Node> node);
 
   /// Complete all available queued work without blocking.
   /**
@@ -175,439 +195,176 @@ public:
    * single-threaded model of execution.
    * Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function
    * to block (which may have unintended consequences).
+   *
+   * \param[in] max_duration The maximum amount of time to spend executing work, or 0 for no limit.
+   * Note that spin_some() may take longer than this time as it only returns once max_duration has
+   * been exceeded.
    */
-  virtual void spin_some()
+  RCLCPP_PUBLIC
+  virtual void
+  spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0));
+
+  RCLCPP_PUBLIC
+  virtual void
+  spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+
+  /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
+  /**
+   * \param[in] future The future to wait on. If SUCCESS, the future is safe to access after this
+   *   function.
+   * \param[in] timeout Optional timeout parameter, which gets passed to Executor::spin_node_once.
+   *   `-1` is block forever, `0` is non-blocking.
+   *   If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return
+   *   code.
+   * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
+   */
+  template<typename ResponseT, typename TimeT = std::milli>
+  FutureReturnCode
+  spin_until_future_complete(
+    std::shared_future<ResponseT> & future,
+    std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
   {
-    while (AnyExecutable::SharedPtr any_exec =
-      get_next_executable(std::chrono::milliseconds::zero()))
-    {
-      execute_any_executable(any_exec);
+    // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
+    // inside a callback executed by an executor.
+
+    // Check the future before entering the while loop.
+    // If the future is already complete, don't try to spin.
+    std::future_status status = future.wait_for(std::chrono::seconds(0));
+    if (status == std::future_status::ready) {
+      return FutureReturnCode::SUCCESS;
     }
+
+    auto end_time = std::chrono::steady_clock::now();
+    std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+      timeout);
+    if (timeout_ns > std::chrono::nanoseconds::zero()) {
+      end_time += timeout_ns;
+    }
+    std::chrono::nanoseconds timeout_left = timeout_ns;
+
+    while (rclcpp::ok(this->context_)) {
+      // Do one item of work.
+      spin_once(timeout_left);
+      // Check if the future is set, return SUCCESS if it is.
+      status = future.wait_for(std::chrono::seconds(0));
+      if (status == std::future_status::ready) {
+        return FutureReturnCode::SUCCESS;
+      }
+      // If the original timeout is < 0, then this is blocking, never TIMEOUT.
+      if (timeout_ns < std::chrono::nanoseconds::zero()) {
+        continue;
+      }
+      // Otherwise check if we still have time to wait, return TIMEOUT if not.
+      auto now = std::chrono::steady_clock::now();
+      if (now >= end_time) {
+        return FutureReturnCode::TIMEOUT;
+      }
+      // Subtract the elapsed time from the original timeout.
+      timeout_left = std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - now);
+    }
+
+    // The future did not complete before ok() returned false, return INTERRUPTED.
+    return FutureReturnCode::INTERRUPTED;
   }
 
+  /// Cancel any running spin* function, causing it to return.
+  /* This function can be called asynchonously from any thread. */
+  RCLCPP_PUBLIC
+  void
+  cancel();
+
   /// Support dynamic switching of the memory strategy.
   /**
    * Switching the memory strategy while the executor is spinning in another threading could have
    * unintended consequences.
    * \param[in] memory_strategy Shared pointer to the memory strategy to set.
    */
+  RCLCPP_PUBLIC
   void
-  set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy)
-  {
-    if (memory_strategy == nullptr) {
-      throw std::runtime_error("Received NULL memory strategy in executor.");
-    }
-    memory_strategy_ = memory_strategy;
-  }
+  set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy);
 
 protected:
+  RCLCPP_PUBLIC
+  void
+  spin_node_once_nanoseconds(
+    rclcpp::node_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,
    * service, client).
    */
+  RCLCPP_PUBLIC
   void
-  execute_any_executable(AnyExecutable::SharedPtr any_exec)
-  {
-    if (!any_exec) {
-      return;
-    }
-    if (any_exec->timer) {
-      execute_timer(any_exec->timer);
-    }
-    if (any_exec->subscription) {
-      execute_subscription(any_exec->subscription);
-    }
-    if (any_exec->subscription_intra_process) {
-      execute_intra_process_subscription(any_exec->subscription_intra_process);
-    }
-    if (any_exec->service) {
-      execute_service(any_exec->service);
-    }
-    if (any_exec->client) {
-      execute_client(any_exec->client);
-    }
-    // Reset the callback_group, regardless of type
-    any_exec->callback_group->can_be_taken_from().store(true);
-    // Wake the wait, because it may need to be recalculated or work that
-    // was previously blocked is now available.
-    rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
-    if (status != RMW_RET_OK) {
-      throw std::runtime_error(rmw_get_error_string_safe());
-    }
-  }
+  execute_any_executable(AnyExecutable & any_exec);
 
+  RCLCPP_PUBLIC
   static void
   execute_subscription(
-    rclcpp::subscription::SubscriptionBase::SharedPtr subscription)
-  {
-    std::shared_ptr<void> message = subscription->create_message();
-    bool taken = false;
-    rmw_message_info_t message_info;
-    auto ret =
-      rmw_take_with_info(subscription->get_subscription_handle(),
-        message.get(), &taken, &message_info);
-    if (ret == RMW_RET_OK) {
-      if (taken) {
-        message_info.from_intra_process = false;
-        subscription->handle_message(message, message_info);
-      }
-    } else {
-      fprintf(stderr,
-        "[rclcpp::error] take failed for subscription on topic '%s': %s\n",
-        subscription->get_topic_name().c_str(), rmw_get_error_string_safe());
-    }
-    subscription->return_message(message);
-  }
+    rclcpp::SubscriptionBase::SharedPtr subscription);
 
+  RCLCPP_PUBLIC
   static void
   execute_intra_process_subscription(
-    rclcpp::subscription::SubscriptionBase::SharedPtr subscription)
-  {
-    rcl_interfaces::msg::IntraProcessMessage ipm;
-    bool taken = false;
-    rmw_message_info_t message_info;
-    rmw_ret_t status = rmw_take_with_info(
-      subscription->get_intra_process_subscription_handle(),
-      &ipm,
-      &taken,
-      &message_info);
-    if (status == RMW_RET_OK) {
-      if (taken) {
-        message_info.from_intra_process = true;
-        subscription->handle_intra_process_message(ipm, message_info);
-      }
-    } else {
-      fprintf(stderr,
-        "[rclcpp::error] take failed for intra process subscription on topic '%s': %s\n",
-        subscription->get_topic_name().c_str(), rmw_get_error_string_safe());
-    }
-  }
+    rclcpp::SubscriptionBase::SharedPtr subscription);
 
+  RCLCPP_PUBLIC
   static void
-  execute_timer(
-    rclcpp::timer::TimerBase::SharedPtr timer)
-  {
-    timer->execute_callback();
-  }
+  execute_timer(rclcpp::TimerBase::SharedPtr timer);
 
+  RCLCPP_PUBLIC
   static void
-  execute_service(
-    rclcpp::service::ServiceBase::SharedPtr service)
-  {
-    std::shared_ptr<void> request_header = service->create_request_header();
-    std::shared_ptr<void> request = service->create_request();
-    bool taken = false;
-    rmw_ret_t status = rmw_take_request(
-      service->get_service_handle(),
-      request_header.get(),
-      request.get(),
-      &taken);
-    if (status == RMW_RET_OK) {
-      if (taken) {
-        service->handle_request(request_header, request);
-      }
-    } else {
-      fprintf(stderr,
-        "[rclcpp::error] take request failed for server of service '%s': %s\n",
-        service->get_service_name().c_str(), rmw_get_error_string_safe());
-    }
-  }
+  execute_service(rclcpp::ServiceBase::SharedPtr service);
 
+  RCLCPP_PUBLIC
   static void
-  execute_client(
-    rclcpp::client::ClientBase::SharedPtr client)
-  {
-    std::shared_ptr<void> request_header = client->create_request_header();
-    std::shared_ptr<void> response = client->create_response();
-    bool taken = false;
-    rmw_ret_t status = rmw_take_response(
-      client->get_client_handle(),
-      request_header.get(),
-      response.get(),
-      &taken);
-    if (status == RMW_RET_OK) {
-      if (taken) {
-        client->handle_response(request_header, response);
-      }
-    } else {
-      fprintf(stderr,
-        "[rclcpp::error] take response failed for client of service '%s': %s\n",
-        client->get_service_name().c_str(), rmw_get_error_string_safe());
-    }
-  }
+  execute_client(rclcpp::ClientBase::SharedPtr client);
 
-/*** Populate class storage from stored weak node pointers and wait. ***/
-
-  template<typename T = std::milli>
+  RCLCPP_PUBLIC
   void
-  wait_for_work(std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
-  {
-    memory_strategy_->clear_active_entities();
+  wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
-    // Collect the subscriptions and timers to be waited on
-    bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_);
-
-    // Clean up any invalid nodes, if they were detected
-    if (has_invalid_weak_nodes) {
-      weak_nodes_.erase(
-        remove_if(weak_nodes_.begin(), weak_nodes_.end(),
-        [](std::weak_ptr<rclcpp::node::Node> i)
-          {
-            return i.expired();
-          }));
-    }
-
-    // Use the number of subscriptions to allocate memory in the handles
-    rmw_subscriptions_t subscriber_handles;
-    subscriber_handles.subscriber_count =
-      memory_strategy_->fill_subscriber_handles(subscriber_handles.subscribers);
-
-    rmw_services_t service_handles;
-    service_handles.service_count =
-      memory_strategy_->fill_service_handles(service_handles.services);
-
-    rmw_clients_t client_handles;
-    client_handles.client_count =
-      memory_strategy_->fill_client_handles(client_handles.clients);
-
-    // The number of guard conditions is fixed at 2: 1 for the ctrl-c guard cond,
-    // and one for the executor's guard cond (interrupt_guard_condition_)
-    size_t number_of_guard_conds = 2;
-    rmw_guard_conditions_t guard_condition_handles;
-    guard_condition_handles.guard_condition_count = number_of_guard_conds;
-    guard_condition_handles.guard_conditions = static_cast<void **>(guard_cond_handles_.data());
-    if (guard_condition_handles.guard_conditions == NULL &&
-      number_of_guard_conds > 0)
-    {
-      // TODO(wjwwood): Use a different error here? maybe std::bad_alloc?
-      throw std::runtime_error("Could not malloc for guard condition pointers.");
-    }
-    // Put the global ctrl-c guard condition in
-    assert(guard_condition_handles.guard_condition_count > 1);
-    guard_condition_handles.guard_conditions[0] = \
-      rclcpp::utilities::get_global_sigint_guard_condition()->data;
-    // Put the executor's guard condition in
-    guard_condition_handles.guard_conditions[1] = \
-      interrupt_guard_condition_->data;
-
-    rmw_time_t * wait_timeout = NULL;
-    rmw_time_t rmw_timeout;
-
-    auto next_timer_duration = get_earliest_timer();
-    // If the next timer timeout must preempt the requested timeout
-    // or if the requested timeout blocks forever, and there exists a valid timer,
-    // replace the requested timeout with the next timeout.
-    bool has_valid_timer = next_timer_duration >= std::chrono::nanoseconds::zero();
-    if ((next_timer_duration < timeout ||
-      timeout < std::chrono::duration<int64_t, T>::zero()) && has_valid_timer)
-    {
-      rmw_timeout.sec =
-        std::chrono::duration_cast<std::chrono::seconds>(next_timer_duration).count();
-      rmw_timeout.nsec = next_timer_duration.count() % (1000 * 1000 * 1000);
-      wait_timeout = &rmw_timeout;
-    } else if (timeout >= std::chrono::duration<int64_t, T>::zero()) {
-      // Convert timeout representation to rmw_time
-      rmw_timeout.sec = std::chrono::duration_cast<std::chrono::seconds>(timeout).count();
-      rmw_timeout.nsec = std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count() %
-        (1000 * 1000 * 1000);
-      wait_timeout = &rmw_timeout;
-    }
-
-    // Now wait on the waitable subscriptions and timers
-    rmw_ret_t status = rmw_wait(
-      &subscriber_handles,
-      &guard_condition_handles,
-      &service_handles,
-      &client_handles,
-      wait_timeout);
-    if (status != RMW_RET_OK && status != RMW_RET_TIMEOUT) {
-      throw std::runtime_error(rmw_get_error_string_safe());
-    }
-    // If ctrl-c guard condition, return directly
-    if (guard_condition_handles.guard_conditions[0] != 0) {
-      // Make sure to free or clean memory
-      memory_strategy_->clear_handles();
-      return;
-    }
-
-    memory_strategy_->revalidate_handles();
-  }
-
-/******************************/
-  rclcpp::node::Node::SharedPtr
-  get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group)
-  {
-    if (!group) {
-      return rclcpp::node::Node::SharedPtr();
-    }
-    for (auto & weak_node : weak_nodes_) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto callback_group = weak_group.lock();
-        if (callback_group == group) {
-          return node;
-        }
-      }
-    }
-    return rclcpp::node::Node::SharedPtr();
-  }
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
+  get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group);
 
+  RCLCPP_PUBLIC
   rclcpp::callback_group::CallbackGroup::SharedPtr
-  get_group_by_timer(
-    rclcpp::timer::TimerBase::SharedPtr timer)
-  {
-    for (auto & weak_node : weak_nodes_) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto group = weak_group.lock();
-        if (!group) {
-          continue;
-        }
-        for (auto & weak_timer : group->get_timer_ptrs()) {
-          auto t = weak_timer.lock();
-          if (t == timer) {
-            return group;
-          }
-        }
-      }
-    }
-    return rclcpp::callback_group::CallbackGroup::SharedPtr();
-  }
+  get_group_by_timer(rclcpp::TimerBase::SharedPtr timer);
 
+  RCLCPP_PUBLIC
   void
-  get_next_timer(AnyExecutable::SharedPtr any_exec)
-  {
-    for (auto & weak_node : weak_nodes_) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto group = weak_group.lock();
-        if (!group || !group->can_be_taken_from().load()) {
-          continue;
-        }
-        for (auto & timer_ref : group->get_timer_ptrs()) {
-          auto timer = timer_ref.lock();
-          if (timer && timer->check_and_trigger()) {
-            any_exec->timer = timer;
-            any_exec->callback_group = group;
-            node = get_node_by_group(group);
-            return;
-          }
-        }
-      }
-    }
-  }
+  get_next_timer(AnyExecutable & any_exec);
 
-  std::chrono::nanoseconds
-  get_earliest_timer()
-  {
-    std::chrono::nanoseconds latest = std::chrono::nanoseconds::max();
-    bool timers_empty = true;
-    for (auto & weak_node : weak_nodes_) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto group = weak_group.lock();
-        if (!group || !group->can_be_taken_from().load()) {
-          continue;
-        }
-        for (auto & timer_ref : group->get_timer_ptrs()) {
-          timers_empty = false;
-          // Check the expected trigger time
-          auto timer = timer_ref.lock();
-          if (timer && timer->time_until_trigger() < latest) {
-            latest = timer->time_until_trigger();
-          }
-        }
-      }
-    }
-    if (timers_empty) {
-      return std::chrono::nanoseconds(-1);
-    }
-    return latest;
-  }
+  RCLCPP_PUBLIC
+  bool
+  get_next_ready_executable(AnyExecutable & any_executable);
 
-  AnyExecutable::SharedPtr
-  get_next_ready_executable()
-  {
-    auto any_exec = memory_strategy_->instantiate_next_executable();
-    // Check the timers to see if there are any that are ready, if so return
-    get_next_timer(any_exec);
-    if (any_exec->timer) {
-      return any_exec;
-    }
-    // Check the subscriptions to see if there are any that are ready
-    memory_strategy_->get_next_subscription(any_exec, weak_nodes_);
-    if (any_exec->subscription || any_exec->subscription_intra_process) {
-      return any_exec;
-    }
-    // Check the services to see if there are any that are ready
-    memory_strategy_->get_next_service(any_exec, weak_nodes_);
-    if (any_exec->service) {
-      return any_exec;
-    }
-    // Check the clients to see if there are any that are ready
-    memory_strategy_->get_next_client(any_exec, weak_nodes_);
-    if (any_exec->client) {
-      return any_exec;
-    }
-    // If there is no ready executable, return a null ptr
-    return nullptr;
-  }
+  RCLCPP_PUBLIC
+  bool
+  get_next_executable(
+    AnyExecutable & any_executable,
+    std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
-  template<typename T = std::milli>
-  AnyExecutable::SharedPtr
-  get_next_executable(std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t,
-    T>(-1))
-  {
-    // Check to see if there are any subscriptions or timers needing service
-    // TODO(wjwwood): improve run to run efficiency of this function
-    auto any_exec = get_next_ready_executable();
-    // If there are none
-    if (!any_exec) {
-      // Wait for subscriptions or timers to work on
-      wait_for_work(timeout);
-      // Try again
-      any_exec = get_next_ready_executable();
-    }
-    // At this point any_exec should be valid with either a valid subscription
-    // or a valid timer, or it should be a null shared_ptr
-    if (any_exec) {
-      // If it is valid, check to see if the group is mutually exclusive or
-      // not, then mark it accordingly
-      if (any_exec->callback_group && any_exec->callback_group->type() == \
-        callback_group::CallbackGroupType::MutuallyExclusive)
-      {
-        // It should not have been taken otherwise
-        assert(any_exec->callback_group->can_be_taken_from().load());
-        // Set to false to indicate something is being run from this group
-        any_exec->callback_group->can_be_taken_from().store(false);
-      }
-    }
-    return any_exec;
-  }
+  /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins.
+  std::atomic_bool spinning;
 
   /// Guard condition for signaling the rmw layer to wake up for special events.
-  rmw_guard_condition_t * interrupt_guard_condition_;
+  rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
+
+  /// Wait set for managing entities that the rmw layer waits on.
+  rcl_wait_set_t wait_set_ = 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);
+  /// The context associated with this executor.
+  std::shared_ptr<rclcpp::Context> context_;
 
-  std::vector<std::weak_ptr<rclcpp::node::Node>> weak_nodes_;
-  std::array<void *, 2> guard_cond_handles_;
+private:
+  RCLCPP_DISABLE_COPY(Executor)
+
+  std::vector<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
 };
 
 }  // namespace executor

rclcpp/include/rclcpp/executors.hpp

@@ -12,76 +12,113 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_EXECUTORS_HPP_
-#define RCLCPP_RCLCPP_EXECUTORS_HPP_
+#ifndef RCLCPP__EXECUTORS_HPP_
+#define RCLCPP__EXECUTORS_HPP_
 
 #include <future>
-#include <rclcpp/executors/multi_threaded_executor.hpp>
-#include <rclcpp/executors/single_threaded_executor.hpp>
-#include <rclcpp/node.hpp>
-#include <rclcpp/utilities.hpp>
+#include <memory>
+
+#include "rclcpp/executors/multi_threaded_executor.hpp"
+#include "rclcpp/executors/single_threaded_executor.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/utilities.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
+
+/// Create a default single-threaded executor and execute any immediately available work.
+/** \param[in] node_ptr Shared pointer to the node to spin. */
+RCLCPP_PUBLIC
+void
+spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+RCLCPP_PUBLIC
+void
+spin_some(rclcpp::Node::SharedPtr node_ptr);
+
+/// Create a default single-threaded executor and spin the specified node.
+/** \param[in] node_ptr Shared pointer to the node to spin. */
+RCLCPP_PUBLIC
+void
+spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+RCLCPP_PUBLIC
+void
+spin(rclcpp::Node::SharedPtr node_ptr);
+
 namespace executors
 {
 
-using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor;
-using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor;
+using rclcpp::executors::MultiThreadedExecutor;
+using rclcpp::executors::SingleThreadedExecutor;
 
-/// Return codes to be used with spin_until_future_complete.
-/**
- * SUCCESS: The future is complete and can be accessed with "get" without blocking.
- * INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error.
- * TIMEOUT: Spinning timed out.
- */
-enum FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT};
-
-/// Spin (blocking) until the future is complete, until the function times out (if applicable),
-/// or until rclcpp is interrupted.
+/// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
 /**
  * \param[in] executor The executor which will spin the node.
  * \param[in] node_ptr The node to spin.
- * \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>
-FutureReturnCode
+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))
 {
-  // TODO(wjwwood): does not work recursively right, can't call spin_node_until_future_complete
+  // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
   // inside a callback executed by an executor.
-
-  // Check the future before entering the while loop.
-  // If the future is already complete, don't try to spin.
-  std::future_status status = future.wait_for(std::chrono::seconds(0));
-
-  auto start_time = std::chrono::system_clock::now();
-
-  while (status != std::future_status::ready && rclcpp::utilities::ok()) {
-    executor.spin_node_once(node_ptr, timeout);
-    if (timeout.count() >= 0) {
-      if (start_time + timeout < std::chrono::system_clock::now()) {
-        return TIMEOUT;
-      }
-    }
-    status = future.wait_for(std::chrono::seconds(0));
-  }
-
-  // If the future completed, and we weren't interrupted by ctrl-C, return the response
-  if (status == std::future_status::ready) {
-    return FutureReturnCode::SUCCESS;
-  }
-  return FutureReturnCode::INTERRUPTED;
+  executor.add_node(node_ptr);
+  auto retcode = executor.spin_until_future_complete(future, timeout);
+  executor.remove_node(node_ptr);
+  return retcode;
 }
 
-} // namespace executors
-} // namespace rclcpp
+template<typename NodeT = rclcpp::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);
+}
 
-#endif /* RCLCPP_RCLCPP_EXECUTORS_HPP_ */
+}  // namespace executors
+
+template<typename FutureT, typename TimeT = std::milli>
+rclcpp::executor::FutureReturnCode
+spin_until_future_complete(
+  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, 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

@@ -15,91 +15,73 @@
 #ifndef RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
 #define RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
 
-#include <rmw/rmw.h>
-
-#include <cassert>
-#include <cstdlib>
 #include <memory>
 #include <mutex>
-#include <vector>
+#include <set>
+#include <thread>
+#include <unordered_map>
 
 #include "rclcpp/executor.hpp"
 #include "rclcpp/macros.hpp"
-#include "rclcpp/node.hpp"
-#include "rclcpp/utilities.hpp"
+#include "rclcpp/memory_strategies.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
 namespace executors
 {
-namespace multi_threaded_executor
-{
 
 class MultiThreadedExecutor : public executor::Executor
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(MultiThreadedExecutor);
+  RCLCPP_SMART_PTR_DEFINITIONS(MultiThreadedExecutor)
 
-  MultiThreadedExecutor(memory_strategy::MemoryStrategy::SharedPtr ms =
-    memory_strategies::create_default_strategy())
-  : executor::Executor(ms)
-  {
-    number_of_threads_ = std::thread::hardware_concurrency();
-    if (number_of_threads_ == 0) {
-      number_of_threads_ = 1;
-    }
-  }
+  /// Constructor for MultiThreadedExecutor.
+  /**
+   * For the yield_before_execute option, when true std::this_thread::yield()
+   * will be called after acquiring work (as an AnyExecutable) and
+   * releasing the spinning lock, but before executing the work.
+   * This is useful for reproducing some bugs related to taking work more than
+   * once.
+   *
+   * \param args common arguments for all executors
+   * \param number_of_threads number of threads to have in the thread pool,
+   *   the default 0 will use the number of cpu cores found instead
+   * \param yield_before_execute if true std::this_thread::yield() is called
+   */
+  RCLCPP_PUBLIC
+  MultiThreadedExecutor(
+    const executor::ExecutorArgs & args = executor::ExecutorArgs(),
+    size_t number_of_threads = 0,
+    bool yield_before_execute = false);
 
-  virtual ~MultiThreadedExecutor() {}
+  RCLCPP_PUBLIC
+  virtual ~MultiThreadedExecutor();
 
+  RCLCPP_PUBLIC
   void
-  spin()
-  {
-    std::vector<std::thread> threads;
-    {
-      std::lock_guard<std::mutex> wait_lock(wait_mutex_);
-      size_t thread_id_ = 1;  // Use a _ suffix to avoid shadowing `rclcpp::thread_id`
-      for (size_t i = number_of_threads_; i > 0; --i) {
-        std::this_thread::sleep_for(std::chrono::milliseconds(100));
-        auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id_++);
-        threads.emplace_back(func);
-      }
-    }
-    for (auto & thread : threads) {
-      thread.join();
-    }
-  }
+  spin();
 
+  RCLCPP_PUBLIC
   size_t
-  get_number_of_threads()
-  {
-    return number_of_threads_;
-  }
+  get_number_of_threads();
+
+protected:
+  RCLCPP_PUBLIC
+  void
+  run(size_t this_thread_number);
 
 private:
-  void run(size_t this_thread_id)
-  {
-    rclcpp::thread_id = this_thread_id;
-    while (rclcpp::utilities::ok()) {
-      executor::AnyExecutable::SharedPtr any_exec;
-      {
-        std::lock_guard<std::mutex> wait_lock(wait_mutex_);
-        if (!rclcpp::utilities::ok()) {
-          return;
-        }
-        any_exec = get_next_executable();
-      }
-      execute_any_executable(any_exec);
-    }
-  }
-
-  RCLCPP_DISABLE_COPY(MultiThreadedExecutor);
+  RCLCPP_DISABLE_COPY(MultiThreadedExecutor)
 
   std::mutex wait_mutex_;
   size_t number_of_threads_;
+  bool yield_before_execute_;
+
+  std::mutex scheduled_timers_mutex_;
+  std::set<TimerBase::SharedPtr> scheduled_timers_;
 };
 
-}  // namespace multi_threaded_executor
 }  // namespace executors
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/executors/single_threaded_executor.hpp

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

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

@@ -12,13 +12,19 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_FUNCTION_TRAITS_HPP_
-#define RCLCPP_RCLCPP_FUNCTION_TRAITS_HPP_
+#ifndef RCLCPP__FUNCTION_TRAITS_HPP_
+#define RCLCPP__FUNCTION_TRAITS_HPP_
+
+#include <functional>
 #include <memory>
+#include <tuple>
 
 namespace rclcpp
 {
 
+namespace function_traits
+{
+
 /* NOTE(esteve):
  * We support service callbacks that can optionally take the request id,
  * which should be possible with two overloaded create_service methods,
@@ -43,12 +49,14 @@ template<typename FunctionT>
 struct function_traits
 {
   using arguments = typename tuple_tail<
-      typename function_traits<decltype( & FunctionT::operator())>::arguments>::type;
+    typename function_traits<decltype( &FunctionT::operator())>::arguments>::type;
 
   static constexpr std::size_t arity = std::tuple_size<arguments>::value;
 
   template<std::size_t N>
   using argument_type = typename std::tuple_element<N, arguments>::type;
+
+  using return_type = typename function_traits<decltype( &FunctionT::operator())>::return_type;
 };
 
 // Free functions
@@ -61,17 +69,59 @@ struct function_traits<ReturnTypeT(Args ...)>
 
   template<std::size_t N>
   using argument_type = typename std::tuple_element<N, arguments>::type;
+
+  using return_type = ReturnTypeT;
 };
 
 // Function pointers
 template<typename ReturnTypeT, typename ... Args>
-struct function_traits<ReturnTypeT (*)(Args ...)>: public function_traits<ReturnTypeT(Args ...)>
+struct function_traits<ReturnTypeT (*)(Args ...)>: function_traits<ReturnTypeT(Args ...)>
+{};
+
+// std::bind for object methods
+template<typename ClassT, typename ReturnTypeT, typename ... Args, typename ... FArgs>
+#if defined _LIBCPP_VERSION  // libc++ (Clang)
+struct function_traits<std::__bind<ReturnTypeT (ClassT::*)(Args ...), FArgs ...>>
+#elif defined _GLIBCXX_RELEASE  // glibc++ (GNU C++ >= 7.1)
+struct function_traits<std::_Bind<ReturnTypeT(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
+struct function_traits<
+  std::_Binder<std::_Unforced, ReturnTypeT(__cdecl ClassT::*)(Args ...), FArgs ...>
+>
+#else
+#error "Unsupported C++ compiler / standard library"
+#endif
+  : function_traits<ReturnTypeT(Args ...)>
+{};
+
+// std::bind for free functions
+template<typename ReturnTypeT, typename ... Args, typename ... FArgs>
+#if defined _LIBCPP_VERSION  // libc++ (Clang)
+struct function_traits<std::__bind<ReturnTypeT( &)(Args ...), FArgs ...>>
+#elif defined __GLIBCXX__  // glibc++ (GNU C++)
+struct function_traits<std::_Bind<ReturnTypeT(*(FArgs ...))(Args ...)>>
+#elif defined _MSC_VER  // MS Visual Studio
+struct function_traits<std::_Binder<std::_Unforced, ReturnTypeT(__cdecl &)(Args ...), FArgs ...>>
+#else
+#error "Unsupported C++ compiler / standard library"
+#endif
+  : function_traits<ReturnTypeT(Args ...)>
 {};
 
 // Lambdas
 template<typename ClassT, typename ReturnTypeT, typename ... Args>
 struct function_traits<ReturnTypeT (ClassT::*)(Args ...) const>
-  : public function_traits<ReturnTypeT(ClassT &, Args ...)>
+  : function_traits<ReturnTypeT(ClassT &, Args ...)>
+{};
+
+template<typename FunctionT>
+struct function_traits<FunctionT &>: function_traits<FunctionT>
+{};
+
+template<typename FunctionT>
+struct function_traits<FunctionT &&>: function_traits<FunctionT>
 {};
 
 /* NOTE(esteve):
@@ -80,21 +130,24 @@ struct function_traits<ReturnTypeT (ClassT::*)(Args ...) const>
  */
 template<std::size_t Arity, typename FunctorT>
 struct arity_comparator : std::integral_constant<
-    bool, (Arity == function_traits<FunctorT>::arity)>{};
+    bool, (Arity == function_traits<FunctorT>::arity)> {};
 
 template<typename FunctorT, typename ... Args>
 struct check_arguments : std::is_same<
     typename function_traits<FunctorT>::arguments,
     std::tuple<Args ...>
-  >
+>
 {};
 
 template<typename FunctorAT, typename FunctorBT>
 struct same_arguments : std::is_same<
     typename function_traits<FunctorAT>::arguments,
     typename function_traits<FunctorBT>::arguments
-  >
+>
 {};
-} /* namespace rclcpp */
 
-#endif /* RCLCPP_RCLCPP_FUNCTION_TRAITS_HPP_ */
+}  // namespace function_traits
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__FUNCTION_TRAITS_HPP_

rclcpp/include/rclcpp/graph_listener.hpp

@@ -0,0 +1,188 @@
+// 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/context.hpp"
+#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
+  explicit GraphListener(std::shared_ptr<rclcpp::Context> parent_context);
+
+  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();
+
+  /// Nothrow version of shutdown(), logs to RCLCPP_ERROR instead.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  shutdown(const std::nothrow_t &) noexcept;
+
+  /// 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)
+
+  /** \internal */
+  void
+  __shutdown(bool);
+
+  rclcpp::Context::SharedPtr parent_context_;
+
+  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();
+  std::shared_ptr<rcl_context_t> interrupt_guard_condition_context_;
+  rcl_guard_condition_t * shutdown_guard_condition_;
+  rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
+};
+
+}  // namespace graph_listener
+}  // namespace rclcpp
+
+#endif  // RCLCPP__GRAPH_LISTENER_HPP_

rclcpp/include/rclcpp/init_options.hpp

@@ -0,0 +1,69 @@
+// Copyright 2018 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__INIT_OPTIONS_HPP_
+#define RCLCPP__INIT_OPTIONS_HPP_
+
+#include <memory>
+
+#include "rcl/init_options.h"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Encapsulation of options for initializing rclcpp.
+class InitOptions
+{
+public:
+  /// If true, the context will be shutdown on SIGINT by the signal handler (if it was installed).
+  bool shutdown_on_sigint = true;
+
+  /// Constructor which allows you to specify the allocator used within the init options.
+  RCLCPP_PUBLIC
+  explicit InitOptions(rcl_allocator_t allocator = rcl_get_default_allocator());
+
+  /// Constructor which is initialized by an existing init_options.
+  RCLCPP_PUBLIC
+  explicit InitOptions(const rcl_init_options_t & init_options);
+
+  /// Copy constructor.
+  RCLCPP_PUBLIC
+  InitOptions(const InitOptions & other);
+
+  /// Assignment operator.
+  RCLCPP_PUBLIC
+  InitOptions &
+  operator=(const InitOptions & other);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~InitOptions();
+
+  /// Return the rcl init options.
+  RCLCPP_PUBLIC
+  const rcl_init_options_t *
+  get_rcl_init_options() const;
+
+protected:
+  void
+  finalize_init_options();
+
+private:
+  std::unique_ptr<rcl_init_options_t> init_options_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__INIT_OPTIONS_HPP_

rclcpp/include/rclcpp/intra_process_manager.hpp

@@ -23,15 +23,17 @@
 #include <exception>
 #include <map>
 #include <memory>
+#include <mutex>
 #include <unordered_map>
 #include <set>
 
 #include "rclcpp/allocator/allocator_deleter.hpp"
-#include "rclcpp/intra_process_manager_state.hpp"
+#include "rclcpp/intra_process_manager_impl.hpp"
 #include "rclcpp/mapped_ring_buffer.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/publisher.hpp"
 #include "rclcpp/subscription.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
@@ -39,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
@@ -53,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
@@ -119,20 +122,21 @@ 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(
-    IntraProcessManagerStateBase::SharedPtr state = create_default_state()
-  )
-  : state_(state)
-  {
-  }
+    IntraProcessManagerImplBase::SharedPtr state = create_default_impl());
+
+  RCLCPP_PUBLIC
+  virtual ~IntraProcessManager();
 
   /// Register a subscription with the manager, returns subscriptions unique id.
-  /* In addition to generating a unique intra process id for the subscription,
+  /**
+   * 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,
@@ -143,27 +147,23 @@ public:
    * \param subscription the Subscription to register.
    * \return an unsigned 64-bit integer which is the subscription's unique id.
    */
+  RCLCPP_PUBLIC
   uint64_t
-  add_subscription(subscription::SubscriptionBase::SharedPtr subscription)
-  {
-    auto id = IntraProcessManager::get_next_unique_id();
-    state_->add_subscription(id, subscription);
-    return id;
-  }
+  add_subscription(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.
    */
+  RCLCPP_PUBLIC
   void
-  remove_subscription(uint64_t intra_process_subscription_id)
-  {
-    state_->remove_subscription(intra_process_subscription_id);
-  }
+  remove_subscription(uint64_t intra_process_subscription_id);
 
   /// Register a publisher with the manager, returns the publisher unique id.
-  /* In addition to generating and returning a unique id for the publisher,
+  /**
+   * 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.
@@ -186,31 +186,33 @@ public:
    */
   template<typename MessageT, typename Alloc>
   uint64_t
-  add_publisher(typename publisher::Publisher<MessageT, Alloc>::SharedPtr publisher,
+  add_publisher(
+    typename Publisher<MessageT, Alloc>::SharedPtr publisher,
     size_t buffer_size = 0)
   {
     auto id = IntraProcessManager::get_next_unique_id();
     size_t size = buffer_size > 0 ? buffer_size : publisher->get_queue_size();
-    auto mrb = mapped_ring_buffer::MappedRingBuffer<MessageT,
-      typename publisher::Publisher<MessageT, Alloc>::MessageAlloc>::make_shared(
-      size, publisher->get_allocator());
-    state_->add_publisher(id, publisher, mrb, size);
+    auto mrb = mapped_ring_buffer::MappedRingBuffer<
+      MessageT,
+      typename Publisher<MessageT, Alloc>::MessageAlloc
+      >::make_shared(size, publisher->get_allocator());
+    impl_->add_publisher(id, publisher, mrb, size);
     return id;
   }
 
   /// 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.
    */
+  RCLCPP_PUBLIC
   void
-  remove_publisher(uint64_t intra_process_publisher_id)
-  {
-    state_->remove_publisher(intra_process_publisher_id);
-  }
+  remove_publisher(uint64_t intra_process_publisher_id);
 
   /// Store a message in the manager, and return the message sequence number.
-  /* The given message is stored in internal storage using the given publisher
+  /**
+   * 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
@@ -239,8 +241,9 @@ public:
    * \param message the message that is being stored.
    * \return the message sequence number.
    */
-  template<typename MessageT, typename Alloc = std::allocator<void>,
-  typename Deleter = std::default_delete<MessageT>>
+  template<
+    typename MessageT, typename Alloc = std::allocator<void>,
+    typename Deleter = std::default_delete<MessageT>>
   uint64_t
   store_intra_process_message(
     uint64_t intra_process_publisher_id,
@@ -249,7 +252,7 @@ public:
     using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
     using TypedMRB = typename mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc>;
     uint64_t message_seq = 0;
-    mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = state_->get_publisher_info_for_id(
+    mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = impl_->get_publisher_info_for_id(
       intra_process_publisher_id, message_seq);
     typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(buffer);
     if (!typed_buffer) {
@@ -261,14 +264,15 @@ public:
     // TODO(wjwwood): do something when a message was displaced. log debug?
     (void)did_replace;  // Avoid unused variable warning.
 
-    state_->store_intra_process_message(intra_process_publisher_id, message_seq);
+    impl_->store_intra_process_message(intra_process_publisher_id, message_seq);
 
     // Return the message sequence which is sent to the subscription.
     return message_seq;
   }
 
   /// 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
@@ -301,8 +305,9 @@ public:
    * \param requesting_subscriptions_intra_process_id the subscription's id.
    * \param message the message typed unique_ptr used to return the message.
    */
-  template<typename MessageT, typename Alloc = std::allocator<void>,
-  typename Deleter = std::default_delete<MessageT>>
+  template<
+    typename MessageT, typename Alloc = std::allocator<void>,
+    typename Deleter = std::default_delete<MessageT>>
   void
   take_intra_process_message(
     uint64_t intra_process_publisher_id,
@@ -315,12 +320,13 @@ public:
     message = nullptr;
 
     size_t target_subs_size = 0;
-    mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = state_->take_intra_process_message(
+    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,
       requesting_subscriptions_intra_process_id,
       target_subs_size
-      );
+    );
     typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(buffer);
     if (!typed_buffer) {
       return;
@@ -336,41 +342,19 @@ public:
   }
 
   /// Return true if the given rmw_gid_t matches any stored Publishers.
+  RCLCPP_PUBLIC
   bool
-  matches_any_publishers(const rmw_gid_t * id) const
-  {
-    return state_->matches_any_publishers(id);
-  }
+  matches_any_publishers(const rmw_gid_t * id) const;
 
 private:
-  static uint64_t get_next_unique_id()
-  {
-    auto next_id = next_unique_id_.fetch_add(1, std::memory_order_relaxed);
-    // Check for rollover (we started at 1).
-    if (0 == next_id) {
-      // This puts a technical limit on the number of times you can add a publisher or subscriber.
-      // But even if you could add (and remove) them at 1 kHz (very optimistic rate)
-      // it would still be a very long time before you could exhaust the pool of id's:
-      //   2^64 / 1000 times per sec / 60 sec / 60 min / 24 hours / 365 days = 584,942,417 years
-      // So around 585 million years. Even at 1 GHz, it would take 585 years.
-      // I think it's safe to avoid trying to handle overflow.
-      // If we roll over then it's most likely a bug.
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-      throw std::overflow_error(
-        "exhausted the unique id's for publishers and subscribers in this process "
-        "(congratulations your computer is either extremely fast or extremely old)");
-      // *INDENT-ON*
-    }
-    return next_id;
-  }
+  RCLCPP_PUBLIC
+  static uint64_t
+  get_next_unique_id();
 
-  static std::atomic<uint64_t> next_unique_id_;
-
-  IntraProcessManagerStateBase::SharedPtr state_;
+  IntraProcessManagerImplBase::SharedPtr impl_;
+  std::mutex take_mutex_;
 };
 
-std::atomic<uint64_t> IntraProcessManager::next_unique_id_ {1};
-
 }  // namespace intra_process_manager
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/intra_process_manager_impl.hpp

@@ -12,42 +12,50 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_
-#define RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_
-
-#include <rclcpp/mapped_ring_buffer.hpp>
-#include <rclcpp/publisher.hpp>
-#include <rclcpp/subscription.hpp>
+#ifndef RCLCPP__INTRA_PROCESS_MANAGER_IMPL_HPP_
+#define RCLCPP__INTRA_PROCESS_MANAGER_IMPL_HPP_
 
 #include <algorithm>
 #include <atomic>
+#include <cstring>
 #include <functional>
 #include <limits>
-#include <memory>
 #include <map>
-#include <unordered_map>
+#include <memory>
+#include <mutex>
 #include <set>
 #include <string>
+#include <unordered_map>
 #include <utility>
 
+#include "rclcpp/macros.hpp"
+#include "rclcpp/mapped_ring_buffer.hpp"
+#include "rclcpp/publisher.hpp"
+#include "rclcpp/subscription.hpp"
+#include "rclcpp/visibility_control.hpp"
+
 namespace rclcpp
 {
 namespace intra_process_manager
 {
 
-class IntraProcessManagerStateBase
+class IntraProcessManagerImplBase
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(IntraProcessManagerStateBase);
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(IntraProcessManagerImplBase)
+
+  IntraProcessManagerImplBase() = default;
+  virtual ~IntraProcessManagerImplBase() = default;
 
   virtual void
-  add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription) = 0;
+  add_subscription(uint64_t id, SubscriptionBase::SharedPtr subscription) = 0;
 
   virtual void
   remove_subscription(uint64_t intra_process_subscription_id) = 0;
 
-  virtual void add_publisher(uint64_t id,
-    publisher::PublisherBase::WeakPtr publisher,
+  virtual void add_publisher(
+    uint64_t id,
+    PublisherBase::WeakPtr publisher,
     mapped_ring_buffer::MappedRingBufferBase::SharedPtr mrb,
     size_t size) = 0;
 
@@ -63,23 +71,32 @@ public:
   store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq) = 0;
 
   virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr
-  take_intra_process_message(uint64_t intra_process_publisher_id,
+  take_intra_process_message(
+    uint64_t intra_process_publisher_id,
     uint64_t message_sequence_number,
     uint64_t requesting_subscriptions_intra_process_id,
     size_t & size) = 0;
 
   virtual bool
   matches_any_publishers(const rmw_gid_t * id) const = 0;
+
+private:
+  RCLCPP_DISABLE_COPY(IntraProcessManagerImplBase)
 };
 
 template<typename Allocator = std::allocator<void>>
-class IntraProcessManagerState : public IntraProcessManagerStateBase
+class IntraProcessManagerImpl : public IntraProcessManagerImplBase
 {
 public:
+  IntraProcessManagerImpl() = default;
+  ~IntraProcessManagerImpl() = default;
+
   void
-  add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription)
+  add_subscription(uint64_t id, 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);
   }
 
@@ -99,8 +116,9 @@ public:
     }
   }
 
-  void add_publisher(uint64_t id,
-    publisher::PublisherBase::WeakPtr publisher,
+  void add_publisher(
+    uint64_t id,
+    PublisherBase::WeakPtr publisher,
     mapped_ring_buffer::MappedRingBufferBase::SharedPtr mrb,
     size_t size)
   {
@@ -127,13 +145,14 @@ public:
     uint64_t intra_process_publisher_id,
     uint64_t & message_seq)
   {
+    std::lock_guard<std::mutex> lock(runtime_mutex_);
     auto it = publishers_.find(intra_process_publisher_id);
     if (it == publishers_.end()) {
-      throw std::runtime_error("store_intra_process_message called with invalid publisher id");
+      throw std::runtime_error("get_publisher_info_for_id called with invalid publisher id");
     }
     PublisherInfo & info = it->second;
     // Calculate the next message sequence number.
-    message_seq = info.sequence_number.fetch_add(1, std::memory_order_relaxed);
+    message_seq = info.sequence_number.fetch_add(1);
 
     return info.buffer;
   }
@@ -141,6 +160,7 @@ public:
   void
   store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq)
   {
+    std::lock_guard<std::mutex> lock(runtime_mutex_);
     auto it = publishers_.find(intra_process_publisher_id);
     if (it == publishers_.end()) {
       throw std::runtime_error("store_intra_process_message called with invalid publisher id");
@@ -154,7 +174,12 @@ public:
     // Figure out what subscriptions should receive the message.
     auto & destined_subscriptions = subscription_ids_by_topic_[publisher->get_topic_name()];
     // Store the list for later comparison.
-    info.target_subscriptions_by_message_sequence[message_seq].clear();
+    if (info.target_subscriptions_by_message_sequence.count(message_seq) == 0) {
+      info.target_subscriptions_by_message_sequence.emplace(
+        message_seq, AllocSet(std::less<uint64_t>(), uint64_allocator));
+    } else {
+      info.target_subscriptions_by_message_sequence[message_seq].clear();
+    }
     std::copy(
       destined_subscriptions.begin(), destined_subscriptions.end(),
       // Memory allocation occurs in info.target_subscriptions_by_message_sequence[message_seq]
@@ -167,12 +192,14 @@ public:
   }
 
   mapped_ring_buffer::MappedRingBufferBase::SharedPtr
-  take_intra_process_message(uint64_t intra_process_publisher_id,
+  take_intra_process_message(
+    uint64_t intra_process_publisher_id,
     uint64_t message_sequence_number,
     uint64_t requesting_subscriptions_intra_process_id,
     size_t & size
   )
   {
+    std::lock_guard<std::mutex> lock(runtime_mutex_);
     PublisherInfo * info;
     {
       auto it = publishers_.find(intra_process_publisher_id);
@@ -222,15 +249,32 @@ public:
   }
 
 private:
+  RCLCPP_DISABLE_COPY(IntraProcessManagerImpl)
+
   template<typename T>
   using RebindAlloc = typename std::allocator_traits<Allocator>::template rebind_alloc<T>;
 
+  RebindAlloc<uint64_t> uint64_allocator;
+
   using AllocSet = std::set<uint64_t, std::less<uint64_t>, RebindAlloc<uint64_t>>;
-  using SubscriptionMap = std::unordered_map<uint64_t, subscription::SubscriptionBase::WeakPtr,
-      std::hash<uint64_t>, std::equal_to<uint64_t>,
-      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>>>;
+  using SubscriptionMap = std::unordered_map<
+    uint64_t, SubscriptionBase::WeakPtr,
+    std::hash<uint64_t>, std::equal_to<uint64_t>,
+    RebindAlloc<std::pair<const uint64_t, SubscriptionBase::WeakPtr>>>;
+
+  struct strcmp_wrapper
+  {
+    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 char * const, AllocSet>>>;
 
   SubscriptionMap subscriptions_;
 
@@ -238,33 +282,36 @@ private:
 
   struct PublisherInfo
   {
-    RCLCPP_DISABLE_COPY(PublisherInfo);
+    RCLCPP_DISABLE_COPY(PublisherInfo)
 
     PublisherInfo() = default;
 
-    publisher::PublisherBase::WeakPtr publisher;
+    PublisherBase::WeakPtr publisher;
     std::atomic<uint64_t> sequence_number;
     mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer;
 
-    using TargetSubscriptionsMap = std::unordered_map<uint64_t, AllocSet,
-        std::hash<uint64_t>, std::equal_to<uint64_t>,
-        RebindAlloc<std::pair<const uint64_t, AllocSet>>>;
+    using TargetSubscriptionsMap = std::unordered_map<
+      uint64_t, AllocSet,
+      std::hash<uint64_t>, std::equal_to<uint64_t>,
+      RebindAlloc<std::pair<const uint64_t, AllocSet>>>;
     TargetSubscriptionsMap target_subscriptions_by_message_sequence;
   };
 
-  using PublisherMap = std::unordered_map<uint64_t, PublisherInfo,
-      std::hash<uint64_t>, std::equal_to<uint64_t>,
-      RebindAlloc<std::pair<const uint64_t, PublisherInfo>>>;
+  using PublisherMap = std::unordered_map<
+    uint64_t, PublisherInfo,
+    std::hash<uint64_t>, std::equal_to<uint64_t>,
+    RebindAlloc<std::pair<const uint64_t, PublisherInfo>>>;
 
   PublisherMap publishers_;
+
+  std::mutex runtime_mutex_;
 };
 
-static IntraProcessManagerStateBase::SharedPtr create_default_state()
-{
-  return std::make_shared<IntraProcessManagerState<>>();
-}
+RCLCPP_PUBLIC
+IntraProcessManagerImplBase::SharedPtr
+create_default_impl();
 
 }  // namespace intra_process_manager
 }  // namespace rclcpp
 
-#endif  // RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_
+#endif  // RCLCPP__INTRA_PROCESS_MANAGER_IMPL_HPP_

rclcpp/include/rclcpp/logger.hpp

@@ -0,0 +1,145 @@
+// 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__LOGGER_HPP_
+#define RCLCPP__LOGGER_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcl/node.h"
+
+/**
+ * \def RCLCPP_LOGGING_ENABLED
+ * When this define evaluates to true (default), logger factory functions will
+ * behave normally.
+ * When false, logger factory functions will create dummy loggers to avoid
+ * computational expense in manipulating objects.
+ * This should be used in combination with `RCLCPP_LOG_MIN_SEVERITY` to compile
+ * out logging macros.
+ */
+// TODO(dhood): determine this automatically from `RCLCPP_LOG_MIN_SEVERITY`
+#ifndef RCLCPP_LOGGING_ENABLED
+#define RCLCPP_LOGGING_ENABLED 1
+#endif
+
+namespace rclcpp
+{
+
+// Forward declaration is used for friend statement.
+namespace node_interfaces
+{
+class NodeLogging;
+}
+
+class Logger;
+
+/// Return a named logger.
+/**
+ * The returned logger's name will include any naming conventions, such as a
+ * name prefix.
+ * Currently there are no such naming conventions but they may be introduced in
+ * the future.
+ *
+ * \param[in] name the name of the logger
+ * \return a logger with the fully-qualified name including naming conventions, or
+ * \return a dummy logger if logging is disabled.
+ */
+RCLCPP_PUBLIC
+Logger
+get_logger(const std::string & name);
+
+/// Return a named logger using an rcl_node_t.
+/**
+ * This is a convenience function that does error checking and returns the node
+ * logger name, or "rclcpp" if it is unable to get the node name.
+ *
+ * \param[in] node the rcl node from which to get the logger name
+ * \return a logger based on the node name, or "rclcpp" if there's an error
+ */
+RCLCPP_PUBLIC
+Logger
+get_node_logger(const rcl_node_t * node);
+
+class Logger
+{
+private:
+  friend Logger rclcpp::get_logger(const std::string & name);
+  friend ::rclcpp::node_interfaces::NodeLogging;
+
+  /// Constructor of a dummy logger.
+  /**
+   * This is used when logging is disabled: see `RCLCPP_LOGGING_ENABLED`.
+   * This cannot be called directly, see `rclcpp::get_logger` instead.
+   */
+  Logger()
+  : name_(nullptr) {}
+
+  /// Constructor of a named logger.
+  /**
+   * This cannot be called directly, see `rclcpp::get_logger` instead.
+   */
+  explicit Logger(const std::string & name)
+  : name_(new std::string(name)) {}
+
+  std::shared_ptr<const std::string> name_;
+
+public:
+  RCLCPP_PUBLIC
+  Logger(const Logger &) = default;
+
+  /// Get the name of this logger.
+  /**
+   * \return the full name of the logger including any prefixes, or
+   * \return `nullptr` if this logger is invalid (e.g. because logging is
+   *   disabled).
+   */
+  RCLCPP_PUBLIC
+  const char *
+  get_name() const
+  {
+    if (!name_) {
+      return nullptr;
+    }
+    return name_->c_str();
+  }
+
+  /// Return a logger that is a descendant of this logger.
+  /**
+   * The child logger's full name will include any hierarchy conventions that
+   * indicate it is a descendant of this logger.
+   * For example, ```get_logger('abc').get_child('def')``` will return a logger
+   * with name `abc.def`.
+   *
+   * \param[in] suffix the child logger's suffix
+   * \return a logger with the fully-qualified name including the suffix, or
+   * \return a dummy logger if this logger is invalid (e.g. because logging is
+   *   disabled).
+   */
+  RCLCPP_PUBLIC
+  Logger
+  get_child(const std::string & suffix)
+  {
+    if (!name_) {
+      return Logger();
+    }
+    return Logger(*name_ + "." + suffix);
+  }
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__LOGGER_HPP_

rclcpp/include/rclcpp/macros.hpp

@@ -12,10 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_MACROS_HPP_
-#define RCLCPP_RCLCPP_MACROS_HPP_
+#include <memory>
+#include <utility>
 
-/* Disables the copy constructor and operator= for the given class.
+#ifndef RCLCPP__MACROS_HPP_
+#define RCLCPP__MACROS_HPP_
+
+/**
+ * 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
+ * Same as RCLCPP_SMART_PTR_DEFINITIONS except 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 except 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__)
@@ -76,11 +101,13 @@
   { \
     return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward<Args>(args) ...)); \
   }
+
 /// Defines aliases and static functions for using the Class with unique_ptrs.
 #define RCLCPP_UNIQUE_PTR_DEFINITIONS(...) \
   __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__) \
   __RCLCPP_MAKE_UNIQUE_DEFINITION(__VA_ARGS__)
 
-#define RCLCPP_INFO(Args) std::cout << Args << std::endl;
+#define RCLCPP_STRING_JOIN(arg1, arg2) RCLCPP_DO_STRING_JOIN(arg1, arg2)
+#define RCLCPP_DO_STRING_JOIN(arg1, arg2) arg1 ## arg2
 
-#endif /* RCLCPP_RCLCPP_MACROS_HPP_ */
+#endif  // RCLCPP__MACROS_HPP_

rclcpp/include/rclcpp/mapped_ring_buffer.hpp

@@ -19,24 +19,28 @@
 #include <cstddef>
 #include <cstdint>
 #include <memory>
+#include <mutex>
+#include <utility>
 #include <vector>
 
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
 namespace mapped_ring_buffer
 {
 
-class MappedRingBufferBase
+class RCLCPP_PUBLIC MappedRingBufferBase
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBufferBase);
+  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.
@@ -55,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>;
@@ -63,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)
@@ -83,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.
@@ -96,6 +103,7 @@ public:
   void
   get_copy_at_key(uint64_t key, ElemUniquePtr & value)
   {
+    std::lock_guard<std::mutex> lock(data_mutex_);
     auto it = get_iterator_of_key(key);
     value = nullptr;
     if (it != elements_.end() && it->in_use) {
@@ -106,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.
@@ -127,6 +136,7 @@ public:
   void
   get_ownership_at_key(uint64_t key, ElemUniquePtr & value)
   {
+    std::lock_guard<std::mutex> lock(data_mutex_);
     auto it = get_iterator_of_key(key);
     value = nullptr;
     if (it != elements_.end() && it->in_use) {
@@ -142,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.
    *
@@ -154,6 +165,7 @@ public:
   void
   pop_at_key(uint64_t key, ElemUniquePtr & value)
   {
+    std::lock_guard<std::mutex> lock(data_mutex_);
     auto it = get_iterator_of_key(key);
     value = nullptr;
     if (it != elements_.end() && it->in_use) {
@@ -163,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.
    *
@@ -176,6 +189,7 @@ public:
   bool
   push_and_replace(uint64_t key, ElemUniquePtr & value)
   {
+    std::lock_guard<std::mutex> lock(data_mutex_);
     bool did_replace = elements_[head_].in_use;
     elements_[head_].key = key;
     elements_[head_].value.swap(value);
@@ -195,11 +209,12 @@ public:
   bool
   has_key(uint64_t key)
   {
+    std::lock_guard<std::mutex> lock(data_mutex_);
     return elements_.end() != get_iterator_of_key(key);
   }
 
 private:
-  RCLCPP_DISABLE_COPY(MappedRingBuffer<T, Alloc>);
+  RCLCPP_DISABLE_COPY(MappedRingBuffer<T, Alloc>)
 
   struct element
   {
@@ -213,17 +228,18 @@ private:
   typename std::vector<element, VectorAlloc>::iterator
   get_iterator_of_key(uint64_t key)
   {
-    // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-    auto it = std::find_if(elements_.begin(), elements_.end(), [key](element & e) -> bool {
-      return e.key == key && e.in_use;
-    });
-    // *INDENT-ON*
+    auto it = std::find_if(
+      elements_.begin(), elements_.end(),
+      [key](element & e) -> bool {
+        return e.key == key && e.in_use;
+      });
     return it;
   }
 
   std::vector<element, VectorAlloc> elements_;
   size_t head_;
   std::shared_ptr<ElemAlloc> allocator_;
+  std::mutex data_mutex_;
 };
 
 }  // namespace mapped_ring_buffer

rclcpp/include/rclcpp/memory_strategies.hpp

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

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -18,9 +18,14 @@
 #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"
+#include "rclcpp/waitable.hpp"
 
 namespace rclcpp
 {
@@ -33,223 +38,96 @@ namespace memory_strategy
  * the rmw implementation after the executor waits for work, based on the number of entities that
  * come through.
  */
-class MemoryStrategy
+class RCLCPP_PUBLIC MemoryStrategy
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy);
-  using WeakNodeVector = std::vector<std::weak_ptr<node::Node>>;
+  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;
+  virtual ~MemoryStrategy() = default;
 
-  // return the new number of services
-  virtual size_t fill_service_handles(void ** & ptr) = 0;
-
-  // return the new number of clients
-  virtual size_t fill_client_handles(void ** & ptr) = 0;
-
-  virtual void clear_active_entities() = 0;
-
-  virtual void clear_handles() = 0;
-  virtual void revalidate_handles() = 0;
   virtual bool collect_entities(const WeakNodeVector & weak_nodes) = 0;
 
-  /// Provide a newly initialized AnyExecutable object.
-  // \return Shared pointer to the fresh executable.
-  virtual executor::AnyExecutable::SharedPtr instantiate_next_executable() = 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 size_t number_of_waitables() const = 0;
+
+  virtual bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) = 0;
+  virtual void clear_handles() = 0;
+  virtual void remove_null_handles(rcl_wait_set_t * wait_set) = 0;
+
+  virtual void add_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
+
+  virtual void remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
 
   virtual void
-  get_next_subscription(executor::AnyExecutable::SharedPtr any_exec,
+  get_next_subscription(
+    rclcpp::executor::AnyExecutable & any_exec,
     const WeakNodeVector & weak_nodes) = 0;
 
   virtual void
-  get_next_service(executor::AnyExecutable::SharedPtr any_exec,
+  get_next_service(
+    rclcpp::executor::AnyExecutable & any_exec,
     const WeakNodeVector & weak_nodes) = 0;
 
   virtual void
-  get_next_client(executor::AnyExecutable::SharedPtr any_exec,
+  get_next_client(
+    rclcpp::executor::AnyExecutable & any_exec,
     const WeakNodeVector & weak_nodes) = 0;
 
-  static rclcpp::subscription::SubscriptionBase::SharedPtr
-  get_subscription_by_handle(void * subscriber_handle,
-    const WeakNodeVector & weak_nodes)
-  {
-    for (auto & weak_node : weak_nodes) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto group = weak_group.lock();
-        if (!group) {
-          continue;
-        }
-        for (auto & weak_subscription : group->get_subscription_ptrs()) {
-          auto subscription = weak_subscription.lock();
-          if (subscription) {
-            if (subscription->get_subscription_handle()->data == subscriber_handle) {
-              return subscription;
-            }
-            if (subscription->get_intra_process_subscription_handle() &&
-              subscription->get_intra_process_subscription_handle()->data == subscriber_handle)
-            {
-              return subscription;
-            }
-          }
-        }
-      }
-    }
-    return nullptr;
-  }
+  virtual void
+  get_next_waitable(
+    rclcpp::executor::AnyExecutable & any_exec,
+    const WeakNodeVector & weak_nodes) = 0;
 
-  static rclcpp::service::ServiceBase::SharedPtr
-  get_service_by_handle(void * service_handle, const WeakNodeVector & weak_nodes)
-  {
-    for (auto & weak_node : weak_nodes) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto group = weak_group.lock();
-        if (!group) {
-          continue;
-        }
-        for (auto & service : group->get_service_ptrs()) {
-          if (service->get_service_handle()->data == service_handle) {
-            return service;
-          }
-        }
-      }
-    }
-    return nullptr;
-  }
+  virtual rcl_allocator_t
+  get_allocator() = 0;
 
-  static rclcpp::client::ClientBase::SharedPtr
-  get_client_by_handle(void * client_handle, const WeakNodeVector & weak_nodes)
-  {
-    for (auto & weak_node : weak_nodes) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto group = weak_group.lock();
-        if (!group) {
-          continue;
-        }
-        for (auto & client : group->get_client_ptrs()) {
-          if (client->get_client_handle()->data == client_handle) {
-            return client;
-          }
-        }
-      }
-    }
-    return nullptr;
-  }
+  static rclcpp::SubscriptionBase::SharedPtr
+  get_subscription_by_handle(
+    std::shared_ptr<const rcl_subscription_t> subscriber_handle,
+    const WeakNodeVector & weak_nodes);
 
-  static rclcpp::node::Node::SharedPtr
-  get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group,
-    const WeakNodeVector & weak_nodes)
-  {
-    if (!group) {
-      return nullptr;
-    }
-    for (auto & weak_node : weak_nodes) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto callback_group = weak_group.lock();
-        if (callback_group == group) {
-          return node;
-        }
-      }
-    }
-    return nullptr;
-  }
+  static rclcpp::ServiceBase::SharedPtr
+  get_service_by_handle(
+    std::shared_ptr<const rcl_service_t> service_handle,
+    const WeakNodeVector & weak_nodes);
+
+  static rclcpp::ClientBase::SharedPtr
+  get_client_by_handle(
+    std::shared_ptr<const rcl_client_t> client_handle,
+    const WeakNodeVector & weak_nodes);
+
+  static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
+  get_node_by_group(
+    rclcpp::callback_group::CallbackGroup::SharedPtr group,
+    const WeakNodeVector & weak_nodes);
 
   static rclcpp::callback_group::CallbackGroup::SharedPtr
   get_group_by_subscription(
-    rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
-    const WeakNodeVector & weak_nodes)
-  {
-    for (auto & weak_node : weak_nodes) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto group = weak_group.lock();
-        if (!group) {
-          continue;
-        }
-        for (auto & weak_sub : group->get_subscription_ptrs()) {
-          auto sub = weak_sub.lock();
-          if (sub == subscription) {
-            return group;
-          }
-        }
-      }
-    }
-    return nullptr;
-  }
+    rclcpp::SubscriptionBase::SharedPtr subscription,
+    const WeakNodeVector & weak_nodes);
 
   static rclcpp::callback_group::CallbackGroup::SharedPtr
   get_group_by_service(
-    rclcpp::service::ServiceBase::SharedPtr service,
-    const WeakNodeVector & weak_nodes)
-  {
-    for (auto & weak_node : weak_nodes) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto group = weak_group.lock();
-        if (!group) {
-          continue;
-        }
-        for (auto & serv : group->get_service_ptrs()) {
-          if (serv == service) {
-            return group;
-          }
-        }
-      }
-    }
-    return nullptr;
-  }
+    rclcpp::ServiceBase::SharedPtr service,
+    const WeakNodeVector & weak_nodes);
 
   static rclcpp::callback_group::CallbackGroup::SharedPtr
-  get_group_by_client(rclcpp::client::ClientBase::SharedPtr client,
-    const WeakNodeVector & weak_nodes)
-  {
-    for (auto & weak_node : weak_nodes) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto group = weak_group.lock();
-        if (!group) {
-          continue;
-        }
-        for (auto & cli : group->get_client_ptrs()) {
-          if (cli == client) {
-            return group;
-          }
-        }
-      }
-    }
-    return nullptr;
-  }
-
+  get_group_by_client(
+    rclcpp::ClientBase::SharedPtr client,
+    const WeakNodeVector & weak_nodes);
 
+  static rclcpp::callback_group::CallbackGroup::SharedPtr
+  get_group_by_waitable(
+    rclcpp::Waitable::SharedPtr waitable,
+    const WeakNodeVector & weak_nodes);
 };
 
 }  // namespace memory_strategy
-
 }  // namespace rclcpp
 
 #endif  // RCLCPP__MEMORY_STRATEGY_HPP_

rclcpp/include/rclcpp/message_memory_strategy.hpp

@@ -16,9 +16,18 @@
 #define RCLCPP__MESSAGE_MEMORY_STRATEGY_HPP_
 
 #include <memory>
+#include <stdexcept>
+
+#include "rcl/types.h"
 
 #include "rclcpp/allocator/allocator_common.hpp"
+#include "rclcpp/exceptions.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcutils/logging_macros.h"
+
+#include "rmw/serialized_message.h"
 
 namespace rclcpp
 {
@@ -26,27 +35,44 @@ 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;
   using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
 
+  using SerializedMessageAllocTraits = allocator::AllocRebind<rcl_serialized_message_t, Alloc>;
+  using SerializedMessageAlloc = typename SerializedMessageAllocTraits::allocator_type;
+  using SerializedMessageDeleter =
+    allocator::Deleter<SerializedMessageAlloc, rcl_serialized_message_t>;
+
+  using BufferAllocTraits = allocator::AllocRebind<char, Alloc>;
+  using BufferAlloc = typename BufferAllocTraits::allocator_type;
+  using BufferDeleter = allocator::Deleter<BufferAlloc, char>;
+
   MessageMemoryStrategy()
   {
     message_allocator_ = std::make_shared<MessageAlloc>();
+    serialized_message_allocator_ = std::make_shared<SerializedMessageAlloc>();
+    buffer_allocator_ = std::make_shared<BufferAlloc>();
+    rcutils_allocator_ = allocator::get_rcl_allocator<char, BufferAlloc>(*buffer_allocator_.get());
   }
 
   explicit MessageMemoryStrategy(std::shared_ptr<Alloc> allocator)
   {
     message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
+    serialized_message_allocator_ = std::make_shared<SerializedMessageAlloc>(*allocator.get());
+    buffer_allocator_ = std::make_shared<BufferAlloc>(*allocator.get());
+    rcutils_allocator_ = allocator::get_rcl_allocator<char, BufferAlloc>(*buffer_allocator_.get());
   }
 
+  virtual ~MessageMemoryStrategy() = default;
+
   /// Default factory method
   static SharedPtr create_default()
   {
@@ -54,21 +80,68 @@ 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());
   }
 
+  virtual std::shared_ptr<rcl_serialized_message_t> borrow_serialized_message(size_t capacity)
+  {
+    auto msg = new rcl_serialized_message_t;
+    *msg = rmw_get_zero_initialized_serialized_message();
+    auto ret = rmw_serialized_message_init(msg, capacity, &rcutils_allocator_);
+    if (ret != RCL_RET_OK) {
+      rclcpp::exceptions::throw_from_rcl_error(ret);
+    }
+
+    auto serialized_msg = std::shared_ptr<rcl_serialized_message_t>(msg,
+        [](rmw_serialized_message_t * msg) {
+          auto ret = rmw_serialized_message_fini(msg);
+          delete msg;
+          if (ret != RCL_RET_OK) {
+            RCUTILS_LOG_ERROR_NAMED(
+              "rclcpp",
+              "failed to destroy serialized message: %s", rcl_get_error_string().str);
+          }
+        });
+
+    return serialized_msg;
+  }
+
+  virtual std::shared_ptr<rcl_serialized_message_t> borrow_serialized_message()
+  {
+    return borrow_serialized_message(default_buffer_capacity_);
+  }
+
+  virtual void set_default_buffer_capacity(size_t capacity)
+  {
+    default_buffer_capacity_ = capacity;
+  }
+
   /// 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();
   }
 
+  virtual void return_serialized_message(std::shared_ptr<rcl_serialized_message_t> & serialized_msg)
+  {
+    serialized_msg.reset();
+  }
+
   std::shared_ptr<MessageAlloc> message_allocator_;
   MessageDeleter message_deleter_;
+
+  std::shared_ptr<SerializedMessageAlloc> serialized_message_allocator_;
+  SerializedMessageDeleter serialized_message_deleter_;
+
+  std::shared_ptr<BufferAlloc> buffer_allocator_;
+  BufferDeleter buffer_deleter_;
+  size_t default_buffer_capacity_ = 0;
+
+  rcutils_allocator_t rcutils_allocator_;
 };
 
 }  // namespace message_memory_strategy

rclcpp/include/rclcpp/node.hpp

@@ -15,84 +15,142 @@
 #ifndef RCLCPP__NODE_HPP_
 #define RCLCPP__NODE_HPP_
 
+#include <atomic>
+#include <condition_variable>
 #include <list>
 #include <map>
 #include <memory>
+#include <mutex>
 #include <string>
 #include <tuple>
 #include <vector>
 
+#include "rcl/error_handling.h"
+#include "rcl/node.h"
+
 #include "rcl_interfaces/msg/list_parameters_result.hpp"
 #include "rcl_interfaces/msg/parameter_descriptor.hpp"
 #include "rcl_interfaces/msg/parameter_event.hpp"
 #include "rcl_interfaces/msg/set_parameters_result.hpp"
-#include "rosidl_generator_cpp/message_type_support.hpp"
 
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/client.hpp"
+#include "rclcpp/clock.hpp"
 #include "rclcpp/context.hpp"
+#include "rclcpp/event.hpp"
+#include "rclcpp/logger.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/message_memory_strategy.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_clock_interface.hpp"
+#include "rclcpp/node_interfaces/node_graph_interface.hpp"
+#include "rclcpp/node_interfaces/node_logging_interface.hpp"
+#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/node_interfaces/node_time_source_interface.hpp"
+#include "rclcpp/node_interfaces/node_timers_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/node_interfaces/node_waitables_interface.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/publisher.hpp"
 #include "rclcpp/service.hpp"
 #include "rclcpp/subscription.hpp"
+#include "rclcpp/subscription_traits.hpp"
+#include "rclcpp/time.hpp"
 #include "rclcpp/timer.hpp"
-
-
-// Forward declaration of ROS middleware class
-namespace rmw
-{
-struct rmw_node_t;
-}  // namespace rmw
+#include "rclcpp/visibility_control.hpp"
 
 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.
    */
-  explicit Node(const std::string & node_name, bool use_intra_process_comms = false);
-
-  /// Create a node based on the node name and a rclcpp::context::Context.
-  /**
-   * \param[in] node_name Name 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.
-   */
-  Node(
-    const std::string & node_name, rclcpp::context::Context::SharedPtr context,
+  RCLCPP_PUBLIC
+  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.
+  /**
+   * \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] arguments Command line arguments that should apply only to this node.
+   * \param[in] initial_parameters a list of initial values for parameters on the node.
+   * This can be used to provide remapping rules that only affect one instance.
+   * \param[in] use_global_arguments False to prevent node using arguments passed to the 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.
+   * \param[in] start_parameter_services True to setup ROS interfaces for accessing parameters
+   * in the node.
+   * \param[in] allow_undeclared_parameters True to allow any parameter name to be set on the node.
+   */
+  RCLCPP_PUBLIC
+  Node(
+    const std::string & node_name,
+    const std::string & namespace_,
+    rclcpp::Context::SharedPtr context,
+    const std::vector<std::string> & arguments,
+    const std::vector<Parameter> & initial_parameters,
+    bool use_global_arguments = true,
+    bool use_intra_process_comms = false,
+    bool start_parameter_services = true,
+    bool allow_undeclared_parameters = false);
+
+  RCLCPP_PUBLIC
+  virtual ~Node();
+
   /// Get the name of the node.
-  // \return The name of the node.
-  const std::string &
-  get_name() const {return name_; }
+  /** \return The name of the node. */
+  RCLCPP_PUBLIC
+  const char *
+  get_name() const;
+
+  /// Get the namespace of the node.
+  /** \return The namespace of the node. */
+  RCLCPP_PUBLIC
+  const char *
+  get_namespace() const;
+
+  /// Get the logger of the node.
+  /** \return The logger of the node. */
+  RCLCPP_PUBLIC
+  rclcpp::Logger
+  get_logger() 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<MessageT, Alloc>>
+  std::shared_ptr<PublisherT>
   create_publisher(
     const std::string & topic_name, size_t qos_history_depth,
     std::shared_ptr<Alloc> allocator = nullptr);
@@ -101,10 +159,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<MessageT, Alloc>>
+  std::shared_ptr<PublisherT>
   create_publisher(
     const std::string & topic_name,
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default,
@@ -118,21 +179,28 @@ 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<
+      typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>>
+  std::shared_ptr<SubscriptionT>
   create_subscription(
     const std::string & topic_name,
-    CallbackT callback,
+    CallbackT && callback,
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr,
     bool ignore_local_publications = false,
-    typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
+    typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
+      typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>::SharedPtr
     msg_mem_strat = nullptr,
     std::shared_ptr<Alloc> allocator = nullptr);
 
@@ -144,21 +212,28 @@ 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<
+      typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>>
+  std::shared_ptr<SubscriptionT>
   create_subscription(
     const std::string & topic_name,
+    CallbackT && callback,
     size_t qos_history_depth,
-    CallbackT callback,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr,
     bool ignore_local_publications = false,
-    typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
+    typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
+      typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>::SharedPtr
     msg_mem_strat = nullptr,
     std::shared_ptr<Alloc> allocator = nullptr);
 
@@ -168,115 +243,313 @@ public:
    * \param[in] callback User-defined callback function.
    * \param[in] group Callback group to execute this timer's callback in.
    */
-  rclcpp::timer::WallTimer::SharedPtr
+  template<typename DurationT = std::milli, typename CallbackT>
+  typename rclcpp::WallTimer<CallbackT>::SharedPtr
   create_wall_timer(
-    std::chrono::nanoseconds period,
-    rclcpp::timer::CallbackType callback,
+    std::chrono::duration<int64_t, DurationT> period,
+    CallbackT callback,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
-  /// Create a timer with a sub-nanosecond precision update period.
-  /**
-   * \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
+  typename rclcpp::Client<ServiceT>::SharedPtr
   create_client(
     const std::string & service_name,
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
   /* Create and return a Service. */
-  template<typename ServiceT, typename FunctorT>
-  typename rclcpp::service::Service<ServiceT>::SharedPtr
+  template<typename ServiceT, typename CallbackT>
+  typename rclcpp::Service<ServiceT>::SharedPtr
   create_service(
     const std::string & service_name,
-    FunctorT callback,
+    CallbackT && callback,
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
-  std::vector<rcl_interfaces::msg::SetParametersResult> set_parameters(
-    const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+  /// Declare and initialize a parameter.
+  /**
+   * This method is used to declare that a parameter exists on this node.
+   * If a run-time user has provided an an initial value then it will be set in this method,
+   * otherwise the default_value will be set.
+   * \param[in] name the name of the parameter
+   * \param[in] default_value An initial value to be used if a run-time user did not override it.
+   * \param[in] read_only if True then this parameter may not be changed after initialization.
+   * \throws std::runtime_error if parameter has already been declared.
+   * \throws std::runtime_error if a parameter name is invalid.
+   * \throws rclcpp::exceptions::InvalidParameterValueException if initial value fails to be set.
+   */
+  RCLCPP_PUBLIC
+  void
+  declare_parameter(
+    const std::string & name,
+    const rclcpp::ParameterValue & default_value = rclcpp::ParameterValue(),
+    bool read_only = false);
 
-  rcl_interfaces::msg::SetParametersResult set_parameters_atomically(
-    const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+  /// Declare and initialize a parameter with a type.
+  /**
+   * See the non-templated declare_parameter() on this class for details.
+   */
+  template<typename ParameterT>
+  void
+  declare_parameter(
+    const std::string & name,
+    const ParameterT & default_value,
+    bool read_only = false);
 
-  std::vector<rclcpp::parameter::ParameterVariant> get_parameters(
-    const std::vector<std::string> & names) const;
+  RCLCPP_PUBLIC
+  std::vector<rcl_interfaces::msg::SetParametersResult>
+  set_parameters(const std::vector<rclcpp::Parameter> & parameters);
 
-  std::vector<rcl_interfaces::msg::ParameterDescriptor> describe_parameters(
-    const std::vector<std::string> & names) const;
+  RCLCPP_PUBLIC
+  rcl_interfaces::msg::SetParametersResult
+  set_parameters_atomically(const std::vector<rclcpp::Parameter> & parameters);
 
-  std::vector<uint8_t> get_parameter_types(
-    const std::vector<std::string> & names) const;
+  template<typename ParameterT>
+  void
+  set_parameter_if_not_set(
+    const std::string & name,
+    const ParameterT & value);
 
-  rcl_interfaces::msg::ListParametersResult list_parameters(
-    const std::vector<std::string> & prefixes, uint64_t depth) const;
+  /// Set a map of parameters with the same prefix.
+  /**
+   * For each key in the map, a parameter with a name of "name.key" will be set
+   * to the value in the map.
+   *
+   * \param[in] name The prefix of the parameters to set.
+   * \param[in] values The parameters to set in the given prefix.
+   */
+  template<typename MapValueT>
+  void
+  set_parameters_if_not_set(
+    const std::string & name,
+    const std::map<std::string, MapValueT> & values);
 
-  std::map<std::string, std::string> get_topic_names_and_types() const;
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::Parameter>
+  get_parameters(const std::vector<std::string> & names) const;
 
-  size_t count_publishers(const std::string & topic_name) const;
+  RCLCPP_PUBLIC
+  rclcpp::Parameter
+  get_parameter(const std::string & name) const;
 
-  size_t count_subscribers(const std::string & topic_name) const;
+  RCLCPP_PUBLIC
+  bool
+  get_parameter(
+    const std::string & name,
+    rclcpp::Parameter & parameter) const;
 
-  const CallbackGroupWeakPtrList & get_callback_groups() 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;
+
+  /// Assign the value of the map parameter if set into the values argument.
+  /**
+   * Parameter names that are part of a map are of the form "name.member".
+   * This API gets all parameters that begin with "name", storing them into the
+   * map with the name of the parameter and their value.
+   * If there are no members in the named map, then the "values" argument is not changed.
+   *
+   * \param[in] name The prefix of the parameters to get.
+   * \param[out] values The map of output values, with one std::string,MapValueT
+   *                    per parameter.
+   * \returns true if values was changed, false otherwise
+   */
+  template<typename MapValueT>
+  bool
+  get_parameters(
+    const std::string & name,
+    std::map<std::string, MapValueT> & values) 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;
+
+  /// Get the parameter value; if not set, set the "alternative value" and store it in the node.
+  /**
+   * If the parameter is set, then the "value" argument is assigned the value
+   * in the parameter.
+   * If the parameter is not set, then the "value" argument is assigned the "alternative_value",
+   * and the parameter is set to the "alternative_value" on the node.
+   *
+   * \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 and parameter if the parameter was not set.
+   */
+  template<typename ParameterT>
+  void
+  get_parameter_or_set(
+    const std::string & name,
+    ParameterT & value,
+    const ParameterT & alternative_value);
+
+  RCLCPP_PUBLIC
+  std::vector<rcl_interfaces::msg::ParameterDescriptor>
+  describe_parameters(const std::vector<std::string> & names) const;
+
+  RCLCPP_PUBLIC
+  std::vector<uint8_t>
+  get_parameter_types(const std::vector<std::string> & names) const;
+
+  RCLCPP_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::vector<std::string>
+  get_node_names() const;
+
+  RCLCPP_PUBLIC
+  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;
+
+  RCLCPP_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 loan just let it go
+   * out of scope.
+   */
+  RCLCPP_PUBLIC
+  rclcpp::Event::SharedPtr
+  get_graph_event();
+
+  /// Wait for a graph event to occur by waiting on an Event to become set.
+  /**
+   * The given Event must be acquire through the get_graph_event() method.
+   *
+   * \throws InvalidEventError if the given event is nullptr
+   * \throws EventNotRegisteredError if the given event was not acquired with
+   *   get_graph_event().
+   */
+  RCLCPP_PUBLIC
+  void
+  wait_for_graph_change(
+    rclcpp::Event::SharedPtr event,
+    std::chrono::nanoseconds timeout);
+
+  RCLCPP_PUBLIC
+  rclcpp::Clock::SharedPtr
+  get_clock();
+
+  RCLCPP_PUBLIC
+  Time
+  now();
+
+  /// Return the Node's internal NodeBaseInterface implementation.
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
+  get_node_base_interface();
+
+  /// Return the Node's internal NodeClockInterface implementation.
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr
+  get_node_clock_interface();
+
+  /// Return the Node's internal NodeGraphInterface implementation.
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr
+  get_node_graph_interface();
+
+  /// Return the Node's internal NodeLoggingInterface implementation.
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr
+  get_node_logging_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 NodeWaitablesInterface implementation.
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr
+  get_node_waitables_interface();
+
+  /// Return the Node's internal NodeParametersInterface implementation.
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr
+  get_node_parameters_interface();
+
+  /// Return the Node's internal NodeParametersInterface implementation.
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr
+  get_node_time_source_interface();
 
 private:
-  RCLCPP_DISABLE_COPY(Node);
-
-  static const rosidl_message_type_support_t * ipm_ts_;
+  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::NodeLoggingInterface::SharedPtr node_logging_;
+  rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_;
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
+  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
+  rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_;
+  rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_;
 
   bool use_intra_process_comms_;
-
-  mutable std::mutex mutex_;
-
-  std::map<std::string, rclcpp::parameter::ParameterVariant> parameters_;
-
-  publisher::Publisher<rcl_interfaces::msg::ParameterEvent>::SharedPtr events_publisher_;
 };
 
-const rosidl_message_type_support_t * Node::ipm_ts_ =
-  rosidl_generator_cpp::get_message_type_support_handle<rcl_interfaces::msg::IntraProcessMessage>();
-
-} /* namespace node */
-} /* namespace rclcpp */
-
-#define RCLCPP_REGISTER_NODE(Class) RMW_EXPORT \
-  rclcpp::node::Node::SharedPtr \
-  create_node() \
-  { \
-    return rclcpp::node::Node::SharedPtr(new Class( \
-               rclcpp::contexts::default_context::DefaultContext:: \
-               make_shared())); \
-  }
+}  // namespace rclcpp
 
 #ifndef RCLCPP__NODE_IMPL_HPP_
 // Template implementations

rclcpp/include/rclcpp/node_impl.hpp

@@ -30,95 +30,29 @@
 #include <utility>
 #include <vector>
 
+#include "rcl/publisher.h"
+#include "rcl/subscription.h"
+
 #include "rcl_interfaces/msg/intra_process_message.hpp"
-#include "rosidl_generator_cpp/message_type_support.hpp"
-#include "rosidl_generator_cpp/service_type_support.hpp"
 
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/intra_process_manager.hpp"
 #include "rclcpp/parameter.hpp"
+#include "rclcpp/create_publisher.hpp"
+#include "rclcpp/create_service.hpp"
+#include "rclcpp/create_subscription.hpp"
+#include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 #ifndef RCLCPP__NODE_HPP_
 #include "node.hpp"
 #endif
 
-using namespace rclcpp;
-using namespace node;
-
-Node::Node(const std::string & node_name, bool use_intra_process_comms)
-: Node(
-    node_name,
-    rclcpp::contexts::default_context::get_global_default_context(),
-    use_intra_process_comms)
-{}
-
-Node::Node(
-  const std::string & node_name,
-  context::Context::SharedPtr context,
-  bool use_intra_process_comms)
-: name_(node_name), context_(context),
-  number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0),
-  use_intra_process_comms_(use_intra_process_comms)
+namespace rclcpp
 {
-  size_t domain_id = 0;
-  char * ros_domain_id = nullptr;
-  const char * env_var = "ROS_DOMAIN_ID";
-#ifndef _WIN32
-  ros_domain_id = getenv(env_var);
-#else
-  size_t ros_domain_id_size;
-  _dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var);
-#endif
-  if (ros_domain_id) {
-    uint32_t number = strtoul(ros_domain_id, NULL, 0);
-    if (number == (std::numeric_limits<uint32_t>::max)()) {
-      throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number");
-    }
-    domain_id = static_cast<size_t>(number);
-#ifdef _WIN32
-    free(ros_domain_id);
-#endif
-  }
 
-  auto node = rmw_create_node(name_.c_str(), domain_id);
-  if (!node) {
-    // *INDENT-OFF*
-    throw std::runtime_error(
-      std::string("could not create node: ") +
-      rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
-  // Initialize node handle shared_ptr with custom deleter.
-  // *INDENT-OFF*
-  node_handle_.reset(node, [](rmw_node_t * node) {
-    auto ret = rmw_destroy_node(node);
-    if (ret != RMW_RET_OK) {
-      fprintf(
-        stderr, "Error in destruction of rmw node handle: %s\n", rmw_get_error_string_safe());
-    }
-  });
-  // *INDENT-ON*
-
-  using rclcpp::callback_group::CallbackGroupType;
-  default_callback_group_ = create_callback_group(
-    CallbackGroupType::MutuallyExclusive);
-  events_publisher_ = create_publisher<rcl_interfaces::msg::ParameterEvent>(
-    "parameter_events", rmw_qos_profile_parameter_events);
-}
-
-rclcpp::callback_group::CallbackGroup::SharedPtr
-Node::create_callback_group(
-  rclcpp::callback_group::CallbackGroupType group_type)
-{
-  using rclcpp::callback_group::CallbackGroup;
-  using rclcpp::callback_group::CallbackGroupType;
-  auto group = CallbackGroup::SharedPtr(new CallbackGroup(group_type));
-  callback_groups_.push_back(group);
-  return group;
-}
-
-template<typename MessageT, typename Alloc>
-typename rclcpp::publisher::Publisher<MessageT, Alloc>::SharedPtr
+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)
@@ -128,11 +62,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)
@@ -140,210 +74,76 @@ 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(), ipm_ts_, (topic_name + "__intra").c_str(), qos_profile);
-    if (!intra_process_publisher_handle) {
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-      throw std::runtime_error(
-        std::string("could not create intra process publisher: ") +
-        rmw_get_error_string_safe());
-      // *INDENT-ON*
-    }
-
-    auto intra_process_manager =
-      context_->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
-    uint64_t intra_process_publisher_id =
-      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);
-  }
-  return publisher;
+  return rclcpp::create_publisher<MessageT, Alloc, PublisherT>(
+    this->node_topics_.get(),
+    topic_name,
+    qos_profile,
+    use_intra_process_comms_,
+    allocator);
 }
 
-bool
-Node::group_in_node(callback_group::CallbackGroup::SharedPtr group)
-{
-  bool group_belongs_to_this_node = false;
-  for (auto & weak_group : this->callback_groups_) {
-    auto cur_group = weak_group.lock();
-    if (cur_group && (cur_group == group)) {
-      group_belongs_to_this_node = true;
-    }
-  }
-  return group_belongs_to_this_node;
-}
-
-template<typename MessageT, typename CallbackT, typename Alloc>
-typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
+template<
+  typename MessageT,
+  typename CallbackT,
+  typename Alloc,
+  typename SubscriptionT>
+std::shared_ptr<SubscriptionT>
 Node::create_subscription(
   const std::string & topic_name,
-  CallbackT callback,
+  CallbackT && callback,
   const rmw_qos_profile_t & qos_profile,
   rclcpp::callback_group::CallbackGroup::SharedPtr group,
   bool ignore_local_publications,
-  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
+  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
+    typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>::SharedPtr
   msg_mem_strat,
-  typename std::shared_ptr<Alloc> allocator)
+  std::shared_ptr<Alloc> allocator)
 {
+  using CallbackMessageT = typename rclcpp::subscription_traits::has_message_type<CallbackT>::type;
+
   if (!allocator) {
     allocator = std::make_shared<Alloc>();
   }
 
-  rclcpp::subscription::AnySubscriptionCallback<MessageT,
-  Alloc> any_subscription_callback(allocator);
-  any_subscription_callback.set(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<CallbackMessageT, 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 namespace rclcpp::subscription;
-
-  auto sub = Subscription<MessageT, Alloc>::make_shared(
-    node_handle_,
-    subscriber_handle,
+  return rclcpp::create_subscription<MessageT, CallbackT, Alloc, CallbackMessageT, 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(), ipm_ts_,
-      (topic_name + "__intra").c_str(), qos_profile, false);
-    if (!subscriber_handle) {
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-      throw std::runtime_error(
-        std::string("could not create intra process subscription: ") + rmw_get_error_string_safe());
-      // *INDENT-ON*
-    }
-    auto intra_process_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_++;
-  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,
+  CallbackT && callback,
   size_t qos_history_depth,
-  CallbackT callback,
   rclcpp::callback_group::CallbackGroup::SharedPtr group,
   bool ignore_local_publications,
-  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
+  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
+    typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>::SharedPtr
   msg_mem_strat,
   std::shared_ptr<Alloc> allocator)
 {
   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>(
     topic_name,
-    callback,
+    std::forward<CallbackT>(callback),
     qos,
     group,
     ignore_local_publications,
@@ -351,326 +151,174 @@ Node::create_subscription(
     allocator);
 }
 
-rclcpp::timer::WallTimer::SharedPtr
+template<typename DurationT, typename CallbackT>
+typename rclcpp::WallTimer<CallbackT>::SharedPtr
 Node::create_wall_timer(
-  std::chrono::nanoseconds period,
-  rclcpp::timer::CallbackType callback,
+  std::chrono::duration<int64_t, DurationT> period,
+  CallbackT callback,
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
-  auto timer = rclcpp::timer::WallTimer::make_shared(period, 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_++;
+  auto timer = rclcpp::WallTimer<CallbackT>::make_shared(
+    std::chrono::duration_cast<std::chrono::nanoseconds>(period),
+    std::move(callback),
+    this->node_base_->get_context());
+  node_timers_->add_timer(timer, group);
   return timer;
 }
 
-// TODO(wjwwood): reenable this once I figure out why the demo doesn't build with it.
-// rclcpp::timer::WallTimer::SharedPtr
-// Node::create_wall_timer(
-//   std::chrono::duration<long double, std::nano> period,
-//   rclcpp::timer::CallbackType callback,
-//   rclcpp::callback_group::CallbackGroup::SharedPtr group)
-// {
-//   return create_wall_timer(
-//     std::chrono::duration_cast<std::chrono::nanoseconds>(period),
-//     callback,
-//     group);
-// }
-
 template<typename ServiceT>
-typename client::Client<ServiceT>::SharedPtr
+typename Client<ServiceT>::SharedPtr
 Node::create_client(
   const std::string & service_name,
+  const rmw_qos_profile_t & qos_profile,
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
-  using rosidl_generator_cpp::get_service_type_support_handle;
-  auto service_type_support_handle =
-    get_service_type_support_handle<ServiceT>();
+  rcl_client_options_t options = rcl_client_get_default_options();
+  options.qos = qos_profile;
 
-  rmw_client_t * client_handle = rmw_create_client(
-    this->node_handle_.get(), service_type_support_handle, service_name.c_str());
-  if (!client_handle) {
-    // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-    throw std::runtime_error(
-      std::string("could not create client: ") +
-      rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
-
-  using namespace rclcpp::client;
+  using rclcpp::Client;
+  using rclcpp::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_++;
-
+  node_services_->add_client(cli_base_ptr, group);
   return cli;
 }
 
-template<typename ServiceT, typename FunctorT>
-typename rclcpp::service::Service<ServiceT>::SharedPtr
+template<typename ServiceT, typename CallbackT>
+typename rclcpp::Service<ServiceT>::SharedPtr
 Node::create_service(
   const std::string & service_name,
-  FunctorT callback,
+  CallbackT && callback,
+  const rmw_qos_profile_t & qos_profile,
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
-  using rosidl_generator_cpp::get_service_type_support_handle;
-  auto service_type_support_handle =
-    get_service_type_support_handle<ServiceT>();
-
-  rclcpp::service::AnyServiceCallback<ServiceT> any_service_callback;
-  any_service_callback.set(callback);
-
-  rmw_service_t * service_handle = rmw_create_service(
-    node_handle_.get(), service_type_support_handle, service_name.c_str());
-  if (!service_handle) {
-    // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-    throw std::runtime_error(
-      std::string("could not create service: ") +
-      rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
-
-  auto serv = service::Service<ServiceT>::make_shared(
-    node_handle_, service_handle, service_name, any_service_callback);
-  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_++;
-  return serv;
+  return rclcpp::create_service<ServiceT, CallbackT>(
+    node_base_, node_services_,
+    service_name, std::forward<CallbackT>(callback), qos_profile, group);
 }
 
-std::vector<rcl_interfaces::msg::SetParametersResult>
-Node::set_parameters(
-  const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
+template<typename CallbackT>
+void
+Node::register_param_change_callback(CallbackT && callback)
 {
-  std::vector<rcl_interfaces::msg::SetParametersResult> results;
-  for (auto p : parameters) {
-    auto result = set_parameters_atomically({{p}});
-    results.push_back(result);
-  }
-  return results;
+  this->node_parameters_->register_param_change_callback(std::forward<CallbackT>(callback));
 }
 
-rcl_interfaces::msg::SetParametersResult
-Node::set_parameters_atomically(
-  const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
+template<typename ParameterT>
+void
+Node::declare_parameter(
+  const std::string & name,
+  const ParameterT & default_value,
+  bool read_only)
 {
-  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>();
+  this->declare_parameter(name, rclcpp::ParameterValue(default_value), read_only);
+}
 
-  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;
+template<typename ParameterT>
+void
+Node::set_parameter_if_not_set(
+  const std::string & name,
+  const ParameterT & value)
+{
+  rclcpp::Parameter parameter;
+  if (!this->get_parameter(name, parameter)) {
+    this->set_parameters({
+        rclcpp::Parameter(name, value),
+      });
   }
-  // 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;
+// this is a partially-specialized version of set_parameter_if_not_set above,
+// where our concrete type for ParameterT is std::map, but the to-be-determined
+// type is the value in the map.
+template<typename MapValueT>
+void
+Node::set_parameters_if_not_set(
+  const std::string & name,
+  const std::map<std::string, MapValueT> & values)
+{
+  std::vector<rclcpp::Parameter> params;
 
-  events_publisher_->publish(parameter_event);
+  for (const auto & val : values) {
+    std::string param_name = name + "." + val.first;
+    rclcpp::Parameter parameter;
+    if (!this->get_parameter(param_name, parameter)) {
+      params.push_back(rclcpp::Parameter(param_name, val.second));
+    }
+  }
+
+  this->set_parameters(params);
+}
+
+template<typename ParameterT>
+bool
+Node::get_parameter(const std::string & name, ParameterT & value) const
+{
+  rclcpp::Parameter parameter;
+  bool result = get_parameter(name, parameter);
+  if (result) {
+    value = parameter.get_value<ParameterT>();
+  }
 
   return result;
 }
 
-std::vector<rclcpp::parameter::ParameterVariant>
+// this is a partially-specialized version of get_parameter above,
+// where our concrete type for ParameterT is std::map, but the to-be-determined
+// type is the value in the map.
+template<typename MapValueT>
+bool
 Node::get_parameters(
-  const std::vector<std::string> & names) const
+  const std::string & name,
+  std::map<std::string, MapValueT> & values) 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));
+  std::map<std::string, rclcpp::Parameter> params;
+  bool result = node_parameters_->get_parameters_by_prefix(name, params);
+  if (result) {
+    for (const auto & param : params) {
+      values[param.first] = param.second.get_value<MapValueT>();
     }
   }
-  return results;
-}
 
-std::vector<rcl_interfaces::msg::ParameterDescriptor>
-Node::describe_parameters(
-  const std::vector<std::string> & names) const
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  std::vector<rcl_interfaces::msg::ParameterDescriptor> results;
-  for (auto & kv : parameters_) {
-    if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
-      return name == kv.first;
-    }))
-    {
-      rcl_interfaces::msg::ParameterDescriptor parameter_descriptor;
-      parameter_descriptor.name = kv.first;
-      parameter_descriptor.type = kv.second.get_type();
-      results.push_back(parameter_descriptor);
-    }
-  }
-  return results;
-}
-
-std::vector<uint8_t>
-Node::get_parameter_types(
-  const std::vector<std::string> & names) const
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  std::vector<uint8_t> results;
-  for (auto & kv : parameters_) {
-    if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
-      return name == kv.first;
-    }))
-    {
-      results.push_back(kv.second.get_type());
-    } else {
-      results.push_back(rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET);
-    }
-  }
-  return results;
-}
-
-rcl_interfaces::msg::ListParametersResult
-Node::list_parameters(
-  const std::vector<std::string> & prefixes, uint64_t depth) const
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  rcl_interfaces::msg::ListParametersResult result;
-
-  // TODO(esteve): define parameter separator, use "." for now
-  for (auto & kv : parameters_) {
-    if (std::any_of(prefixes.cbegin(), prefixes.cend(), [&kv, &depth](const std::string & prefix) {
-      if (kv.first == prefix) {
-        return true;
-      } else if (kv.first.find(prefix + ".") == 0) {
-        size_t length = prefix.length();
-        std::string substr = kv.first.substr(length);
-        // Cast as unsigned integer to avoid warning
-        return static_cast<uint64_t>(std::count(substr.begin(), substr.end(), '.')) < depth;
-      }
-      return false;
-    }))
-    {
-      result.names.push_back(kv.first);
-      size_t last_separator = kv.first.find_last_of('.');
-      if (std::string::npos != last_separator) {
-        std::string prefix = kv.first.substr(0, last_separator);
-        if (std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) ==
-          result.prefixes.cend())
-        {
-          result.prefixes.push_back(prefix);
-        }
-      }
-    }
-  }
   return result;
 }
 
-std::map<std::string, std::string>
-Node::get_topic_names_and_types() const
+template<typename ParameterT>
+bool
+Node::get_parameter_or(
+  const std::string & name,
+  ParameterT & value,
+  const ParameterT & alternative_value) const
 {
-  rmw_topic_names_and_types_t topic_names_and_types;
-  topic_names_and_types.topic_count = 0;
-  topic_names_and_types.topic_names = nullptr;
-  topic_names_and_types.type_names = nullptr;
-
-  auto ret = rmw_get_topic_names_and_types(node_handle_.get(), &topic_names_and_types);
-  if (ret != RMW_RET_OK) {
-    // *INDENT-OFF*
-    throw std::runtime_error(
-      std::string("could not get topic names and types: ") + rmw_get_error_string_safe());
-    // *INDENT-ON*
+  bool got_parameter = get_parameter(name, value);
+  if (!got_parameter) {
+    value = alternative_value;
   }
-
-  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;
+  return got_parameter;
 }
 
-size_t
-Node::count_publishers(const std::string & topic_name) const
+template<typename ParameterT>
+void
+Node::get_parameter_or_set(
+  const std::string & name,
+  ParameterT & value,
+  const ParameterT & alternative_value)
 {
-  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*
+  bool got_parameter = get_parameter(name, value);
+  if (!got_parameter) {
+    this->set_parameters({
+        rclcpp::Parameter(name, alternative_value),
+      });
+    value = alternative_value;
   }
-  return count;
 }
 
-size_t
-Node::count_subscribers(const std::string & topic_name) const
-{
-  size_t count;
-  auto ret = rmw_count_subscribers(node_handle_.get(), topic_name.c_str(), &count);
-  if (ret != RMW_RET_OK) {
-    // *INDENT-OFF*
-    throw std::runtime_error(
-      std::string("could not count subscribers: ") + rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
-  return count;
-}
-
-
-const Node::CallbackGroupWeakPtrList &
-Node::get_callback_groups() const
-{
-  return callback_groups_;
-}
+}  // namespace rclcpp
 
 #endif  // RCLCPP__NODE_IMPL_HPP_

rclcpp/include/rclcpp/node_interfaces/get_node_parameters_interface.hpp

@@ -0,0 +1,162 @@
+// Copyright 2018 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__GET_NODE_PARAMETERS_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__GET_NODE_PARAMETERS_INTERFACE_HPP_
+
+#include <memory>
+#include <utility>
+#include <type_traits>
+
+#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
+
+/// This header provides the get_node_parameters_interface() template function.
+/**
+ * This function is useful for getting the NodeParametersInterface pointer from
+ * various kinds of Node-like classes.
+ *
+ * It's able to get the NodeParametersInterface pointer so long as the class
+ * has a method called ``get_node_parameters_interface()`` which returns
+ * either a pointer (const or not) to a NodeParametersInterface or a
+ * std::shared_ptr to a NodeParametersInterface.
+ */
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+namespace detail
+{
+
+// This is a meta-programming checker for if a given Node-like object has a
+// getter called get_node_parameters_interface() which returns various types,
+// e.g. const pointer or a shared pointer.
+template<typename NodeType, typename ReturnType>
+struct has_get_node_parameters_interface
+{
+private:
+  template<typename T>
+  static constexpr
+  auto
+  check(T *)->typename std::is_same<
+    decltype(std::declval<T>().get_node_parameters_interface()),
+    ReturnType
+  >::type;
+
+  template<typename>
+  static constexpr
+  std::false_type
+  check(...);
+
+public:
+  using type = decltype(check<NodeType>(nullptr));
+  static constexpr bool value = type::value;
+};
+
+// If NodeType is a pointer to NodeParametersInterface already (just normal function overload).
+inline
+const rclcpp::node_interfaces::NodeParametersInterface *
+get_node_parameters_interface_from_pointer(
+  const rclcpp::node_interfaces::NodeParametersInterface * pointer)
+{
+  return pointer;
+}
+
+// If NodeType has a method called get_node_parameters_interface() which returns a shared pointer.
+template<
+  typename NodeType,
+  typename std::enable_if<has_get_node_parameters_interface<
+    typename std::remove_pointer<NodeType>::type,
+    std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface>
+  >::value, int>::type = 0
+>
+const rclcpp::node_interfaces::NodeParametersInterface *
+get_node_parameters_interface_from_pointer(NodeType node_pointer)
+{
+  return node_pointer->get_node_parameters_interface().get();
+}
+
+// If NodeType has a method called get_node_parameters_interface() which returns a pointer.
+template<
+  typename NodeType,
+  typename std::enable_if<has_get_node_parameters_interface<
+    typename std::remove_pointer<NodeType>::type,
+    rclcpp::node_interfaces::NodeParametersInterface *
+  >::value, int>::type = 0
+>
+const rclcpp::node_interfaces::NodeParametersInterface *
+get_node_parameters_interface_from_pointer(NodeType node_pointer)
+{
+  return node_pointer->get_node_parameters_interface();
+}
+
+// If NodeType has a method called get_node_parameters_interface() which returns a const pointer.
+template<
+  typename NodeType,
+  typename std::enable_if<has_get_node_parameters_interface<
+    typename std::remove_pointer<NodeType>::type,
+    const rclcpp::node_interfaces::NodeParametersInterface *
+  >::value, int>::type = 0
+>
+const rclcpp::node_interfaces::NodeParametersInterface *
+get_node_parameters_interface_from_pointer(NodeType node_pointer)
+{
+  return node_pointer->get_node_parameters_interface();
+}
+
+// Forward shared_ptr's to const node pointer signatures.
+template<
+  typename NodeType,
+  typename std::enable_if<std::is_same<
+    NodeType,
+    typename std::shared_ptr<typename std::remove_pointer<NodeType>::type::element_type> *
+  >::value, int>::type = 0
+>
+const rclcpp::node_interfaces::NodeParametersInterface *
+get_node_parameters_interface_from_pointer(NodeType node_shared_pointer)
+{
+  return get_node_parameters_interface_from_pointer(node_shared_pointer->get());
+}
+
+}  // namespace detail
+
+/// Get the NodeParametersInterface as a const pointer from a pointer to a "Node like" object.
+template<
+  typename NodeType,
+  typename std::enable_if<std::is_pointer<NodeType>::value, int>::type = 0
+>
+const rclcpp::node_interfaces::NodeParametersInterface *
+get_node_parameters_interface(NodeType && node_pointer)
+{
+  // Forward pointers to detail implmentation directly.
+  return detail::get_node_parameters_interface_from_pointer(std::forward<NodeType>(node_pointer));
+}
+
+/// Get the NodeParametersInterface as a const pointer from a "Node like" object.
+template<
+  typename NodeType,
+  typename std::enable_if<!std::is_pointer<NodeType>::value, int>::type = 0
+>
+const rclcpp::node_interfaces::NodeParametersInterface *
+get_node_parameters_interface(NodeType && node_reference)
+{
+  // Forward const-references to detail implmentation as a pointer.
+  return detail::get_node_parameters_interface_from_pointer(&node_reference);
+}
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__GET_NODE_PARAMETERS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -0,0 +1,141 @@
+// 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::SharedPtr context,
+    const std::vector<std::string> & arguments,
+    bool use_global_arguments);
+
+  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::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::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,150 @@
+// 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)
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeBaseInterface() = default;
+
+  /// 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::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_clock.hpp

@@ -0,0 +1,73 @@
+// 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__NODE_INTERFACES__NODE_CLOCK_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_clock_interface.hpp"
+#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/time_source.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeClock part of the Node API.
+class NodeClock : public NodeClockInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeClock)
+
+  RCLCPP_PUBLIC
+  explicit NodeClock(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
+    rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeClock();
+
+  /// Get a clock which will be kept up to date by the node.
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::Clock::SharedPtr
+  get_clock();
+
+private:
+  RCLCPP_DISABLE_COPY(NodeClock)
+
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
+  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
+
+  rclcpp::Clock::SharedPtr ros_clock_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_

rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp

@@ -0,0 +1,48 @@
+// 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__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeClock part of the Node API.
+class NodeClockInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeClockInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeClockInterface() = default;
+
+  /// Get a ROS clock which will be kept up to date by the node.
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::Clock::SharedPtr
+  get_clock() = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

@@ -0,0 +1,145 @@
+// 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 <utility>
+#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
+  std::vector<std::pair<std::string, std::string>>
+  get_node_names_and_namespaces() 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::SharedPtr
+  get_graph_event();
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  wait_for_graph_change(
+    rclcpp::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::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,158 @@
+// 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 <utility>
+#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)
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeGraphInterface() = default;
+
+  /// 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 a vector of existing node names and namespaces (pair of string).
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<std::pair<std::string, std::string>>
+  get_node_names_and_namespaces() 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::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::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_logging.hpp

@@ -0,0 +1,66 @@
+// 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__NODE_INTERFACES__NODE_LOGGING_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_LOGGING_HPP_
+
+#include <memory>
+
+#include "rclcpp/logger.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_logging_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeLogging part of the Node API.
+class NodeLogging : public NodeLoggingInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeLoggingInterface)
+
+  RCLCPP_PUBLIC
+  explicit NodeLogging(rclcpp::node_interfaces::NodeBaseInterface * node_base);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeLogging();
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::Logger
+  get_logger() const;
+
+  RCLCPP_PUBLIC
+  virtual
+  const char *
+  get_logger_name() const;
+
+private:
+  RCLCPP_DISABLE_COPY(NodeLogging)
+
+  /// Handle to the NodeBaseInterface given in the constructor.
+  rclcpp::node_interfaces::NodeBaseInterface * node_base_;
+
+  rclcpp::Logger logger_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_LOGGING_HPP_

rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp

@@ -0,0 +1,58 @@
+// 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__NODE_INTERFACES__NODE_LOGGING_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_LOGGING_INTERFACE_HPP_
+
+#include <memory>
+
+#include "rclcpp/logger.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeLogging part of the Node API.
+class NodeLoggingInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeLoggingInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeLoggingInterface() = default;
+
+  /// Return the logger of the node.
+  /** \return The logger of the node. */
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::Logger
+  get_logger() const = 0;
+
+  /// Return the logger name associated with the node.
+  /** \return The logger name associated with the node. */
+  RCLCPP_PUBLIC
+  virtual
+  const char *
+  get_logger_name() const = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_LOGGING_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -0,0 +1,155 @@
+// 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 <memory>
+#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/parameter_service.hpp"
+#include "rclcpp/publisher.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+// Internal struct for holding useful info about parameters
+struct ParameterInfo
+{
+  /// True if a user called declare_parameter()
+  bool is_declared = false;
+
+  /// Current value of the parameter.
+  rclcpp::ParameterValue value;
+
+  /// A description of the parameter
+  rcl_interfaces::msg::ParameterDescriptor descriptor;
+};
+
+/// Implementation of the NodeParameters part of the Node API.
+class NodeParameters : public NodeParametersInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeParameters)
+
+  RCLCPP_PUBLIC
+  NodeParameters(
+    const node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    const node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+    const node_interfaces::NodeServicesInterface::SharedPtr node_services,
+    const node_interfaces::NodeClockInterface::SharedPtr node_clock,
+    const std::vector<Parameter> & initial_parameters,
+    bool use_intra_process,
+    bool start_parameter_services,
+    bool allow_undeclared_parameters);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeParameters();
+
+  RCLCPP_PUBLIC
+  void
+  declare_parameter(
+    const std::string & name,
+    const rclcpp::ParameterValue & default_value,
+    bool read_only) override;
+
+  RCLCPP_PUBLIC
+  std::vector<rcl_interfaces::msg::SetParametersResult>
+  set_parameters(
+    const std::vector<rclcpp::Parameter> & parameters) override;
+
+  RCLCPP_PUBLIC
+  rcl_interfaces::msg::SetParametersResult
+  set_parameters_atomically(
+    const std::vector<rclcpp::Parameter> & parameters) override;
+
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::Parameter>
+  get_parameters(const std::vector<std::string> & names) const override;
+
+  RCLCPP_PUBLIC
+  rclcpp::Parameter
+  get_parameter(const std::string & name) const override;
+
+  RCLCPP_PUBLIC
+  bool
+  get_parameter(
+    const std::string & name,
+    rclcpp::Parameter & parameter) const override;
+
+  RCLCPP_PUBLIC
+  bool
+  get_parameters_by_prefix(
+    const std::string & prefix,
+    std::map<std::string, rclcpp::Parameter> & parameters) const override;
+
+  RCLCPP_PUBLIC
+  std::vector<rcl_interfaces::msg::ParameterDescriptor>
+  describe_parameters(const std::vector<std::string> & names) const override;
+
+  RCLCPP_PUBLIC
+  std::vector<uint8_t>
+  get_parameter_types(const std::vector<std::string> & names) const override;
+
+  RCLCPP_PUBLIC
+  rcl_interfaces::msg::ListParametersResult
+  list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const override;
+
+  RCLCPP_PUBLIC
+  void
+  register_param_change_callback(ParametersCallbackFunction callback) override;
+
+private:
+  RCLCPP_DISABLE_COPY(NodeParameters)
+
+  mutable std::mutex mutex_;
+
+  ParametersCallbackFunction parameters_callback_ = nullptr;
+
+  std::map<std::string, ParameterInfo> parameters_;
+
+  std::map<std::string, rclcpp::ParameterValue> initial_parameter_values_;
+
+  bool allow_undeclared_ = false;
+
+  Publisher<rcl_interfaces::msg::ParameterEvent>::SharedPtr events_publisher_;
+
+  std::shared_ptr<ParameterService> parameter_service_;
+
+  std::string combined_name_;
+
+  node_interfaces::NodeClockInterface::SharedPtr node_clock_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -0,0 +1,154 @@
+// 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 <map>
+#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)
+
+  /// Declare and initialize a parameter.
+  /* This method is used to declare that a parameter exists on this node.
+   * If a run-time user has provided an an initial value then it will be set in this method,
+   * otherwise the default_value will be set.
+   * \param[in] name the name of the parameter
+   * \param[in] default_value An initial value to be used if a run-time user did not override it.
+   * \param[in] read_only if True then this parameter may not be changed after initialization.
+   * \throws std::runtime_error if parameter has already been declared.
+   * \throws std::runtime_error if a parameter name is invalid.
+   * \throws rclcpp::exceptions::InvalidParameterValueException if initial value fails to be set.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  declare_parameter(
+    const std::string & name,
+    const rclcpp::ParameterValue & default_value = rclcpp::ParameterValue(),
+    bool read_only = false) = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeParametersInterface() = default;
+
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<rcl_interfaces::msg::SetParametersResult>
+  set_parameters(
+    const std::vector<rclcpp::Parameter> & parameters) = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  rcl_interfaces::msg::SetParametersResult
+  set_parameters_atomically(
+    const std::vector<rclcpp::Parameter> & parameters) = 0;
+
+  /// Get descriptions of parameters given their names.
+  /*
+   * \param[in] names a list of parameter names to check.
+   * \return the list of parameters that were found.
+   * Any parameter not found is omitted from the returned list.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<rclcpp::Parameter>
+  get_parameters(const std::vector<std::string> & names) const = 0;
+
+  /// Get the description of one parameter given a name.
+  /*
+   * \param[in] name the name of the parameter to look for.
+   * \return the parameter if it exists on the node.
+   * \throws std::out_of_range if the parameter does not exist on the node.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::Parameter
+  get_parameter(const std::string & name) const = 0;
+
+  /// Get the description of one parameter given a name.
+  /*
+   * \param[in] name the name of the parameter to look for.
+   * \param[out] parameter the description if parameter exists on the node.
+   * \return true if the parameter exists on the node, or
+   * \return false if the parameter does not exist.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  get_parameter(
+    const std::string & name,
+    rclcpp::Parameter & parameter) const = 0;
+
+  /// Get all parameters that have the specified prefix into the parameters map.
+  /*
+   * \param[in] prefix the name of the prefix to look for.
+   * \param[out] parameters a map of parameters that matched the prefix.
+   * \return true if any parameters with the prefix exists on the node, or
+   * \return false otherwise.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  get_parameters_by_prefix(
+    const std::string & prefix,
+    std::map<std::string, rclcpp::Parameter> & parameters) 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> &)>;
+
+  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::ClientBase::SharedPtr client_base_ptr,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group);
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_service(
+    rclcpp::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,57 @@
+// 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
+  ~NodeServicesInterface() = default;
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_client(
+    rclcpp::ClientBase::SharedPtr client_base_ptr,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_service(
+    rclcpp::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_time_source.hpp

@@ -0,0 +1,72 @@
+// Copyright 2018 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_TIME_SOURCE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_clock_interface.hpp"
+#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
+#include "rclcpp/node_interfaces/node_time_source_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/time_source.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeTimeSource part of the Node API.
+class NodeTimeSource : public NodeTimeSourceInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimeSource)
+
+  RCLCPP_PUBLIC
+  explicit NodeTimeSource(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
+    rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
+    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters
+  );
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTimeSource();
+
+private:
+  RCLCPP_DISABLE_COPY(NodeTimeSource)
+
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
+  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
+
+  rclcpp::TimeSource time_source_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp

@@ -0,0 +1,42 @@
+// Copyright 2018 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_TIME_SOURCE_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeTimeSource part of the Node API.
+class NodeTimeSourceInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimeSourceInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTimeSourceInterface() = default;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_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::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,50 @@
+// 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)
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTimersInterface() = default;
+
+  /// Add a timer to the node.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_timer(
+    rclcpp::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::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::PublisherBase::SharedPtr publisher);
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::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::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,82 @@
+// 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
+  ~NodeTopicsInterface() = default;
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::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::PublisherBase::SharedPtr publisher) = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::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::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/node_interfaces/node_waitables.hpp

@@ -0,0 +1,67 @@
+// Copyright 2018 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_WAITABLES_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_WAITABLES_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/client.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_waitables_interface.hpp"
+#include "rclcpp/waitable.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeWaitables part of the Node API.
+class NodeWaitables : public NodeWaitablesInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeWaitables)
+
+  RCLCPP_PUBLIC
+  explicit NodeWaitables(rclcpp::node_interfaces::NodeBaseInterface * node_base);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeWaitables();
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_waitable(
+    rclcpp::Waitable::SharedPtr waitable_base_ptr,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group);
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  remove_waitable(
+    rclcpp::Waitable::SharedPtr waitable_ptr,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group) noexcept;
+
+private:
+  RCLCPP_DISABLE_COPY(NodeWaitables)
+
+  rclcpp::node_interfaces::NodeBaseInterface * node_base_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_HPP_

rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp

@@ -0,0 +1,57 @@
+// Copyright 2018 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_WAITABLES_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeWaitables part of the Node API.
+class NodeWaitablesInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeWaitablesInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeWaitablesInterface() = default;
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_waitable(
+    rclcpp::Waitable::SharedPtr waitable_ptr,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0;
+
+  /// \note this function should not throw because it may be called in destructors
+  RCLCPP_PUBLIC
+  virtual
+  void
+  remove_waitable(
+    rclcpp::Waitable::SharedPtr waitable_ptr,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group) noexcept = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_

rclcpp/include/rclcpp/parameter.hpp

@@ -12,326 +12,218 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_PARAMETER_HPP_
-#define RCLCPP_RCLCPP_PARAMETER_HPP_
+#ifndef RCLCPP__PARAMETER_HPP_
+#define RCLCPP__PARAMETER_HPP_
 
+#include <iostream>
 #include <ostream>
 #include <sstream>
 #include <string>
+#include <vector>
 
-#include <rmw/rmw.h>
-
-#include <rcl_interfaces/msg/parameter.hpp>
-#include <rcl_interfaces/msg/parameter_type.hpp>
-#include <rcl_interfaces/msg/parameter_value.hpp>
+#include "rcl_interfaces/msg/parameter.hpp"
+#include "rclcpp/parameter_value.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
 
-namespace parameter
+class Parameter;
+
+namespace node_interfaces
+{
+struct ParameterInfo;
+}  // namespace node_interfaces
+
+namespace detail
 {
 
-enum ParameterType
-{
-  PARAMETER_NOT_SET=rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET,
-  PARAMETER_BOOL=rcl_interfaces::msg::ParameterType::PARAMETER_BOOL,
-  PARAMETER_INTEGER=rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER,
-  PARAMETER_DOUBLE=rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE,
-  PARAMETER_STRING=rcl_interfaces::msg::ParameterType::PARAMETER_STRING,
-  PARAMETER_BYTES=rcl_interfaces::msg::ParameterType::PARAMETER_BYTES,
-};
+// This helper function is required because you cannot do specialization on a
+// class method, so instead we specialize this template function and call it
+// from the unspecialized, but dependent, class method.
+template<typename T>
+auto
+get_value_helper(const rclcpp::Parameter * parameter);
 
-// Structure to store an arbitrary parameter with templated get/set methods
-class ParameterVariant
+}  // namespace detail
+
+/// Structure to store an arbitrary parameter with templated get/set methods.
+class Parameter
 {
 public:
-  ParameterVariant()
-  : name_("")
+  RCLCPP_PUBLIC
+  Parameter();
+
+  RCLCPP_PUBLIC
+  Parameter(const std::string & name, const ParameterValue & value);
+
+  template<typename ValueTypeT>
+  explicit Parameter(const std::string & name, ValueTypeT value)
+  : Parameter(name, ParameterValue(value))
   {
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET;
-  }
-  explicit ParameterVariant(const std::string & name, const bool bool_value)
-  : name_(name)
-  {
-    value_.bool_value = bool_value;
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
-  }
-  explicit ParameterVariant(const std::string & name, const int int_value)
-  : name_(name)
-  {
-    value_.integer_value = int_value;
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
-  }
-  explicit ParameterVariant(const std::string & name, const int64_t int_value)
-  : name_(name)
-  {
-    value_.integer_value = int_value;
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
-  }
-  explicit ParameterVariant(const std::string & name, const float double_value)
-  : name_(name)
-  {
-    value_.double_value = double_value;
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
-  }
-  explicit ParameterVariant(const std::string & name, const double double_value)
-  : name_(name)
-  {
-    value_.double_value = double_value;
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
-  }
-  explicit ParameterVariant(const std::string & name, const std::string & string_value)
-  : name_(name)
-  {
-    value_.string_value = string_value;
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
-  }
-  explicit ParameterVariant(const std::string & name, const char * string_value)
-  : ParameterVariant(name, std::string(string_value)) {}
-  explicit ParameterVariant(const std::string & name, const std::vector<uint8_t> & bytes_value)
-  : name_(name)
-  {
-    value_.bytes_value = bytes_value;
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BYTES;
   }
 
-  inline ParameterType get_type() const {return static_cast<ParameterType>(value_.type); }
+  RCLCPP_PUBLIC
+  explicit Parameter(const rclcpp::node_interfaces::ParameterInfo & parameter_info);
 
-  inline std::string get_type_name() const
-  {
-    switch (get_type()) {
-      case rclcpp::parameter::ParameterType::PARAMETER_BOOL:
-        return "bool";
-      case rclcpp::parameter::ParameterType::PARAMETER_INTEGER:
-        return "integer";
-      case rclcpp::parameter::ParameterType::PARAMETER_DOUBLE:
-        return "double";
-      case rclcpp::parameter::ParameterType::PARAMETER_STRING:
-        return "string";
-      case rclcpp::parameter::ParameterType::PARAMETER_BYTES:
-        return "bytes";
-      case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET:
-        return "not set";
-      default:
-        // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-        throw std::runtime_error(
-          "Unexpected type from ParameterVariant: " + std::to_string(get_type()));
-        // *INDENT-ON*
-    }
-  }
+  RCLCPP_PUBLIC
+  ParameterType
+  get_type() const;
 
-  inline std::string get_name() const & {return name_; }
+  RCLCPP_PUBLIC
+  std::string
+  get_type_name() const;
 
-  inline rcl_interfaces::msg::ParameterValue get_parameter_value() const
-  {
-    return value_;
-  }
+  RCLCPP_PUBLIC
+  const std::string &
+  get_name() const;
 
-  template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_INTEGER, int64_t>::type
+  RCLCPP_PUBLIC
+  rcl_interfaces::msg::ParameterValue
+  get_value_message() const;
+
+  /// Get the internal storage for the parameter value.
+  RCLCPP_PUBLIC
+  const rclcpp::ParameterValue &
+  get_parameter_value() const;
+
+  /// Get value of parameter using rclcpp::ParameterType as template argument.
+  template<ParameterType ParamT>
+  decltype(auto)
   get_value() const
   {
-    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) {
-      // TODO: use custom exception
-      throw std::runtime_error("Invalid type");
-    }
-    return value_.integer_value;
-  }
-  template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_DOUBLE, double>::type
-  get_value() const
-  {
-    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) {
-      // TODO: use custom exception
-      throw std::runtime_error("Invalid type");
-    }
-    return value_.double_value;
-  }
-  template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_STRING, const std::string &>::type
-  get_value() const
-  {
-    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_STRING) {
-      // TODO: use custom exception
-      throw std::runtime_error("Invalid type");
-    }
-    return value_.string_value;
-  }
-  template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_BOOL, bool>::type
-  get_value() const
-  {
-    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) {
-      // TODO: use custom exception
-      throw std::runtime_error("Invalid type");
-    }
-    return value_.bool_value;
-  }
-  template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_BYTES,
-  const std::vector<uint8_t> &>::type
-  get_value() const
-  {
-    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BYTES) {
-      // TODO: use custom exception
-      throw std::runtime_error("Invalid type");
-    }
-    return value_.bytes_value;
+    return value_.get<ParamT>();
   }
 
-  int64_t as_int() const {return get_value<ParameterType::PARAMETER_INTEGER>(); }
+  /// Get value of parameter using c++ types as template argument.
+  template<typename T>
+  decltype(auto)
+  get_value() const;
 
-  double as_double() const {return get_value<ParameterType::PARAMETER_DOUBLE>(); }
+  RCLCPP_PUBLIC
+  bool
+  as_bool() const;
 
-  const std::string & as_string() const {return get_value<ParameterType::PARAMETER_STRING>(); }
+  RCLCPP_PUBLIC
+  int64_t
+  as_int() const;
 
-  bool as_bool() const {return get_value<ParameterType::PARAMETER_BOOL>(); }
+  RCLCPP_PUBLIC
+  double
+  as_double() const;
 
-  const std::vector<uint8_t> & as_bytes() const
-  {
-    return get_value<ParameterType::PARAMETER_BYTES>();
-  }
+  RCLCPP_PUBLIC
+  const std::string &
+  as_string() const;
 
-  static ParameterVariant from_parameter(const rcl_interfaces::msg::Parameter & parameter)
-  {
-    switch (parameter.value.type) {
-      case PARAMETER_BOOL:
-        return ParameterVariant(parameter.name, parameter.value.bool_value);
-      case PARAMETER_INTEGER:
-        return ParameterVariant(parameter.name, parameter.value.integer_value);
-      case PARAMETER_DOUBLE:
-        return ParameterVariant(parameter.name, parameter.value.double_value);
-      case PARAMETER_STRING:
-        return ParameterVariant(parameter.name, parameter.value.string_value);
-      case PARAMETER_BYTES:
-        return ParameterVariant(parameter.name, parameter.value.bytes_value);
-      case PARAMETER_NOT_SET:
-        throw std::runtime_error("Type from ParameterValue is not set");
-      default:
-        // TODO(wjwwood): use custom exception
-        // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-        throw std::runtime_error(
-          "Unexpected type from ParameterVariant: " + std::to_string(parameter.value.type));
-        // *INDENT-ON*
-    }
-  }
+  RCLCPP_PUBLIC
+  const std::vector<uint8_t> &
+  as_byte_array() const;
 
-  rcl_interfaces::msg::Parameter to_parameter()
-  {
-    rcl_interfaces::msg::Parameter parameter;
-    parameter.name = name_;
-    parameter.value = value_;
-    return parameter;
-  }
+  RCLCPP_PUBLIC
+  const std::vector<bool> &
+  as_bool_array() const;
 
-  std::string value_to_string() const
-  {
-    switch (get_type()) {
-      case rclcpp::parameter::ParameterType::PARAMETER_BOOL:
-        return as_bool() ? "true" : "false";
-      case rclcpp::parameter::ParameterType::PARAMETER_INTEGER:
-        return std::to_string(as_int());
-      case rclcpp::parameter::ParameterType::PARAMETER_DOUBLE:
-        return std::to_string(as_double());
-      case rclcpp::parameter::ParameterType::PARAMETER_STRING:
-        return as_string();
-      case rclcpp::parameter::ParameterType::PARAMETER_BYTES:
-        {
-          std::stringstream bytes;
-          bool first_byte = true;
-          bytes << "[" << std::hex;
-          for (auto & byte : as_bytes()) {
-            bytes << "0x" << byte;
-            if (!first_byte) {
-              bytes << ", ";
-            } else {
-              first_byte = false;
-            }
-          }
-          return bytes.str();
-        }
-      case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET:
-        return "not set";
-      default:
-        // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-        throw std::runtime_error(
-          "Unexpected type from ParameterVariant: " + std::to_string(get_type()));
-        // *INDENT-ON*
-    }
-  }
+  RCLCPP_PUBLIC
+  const std::vector<int64_t> &
+  as_integer_array() const;
+
+  RCLCPP_PUBLIC
+  const std::vector<double> &
+  as_double_array() const;
+
+  RCLCPP_PUBLIC
+  const std::vector<std::string> &
+  as_string_array() const;
+
+  RCLCPP_PUBLIC
+  static Parameter
+  from_parameter_msg(const rcl_interfaces::msg::Parameter & parameter);
+
+  RCLCPP_PUBLIC
+  rcl_interfaces::msg::Parameter
+  to_parameter_msg() const;
+
+  RCLCPP_PUBLIC
+  std::string
+  value_to_string() const;
 
 private:
   std::string name_;
-  rcl_interfaces::msg::ParameterValue value_;
+  ParameterValue value_;
 };
 
+/// Return a json encoded version of the parameter intended for a dict.
+RCLCPP_PUBLIC
+std::string
+_to_json_dict_entry(const Parameter & param);
 
-/* Return a json encoded version of the parameter intended for a dict. */
-std::string _to_json_dict_entry(const ParameterVariant & param)
+RCLCPP_PUBLIC
+std::ostream &
+operator<<(std::ostream & os, const rclcpp::Parameter & pv);
+
+RCLCPP_PUBLIC
+std::ostream &
+operator<<(std::ostream & os, const std::vector<Parameter> & parameters);
+
+namespace detail
 {
-  std::stringstream ss;
-  ss << "\"" << param.get_name() << "\": ";
-  ss << "{\"type\": \"" << param.get_type_name() << "\", ";
-  ss << "\"value\": \"" << param.value_to_string() << "\"}";
-  return ss.str();
+
+template<typename T>
+auto
+get_value_helper(const rclcpp::Parameter * parameter)
+{
+  return parameter->get_parameter_value().get<T>();
 }
 
+// Specialization allowing Parameter::get() to return a const ref to the parameter value object.
+template<>
+inline
+auto
+get_value_helper<rclcpp::ParameterValue>(const rclcpp::Parameter * parameter)
+{
+  return parameter->get_parameter_value();
+}
 
-} /* namespace parameter */
+// Specialization allowing Parameter::get() to return a const ref to the parameter itself.
+template<>
+inline
+auto
+get_value_helper<rclcpp::Parameter>(const rclcpp::Parameter * parameter)
+{
+  // Use this labmda to ensure it's a const reference being returned (and not a copy).
+  auto type_enforcing_lambda =
+    [&parameter]() -> const rclcpp::Parameter & {
+      return *parameter;
+    };
+  return type_enforcing_lambda();
+}
 
-} /* namespace rclcpp */
+}  // namespace detail
+
+template<typename T>
+decltype(auto)
+Parameter::get_value() const
+{
+  // use the helper to specialize for the ParameterValue and Parameter cases.
+  return detail::get_value_helper<T>(this);
+}
+
+}  // namespace rclcpp
 
 namespace std
 {
-/* Return a json encoded version of the parameter intended for a list. */
-inline std::string to_string(const rclcpp::parameter::ParameterVariant & param)
-{
-  std::stringstream ss;
-  ss << "{\"name\": \"" << param.get_name() << "\", ";
-  ss << "\"type\": \"" << param.get_type_name() << "\", ";
-  ss << "\"value\": \"" << param.value_to_string() << "\"}";
-  return ss.str();
-}
 
-/* Return a json encoded version of a vector of parameters, as a string*/
-inline std::string to_string(const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
-{
-  std::stringstream ss;
-  ss << "{";
-  bool first = true;
-  for (const auto & pv : parameters) {
-    if (first == false) {
-      ss << ", ";
-    } else {
-      first = false;
-    }
-    ss << rclcpp::parameter::_to_json_dict_entry(pv);
-  }
-  ss << "}";
-  return ss.str();
-}
+/// Return a json encoded version of the parameter intended for a list.
+RCLCPP_PUBLIC
+std::string
+to_string(const rclcpp::Parameter & param);
 
-} /* namespace std */
+/// Return a json encoded version of a vector of parameters, as a string.
+RCLCPP_PUBLIC
+std::string
+to_string(const std::vector<rclcpp::Parameter> & parameters);
 
-namespace rclcpp
-{
-namespace parameter
-{
+}  // namespace std
 
-std::ostream & operator<<(std::ostream & os, const rclcpp::parameter::ParameterVariant & pv)
-{
-  os << std::to_string(pv);
-  return os;
-}
-
-std::ostream & operator<<(std::ostream & os, const std::vector<ParameterVariant> & parameters)
-{
-  os << std::to_string(parameters);
-  return os;
-}
-
-} /* namespace parameter */
-
-} /* namespace rclcpp */
-
-#endif /* RCLCPP_RCLCPP_PARAMETER_HPP_ */
+#endif  // RCLCPP__PARAMETER_HPP_

rclcpp/include/rclcpp/parameter_client.hpp

@@ -12,239 +12,157 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_PARAMETER_CLIENT_HPP_
-#define RCLCPP_RCLCPP_PARAMETER_CLIENT_HPP_
+#ifndef RCLCPP__PARAMETER_CLIENT_HPP_
+#define RCLCPP__PARAMETER_CLIENT_HPP_
 
+#include <memory>
 #include <string>
+#include <utility>
+#include <vector>
 
-#include <rmw/rmw.h>
-
-#include <rclcpp/executors.hpp>
-#include <rclcpp/macros.hpp>
-#include <rclcpp/node.hpp>
-#include <rclcpp/parameter.hpp>
-
-#include <rcl_interfaces/msg/parameter.hpp>
-#include <rcl_interfaces/msg/parameter_event.hpp>
-#include <rcl_interfaces/msg/parameter_value.hpp>
-#include <rcl_interfaces/srv/describe_parameters.hpp>
-#include <rcl_interfaces/srv/get_parameters.hpp>
-#include <rcl_interfaces/srv/get_parameter_types.hpp>
-#include <rcl_interfaces/srv/list_parameters.hpp>
-#include <rcl_interfaces/srv/set_parameters.hpp>
-#include <rcl_interfaces/srv/set_parameters_atomically.hpp>
+#include "rcl_interfaces/msg/parameter.hpp"
+#include "rcl_interfaces/msg/parameter_event.hpp"
+#include "rcl_interfaces/msg/parameter_value.hpp"
+#include "rcl_interfaces/srv/describe_parameters.hpp"
+#include "rcl_interfaces/srv/get_parameter_types.hpp"
+#include "rcl_interfaces/srv/get_parameters.hpp"
+#include "rcl_interfaces/srv/list_parameters.hpp"
+#include "rcl_interfaces/srv/set_parameters.hpp"
+#include "rcl_interfaces/srv/set_parameters_atomically.hpp"
+#include "rclcpp/executors.hpp"
+#include "rclcpp/create_subscription.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rmw/rmw.h"
 
 namespace rclcpp
 {
 
-namespace parameter_client
-{
-
 class AsyncParametersClient
 {
-
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(AsyncParametersClient);
+  RCLCPP_SMART_PTR_DEFINITIONS(AsyncParametersClient)
 
-  AsyncParametersClient(const rclcpp::node::Node::SharedPtr node,
-    const std::string & remote_node_name = "")
-  : node_(node)
-  {
-    if (remote_node_name != "") {
-      remote_node_name_ = remote_node_name;
-    } else {
-      remote_node_name_ = node_->get_name();
-    }
-    get_parameters_client_ = node_->create_client<rcl_interfaces::srv::GetParameters>(
-      remote_node_name_ + "__get_parameters");
-    get_parameter_types_client_ = node_->create_client<rcl_interfaces::srv::GetParameterTypes>(
-      remote_node_name_ + "__get_parameter_types");
-    set_parameters_client_ = node_->create_client<rcl_interfaces::srv::SetParameters>(
-      remote_node_name_ + "__set_parameters");
-    list_parameters_client_ = node_->create_client<rcl_interfaces::srv::ListParameters>(
-      remote_node_name_ + "__list_parameters");
-    describe_parameters_client_ = node_->create_client<rcl_interfaces::srv::DescribeParameters>(
-      remote_node_name_ + "__describe_parameters");
-  }
+  RCLCPP_PUBLIC
+  AsyncParametersClient(
+    const rclcpp::node_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);
 
-  std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>
+  RCLCPP_PUBLIC
+  AsyncParametersClient(
+    const rclcpp::Node::SharedPtr node,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+
+  RCLCPP_PUBLIC
+  AsyncParametersClient(
+    rclcpp::Node * node,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+
+  RCLCPP_PUBLIC
+  std::shared_future<std::vector<rclcpp::Parameter>>
   get_parameters(
     const std::vector<std::string> & names,
-    std::function<void(
-      std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>)> callback = nullptr)
-  {
-    auto promise_result =
-      std::make_shared<std::promise<std::vector<rclcpp::parameter::ParameterVariant>>>();
-    auto future_result = promise_result->get_future().share();
+    std::function<
+      void(std::shared_future<std::vector<rclcpp::Parameter>>)
+    > callback = nullptr);
 
-    auto request = std::make_shared<rcl_interfaces::srv::GetParameters::Request>();
-    request->names = names;
-
-    get_parameters_client_->async_send_request(
-      request,
-      [request, promise_result, future_result, &callback](
-        rclcpp::client::Client<rcl_interfaces::srv::GetParameters>::SharedFuture cb_f) {
-          std::vector<rclcpp::parameter::ParameterVariant> parameter_variants;
-          auto & pvalues = cb_f.get()->values;
-
-          for (auto & pvalue : pvalues) {
-            auto i = &pvalue - &pvalues[0];
-            rcl_interfaces::msg::Parameter parameter;
-            parameter.name = request->names[i];
-            parameter.value = pvalue;
-            parameter_variants.push_back(rclcpp::parameter::ParameterVariant::from_parameter(
-              parameter));
-          }
-
-          promise_result->set_value(parameter_variants);
-          if (callback != nullptr) {
-            callback(future_result);
-          }
-        }
-    );
-
-    return future_result;
-  }
-
-  std::shared_future<std::vector<rclcpp::parameter::ParameterType>>
+  RCLCPP_PUBLIC
+  std::shared_future<std::vector<rclcpp::ParameterType>>
   get_parameter_types(
     const std::vector<std::string> & names,
-    std::function<void(
-      std::shared_future<std::vector<rclcpp::parameter::ParameterType>>)> callback = nullptr)
-  {
-    auto promise_result =
-      std::make_shared<std::promise<std::vector<rclcpp::parameter::ParameterType>>>();
-    auto future_result = promise_result->get_future().share();
-
-    auto request = std::make_shared<rcl_interfaces::srv::GetParameterTypes::Request>();
-    request->names = names;
-
-    get_parameter_types_client_->async_send_request(
-      request,
-      [promise_result, future_result, &callback](
-        rclcpp::client::Client<rcl_interfaces::srv::GetParameterTypes>::SharedFuture cb_f) {
-          std::vector<rclcpp::parameter::ParameterType> types;
-          auto & pts = cb_f.get()->types;
-          for (auto & pt : pts) {
-            pts.push_back(static_cast<rclcpp::parameter::ParameterType>(pt));
-          }
-          promise_result->set_value(types);
-          if (callback != nullptr) {
-            callback(future_result);
-          }
-        }
-    );
-
-    return future_result;
-  }
+    std::function<
+      void(std::shared_future<std::vector<rclcpp::ParameterType>>)
+    > callback = nullptr);
 
+  RCLCPP_PUBLIC
   std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>
   set_parameters(
-    const std::vector<rclcpp::parameter::ParameterVariant> & parameters,
-    std::function<void(std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>)> callback =
-    nullptr)
-  {
-    auto promise_result =
-      std::make_shared<std::promise<std::vector<rcl_interfaces::msg::SetParametersResult>>>();
-    auto future_result = promise_result->get_future().share();
-
-    auto request = std::make_shared<rcl_interfaces::srv::SetParameters::Request>();
-
-    std::transform(parameters.begin(), parameters.end(), std::back_inserter(
-        request->parameters), [](
-        rclcpp::parameter::ParameterVariant p) {return p.to_parameter(); });
-
-    set_parameters_client_->async_send_request(
-      request,
-      [promise_result, future_result, &callback](
-        rclcpp::client::Client<rcl_interfaces::srv::SetParameters>::SharedFuture cb_f) {
-          promise_result->set_value(cb_f.get()->results);
-          if (callback != nullptr) {
-            callback(future_result);
-          }
-        }
-    );
-
-    return future_result;
-  }
+    const std::vector<rclcpp::Parameter> & parameters,
+    std::function<
+      void(std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>)
+    > callback = nullptr);
 
+  RCLCPP_PUBLIC
   std::shared_future<rcl_interfaces::msg::SetParametersResult>
   set_parameters_atomically(
-    const std::vector<rclcpp::parameter::ParameterVariant> & parameters,
-    std::function<void(std::shared_future<rcl_interfaces::msg::SetParametersResult>)> callback =
-    nullptr)
-  {
-    auto promise_result =
-      std::make_shared<std::promise<rcl_interfaces::msg::SetParametersResult>>();
-    auto future_result = promise_result->get_future().share();
-
-    auto request = std::make_shared<rcl_interfaces::srv::SetParametersAtomically::Request>();
-
-    std::transform(parameters.begin(), parameters.end(), std::back_inserter(
-        request->parameters), [](
-        rclcpp::parameter::ParameterVariant p) {return p.to_parameter(); });
-
-    set_parameters_atomically_client_->async_send_request(
-      request,
-      [promise_result, future_result, &callback](
-        rclcpp::client::Client<rcl_interfaces::srv::SetParametersAtomically>::SharedFuture cb_f) {
-          promise_result->set_value(cb_f.get()->result);
-          if (callback != nullptr) {
-            callback(future_result);
-          }
-        }
-    );
-
-    return future_result;
-  }
+    const std::vector<rclcpp::Parameter> & parameters,
+    std::function<
+      void(std::shared_future<rcl_interfaces::msg::SetParametersResult>)
+    > callback = nullptr);
 
+  RCLCPP_PUBLIC
   std::shared_future<rcl_interfaces::msg::ListParametersResult>
   list_parameters(
     const std::vector<std::string> & prefixes,
     uint64_t depth,
-    std::function<void(std::shared_future<rcl_interfaces::msg::ListParametersResult>)> callback =
-    nullptr)
+    std::function<
+      void(std::shared_future<rcl_interfaces::msg::ListParametersResult>)
+    > callback = nullptr);
+
+  template<
+    typename CallbackT,
+    typename Alloc = std::allocator<void>,
+    typename SubscriptionT =
+    rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent, Alloc>>
+  typename rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
+  on_parameter_event(CallbackT && callback)
   {
-    auto promise_result =
-      std::make_shared<std::promise<rcl_interfaces::msg::ListParametersResult>>();
-    auto future_result = promise_result->get_future().share();
+    using rclcpp::message_memory_strategy::MessageMemoryStrategy;
+    auto msg_mem_strat =
+      MessageMemoryStrategy<rcl_interfaces::msg::ParameterEvent, Alloc>::create_default();
 
-    auto request = std::make_shared<rcl_interfaces::srv::ListParameters::Request>();
-    request->prefixes = prefixes;
-    request->depth = depth;
+    using rcl_interfaces::msg::ParameterEvent;
+    return rclcpp::create_subscription<
+      ParameterEvent, CallbackT, Alloc, ParameterEvent, 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>());
+  }
 
-    list_parameters_client_->async_send_request(
-      request,
-      [promise_result, future_result, &callback](
-        rclcpp::client::Client<rcl_interfaces::srv::ListParameters>::SharedFuture cb_f) {
-          promise_result->set_value(cb_f.get()->result);
-          if (callback != nullptr) {
-            callback(future_result);
-          }
-        }
+  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)
     );
-
-    return future_result;
   }
 
-  template<typename FunctorT>
-  typename rclcpp::subscription::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
-  on_parameter_event(FunctorT callback)
-  {
-    return node_->create_subscription<rcl_interfaces::msg::ParameterEvent>(
-      "parameter_events", callback, rmw_qos_profile_parameter_events);
-  }
+protected:
+  RCLCPP_PUBLIC
+  bool
+  wait_for_service_nanoseconds(std::chrono::nanoseconds timeout);
 
 private:
-  const rclcpp::node::Node::SharedPtr node_;
-  rclcpp::client::Client<rcl_interfaces::srv::GetParameters>::SharedPtr get_parameters_client_;
-  rclcpp::client::Client<rcl_interfaces::srv::GetParameterTypes>::SharedPtr
+  const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface_;
+  rclcpp::Client<rcl_interfaces::srv::GetParameters>::SharedPtr get_parameters_client_;
+  rclcpp::Client<rcl_interfaces::srv::GetParameterTypes>::SharedPtr
     get_parameter_types_client_;
-  rclcpp::client::Client<rcl_interfaces::srv::SetParameters>::SharedPtr set_parameters_client_;
-  rclcpp::client::Client<rcl_interfaces::srv::SetParametersAtomically>::SharedPtr
+  rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr set_parameters_client_;
+  rclcpp::Client<rcl_interfaces::srv::SetParametersAtomically>::SharedPtr
     set_parameters_atomically_client_;
-  rclcpp::client::Client<rcl_interfaces::srv::ListParameters>::SharedPtr list_parameters_client_;
-  rclcpp::client::Client<rcl_interfaces::srv::DescribeParameters>::SharedPtr
+  rclcpp::Client<rcl_interfaces::srv::ListParameters>::SharedPtr list_parameters_client_;
+  rclcpp::Client<rcl_interfaces::srv::DescribeParameters>::SharedPtr
     describe_parameters_client_;
   std::string remote_node_name_;
 };
@@ -252,108 +170,131 @@ private:
 class SyncParametersClient
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient);
+  RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient)
 
-  SyncParametersClient(
-    rclcpp::node::Node::SharedPtr node)
-  : node_(node)
-  {
-    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
-    async_parameters_client_ = std::make_shared<AsyncParametersClient>(node);
-  }
+  RCLCPP_PUBLIC
+  explicit SyncParametersClient(
+    rclcpp::Node::SharedPtr node,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
+  RCLCPP_PUBLIC
   SyncParametersClient(
     rclcpp::executor::Executor::SharedPtr executor,
-    rclcpp::node::Node::SharedPtr node)
-  : executor_(executor), node_(node)
-  {
-    async_parameters_client_ = std::make_shared<AsyncParametersClient>(node);
-  }
+    rclcpp::Node::SharedPtr node,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
-  std::vector<rclcpp::parameter::ParameterVariant>
-  get_parameters(const std::vector<std::string> & parameter_names)
+  RCLCPP_PUBLIC
+  explicit SyncParametersClient(
+    rclcpp::Node * node,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+
+  RCLCPP_PUBLIC
+  SyncParametersClient(
+    rclcpp::executor::Executor::SharedPtr executor,
+    rclcpp::Node * node,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+
+  RCLCPP_PUBLIC
+  SyncParametersClient(
+    rclcpp::executor::Executor::SharedPtr executor,
+    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
+  std::vector<rclcpp::Parameter>
+  get_parameters(const std::vector<std::string> & parameter_names);
+
+  RCLCPP_PUBLIC
+  bool
+  has_parameter(const std::string & parameter_name);
+
+  template<typename T>
+  T
+  get_parameter_impl(
+    const std::string & parameter_name, std::function<T()> parameter_not_found_handler)
   {
-    auto f = async_parameters_client_->get_parameters(parameter_names);
-    if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
-      rclcpp::executors::FutureReturnCode::SUCCESS)
-    {
-      return f.get();
+    std::vector<std::string> names;
+    names.push_back(parameter_name);
+    auto vars = get_parameters(names);
+    if ((vars.size() != 1) || (vars[0].get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET)) {
+      return parameter_not_found_handler();
+    } else {
+      return static_cast<T>(vars[0].get_value<T>());
     }
-    // Return an empty vector if unsuccessful
-    return std::vector<rclcpp::parameter::ParameterVariant>();
   }
 
-  std::vector<rclcpp::parameter::ParameterType>
-  get_parameter_types(const std::vector<std::string> & parameter_names)
+  template<typename T>
+  T
+  get_parameter(const std::string & parameter_name, const T & default_value)
   {
-    auto f = async_parameters_client_->get_parameter_types(parameter_names);
-
-    if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
-      rclcpp::executors::FutureReturnCode::SUCCESS)
-    {
-      return f.get();
-    }
-    return std::vector<rclcpp::parameter::ParameterType>();
+    return get_parameter_impl(
+      parameter_name,
+      std::function<T()>([&default_value]() -> T {return default_value;}));
   }
 
+  template<typename T>
+  T
+  get_parameter(const std::string & parameter_name)
+  {
+    return get_parameter_impl(
+      parameter_name,
+      std::function<T()>([]() -> T {throw std::runtime_error("Parameter not set");}));
+  }
+
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::ParameterType>
+  get_parameter_types(const std::vector<std::string> & parameter_names);
+
+  RCLCPP_PUBLIC
   std::vector<rcl_interfaces::msg::SetParametersResult>
-  set_parameters(const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
-  {
-    auto f = async_parameters_client_->set_parameters(parameters);
-
-    if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
-      rclcpp::executors::FutureReturnCode::SUCCESS)
-    {
-      return f.get();
-    }
-    return std::vector<rcl_interfaces::msg::SetParametersResult>();
-  }
+  set_parameters(const std::vector<rclcpp::Parameter> & parameters);
 
+  RCLCPP_PUBLIC
   rcl_interfaces::msg::SetParametersResult
-  set_parameters_atomically(const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
-  {
-    auto f = async_parameters_client_->set_parameters_atomically(parameters);
-
-    if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
-      rclcpp::executors::FutureReturnCode::SUCCESS)
-    {
-      return f.get();
-    }
-
-    throw std::runtime_error("Unable to get result of set parameters service call.");
-  }
+  set_parameters_atomically(const std::vector<rclcpp::Parameter> & parameters);
 
+  RCLCPP_PUBLIC
   rcl_interfaces::msg::ListParametersResult
   list_parameters(
     const std::vector<std::string> & parameter_prefixes,
-    uint64_t depth)
+    uint64_t depth);
+
+  template<typename CallbackT>
+  typename rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
+  on_parameter_event(CallbackT && callback)
   {
-    auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth);
-
-    if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
-      rclcpp::executors::FutureReturnCode::SUCCESS)
-    {
-      return f.get();
-    }
-
-    throw std::runtime_error("Unable to get result of list parameters service call.");
+    return async_parameters_client_->on_parameter_event(std::forward<CallbackT>(callback));
   }
 
-  template<typename FunctorT>
-  typename rclcpp::subscription::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
-  on_parameter_event(FunctorT callback)
+  RCLCPP_PUBLIC
+  bool
+  service_is_ready() const
   {
-    return async_parameters_client_->on_parameter_event(callback);
+    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_;
+  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_;
   AsyncParametersClient::SharedPtr async_parameters_client_;
 };
 
-} /* namespace parameter_client */
+}  // namespace rclcpp
 
-} /* namespace rclcpp */
-
-#endif /* RCLCPP_RCLCPP_PARAMETER_CLIENT_HPP_ */
+#endif  // RCLCPP__PARAMETER_CLIENT_HPP_

rclcpp/include/rclcpp/parameter_events_filter.hpp

@@ -0,0 +1,78 @@
+// 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_EVENTS_FILTER_HPP_
+#define RCLCPP__PARAMETER_EVENTS_FILTER_HPP_
+
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "rcl_interfaces/msg/parameter.hpp"
+#include "rcl_interfaces/msg/parameter_event.hpp"
+#include "rcl_interfaces/msg/parameter_value.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rmw/rmw.h"
+
+namespace rclcpp
+{
+
+class ParameterEventsFilter
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventsFilter)
+  enum class EventType {NEW, DELETED, CHANGED};  ///< An enum for the type of event.
+  /// Used for the listed results
+  using EventPair = std::pair<EventType, rcl_interfaces::msg::Parameter *>;
+
+  /// Construct a filtered view of a parameter event.
+  /**
+   * \param[in] event The parameter event message to filter.
+   * \param[in] names A list of parameter names of interest.
+   * \param[in] types A list of the types of parameter events of iterest.
+  *    EventType NEW, DELETED, or CHANGED
+  *
+  * Example Usage:
+  * If you have recieved a parameter event and are only interested in parameters foo and
+  * bar being added or changed but don't care about deletion.
+  *    auto res = rclcpp::ParameterEventsFilter(
+  *      event_shared_ptr,
+  *      {"foo", "bar"},
+  *      {rclcpp::ParameterEventsFilter::EventType::NEW, rclcpp::ParameterEventsFilter::EventType::CHANGED});
+  */
+  RCLCPP_PUBLIC
+  ParameterEventsFilter(
+    rcl_interfaces::msg::ParameterEvent::SharedPtr event,
+    const std::vector<std::string> & names,
+    const std::vector<EventType> & types);
+
+  /// Get the result of the filter
+  /**
+   * \return A std::vector<EventPair> of all matching parameter changes in this event.
+   */
+  RCLCPP_PUBLIC
+  const std::vector<EventPair> & get_events() const;
+
+private:
+  // access only allowed via const accessor.
+  std::vector<EventPair> result_;  ///< Storage of the resultant vector
+  rcl_interfaces::msg::ParameterEvent::SharedPtr event_;  ///< Keep event in scope
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__PARAMETER_EVENTS_FILTER_HPP_

rclcpp/include/rclcpp/parameter_map.hpp

@@ -0,0 +1,53 @@
+// Copyright 2018 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_MAP_HPP_
+#define RCLCPP__PARAMETER_MAP_HPP_
+
+#include <rcl_yaml_param_parser/types.h>
+
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/parameter_value.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// A map of fully qualified node names to a list of parameters
+using ParameterMap = std::unordered_map<std::string, std::vector<Parameter>>;
+
+/// Convert parameters from rcl_yaml_param_parser into C++ class instances.
+/// \param[in] c_params C structures containing parameters for multiple nodes.
+/// \returns a map where the keys are fully qualified node names and values a list of parameters.
+/// \throws InvalidParametersException if the `rcl_params_t` is inconsistent or invalid.
+RCLCPP_PUBLIC
+ParameterMap
+parameter_map_from(const rcl_params_t * const c_params);
+
+/// Convert parameter value from rcl_yaml_param_parser into a C++ class instance.
+/// \param[in] c_value C structure containing a value of a parameter.
+/// \returns an instance of a parameter value
+/// \throws InvalidParameterValueException if the `rcl_variant_t` is inconsistent or invalid.
+RCLCPP_PUBLIC
+ParameterValue
+parameter_value_from(const rcl_variant_t * const c_value);
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__PARAMETER_MAP_HPP_

rclcpp/include/rclcpp/parameter_service.hpp

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

rclcpp/include/rclcpp/parameter_value.hpp

@@ -0,0 +1,317 @@
+// Copyright 2018 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_VALUE_HPP_
+#define RCLCPP__PARAMETER_VALUE_HPP_
+
+#include <exception>
+#include <iostream>
+#include <ostream>
+#include <sstream>
+#include <string>
+#include <vector>
+
+#include "rcl_interfaces/msg/parameter_type.hpp"
+#include "rcl_interfaces/msg/parameter_value.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+enum ParameterType : uint8_t
+{
+  PARAMETER_NOT_SET = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET,
+  PARAMETER_BOOL = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL,
+  PARAMETER_INTEGER = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER,
+  PARAMETER_DOUBLE = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE,
+  PARAMETER_STRING = rcl_interfaces::msg::ParameterType::PARAMETER_STRING,
+  PARAMETER_BYTE_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY,
+  PARAMETER_BOOL_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY,
+  PARAMETER_INTEGER_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY,
+  PARAMETER_DOUBLE_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY,
+  PARAMETER_STRING_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY,
+};
+
+/// Return the name of a parameter type
+RCLCPP_PUBLIC
+std::string
+to_string(ParameterType type);
+
+RCLCPP_PUBLIC
+std::ostream &
+operator<<(std::ostream & os, ParameterType type);
+
+/// Indicate the parameter type does not match the expected type.
+class ParameterTypeException : public std::runtime_error
+{
+public:
+  /// Construct an instance.
+  /**
+   * \param[in] expected the expected parameter type.
+   * \param[in] actual the actual parameter type.
+   */
+  RCLCPP_PUBLIC
+  ParameterTypeException(ParameterType expected, ParameterType actual)
+  : std::runtime_error("expected [" + to_string(expected) + "] got [" + to_string(actual) + "]")
+  {}
+};
+
+/// Store the type and value of a parameter.
+class ParameterValue
+{
+public:
+  /// Construct a parameter value with type PARAMETER_NOT_SET.
+  RCLCPP_PUBLIC
+  ParameterValue();
+  /// Construct a parameter value from a message.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const rcl_interfaces::msg::ParameterValue & value);
+  /// Construct a parameter value with type PARAMETER_BOOL.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const bool bool_value);
+  /// Construct a parameter value with type PARAMETER_INTEGER.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const int int_value);
+  /// Construct a parameter value with type PARAMETER_INTEGER.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const int64_t int_value);
+  /// Construct a parameter value with type PARAMETER_DOUBLE.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const float double_value);
+  /// Construct a parameter value with type PARAMETER_DOUBLE.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const double double_value);
+  /// Construct a parameter value with type PARAMETER_STRING.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::string & string_value);
+  /// Construct a parameter value with type PARAMETER_STRING.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const char * string_value);
+  /// Construct a parameter value with type PARAMETER_BYTE_ARRAY.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::vector<uint8_t> & byte_array_value);
+  /// Construct a parameter value with type PARAMETER_BOOL_ARRAY.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::vector<bool> & bool_array_value);
+  /// Construct a parameter value with type PARAMETER_INTEGER_ARRAY.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::vector<int> & int_array_value);
+  /// Construct a parameter value with type PARAMETER_INTEGER_ARRAY.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::vector<int64_t> & int_array_value);
+  /// Construct a parameter value with type PARAMETER_DOUBLE_ARRAY.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::vector<float> & double_array_value);
+  /// Construct a parameter value with type PARAMETER_DOUBLE_ARRAY.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::vector<double> & double_array_value);
+  /// Construct a parameter value with type PARAMETER_STRING_ARRAY.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::vector<std::string> & string_array_value);
+
+  /// Return an enum indicating the type of the set value.
+  RCLCPP_PUBLIC
+  ParameterType
+  get_type() const;
+
+  /// Return a message populated with the parameter value
+  RCLCPP_PUBLIC
+  rcl_interfaces::msg::ParameterValue
+  to_value_msg() const;
+
+  // The following get() variants require the use of ParameterType
+
+  template<ParameterType type>
+  typename std::enable_if<type == ParameterType::PARAMETER_BOOL, bool>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) {
+      throw ParameterTypeException(ParameterType::PARAMETER_BOOL, get_type());
+    }
+    return value_.bool_value;
+  }
+
+  template<ParameterType type>
+  typename std::enable_if<type == ParameterType::PARAMETER_INTEGER, int64_t>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) {
+      throw ParameterTypeException(ParameterType::PARAMETER_INTEGER, get_type());
+    }
+    return value_.integer_value;
+  }
+
+  template<ParameterType type>
+  typename std::enable_if<type == ParameterType::PARAMETER_DOUBLE, double>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) {
+      throw ParameterTypeException(ParameterType::PARAMETER_DOUBLE, get_type());
+    }
+    return value_.double_value;
+  }
+
+  template<ParameterType type>
+  typename std::enable_if<type == ParameterType::PARAMETER_STRING, const std::string &>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_STRING) {
+      throw ParameterTypeException(ParameterType::PARAMETER_STRING, get_type());
+    }
+    return value_.string_value;
+  }
+
+  template<ParameterType type>
+  typename std::enable_if<
+    type == ParameterType::PARAMETER_BYTE_ARRAY, const std::vector<uint8_t> &>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY) {
+      throw ParameterTypeException(ParameterType::PARAMETER_BYTE_ARRAY, get_type());
+    }
+    return value_.byte_array_value;
+  }
+
+  template<ParameterType type>
+  typename std::enable_if<
+    type == ParameterType::PARAMETER_BOOL_ARRAY, const std::vector<bool> &>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY) {
+      throw ParameterTypeException(ParameterType::PARAMETER_BOOL_ARRAY, get_type());
+    }
+    return value_.bool_array_value;
+  }
+
+  template<ParameterType type>
+  typename std::enable_if<
+    type == ParameterType::PARAMETER_INTEGER_ARRAY, const std::vector<int64_t> &>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY) {
+      throw ParameterTypeException(ParameterType::PARAMETER_INTEGER_ARRAY, get_type());
+    }
+    return value_.integer_array_value;
+  }
+
+  template<ParameterType type>
+  typename std::enable_if<
+    type == ParameterType::PARAMETER_DOUBLE_ARRAY, const std::vector<double> &>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY) {
+      throw ParameterTypeException(ParameterType::PARAMETER_DOUBLE_ARRAY, get_type());
+    }
+    return value_.double_array_value;
+  }
+
+  template<ParameterType type>
+  typename std::enable_if<
+    type == ParameterType::PARAMETER_STRING_ARRAY, const std::vector<std::string> &>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY) {
+      throw ParameterTypeException(ParameterType::PARAMETER_STRING_ARRAY, get_type());
+    }
+    return value_.string_array_value;
+  }
+
+  // The following get() variants allow the use of primitive types
+
+  template<typename type>
+  typename std::enable_if<std::is_same<type, bool>::value, bool>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_BOOL>();
+  }
+
+  template<typename type>
+  typename std::enable_if<
+    std::is_integral<type>::value && !std::is_same<type, bool>::value, int64_t>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_INTEGER>();
+  }
+
+  template<typename type>
+  typename std::enable_if<std::is_floating_point<type>::value, double>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_DOUBLE>();
+  }
+
+  template<typename type>
+  typename std::enable_if<std::is_convertible<type, std::string>::value, const std::string &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_STRING>();
+  }
+
+  template<typename type>
+  typename std::enable_if<
+    std::is_convertible<
+      type, const std::vector<uint8_t> &>::value, const std::vector<uint8_t> &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_BYTE_ARRAY>();
+  }
+
+  template<typename type>
+  typename std::enable_if<
+    std::is_convertible<
+      type, const std::vector<bool> &>::value, const std::vector<bool> &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_BOOL_ARRAY>();
+  }
+
+  template<typename type>
+  typename std::enable_if<
+    std::is_convertible<
+      type, const std::vector<int64_t> &>::value, const std::vector<int64_t> &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_INTEGER_ARRAY>();
+  }
+
+  template<typename type>
+  typename std::enable_if<
+    std::is_convertible<
+      type, const std::vector<double> &>::value, const std::vector<double> &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_DOUBLE_ARRAY>();
+  }
+
+  template<typename type>
+  typename std::enable_if<
+    std::is_convertible<
+      type, const std::vector<std::string> &>::value, const std::vector<std::string> &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_STRING_ARRAY>();
+  }
+
+private:
+  rcl_interfaces::msg::ParameterValue value_;
+};
+
+/// Return the value of a parameter as a string
+RCLCPP_PUBLIC
+std::string
+to_string(const ParameterValue & type);
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__PARAMETER_VALUE_HPP_

rclcpp/include/rclcpp/publisher.hpp

@@ -18,114 +18,94 @@
 #include <rmw/error_handling.h>
 #include <rmw/rmw.h>
 
+#include <functional>
 #include <iostream>
 #include <memory>
-#include <mutex>
 #include <sstream>
 #include <string>
 
-#include "rcl_interfaces/msg/intra_process_message.hpp"
-#include "rmw/impl/cpp/demangle.hpp"
+#include "rcl/error_handling.h"
+#include "rcl/publisher.h"
 
+#include "rcl_interfaces/msg/intra_process_message.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
-{
-class Node;
-}  // namespace node
-
-namespace publisher
+// Forward declaration is used for friend statement.
+namespace node_interfaces
 {
+class NodeTopicsInterface;
+}
 
 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)
-  : node_handle_(node_handle), publisher_handle_(publisher_handle),
-    intra_process_publisher_handle_(nullptr),
-    topic_(topic), queue_size_(queue_size),
-    intra_process_publisher_id_(0), store_intra_process_message_(nullptr)
-  {
-    // Life time of this object is tied to the publisher handle.
-    if (rmw_get_gid_for_publisher(publisher_handle_, &rmw_gid_) != RMW_RET_OK) {
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-      throw std::runtime_error(
-        std::string("failed to get publisher gid: ") + rmw_get_error_string_safe());
-      // *INDENT-ON*
-    }
-  }
+    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.
-  virtual ~PublisherBase()
-  {
-    if (intra_process_publisher_handle_) {
-      if (rmw_destroy_publisher(node_handle_.get(), intra_process_publisher_handle_)) {
-        fprintf(
-          stderr,
-          "Error in destruction of intra process rmw publisher handle: %s\n",
-          rmw_get_error_string_safe());
-      }
-    }
-    if (publisher_handle_) {
-      if (rmw_destroy_publisher(node_handle_.get(), publisher_handle_) != RMW_RET_OK) {
-        fprintf(
-          stderr,
-          "Error in destruction of rmw publisher handle: %s\n",
-          rmw_get_error_string_safe());
-      }
-    }
-  }
+  RCLCPP_PUBLIC
+  virtual ~PublisherBase();
 
   /// Get the topic that this publisher publishes on.
-  // \return The topic name.
-  const std::string &
-  get_topic_name() const
-  {
-    return topic_;
-  }
+  /** \return The topic name. */
+  RCLCPP_PUBLIC
+  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
-  {
-    return queue_size_;
-  }
+  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
-  {
-    return rmw_gid_;
-  }
+  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
-  {
-    return intra_process_rmw_gid_;
-  }
+  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.
   /**
@@ -133,11 +113,9 @@ public:
    * \param[in] gid Reference to a gid.
    * \return True if the publisher's gid matches the input.
    */
+  RCLCPP_PUBLIC
   bool
-  operator==(const rmw_gid_t & gid) const
-  {
-    return *this == &gid;
-  }
+  operator==(const rmw_gid_t & gid) const;
 
   /// Compare this publisher to a pointer gid.
   /**
@@ -145,98 +123,69 @@ public:
    * \param[in] gid A pointer to a gid.
    * \return True if this publisher's gid matches the input.
    */
+  RCLCPP_PUBLIC
   bool
-  operator==(const rmw_gid_t * gid) const
-  {
-    bool result = false;
-    auto ret = rmw_compare_gids_equal(gid, &this->get_gid(), &result);
-    if (ret != RMW_RET_OK) {
-      throw std::runtime_error(
-              std::string("failed to compare gids: ") + rmw_get_error_string_safe());
-    }
-    if (!result) {
-      ret = rmw_compare_gids_equal(gid, &this->get_intra_process_gid(), &result);
-      if (ret != RMW_RET_OK) {
-        throw std::runtime_error(
-                std::string("failed to compare gids: ") + rmw_get_error_string_safe());
-      }
-    }
-    return result;
-  }
+  operator==(const rmw_gid_t * gid) const;
 
-  typedef std::function<uint64_t(uint64_t, void *, const std::type_info &)> StoreMessageCallbackT;
+  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)
-  {
-    intra_process_publisher_id_ = intra_process_publisher_id;
-    store_intra_process_message_ = callback;
-    intra_process_publisher_handle_ = intra_process_publisher_handle;
-    // Life time of this object is tied to the publisher handle.
-    auto ret = rmw_get_gid_for_publisher(intra_process_publisher_handle_, &intra_process_rmw_gid_);
-    if (ret != RMW_RET_OK) {
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-      throw std::runtime_error(
-        std::string("failed to create intra process publisher gid: ") +
-        rmw_get_error_string_safe());
-      // *INDENT-ON*
-    }
-  }
+    const rcl_publisher_options_t & intra_process_options);
 
-  std::shared_ptr<rmw_node_t> node_handle_;
+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_;
 
   rmw_gid_t rmw_gid_;
   rmw_gid_t intra_process_rmw_gid_;
-
-  std::mutex intra_process_publish_mutex_;
 };
 
 /// A publisher publishes messages of any type to a topic.
 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());
@@ -250,21 +199,24 @@ public:
       //   See: http://stackoverflow.com/questions/11002641/dynamic-casting-for-unique-ptr
       MessageT * msg_ptr = msg.get();
       msg.release();
-      uint64_t message_seq;
-      {
-        std::lock_guard<std::mutex> lock(intra_process_publish_mutex_);
-        message_seq =
-          store_intra_process_message_(intra_process_publisher_id_, msg_ptr, typeid(MessageT));
-      }
+      uint64_t message_seq =
+        store_intra_process_message_(intra_process_publisher_id_, msg_ptr, typeid(MessageT));
       rcl_interfaces::msg::IntraProcessMessage ipm;
       ipm.publisher_id = intra_process_publisher_id_;
       ipm.message_sequence = message_seq;
-      auto status = rmw_publish(intra_process_publisher_handle_, &ipm);
-      if (status != RMW_RET_OK) {
-        // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-        throw std::runtime_error(
-          std::string("failed to publish intra process message: ") + rmw_get_error_string_safe());
-        // *INDENT-ON*
+      auto status = rcl_publish(&intra_process_publisher_handle_, &ipm);
+      if (RCL_RET_PUBLISHER_INVALID == status) {
+        rcl_reset_error();  // next call will reset error message if not context
+        if (rcl_publisher_is_valid_except_context(&intra_process_publisher_handle_)) {
+          rcl_context_t * context = rcl_publisher_get_context(&intra_process_publisher_handle_);
+          if (nullptr != context && !rcl_context_is_valid(context)) {
+            // publisher is invalid due to context being shutdown
+            return;
+          }
+        }
+      }
+      if (RCL_RET_OK != status) {
+        rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish intra process message");
       }
     } else {
       // Always destroy the message, even if we don't consume it, for consistency.
@@ -272,7 +224,7 @@ public:
     }
   }
 
-  void
+  virtual void
   publish(const std::shared_ptr<MessageT> & msg)
   {
     // Avoid allocating when not using intra process.
@@ -291,7 +243,7 @@ public:
     return this->publish(unique_msg);
   }
 
-  void
+  virtual void
   publish(std::shared_ptr<const MessageT> msg)
   {
     // Avoid allocating when not using intra process.
@@ -310,7 +262,7 @@ public:
     return this->publish(unique_msg);
   }
 
-  void
+  virtual void
   publish(const MessageT & msg)
   {
     // Avoid allocating when not using intra process.
@@ -325,6 +277,34 @@ 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);
+  }
+
+  void
+  publish(const rcl_serialized_message_t * serialized_msg)
+  {
+    if (store_intra_process_message_) {
+      // TODO(Karsten1987): support serialized message passed by intraprocess
+      throw std::runtime_error("storing serialized messages in intra process is not supported yet");
+    }
+    auto status = rcl_publish_serialized_message(&publisher_handle_, serialized_msg);
+    if (RCL_RET_OK != status) {
+      rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish serialized message");
+    }
+  }
+
+  void
+  publish(std::shared_ptr<const rcl_serialized_message_t> serialized_msg)
+  {
+    return this->publish(serialized_msg.get());
+  }
+
   std::shared_ptr<MessageAlloc> get_allocator() const
   {
     return message_allocator_;
@@ -334,12 +314,19 @@ 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)
-      throw std::runtime_error(
-        std::string("failed to publish message: ") + rmw_get_error_string_safe());
-      // *INDENT-ON*
+    auto status = rcl_publish(&publisher_handle_, msg);
+    if (RCL_RET_PUBLISHER_INVALID == status) {
+      rcl_reset_error();  // next call will reset error message if not context
+      if (rcl_publisher_is_valid_except_context(&publisher_handle_)) {
+        rcl_context_t * context = rcl_publisher_get_context(&publisher_handle_);
+        if (nullptr != context && !rcl_context_is_valid(context)) {
+          // publisher is invalid due to context being shutdown
+          return;
+        }
+      }
+    }
+    if (RCL_RET_OK != status) {
+      rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish message");
     }
   }
 
@@ -348,7 +335,6 @@ protected:
   MessageDeleter message_deleter_;
 };
 
-}  // 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::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::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::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::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::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<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

@@ -12,28 +12,27 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_RATE_HPP_
-#define RCLCPP_RCLCPP_RATE_HPP_
+#ifndef RCLCPP__RATE_HPP_
+#define RCLCPP__RATE_HPP_
 
 #include <chrono>
 #include <memory>
 #include <thread>
 
-#include <rclcpp/macros.hpp>
-#include <rclcpp/utilities.hpp>
+#include "rclcpp/macros.hpp"
+#include "rclcpp/utilities.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
-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() = 0;
+  virtual bool is_steady() const = 0;
   virtual void reset() = 0;
 };
 
@@ -45,13 +44,13 @@ template<class Clock = std::chrono::high_resolution_clock>
 class GenericRate : public RateBase
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(GenericRate);
+  RCLCPP_SMART_PTR_DEFINITIONS(GenericRate)
 
-  GenericRate(double rate)
+  explicit GenericRate(double rate)
   : GenericRate<Clock>(
       duration_cast<nanoseconds>(duration<double>(1.0 / rate)))
   {}
-  GenericRate(std::chrono::nanoseconds period)
+  explicit GenericRate(std::chrono::nanoseconds period)
   : period_(period), last_interval_(Clock::now())
   {}
 
@@ -83,12 +82,12 @@ public:
       return false;
     }
     // Sleep (will get interrupted by ctrl-c, may not sleep full time)
-    rclcpp::utilities::sleep_for(time_to_sleep);
+    rclcpp::sleep_for(time_to_sleep);
     return true;
   }
 
   virtual bool
-  is_steady()
+  is_steady() const
   {
     return Clock::is_steady;
   }
@@ -105,17 +104,16 @@ public:
   }
 
 private:
-  RCLCPP_DISABLE_COPY(GenericRate);
+  RCLCPP_DISABLE_COPY(GenericRate)
 
   std::chrono::nanoseconds period_;
-  std::chrono::time_point<Clock> last_interval_;
-
+  using ClockDurationNano = std::chrono::duration<typename Clock::rep, std::nano>;
+  std::chrono::time_point<Clock, ClockDurationNano> last_interval_;
 };
 
 using Rate = GenericRate<std::chrono::system_clock>;
 using WallRate = GenericRate<std::chrono::steady_clock>;
 
-} // namespace rate
-} // namespace rclcpp
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_RATE_HPP_ */
+#endif  // RCLCPP__RATE_HPP_

rclcpp/include/rclcpp/rclcpp.hpp

@@ -12,104 +12,145 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_RCLCPP_HPP_
-#define RCLCPP_RCLCPP_RCLCPP_HPP_
+/** \mainpage rclcpp: ROS Client Library for C++
+ *
+ * `rclcpp` provides the canonical C++ API for interacting with ROS.
+ * It consists of these main components:
+ *
+ * - Node
+ *   - rclcpp::Node
+ *   - rclcpp/node.hpp
+ * - Publisher
+ *   - rclcpp::Node::create_publisher()
+ *   - rclcpp::Publisher
+ *   - rclcpp::Publisher::publish()
+ *   - rclcpp/publisher.hpp
+ * - Subscription
+ *   - rclcpp::Node::create_subscription()
+ *   - rclcpp::Subscription
+ *   - rclcpp/subscription.hpp
+ * - Service Client
+ *   - rclcpp::Node::create_client()
+ *   - rclcpp::Client
+ *   - rclcpp/client.hpp
+ * - Service Server
+ *   - rclcpp::Node::create_service()
+ *   - rclcpp::Service
+ *   - rclcpp/service.hpp
+ * - Timer
+ *   - rclcpp::Node::create_wall_timer()
+ *   - rclcpp::WallTimer
+ *   - rclcpp::TimerBase
+ *   - rclcpp/timer.hpp
+ * - Parameters:
+ *   - rclcpp::Node::set_parameters()
+ *   - rclcpp::Node::get_parameters()
+ *   - rclcpp::Node::get_parameter()
+ *   - rclcpp::Node::describe_parameters()
+ *   - rclcpp::Node::list_parameters()
+ *   - rclcpp::Node::register_param_change_callback()
+ *   - rclcpp::Parameter
+ *   - rclcpp::ParameterValue
+ *   - rclcpp::AsyncParametersClient
+ *   - rclcpp::SyncParametersClient
+ *   - rclcpp/parameter.hpp
+ *   - rclcpp/parameter_value.hpp
+ *   - rclcpp/parameter_client.hpp
+ *   - rclcpp/parameter_service.hpp
+ * - Rate:
+ *   - rclcpp::Rate
+ *   - rclcpp::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::SingleThreadedExecutor
+ *   - rclcpp::executors::SingleThreadedExecutor::add_node()
+ *   - rclcpp::executors::SingleThreadedExecutor::spin()
+ *   - rclcpp::executors::MultiThreadedExecutor
+ *   - rclcpp::executors::MultiThreadedExecutor::add_node()
+ *   - rclcpp::executors::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::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::get_graph_event()
+ *   - rclcpp::Node::wait_for_graph_change()
+ *   - rclcpp::Event
+ * - List topic names and types:
+ *   - rclcpp::Node::get_topic_names_and_types()
+ * - Get the number of publishers or subscribers on a topic:
+ *   - rclcpp::Node::count_publishers()
+ *   - rclcpp::Node::count_subscribers()
+ *
+ * And components related to logging:
+ *
+ * - Logging macros:
+ *   - Some examples (not exhaustive):
+ *     - RCLCPP_DEBUG()
+ *     - RCLCPP_INFO()
+ *     - RCLCPP_WARN_ONCE()
+ *     - RCLCPP_ERROR_SKIPFIRST()
+ *   - rclcpp/logging.hpp
+ * - Logger:
+ *   - rclcpp::Logger
+ *   - rclcpp/logger.hpp
+ *   - rclcpp::Node::get_logger()
+ *
+ * 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
+ *   - 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_
 
 #include <csignal>
 #include <memory>
 
-#include <rclcpp/node.hpp>
-#include <rclcpp/parameter.hpp>
-#include <rclcpp/parameter_client.hpp>
-#include <rclcpp/parameter_service.hpp>
-#include <rclcpp/executors.hpp>
-#include <rclcpp/rate.hpp>
-#include <rclcpp/utilities.hpp>
+#include "rclcpp/executors.hpp"
+#include "rclcpp/logging.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/parameter_client.hpp"
+#include "rclcpp/parameter_service.hpp"
+#include "rclcpp/rate.hpp"
+#include "rclcpp/time.hpp"
+#include "rclcpp/utilities.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
 
-namespace rclcpp
-{
-// NOLINTNEXTLINE(runtime/int)
-const std::chrono::seconds operator"" _s(unsigned long long s)
-{
-  return std::chrono::seconds(s);
-}
-const std::chrono::nanoseconds operator"" _s(long double s)
-{
-  return std::chrono::duration_cast<std::chrono::nanoseconds>(
-    std::chrono::duration<long double>(s));
-}
-
-const std::chrono::nanoseconds
-// NOLINTNEXTLINE(runtime/int)
-operator"" _ms(unsigned long long ms)
-{
-  return std::chrono::milliseconds(ms);
-}
-const std::chrono::nanoseconds
-operator"" _ms(long double ms)
-{
-  return std::chrono::duration_cast<std::chrono::nanoseconds>(
-    std::chrono::duration<long double, std::milli>(ms));
-}
-
-const std::chrono::nanoseconds
-// NOLINTNEXTLINE(runtime/int)
-operator"" _ns(unsigned long long ns)
-{
-  return std::chrono::nanoseconds(ns);
-}
-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 escalations.
-// For example, this next line escalates type "rclcpp:node::Node" to "rclcpp::Node"
-using rclcpp::node::Node;
-using rclcpp::publisher::Publisher;
-using rclcpp::subscription::SubscriptionBase;
-using rclcpp::subscription::Subscription;
-using rclcpp::rate::GenericRate;
-using rclcpp::rate::WallRate;
-using rclcpp::timer::GenericTimer;
-using rclcpp::timer::TimerBase;
-using rclcpp::timer::WallTimer;
-using ContextSharedPtr = rclcpp::context::Context::SharedPtr;
-using rclcpp::utilities::ok;
-using rclcpp::utilities::shutdown;
-using rclcpp::utilities::init;
-using rclcpp::utilities::sleep_for;
-
-/// Create a default single-threaded executor and execute any immediately available work.
-// \param[in] node_ptr Shared pointer to the node to spin.
-void spin_some(Node::SharedPtr node_ptr)
-{
-  rclcpp::executors::SingleThreadedExecutor executor;
-  executor.spin_node_some(node_ptr);
-}
-
-/// Create a default single-threaded executor and spin the specified node.
-// \param[in] node_ptr Shared pointer to the node to spin.
-void spin(Node::SharedPtr node_ptr)
-{
-  rclcpp::executors::SingleThreadedExecutor executor;
-  executor.add_node(node_ptr);
-  executor.spin();
-}
-
-template<typename FutureT, typename TimeT = std::milli>
-rclcpp::executors::FutureReturnCode
-spin_until_future_complete(
-  Node::SharedPtr node_ptr, std::shared_future<FutureT> & future,
-  std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
-{
-  rclcpp::executors::SingleThreadedExecutor executor;
-  return rclcpp::executors::spin_node_until_future_complete<FutureT>(
-    executor, node_ptr, future, timeout);
-}
-
-} /* namespace rclcpp */
-
-#endif /* RCLCPP_RCLCPP_RCLCPP_HPP_ */
+#endif  // RCLCPP__RCLCPP_HPP_

rclcpp/include/rclcpp/scope_exit.hpp

@@ -0,0 +1,52 @@
+// Copyright 2015 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// Based on: http://the-witness.net/news/2012/11/scopeexit-in-c11/
+// But I changed the lambda to include by reference rather than value, see:
+// http://the-witness.net/news/2012/11/scopeexit-in-c11/comment-page-1/#comment-86873
+
+#ifndef RCLCPP__SCOPE_EXIT_HPP_
+#define RCLCPP__SCOPE_EXIT_HPP_
+
+#include <functional>
+
+#include "rclcpp/macros.hpp"
+
+namespace rclcpp
+{
+
+template<typename Callable>
+struct ScopeExit
+{
+  explicit ScopeExit(Callable callable)
+  : callable_(callable) {}
+  ~ScopeExit() {callable_();}
+
+private:
+  Callable callable_;
+};
+
+template<typename Callable>
+ScopeExit<Callable>
+make_scope_exit(Callable callable)
+{
+  return ScopeExit<Callable>(callable);
+}
+
+}  // namespace rclcpp
+
+#define RCLCPP_SCOPE_EXIT(code) \
+  auto RCLCPP_STRING_JOIN(scope_exit_, __LINE__) = rclcpp::make_scope_exit([&]() {code;})
+
+#endif  // RCLCPP__SCOPE_EXIT_HPP_

rclcpp/include/rclcpp/service.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_SERVICE_HPP_
-#define RCLCPP_RCLCPP_SERVICE_HPP_
+#ifndef RCLCPP__SERVICE_HPP_
+#define RCLCPP__SERVICE_HPP_
 
 #include <functional>
 #include <iostream>
@@ -21,138 +21,224 @@
 #include <sstream>
 #include <string>
 
-#include <rmw/error_handling.h>
-#include <rmw/rmw.h>
+#include "rcl/error_handling.h"
+#include "rcl/service.h"
 
-#include <rclcpp/macros.hpp>
-#include <rclcpp/any_service_callback.hpp>
+#include "rclcpp/any_service_callback.hpp"
+#include "rclcpp/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 "rclcpp/logging.hpp"
+#include "rmw/error_handling.h"
+#include "rmw/rmw.h"
 
 namespace rclcpp
 {
 
-namespace service
-{
-
 class ServiceBase
 {
-
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase);
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(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)
-  {}
+  RCLCPP_PUBLIC
+  explicit ServiceBase(
+    std::shared_ptr<rcl_node_t> node_handle);
 
-  virtual ~ServiceBase()
-  {
-    if (service_handle_) {
-      if (rmw_destroy_service(service_handle_) != RMW_RET_OK) {
-        std::stringstream ss;
-        ss << "Error in destruction of rmw service_handle_ handle: " <<
-          rmw_get_error_string_safe() << '\n';
-        (std::cerr << ss.str()).flush();
-      }
-    }
-  }
+  RCLCPP_PUBLIC
+  virtual ~ServiceBase();
 
-  std::string get_service_name()
-  {
-    return this->service_name_;
-  }
+  RCLCPP_PUBLIC
+  const char *
+  get_service_name();
 
-  const rmw_service_t * get_service_handle()
-  {
-    return this->service_handle_;
-  }
+  RCLCPP_PUBLIC
+  std::shared_ptr<rcl_service_t>
+  get_service_handle();
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<const rcl_service_t>
+  get_service_handle() const;
 
   virtual std::shared_ptr<void> create_request() = 0;
-  virtual std::shared_ptr<void> create_request_header() = 0;
+  virtual std::shared_ptr<rmw_request_id_t> create_request_header() = 0;
   virtual void handle_request(
-    std::shared_ptr<void> request_header,
+    std::shared_ptr<rmw_request_id_t> request_header,
     std::shared_ptr<void> request) = 0;
 
-private:
-  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_;
-  std::string service_name_;
+  RCLCPP_PUBLIC
+  const rcl_node_t *
+  get_rcl_node_handle() const;
 
+  std::shared_ptr<rcl_node_t> node_handle_;
+
+  std::shared_ptr<rcl_service_t> service_handle_;
+  bool owns_rcl_handle_ = true;
 };
 
-using namespace any_service_callback;
-
 template<typename ServiceT>
 class Service : public ServiceBase
 {
 public:
   using CallbackType = std::function<
-      void(
-        const std::shared_ptr<typename ServiceT::Request>,
-        std::shared_ptr<typename ServiceT::Response>)>;
+    void (
+      const std::shared_ptr<typename ServiceT::Request>,
+      std::shared_ptr<typename ServiceT::Response>)>;
 
   using CallbackWithHeaderType = std::function<
-      void(
-        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);
+    void (
+      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)
 
   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), any_callback_(any_callback)
+  {
+    using rosidl_typesupport_cpp::get_service_type_support_handle;
+    auto service_type_support_handle = get_service_type_support_handle<ServiceT>();
+
+    std::weak_ptr<rcl_node_t> weak_node_handle(node_handle_);
+    // rcl does the static memory allocation here
+    service_handle_ = std::shared_ptr<rcl_service_t>(
+      new rcl_service_t, [weak_node_handle](rcl_service_t * service)
+      {
+        auto handle = weak_node_handle.lock();
+        if (handle) {
+          if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) {
+            RCLCPP_ERROR(
+              rclcpp::get_node_logger(handle.get()).get_child("rclcpp"),
+              "Error in destruction of rcl service handle: %s",
+              rcl_get_error_string().str);
+            rcl_reset_error();
+          }
+        } else {
+          RCLCPP_ERROR(
+            rclcpp::get_logger("rclcpp"),
+            "Error in destruction of rcl service handle: "
+            "the Node Handle was destructed too early. You will leak memory");
+        }
+        delete service;
+      });
+    *service_handle_.get() = rcl_get_zero_initialized_service();
+
+    rcl_ret_t ret = rcl_service_init(
+      service_handle_.get(),
+      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,
+    std::shared_ptr<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
+    if (!rcl_service_is_valid(service_handle.get())) {
+      // *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(
+    std::shared_ptr<rcl_node_t> node_handle,
+    rcl_service_t * service_handle,
+    AnyServiceCallback<ServiceT> any_callback)
+  : ServiceBase(node_handle),
+    any_callback_(any_callback)
+  {
+    // check if service handle was initialized
+    if (!rcl_service_is_valid(service_handle)) {
+      // *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*
+    }
+
+    // In this case, rcl owns the service handle memory
+    service_handle_ = std::shared_ptr<rcl_service_t>(new rcl_service_t);
+    service_handle_->impl = service_handle->impl;
+  }
 
   Service() = delete;
 
+  virtual ~Service()
+  {
+  }
+
   std::shared_ptr<void> create_request()
   {
     return std::shared_ptr<void>(new typename ServiceT::Request());
   }
 
-  std::shared_ptr<void> create_request_header()
+  std::shared_ptr<rmw_request_id_t> create_request_header()
   {
     // TODO(wjwwood): This should probably use rmw_request_id's allocator.
     //                (since it is a C type)
-    return std::shared_ptr<void>(new rmw_request_id_t);
+    return std::shared_ptr<rmw_request_id_t>(new rmw_request_id_t);
   }
 
-  void handle_request(std::shared_ptr<void> request_header, std::shared_ptr<void> request)
+  void handle_request(
+    std::shared_ptr<rmw_request_id_t> request_header,
+    std::shared_ptr<void> request)
   {
     auto typed_request = std::static_pointer_cast<typename ServiceT::Request>(request);
-    auto typed_request_header = std::static_pointer_cast<rmw_request_id_t>(request_header);
     auto response = std::shared_ptr<typename ServiceT::Response>(new typename ServiceT::Response);
-    any_callback_.dispatch(typed_request_header, typed_request, response);
-    send_response(typed_request_header, response);
+    any_callback_.dispatch(request_header, typed_request, response);
+    send_response(request_header, response);
   }
 
   void send_response(
     std::shared_ptr<rmw_request_id_t> req_id,
     std::shared_ptr<typename ServiceT::Response> response)
   {
-    rmw_ret_t status = rmw_send_response(get_service_handle(), req_id.get(), response.get());
-    if (status != RMW_RET_OK) {
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-      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().get(), 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_;
 };
 
-} /* namespace service */
-} /* namespace rclcpp */
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_SERVICE_HPP_ */
+#endif  // RCLCPP__SERVICE_HPP_

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -18,16 +18,21 @@
 #include <memory>
 #include <vector>
 
+#include "rcl/allocator.h"
+
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/memory_strategy.hpp"
 #include "rclcpp/node.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcutils/logging_macros.h"
+
+#include "rmw/types.h"
 
 namespace rclcpp
 {
-
 namespace memory_strategies
 {
-
 namespace allocator_memory_strategy
 {
 
@@ -41,103 +46,108 @@ 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;
-  using ExecDeleter = allocator::Deleter<ExecAlloc, executor::AnyExecutable>;
   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());
     allocator_ = std::make_shared<VoidAlloc>(*allocator.get());
   }
 
   AllocatorMemoryStrategy()
   {
-    executable_allocator_ = std::make_shared<ExecAlloc>();
     allocator_ = std::make_shared<VoidAlloc>();
   }
 
-  size_t fill_subscriber_handles(void ** & ptr)
+  void add_guard_condition(const rcl_guard_condition_t * guard_condition)
   {
-    for (auto & subscription : subscriptions_) {
-      subscriber_handles_.push_back(subscription->get_subscription_handle()->data);
-      if (subscription->get_intra_process_subscription_handle()) {
-        subscriber_handles_.push_back(subscription->get_intra_process_subscription_handle()->data);
+    for (const auto & existing_guard_condition : guard_conditions_) {
+      if (existing_guard_condition == guard_condition) {
+        return;
       }
     }
-    ptr = subscriber_handles_.data();
-    return subscriber_handles_.size();
+    guard_conditions_.push_back(guard_condition);
   }
 
-  // return the new number of services
-  size_t fill_service_handles(void ** & ptr)
+  void remove_guard_condition(const rcl_guard_condition_t * guard_condition)
   {
-    for (auto & service : services_) {
-      service_handles_.push_back(service->get_service_handle()->data);
+    for (auto it = guard_conditions_.begin(); it != guard_conditions_.end(); ++it) {
+      if (*it == guard_condition) {
+        guard_conditions_.erase(it);
+        break;
+      }
     }
-    ptr = service_handles_.data();
-    return service_handles_.size();
-  }
-
-  // return the new number of clients
-  size_t fill_client_handles(void ** & ptr)
-  {
-    for (auto & client : clients_) {
-      client_handles_.push_back(client->get_client_handle()->data);
-    }
-    ptr = client_handles_.data();
-    return client_handles_.size();
-  }
-
-  void clear_active_entities()
-  {
-    subscriptions_.clear();
-    services_.clear();
-    clients_.clear();
   }
 
   void clear_handles()
   {
-    subscriber_handles_.clear();
+    subscription_handles_.clear();
     service_handles_.clear();
     client_handles_.clear();
+    timer_handles_.clear();
+    waitable_handles_.clear();
   }
 
-  void revalidate_handles()
+  virtual void remove_null_handles(rcl_wait_set_t * wait_set)
   {
-    {
-      VectorRebind<void *> temp;
-      for (auto & subscriber_handle : subscriber_handles_) {
-        if (subscriber_handle) {
-          temp.push_back(subscriber_handle);
-        }
+    // TODO(jacobperron): Check if wait set sizes are what we expect them to be?
+    //                    e.g. wait_set->size_of_clients == client_handles_.size()
+
+    // Important to use subscription_handles_.size() instead of wait set's size since
+    // there may be more subscriptions in the wait set due to Waitables added to the end.
+    // The same logic applies for other entities.
+    for (size_t i = 0; i < subscription_handles_.size(); ++i) {
+      if (!wait_set->subscriptions[i]) {
+        subscription_handles_[i].reset();
       }
-      subscriber_handles_.swap(temp);
     }
-    {
-      VectorRebind<void *> temp;
-      for (auto & service_handle : service_handles_) {
-        if (service_handle) {
-          temp.push_back(service_handle);
-        }
+    for (size_t i = 0; i < service_handles_.size(); ++i) {
+      if (!wait_set->services[i]) {
+        service_handles_[i].reset();
       }
-      service_handles_.swap(temp);
     }
-    {
-      VectorRebind<void *> temp;
-      for (auto & client_handle : client_handles_) {
-        if (client_handle) {
-          temp.push_back(client_handle);
-        }
+    for (size_t i = 0; i < client_handles_.size(); ++i) {
+      if (!wait_set->clients[i]) {
+        client_handles_[i].reset();
       }
-      client_handles_.swap(temp);
     }
+    for (size_t i = 0; i < timer_handles_.size(); ++i) {
+      if (!wait_set->timers[i]) {
+        timer_handles_[i].reset();
+      }
+    }
+    for (size_t i = 0; i < waitable_handles_.size(); ++i) {
+      if (!waitable_handles_[i]->is_ready(wait_set)) {
+        waitable_handles_[i].reset();
+      }
+    }
+
+    subscription_handles_.erase(
+      std::remove(subscription_handles_.begin(), subscription_handles_.end(), nullptr),
+      subscription_handles_.end()
+    );
+
+    service_handles_.erase(
+      std::remove(service_handles_.begin(), service_handles_.end(), nullptr),
+      service_handles_.end()
+    );
+
+    client_handles_.erase(
+      std::remove(client_handles_.begin(), client_handles_.end(), nullptr),
+      client_handles_.end()
+    );
+
+    timer_handles_.erase(
+      std::remove(timer_handles_.begin(), timer_handles_.end(), nullptr),
+      timer_handles_.end()
+    );
+
+    waitable_handles_.erase(
+      std::remove(waitable_handles_.begin(), waitable_handles_.end(), nullptr),
+      waitable_handles_.end()
+    );
   }
 
   bool collect_entities(const WeakNodeVector & weak_nodes)
@@ -146,7 +156,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()) {
@@ -157,17 +167,35 @@ 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 & client : group->get_client_ptrs()) {
+        for (auto & weak_client : group->get_client_ptrs()) {
+          auto client = weak_client.lock();
           if (client) {
-            clients_.push_back(client);
+            client_handles_.push_back(client->get_client_handle());
+          }
+        }
+        for (auto & weak_timer : group->get_timer_ptrs()) {
+          auto timer = weak_timer.lock();
+          if (timer) {
+            timer_handles_.push_back(timer->get_timer_handle());
+          }
+        }
+        for (auto & weak_waitable : group->get_waitable_ptrs()) {
+          auto waitable = weak_waitable.lock();
+          if (waitable) {
+            waitable_handles_.push_back(waitable);
           }
         }
       }
@@ -175,33 +203,85 @@ public:
     return has_invalid_weak_nodes;
   }
 
-
-  /// Provide a newly initialized AnyExecutable object.
-  // \return Shared pointer to the fresh executable.
-  executor::AnyExecutable::SharedPtr instantiate_next_executable()
+  bool add_handles_to_wait_set(rcl_wait_set_t * wait_set)
   {
-    return std::allocate_shared<executor::AnyExecutable>(*executable_allocator_.get());
+    for (auto subscription : subscription_handles_) {
+      if (rcl_wait_set_add_subscription(wait_set, subscription.get(), NULL) != RCL_RET_OK) {
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Couldn't add subscription to wait set: %s", rcl_get_error_string().str);
+        return false;
+      }
+    }
+
+    for (auto client : client_handles_) {
+      if (rcl_wait_set_add_client(wait_set, client.get(), NULL) != RCL_RET_OK) {
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Couldn't add client to wait set: %s", rcl_get_error_string().str);
+        return false;
+      }
+    }
+
+    for (auto service : service_handles_) {
+      if (rcl_wait_set_add_service(wait_set, service.get(), NULL) != RCL_RET_OK) {
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Couldn't add service to wait set: %s", rcl_get_error_string().str);
+        return false;
+      }
+    }
+
+    for (auto timer : timer_handles_) {
+      if (rcl_wait_set_add_timer(wait_set, timer.get(), NULL) != RCL_RET_OK) {
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Couldn't add timer to wait set: %s", rcl_get_error_string().str);
+        return false;
+      }
+    }
+
+    for (auto guard_condition : guard_conditions_) {
+      if (rcl_wait_set_add_guard_condition(wait_set, guard_condition, NULL) != RCL_RET_OK) {
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Couldn't add guard_condition to wait set: %s",
+          rcl_get_error_string().str);
+        return false;
+      }
+    }
+
+    for (auto waitable : waitable_handles_) {
+      if (!waitable->add_to_wait_set(wait_set)) {
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Couldn't add waitable to wait set: %s", rcl_get_error_string().str);
+        return false;
+      }
+    }
+    return true;
   }
 
   virtual void
-  get_next_subscription(executor::AnyExecutable::SharedPtr any_exec,
+  get_next_subscription(
+    executor::AnyExecutable & 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);
+          it = subscription_handles_.erase(it);
           continue;
         }
         if (!group->can_be_taken_from().load()) {
@@ -212,22 +292,23 @@ public:
         }
         // Otherwise it is safe to set and return the any_exec
         if (is_intra_process) {
-          any_exec->subscription_intra_process = subscription;
+          any_exec.subscription_intra_process = subscription;
         } else {
-          any_exec->subscription = subscription;
+          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.callback_group = group;
+        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);
+      it = subscription_handles_.erase(it);
     }
   }
 
   virtual void
-  get_next_service(executor::AnyExecutable::SharedPtr any_exec,
+  get_next_service(
+    executor::AnyExecutable & any_exec,
     const WeakNodeVector & weak_nodes)
   {
     auto it = service_handles_.begin();
@@ -239,7 +320,7 @@ public:
         if (!group) {
           // Group was not found, meaning the service is not valid...
           // Remove it from the ready list and continue looking
-          service_handles_.erase(it);
+          it = service_handles_.erase(it);
           continue;
         }
         if (!group->can_be_taken_from().load()) {
@@ -249,19 +330,19 @@ public:
           continue;
         }
         // 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.service = service;
+        any_exec.callback_group = group;
+        any_exec.node_base = get_node_by_group(group, weak_nodes);
         service_handles_.erase(it);
         return;
       }
       // Else, the service is no longer valid, remove it and continue
-      service_handles_.erase(it);
+      it = service_handles_.erase(it);
     }
   }
 
   virtual void
-  get_next_client(executor::AnyExecutable::SharedPtr any_exec, const WeakNodeVector & weak_nodes)
+  get_next_client(executor::AnyExecutable & any_exec, const WeakNodeVector & weak_nodes)
   {
     auto it = client_handles_.begin();
     while (it != client_handles_.end()) {
@@ -272,7 +353,7 @@ public:
         if (!group) {
           // Group was not found, meaning the service is not valid...
           // Remove it from the ready list and continue looking
-          client_handles_.erase(it);
+          it = client_handles_.erase(it);
           continue;
         }
         if (!group->can_be_taken_from().load()) {
@@ -282,37 +363,123 @@ public:
           continue;
         }
         // 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.client = client;
+        any_exec.callback_group = group;
+        any_exec.node_base = get_node_by_group(group, weak_nodes);
         client_handles_.erase(it);
         return;
       }
       // Else, the service is no longer valid, remove it and continue
-      client_handles_.erase(it);
+      it = client_handles_.erase(it);
     }
   }
 
+  virtual void
+  get_next_waitable(executor::AnyExecutable & any_exec, const WeakNodeVector & weak_nodes)
+  {
+    auto it = waitable_handles_.begin();
+    while (it != waitable_handles_.end()) {
+      auto waitable = *it;
+      if (waitable) {
+        // Find the group for this handle and see if it can be serviced
+        auto group = get_group_by_waitable(waitable, weak_nodes);
+        if (!group) {
+          // Group was not found, meaning the waitable is not valid...
+          // Remove it from the ready list and continue looking
+          it = waitable_handles_.erase(it);
+          continue;
+        }
+        if (!group->can_be_taken_from().load()) {
+          // Group is mutually exclusive and is being used, so skip it for now
+          // Leave it to be checked next time, but continue searching
+          ++it;
+          continue;
+        }
+        // Otherwise it is safe to set and return the any_exec
+        any_exec.waitable = waitable;
+        any_exec.callback_group = group;
+        any_exec.node_base = get_node_by_group(group, weak_nodes);
+        waitable_handles_.erase(it);
+        return;
+      }
+      // Else, the waitable is no longer valid, remove it and continue
+      it = waitable_handles_.erase(it);
+    }
+  }
+
+  virtual rcl_allocator_t get_allocator()
+  {
+    return rclcpp::allocator::get_rcl_allocator<void *, VoidAlloc>(*allocator_.get());
+  }
+
+  size_t number_of_ready_subscriptions() const
+  {
+    size_t number_of_subscriptions = subscription_handles_.size();
+    for (auto waitable : waitable_handles_) {
+      number_of_subscriptions += waitable->get_number_of_ready_subscriptions();
+    }
+    return number_of_subscriptions;
+  }
+
+  size_t number_of_ready_services() const
+  {
+    size_t number_of_services = service_handles_.size();
+    for (auto waitable : waitable_handles_) {
+      number_of_services += waitable->get_number_of_ready_services();
+    }
+    return number_of_services;
+  }
+
+  size_t number_of_ready_clients() const
+  {
+    size_t number_of_clients = client_handles_.size();
+    for (auto waitable : waitable_handles_) {
+      number_of_clients += waitable->get_number_of_ready_clients();
+    }
+    return number_of_clients;
+  }
+
+  size_t number_of_guard_conditions() const
+  {
+    size_t number_of_guard_conditions = guard_conditions_.size();
+    for (auto waitable : waitable_handles_) {
+      number_of_guard_conditions += waitable->get_number_of_ready_guard_conditions();
+    }
+    return number_of_guard_conditions;
+  }
+
+  size_t number_of_ready_timers() const
+  {
+    size_t number_of_timers = timer_handles_.size();
+    for (auto waitable : waitable_handles_) {
+      number_of_timers += waitable->get_number_of_ready_timers();
+    }
+    return number_of_timers;
+  }
+
+  size_t number_of_waitables() const
+  {
+    return waitable_handles_.size();
+  }
+
 private:
   template<typename T>
   using VectorRebind =
-      std::vector<T, typename std::allocator_traits<Alloc>::template rebind_alloc<T>>;
+    std::vector<T, typename std::allocator_traits<Alloc>::template rebind_alloc<T>>;
 
-  VectorRebind<rclcpp::subscription::SubscriptionBase::SharedPtr> subscriptions_;
-  VectorRebind<rclcpp::service::ServiceBase::SharedPtr> services_;
-  VectorRebind<rclcpp::client::ClientBase::SharedPtr> clients_;
+  VectorRebind<const rcl_guard_condition_t *> guard_conditions_;
 
-  VectorRebind<void *> subscriber_handles_;
-  VectorRebind<void *> service_handles_;
-  VectorRebind<void *> client_handles_;
+  VectorRebind<std::shared_ptr<const rcl_subscription_t>> subscription_handles_;
+  VectorRebind<std::shared_ptr<const rcl_service_t>> service_handles_;
+  VectorRebind<std::shared_ptr<const rcl_client_t>> client_handles_;
+  VectorRebind<std::shared_ptr<const rcl_timer_t>> timer_handles_;
+  VectorRebind<std::shared_ptr<Waitable>> waitable_handles_;
 
-  std::shared_ptr<ExecAlloc> executable_allocator_;
   std::shared_ptr<VoidAlloc> allocator_;
 };
 
 }  // namespace allocator_memory_strategy
 }  // namespace memory_strategies
-
 }  // namespace rclcpp
 
 #endif  // RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_

rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp

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

rclcpp/include/rclcpp/subscription.hpp

@@ -24,164 +24,170 @@
 #include <sstream>
 #include <string>
 
+#include "rcl/error_handling.h"
+#include "rcl/subscription.h"
+
 #include "rcl_interfaces/msg/intra_process_message.hpp"
 
+#include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/message_memory_strategy.hpp"
-#include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/subscription_traits.hpp"
+#include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
 
-namespace node
-{
-class Node;
-}  // namespace node
-
-namespace subscription
+namespace node_interfaces
 {
+class NodeTopicsInterface;
+}  // namespace node_interfaces
 
 /// Virtual base class for subscriptions. This pattern allows us to iterate over different template
 /// specializations of Subscription, among other things.
 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.
+   * \param[in] is_serialized is true if the message will be delivered still serialized
    */
+  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)
-  : intra_process_subscription_handle_(nullptr),
-    node_handle_(node_handle),
-    subscription_handle_(subscription_handle),
-    topic_name_(topic_name),
-    ignore_local_publications_(ignore_local_publications)
-  {
-    // To avoid unused private member warnings.
-    (void)ignore_local_publications_;
-  }
+    const rcl_subscription_options_t & subscription_options,
+    bool is_serialized = false);
 
   /// Default destructor.
-  virtual ~SubscriptionBase()
-  {
-    if (subscription_handle_) {
-      if (rmw_destroy_subscription(node_handle_.get(), subscription_handle_) != RMW_RET_OK) {
-        std::stringstream ss;
-        ss << "Error in destruction of rmw subscription handle: " <<
-          rmw_get_error_string_safe() << '\n';
-        (std::cerr << ss.str()).flush();
-      }
-    }
-    if (intra_process_subscription_handle_) {
-      auto ret = rmw_destroy_subscription(node_handle_.get(), intra_process_subscription_handle_);
-      if (ret != RMW_RET_OK) {
-        std::stringstream ss;
-        ss << "Error in destruction of rmw intra process subscription handle: " <<
-          rmw_get_error_string_safe() << '\n';
-        (std::cerr << ss.str()).flush();
-      }
-    }
-  }
+  RCLCPP_PUBLIC
+  virtual ~SubscriptionBase();
 
   /// Get the topic that this subscription is subscribed on.
-  const std::string & get_topic_name() const
-  {
-    return this->topic_name_;
-  }
+  RCLCPP_PUBLIC
+  const char *
+  get_topic_name() const;
 
-  const rmw_subscription_t * get_subscription_handle() const
-  {
-    return subscription_handle_;
-  }
+  RCLCPP_PUBLIC
+  std::shared_ptr<rcl_subscription_t>
+  get_subscription_handle();
 
-  const rmw_subscription_t * get_intra_process_subscription_handle() const
-  {
-    return intra_process_subscription_handle_;
-  }
+  RCLCPP_PUBLIC
+  const std::shared_ptr<rcl_subscription_t>
+  get_subscription_handle() const;
+
+  RCLCPP_PUBLIC
+  virtual const std::shared_ptr<rcl_subscription_t>
+  get_intra_process_subscription_handle() const;
 
   /// Borrow a new message.
-  // \return Shared pointer to the fresh message.
-  virtual std::shared_ptr<void> create_message() = 0;
+  /** \return Shared pointer to the fresh message. */
+  virtual std::shared_ptr<void>
+  create_message() = 0;
+
+  /// Borrow a new serialized message
+  /** \return Shared pointer to a rcl_message_serialized_t. */
+  virtual std::shared_ptr<rcl_serialized_message_t>
+  create_serialized_message() = 0;
+
   /// Check if we need to handle the message, and execute the callback if we do.
   /**
    * \param[in] message Shared pointer to the message to handle.
    * \param[in] message_info Metadata associated with this message.
    */
-  virtual void handle_message(
-    std::shared_ptr<void> & message,
-    const rmw_message_info_t & message_info) = 0;
+  virtual void
+  handle_message(std::shared_ptr<void> & message, const rmw_message_info_t & message_info) = 0;
 
   /// Return the message borrowed in create_message.
-  // \param[in] Shared pointer to the returned message.
-  virtual void return_message(std::shared_ptr<void> & message) = 0;
-  virtual void handle_intra_process_message(
+  /** \param[in] message Shared pointer to the returned message. */
+  virtual void
+  return_message(std::shared_ptr<void> & message) = 0;
+
+  /// Return the message borrowed in create_serialized_message.
+  /** \param[in] message Shared pointer to the returned message. */
+  virtual void
+  return_serialized_message(std::shared_ptr<rcl_serialized_message_t> & message) = 0;
+
+  virtual void
+  handle_intra_process_message(
     rcl_interfaces::msg::IntraProcessMessage & ipm,
     const rmw_message_info_t & message_info) = 0;
 
+  const rosidl_message_type_support_t &
+  get_message_type_support_handle() const;
+
+  bool
+  is_serialized() const;
+
 protected:
-  rmw_subscription_t * intra_process_subscription_handle_;
+  std::shared_ptr<rcl_subscription_t> intra_process_subscription_handle_;
+  std::shared_ptr<rcl_subscription_t> subscription_handle_;
+  std::shared_ptr<rcl_node_t> node_handle_;
 
 private:
-  RCLCPP_DISABLE_COPY(SubscriptionBase);
+  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_;
+  rosidl_message_type_support_t type_support_;
+  bool is_serialized_;
 };
 
-using namespace any_subscription_callback;
-
 /// Subscription implementation, templated on the type of message this subscription receives.
-template<typename MessageT, typename Alloc = std::allocator<void>>
+template<
+  typename CallbackMessageT,
+  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>;
+  using MessageAllocTraits = allocator::AllocRebind<CallbackMessageT, Alloc>;
   using MessageAlloc = typename MessageAllocTraits::allocator_type;
-  using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
-  using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
+  using MessageDeleter = allocator::Deleter<MessageAlloc, CallbackMessageT>;
+  using MessageUniquePtr = std::unique_ptr<CallbackMessageT, 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] 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] 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 rosidl_message_type_support_t & type_support_handle,
     const std::string & topic_name,
-    bool ignore_local_publications,
-    AnySubscriptionCallback<MessageT, Alloc> callback,
-    typename message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr memory_strategy =
-    message_memory_strategy::MessageMemoryStrategy<MessageT,
+    const rcl_subscription_options_t & subscription_options,
+    AnySubscriptionCallback<CallbackMessageT, Alloc> callback,
+    typename message_memory_strategy::MessageMemoryStrategy<CallbackMessageT, Alloc>::SharedPtr
+    memory_strategy = message_memory_strategy::MessageMemoryStrategy<CallbackMessageT,
     Alloc>::create_default())
-  : SubscriptionBase(node_handle, subscription_handle, topic_name, ignore_local_publications),
+  : SubscriptionBase(
+      node_handle,
+      type_support_handle,
+      topic_name,
+      subscription_options,
+      rclcpp::subscription_traits::is_serialized_subscription_argument<CallbackMessageT>::value),
     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.
   /**
@@ -189,11 +195,12 @@ public:
    * \param[in] message_memory_strategy Shared pointer to the memory strategy to set.
    */
   void set_message_memory_strategy(
-    typename message_memory_strategy::MessageMemoryStrategy<MessageT,
+    typename message_memory_strategy::MessageMemoryStrategy<CallbackMessageT,
     Alloc>::SharedPtr message_memory_strategy)
   {
     message_memory_strategy_ = message_memory_strategy;
   }
+
   std::shared_ptr<void> create_message()
   {
     /* The default message memory strategy provides a dynamically allocated message on each call to
@@ -203,6 +210,11 @@ public:
     return message_memory_strategy_->borrow_message();
   }
 
+  std::shared_ptr<rcl_serialized_message_t> create_serialized_message()
+  {
+    return message_memory_strategy_->borrow_serialized_message();
+  }
+
   void handle_message(std::shared_ptr<void> & message, const rmw_message_info_t & message_info)
   {
     if (matches_any_intra_process_publishers_) {
@@ -212,16 +224,23 @@ public:
         return;
       }
     }
-    auto typed_message = std::static_pointer_cast<MessageT>(message);
+    auto typed_message = std::static_pointer_cast<CallbackMessageT>(message);
     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);
+    auto typed_message = std::static_pointer_cast<CallbackMessageT>(message);
     message_memory_strategy_->return_message(typed_message);
   }
 
+  void return_serialized_message(std::shared_ptr<rcl_serialized_message_t> & message)
+  {
+    message_memory_strategy_->return_serialized_message(message);
+  }
+
   void handle_intra_process_message(
     rcl_interfaces::msg::IntraProcessMessage & ipm,
     const rmw_message_info_t & message_info)
@@ -248,37 +267,65 @@ 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_.get(),
+      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 std::shared_ptr<rcl_subscription_t>
+  get_intra_process_subscription_handle() const
+  {
+    if (!get_intra_process_message_callback_) {
+      return nullptr;
+    }
+    return intra_process_subscription_handle_;
+  }
 
-  AnySubscriptionCallback<MessageT, Alloc> any_callback_;
-  typename message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
-  message_memory_strategy_;
+private:
+  RCLCPP_DISABLE_COPY(Subscription)
+
+  AnySubscriptionCallback<CallbackMessageT, Alloc> any_callback_;
+  typename message_memory_strategy::MessageMemoryStrategy<CallbackMessageT, Alloc>::SharedPtr
+    message_memory_strategy_;
 
   GetMessageCallbackType get_intra_process_message_callback_;
   MatchesAnyPublishersCallbackType matches_any_intra_process_publishers_;
   uint64_t intra_process_subscription_id_;
 };
 
-}  // namespace subscription
 }  // namespace rclcpp
 
 #endif  // RCLCPP__SUBSCRIPTION_HPP_

rclcpp/include/rclcpp/subscription_factory.hpp

@@ -0,0 +1,180 @@
+// 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/subscription_traits.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::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::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 CallbackMessageT,
+  typename SubscriptionT>
+SubscriptionFactory
+create_subscription_factory(
+  CallbackT && callback,
+  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
+    CallbackMessageT, Alloc>::SharedPtr
+  msg_mem_strat,
+  std::shared_ptr<Alloc> allocator)
+{
+  SubscriptionFactory factory;
+
+  using rclcpp::AnySubscriptionCallback;
+  AnySubscriptionCallback<CallbackMessageT, Alloc> any_subscription_callback(allocator);
+  any_subscription_callback.set(std::forward<CallbackT>(callback));
+
+  auto message_alloc =
+    std::make_shared<typename Subscription<CallbackMessageT, 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::SubscriptionBase::SharedPtr
+    {
+      subscription_options.allocator =
+        rclcpp::allocator::get_rcl_allocator<CallbackMessageT>(*message_alloc.get());
+
+      using rclcpp::Subscription;
+      using rclcpp::SubscriptionBase;
+
+      auto sub = Subscription<CallbackMessageT, Alloc>::make_shared(
+        node_base->get_shared_rcl_node_handle(),
+        *rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>(),
+        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::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<CallbackMessageT>(
+        *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<CallbackMessageT, 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<CallbackMessageT, 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/subscription_traits.hpp

@@ -0,0 +1,80 @@
+// 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__SUBSCRIPTION_TRAITS_HPP_
+#define RCLCPP__SUBSCRIPTION_TRAITS_HPP_
+
+#include <memory>
+
+#include "rclcpp/function_traits.hpp"
+
+namespace rclcpp
+{
+namespace subscription_traits
+{
+
+/*
+ * The current version of uncrustify has a misinterpretion here
+ * between `:` used for inheritance vs for initializer list
+ * The result is that whenever a templated struct is used,
+ * the colon has to be without any whitespace next to it whereas
+ * when no template is used, the colon has to be separated by a space.
+ * Cheers!
+ */
+template<typename T>
+struct is_serialized_subscription_argument : std::false_type
+{};
+
+template<>
+struct is_serialized_subscription_argument<rcl_serialized_message_t>: std::true_type
+{};
+
+template<>
+struct is_serialized_subscription_argument<std::shared_ptr<rcl_serialized_message_t>>
+  : std::true_type
+{};
+
+template<typename T>
+struct is_serialized_subscription : is_serialized_subscription_argument<T>
+{};
+
+template<typename CallbackT>
+struct is_serialized_callback
+  : is_serialized_subscription_argument<
+    typename rclcpp::function_traits::function_traits<CallbackT>::template argument_type<0>>
+{};
+
+template<typename MessageT>
+struct extract_message_type
+{
+  using type = typename std::remove_cv<MessageT>::type;
+};
+
+template<typename MessageT>
+struct extract_message_type<std::shared_ptr<MessageT>>: extract_message_type<MessageT>
+{};
+
+template<typename MessageT, typename Deleter>
+struct extract_message_type<std::unique_ptr<MessageT, Deleter>>: extract_message_type<MessageT>
+{};
+
+template<typename CallbackT>
+struct has_message_type : extract_message_type<
+    typename rclcpp::function_traits::function_traits<CallbackT>::template argument_type<0>>
+{};
+
+}  // namespace subscription_traits
+}  // namespace rclcpp
+
+#endif  // RCLCPP__SUBSCRIPTION_TRAITS_HPP_

rclcpp/include/rclcpp/time.hpp

@@ -0,0 +1,130 @@
+// 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"
+
+#include "rclcpp/duration.hpp"
+
+namespace rclcpp
+{
+
+class Clock;
+
+class Time
+{
+public:
+  RCLCPP_PUBLIC
+  Time(int32_t seconds, uint32_t nanoseconds, rcl_clock_type_t clock_type = RCL_SYSTEM_TIME);
+
+  RCLCPP_PUBLIC
+  explicit Time(int64_t nanoseconds = 0, rcl_clock_type_t clock = RCL_SYSTEM_TIME);
+
+  RCLCPP_PUBLIC
+  Time(const Time & rhs);
+
+  RCLCPP_PUBLIC
+  Time(
+    const builtin_interfaces::msg::Time & time_msg,
+    rcl_clock_type_t ros_time = RCL_ROS_TIME);
+
+  RCLCPP_PUBLIC
+  explicit Time(const rcl_time_point_t & time_point);
+
+  RCLCPP_PUBLIC
+  virtual ~Time();
+
+  RCLCPP_PUBLIC
+  operator builtin_interfaces::msg::Time() const;
+
+  RCLCPP_PUBLIC
+  Time &
+  operator=(const Time & rhs);
+
+  RCLCPP_PUBLIC
+  Time &
+  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
+  bool
+  operator>(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  Time
+  operator+(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  Duration
+  operator-(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  Time
+  operator-(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  rcl_time_point_value_t
+  nanoseconds() const;
+
+  RCLCPP_PUBLIC
+  static Time
+  max();
+
+  /// \return the seconds since epoch as a floating point number.
+  /// \warning Depending on sizeof(double) there could be significant precision loss.
+  /// When an exact time is required use nanoseconds() instead.
+  RCLCPP_PUBLIC
+  double
+  seconds() const;
+
+  RCLCPP_PUBLIC
+  rcl_clock_type_t
+  get_clock_type() const;
+
+private:
+  rcl_time_point_t rcl_time_;
+  friend Clock;  // Allow clock to manipulate internal data
+};
+
+Time
+operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs);
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__TIME_HPP_

rclcpp/include/rclcpp/time_source.hpp

@@ -0,0 +1,145 @@
+// 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_SOURCE_HPP_
+#define RCLCPP__TIME_SOURCE_HPP_
+
+#include <memory>
+#include <vector>
+
+#include "rcl/time.h"
+
+#include "builtin_interfaces/msg/time.hpp"
+#include "rosgraph_msgs/msg/clock.hpp"
+#include "rcl_interfaces/msg/parameter_event.hpp"
+
+#include "rclcpp/node.hpp"
+#include "rclcpp/parameter_client.hpp"
+#include "rclcpp/parameter_events_filter.hpp"
+
+
+namespace rclcpp
+{
+class Clock;
+
+class TimeSource
+{
+public:
+  RCLCPP_PUBLIC
+  explicit TimeSource(rclcpp::Node::SharedPtr node);
+
+  RCLCPP_PUBLIC
+  TimeSource();
+
+  RCLCPP_PUBLIC
+  void attachNode(rclcpp::Node::SharedPtr node);
+
+  RCLCPP_PUBLIC
+  void attachNode(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+    rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
+    rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface,
+    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface);
+
+  RCLCPP_PUBLIC
+  void detachNode();
+
+  /// Attach a clock to the time source to be updated
+  /**
+   * \throws std::invalid_argument if node is nullptr
+   */
+  RCLCPP_PUBLIC
+  void attachClock(rclcpp::Clock::SharedPtr clock);
+
+  RCLCPP_PUBLIC
+  void detachClock(rclcpp::Clock::SharedPtr clock);
+
+  RCLCPP_PUBLIC
+  ~TimeSource();
+
+private:
+  // Preserve the node reference
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
+  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
+
+  // Store (and update on node attach) logger for logging.
+  Logger logger_;
+
+  // The subscription for the clock callback
+  using MessageT = rosgraph_msgs::msg::Clock;
+  using Alloc = std::allocator<void>;
+  using SubscriptionT = rclcpp::Subscription<MessageT, Alloc>;
+  std::shared_ptr<SubscriptionT> clock_subscription_;
+  std::mutex clock_sub_lock_;
+
+  // The clock callback itself
+  void clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg);
+
+  // Create the subscription for the clock topic
+  void create_clock_sub();
+
+  // Destroy the subscription for the clock topic
+  void destroy_clock_sub();
+
+  // Parameter Client pointer
+  std::shared_ptr<rclcpp::AsyncParametersClient> parameter_client_;
+
+  // Parameter Event subscription
+  using ParamMessageT = rcl_interfaces::msg::ParameterEvent;
+  using ParamSubscriptionT = rclcpp::Subscription<ParamMessageT, Alloc>;
+  std::shared_ptr<ParamSubscriptionT> parameter_subscription_;
+
+  // Callback for parameter updates
+  void on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event);
+
+  // An enum to hold the parameter state
+  enum UseSimTimeParameterState {UNSET, SET_TRUE, SET_FALSE};
+  UseSimTimeParameterState parameter_state_;
+
+  // An internal method to use in the clock callback that iterates and enables all clocks
+  void enable_ros_time();
+  // An internal method to use in the clock callback that iterates and disables all clocks
+  void disable_ros_time();
+
+  // Internal helper functions used inside iterators
+  static void enable_ros_time(rclcpp::Clock::SharedPtr clock);
+  static void disable_ros_time(rclcpp::Clock::SharedPtr clock);
+  static void set_clock(
+    const builtin_interfaces::msg::Time::SharedPtr msg,
+    bool set_ros_time_enabled,
+    rclcpp::Clock::SharedPtr clock);
+
+  // Local storage of validity of ROS time
+  // This is needed when new clocks are added.
+  bool ros_time_active_;
+  // Last set message to be passed to newly registered clocks
+  rosgraph_msgs::msg::Clock::SharedPtr last_msg_set_;
+
+  // A lock to protect iterating the associated_clocks_ field.
+  std::mutex clock_list_lock_;
+  // A vector to store references to associated clocks.
+  std::vector<rclcpp::Clock::SharedPtr> associated_clocks_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__TIME_SOURCE_HPP_

rclcpp/include/rclcpp/timer.hpp

@@ -12,107 +12,117 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_TIMER_HPP_
-#define RCLCPP_RCLCPP_TIMER_HPP_
+#ifndef RCLCPP__TIMER_HPP_
+#define RCLCPP__TIMER_HPP_
 
 #include <chrono>
 #include <functional>
 #include <memory>
 #include <sstream>
 #include <thread>
+#include <type_traits>
+#include <utility>
 
-#include <rmw/error_handling.h>
-#include <rmw/rmw.h>
+#include "rclcpp/clock.hpp"
+#include "rclcpp/context.hpp"
+#include "rclcpp/function_traits.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/rate.hpp"
+#include "rclcpp/utilities.hpp"
+#include "rclcpp/visibility_control.hpp"
 
-#include <rclcpp/macros.hpp>
-#include <rclcpp/rate.hpp>
-#include <rclcpp/utilities.hpp>
+#include "rcl/error_handling.h"
+#include "rcl/timer.h"
+
+#include "rmw/error_handling.h"
+#include "rmw/rmw.h"
 
 namespace rclcpp
 {
 
-namespace timer
-{
-
-using CallbackType = std::function<void()>;
-
 class TimerBase
 {
-
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase);
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase)
 
-  TimerBase(std::chrono::nanoseconds period, CallbackType callback)
-  : period_(period),
-    callback_(callback),
-    canceled_(false)
-  {
-  }
+  RCLCPP_PUBLIC
+  explicit TimerBase(
+    Clock::SharedPtr clock,
+    std::chrono::nanoseconds period,
+    rclcpp::Context::SharedPtr context);
 
-  virtual ~TimerBase()
-  {
-  }
+  RCLCPP_PUBLIC
+  ~TimerBase();
 
+  RCLCPP_PUBLIC
   void
-  cancel()
-  {
-    this->canceled_ = true;
-  }
+  cancel();
 
-  void execute_callback() const
-  {
-    callback_();
-  }
+  RCLCPP_PUBLIC
+  void
+  reset();
 
-  const CallbackType & get_callback() const
-  {
-    return callback_;
-  }
+  RCLCPP_PUBLIC
+  virtual void
+  execute_callback() = 0;
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<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_;
-  CallbackType callback_;
-
-  bool canceled_;
-
+  Clock::SharedPtr clock_;
+  std::shared_ptr<rcl_timer_t> timer_handle_;
 };
 
-/// Generic timer templated on the clock type. Periodically executes a user-specified callback.
-template<class Clock = std::chrono::high_resolution_clock>
+
+using VoidCallbackType = std::function<void ()>;
+using TimerCallbackType = std::function<void (TimerBase &)>;
+
+/// Generic timer. Periodically executes a user-specified callback.
+template<
+  typename FunctorT,
+  typename std::enable_if<
+    rclcpp::function_traits::same_arguments<FunctorT, VoidCallbackType>::value ||
+    rclcpp::function_traits::same_arguments<FunctorT, TimerCallbackType>::value
+  >::type * = nullptr
+>
 class GenericTimer : public TimerBase
 {
-
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(GenericTimer);
+  RCLCPP_SMART_PTR_DEFINITIONS(GenericTimer)
 
   /// Default constructor.
   /**
+   * \param[in] clock The clock providing the current time.
    * \param[in] period The interval at which the timer fires.
    * \param[in] callback User-specified callback function.
    */
-  GenericTimer(std::chrono::nanoseconds period, CallbackType callback)
-  : TimerBase(period, callback), loop_rate_(period)
+  explicit GenericTimer(
+    Clock::SharedPtr clock, std::chrono::nanoseconds period, FunctorT && callback,
+    rclcpp::Context::SharedPtr context
+  )
+  : TimerBase(clock, period, context), callback_(std::forward<FunctorT>(callback))
   {
-    /* Subtracting the loop rate period ensures that the callback gets triggered
-       on the first call to check_and_trigger. */
-    last_triggered_time_ = Clock::now() - period;
   }
 
   /// Default destructor.
@@ -122,56 +132,80 @@ public:
     cancel();
   }
 
+  void
+  execute_callback() override
+  {
+    rcl_ret_t ret = rcl_timer_call(timer_handle_.get());
+    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<>();
+  }
+
+  // void specialization
+  template<
+    typename CallbackT = FunctorT,
+    typename std::enable_if<
+      rclcpp::function_traits::same_arguments<CallbackT, VoidCallbackType>::value
+    >::type * = nullptr
+  >
+  void
+  execute_callback_delegate()
+  {
+    callback_();
+  }
+
+  template<
+    typename CallbackT = FunctorT,
+    typename std::enable_if<
+      rclcpp::function_traits::same_arguments<CallbackT, TimerCallbackType>::value
+    >::type * = nullptr
+  >
+  void
+  execute_callback_delegate()
+  {
+    callback_(*this);
+  }
+
   bool
-  check_and_trigger()
+  is_steady() override
   {
-    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;
+    return clock_->get_clock_type() == RCL_STEADY_TIME;
   }
 
-  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()
-  {
-    return Clock::is_steady;
-  }
-
-private:
-  RCLCPP_DISABLE_COPY(GenericTimer);
-
-  rclcpp::rate::GenericRate<Clock> loop_rate_;
-  std::chrono::time_point<Clock> last_triggered_time_;
+protected:
+  RCLCPP_DISABLE_COPY(GenericTimer)
 
+  FunctorT callback_;
 };
 
-using WallTimer = GenericTimer<std::chrono::steady_clock>;
+template<
+  typename FunctorT,
+  typename std::enable_if<
+    rclcpp::function_traits::same_arguments<FunctorT, VoidCallbackType>::value ||
+    rclcpp::function_traits::same_arguments<FunctorT, TimerCallbackType>::value
+  >::type * = nullptr
+>
+class WallTimer : public GenericTimer<FunctorT>
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(WallTimer)
 
-} /* namespace timer */
-} /* namespace rclcpp */
+  WallTimer(
+    std::chrono::nanoseconds period,
+    FunctorT && callback,
+    rclcpp::Context::SharedPtr context)
+  : GenericTimer<FunctorT>(
+      std::make_shared<Clock>(RCL_STEADY_TIME), period, std::move(callback), context)
+  {}
 
-#endif /* RCLCPP_RCLCPP_TIMER_HPP_ */
+protected:
+  RCLCPP_DISABLE_COPY(WallTimer)
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__TIMER_HPP_

rclcpp/include/rclcpp/type_support_decl.hpp

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

rclcpp/include/rclcpp/unused_parameters_checker.hpp

@@ -0,0 +1,115 @@
+// Copyright 2018 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__UNUSED_PARAMETERS_CHECKER_HPP_
+#define RCLCPP__UNUSED_PARAMETERS_CHECKER_HPP_
+
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "rclcpp/node_interfaces/get_node_parameters_interface.hpp"
+#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Check a Node-like class for unused parameters.
+/**
+ * This class can be used to detect misconfigurations and typos by ensuring all
+ * initial parameter values that were passed to the Node-like object were used.
+ * So this class's methods are used after or during Node construction and after
+ * all parameters have been declared.
+ *
+ * This class must not outlive the Node that it's being used with.
+ */
+class UnusedParametersChecker
+{
+public:
+  template<typename NodeType>
+  explicit UnusedParametersChecker(NodeType && node_like)
+  : node_parameters_interface_(
+      rclcpp::node_interfaces::get_node_parameters_interface(std::forward<NodeType>(node_like)))
+  {}
+
+  /// Warn if any initial parameter values have not been used.
+  /**
+   * This function will complain with a RCLCPP_WARN if any provided initial
+   * parameter values have not been used.
+   *
+   * \throws std::bad_alloc when trying to create an error message
+   */
+  RCLCPP_PUBLIC
+  void
+  check_and_warn() const;
+
+  /// Throw an UnusedParameterExecption if any initial parameter values have not been used.
+  /**
+   * This function will throw an exception if any provided initial
+   * parameter values have not been used.
+   *
+   * \throws std::bad_alloc when trying to create an error message
+   * \throws rclcpp::UnusedParametersException when there are unused parameters
+   */
+  RCLCPP_PUBLIC
+  void
+  check_and_throw() const;
+
+  /// Return the number of unused initial parameter values.
+  /**
+   * Similar to get_unused_initial_parameter_values(), but it returns the
+   * number of unused parameter values rather than a vector of the unused
+   * parameters (which involves allocating storage for the copies).
+   * This function is faster and avoids memory allocation while checking for a
+   * problem, and if one is detected then get_unused_initial_parameter_values()
+   * may be used to format a useful error message.
+   *
+   * \returns the number of unused initial parameter values
+   */
+  RCLCPP_PUBLIC
+  size_t
+  count_unused_initial_parameter_values() const;
+
+  /// Return the list of unused initial parameter values.
+  /**
+   * A common case where this returns a non-empty vector, is when someone makes
+   * a typo when setting the parameters from outside the node.
+   * For example, if they use `ip_addr` rather than the expected `ip_address`.
+   *
+   * \returns vector of parameters which where passed to the node but where
+   *   not declared before this function was called.
+   * \throws std::bad_alloc
+   */
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::Parameter>
+  get_unused_initial_parameter_values() const;
+
+private:
+  const rclcpp::node_interfaces::NodeParametersInterface * node_parameters_interface_;
+};
+
+/// Thrown when throw_if_unused_initialized_parameter_values() finds unused parameters.
+class UnusedParametersException : public std::runtime_error
+{
+public:
+  explicit UnusedParametersException(const std::vector<rclcpp::Parameter> & unused_parameters);
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__UNUSED_PARAMETERS_CHECKER_HPP_

rclcpp/include/rclcpp/utilities.hpp

@@ -12,215 +12,296 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_UTILITIES_HPP_
-#define RCLCPP_RCLCPP_UTILITIES_HPP_
+#ifndef RCLCPP__UTILITIES_HPP_
+#define RCLCPP__UTILITIES_HPP_
 
-// TODO(wjwwood): remove
-#include <iostream>
-
-#include <cerrno>
 #include <chrono>
-#include <condition_variable>
-#include <csignal>
-#include <cstring>
-#include <mutex>
-#include <string.h>
-#include <thread>
+#include <functional>
+#include <limits>
+#include <vector>
 
-#include <rmw/error_handling.h>
-#include <rmw/macros.h>
-#include <rmw/rmw.h>
+#include "rclcpp/context.hpp"
+#include "rclcpp/init_options.hpp"
+#include "rclcpp/visibility_control.hpp"
 
-// Determine if sigaction is available
-#if __APPLE__ || _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _POSIX_SOURCE
-#define HAS_SIGACTION
-#endif
+#include "rmw/macros.h"
+#include "rmw/rmw.h"
 
-namespace
+#ifdef ANDROID
+#include <string>
+#include <sstream>
+
+namespace std
 {
-/// Represent the status of the global interrupt signal.
-volatile sig_atomic_t g_signal_status = 0;
-/// Guard condition for interrupting the rmw implementation when the global interrupt signal fired.
-rmw_guard_condition_t * g_sigint_guard_cond_handle = \
-  rmw_create_guard_condition();
-/// Condition variable for timed sleep (see sleep_for).
-std::condition_variable g_interrupt_condition_variable;
-std::atomic<bool> g_is_interrupted(false);
-/// Mutex for protecting the global condition variable.
-std::mutex g_interrupt_mutex;
-
-#ifdef HAS_SIGACTION
-struct sigaction old_action;
-#else
-void (* old_signal_handler)(int) = 0;
-#endif
-
-/// Handle the interrupt signal.
-/** When the interrupt signal fires, the signal handler notifies the condition variable to wake up
- * and triggers the interrupt guard condition, so that all global threads managed by rclcpp
- * are interrupted.
- */
-void
-#ifdef HAS_SIGACTION
-signal_handler(int signal_value, siginfo_t * siginfo, void * context)
-#else
-signal_handler(int signal_value)
-#endif
+template<typename T>
+std::string to_string(T value)
 {
-  // TODO(wjwwood): remove
-  std::cout << "signal_handler(" << signal_value << ")" << std::endl;
-#ifdef HAS_SIGACTION
-  if (old_action.sa_flags & SA_SIGINFO) {
-    if (old_action.sa_sigaction != NULL) {
-      old_action.sa_sigaction(signal_value, siginfo, context);
-    }
-  } else {
-    // *INDENT-OFF*
-    if (
-      old_action.sa_handler != NULL && // Is set
-      old_action.sa_handler != SIG_DFL && // Is not default
-      old_action.sa_handler != SIG_IGN) // Is not ignored
-    // *INDENT-ON*
-    {
-      old_action.sa_handler(signal_value);
-    }
-  }
-#else
-  if (old_signal_handler) {
-    old_signal_handler(signal_value);
-  }
-#endif
-  g_signal_status = signal_value;
-  rmw_ret_t status = rmw_trigger_guard_condition(g_sigint_guard_cond_handle);
-  if (status != RMW_RET_OK) {
-    fprintf(stderr,
-      "[rclcpp::error] failed to trigger guard condition: %s\n", rmw_get_error_string_safe());
-  }
-  g_is_interrupted.store(true);
-  g_interrupt_condition_variable.notify_all();
+  std::ostringstream os;
+  os << value;
+  return os.str();
 }
-} // namespace
+}
+#endif
 
 namespace rclcpp
 {
+/// Initialize communications via the rmw implementation and set up a global signal handler.
+/**
+ * Initializes the global context which is accessible via the function
+ * rclcpp::contexts::default_context::get_global_default_context().
+ * Also, installs the global signal handlers with the function
+ * rclcpp::install_signal_handlers().
+ *
+ * \sa rclcpp::Context::init() for more details on arguments and possible exceptions
+ */
+RCLCPP_PUBLIC
+void
+init(int argc, char const * const argv[], const InitOptions & init_options = InitOptions());
 
-RMW_THREAD_LOCAL size_t thread_id = 0;
+/// Install the global signal handler for rclcpp.
+/**
+ * This function should only need to be run one time per process.
+ * It is implicitly run by rclcpp::init(), and therefore this function does not
+ * need to be run manually if rclcpp::init() has already been run.
+ *
+ * The signal handler will shutdown all initialized context.
+ * It will also interrupt any blocking functions in ROS allowing them react to
+ * any changes in the state of the system (like shutdown).
+ *
+ * This function is thread-safe.
+ *
+ * \return true if signal handler was installed by this function, false if already installed.
+ */
+RCLCPP_PUBLIC
+bool
+install_signal_handlers();
 
-namespace utilities
-{
+/// Return true if the signal handlers are installed, otherwise false.
+RCLCPP_PUBLIC
+bool
+signal_handlers_installed();
+
+/// Uninstall the global signal handler for rclcpp.
+/**
+ * This function does not necessarily need to be called, but can be used to
+ * undo what rclcpp::install_signal_handlers() or rclcpp::init() do with
+ * respect to signal handling.
+ * If you choose to use it, this function only needs to be run one time.
+ * It is implicitly run by rclcpp::shutdown(), and therefore this function does
+ * not need to be run manually if rclcpp::shutdown() has already been run.
+ *
+ * This function is thread-safe.
+ *
+ * \return true if signal handler was uninstalled by this function, false if was not installed.
+ */
+RCLCPP_PUBLIC
+bool
+uninstall_signal_handlers();
 
 /// Initialize communications via the rmw implementation and set up a global signal handler.
 /**
- * \param[in] argc Number of arguments.
- * \param[in] argv Argument vector. Will eventually be used for passing options to rclcpp.
+ * Additionally removes ROS-specific arguments from the argument vector.
+ *
+ * \sa rclcpp::Context::init() for more details on arguments and possible exceptions
+ * \returns Members of the argument vector that are not ROS arguments.
  */
-void
-init(int argc, char * argv[])
-{
-  (void)argc;
-  (void)argv;
-  g_is_interrupted.store(false);
-  rmw_ret_t status = rmw_init();
-  if (status != RMW_RET_OK) {
-    // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-    throw std::runtime_error(
-      std::string("failed to initialize rmw implementation: ") + rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
-#ifdef HAS_SIGACTION
-  struct sigaction action;
-  memset(&action, 0, sizeof(action));
-  sigemptyset(&action.sa_mask);
-  action.sa_sigaction = ::signal_handler;
-  action.sa_flags = SA_SIGINFO;
-  ssize_t ret = sigaction(SIGINT, &action, &old_action);
-  if (ret == -1)
-#else
-  ::old_signal_handler = std::signal(SIGINT, ::signal_handler);
-  if (::old_signal_handler == SIG_ERR)
-#endif
-  {
-    const size_t error_length = 1024;
-    char error_string[error_length];
-#ifndef _WIN32
-    auto rc = strerror_r(errno, error_string, error_length);
-    if (rc) {
-      // *INDENT-OFF*
-      throw std::runtime_error(
-        "Failed to set SIGINT signal handler: (" + std::to_string(errno) +
-        ") unable to retrieve error string");
-      // *INDENT-ON*
-    }
+RCLCPP_PUBLIC
+std::vector<std::string>
+init_and_remove_ros_arguments(
+  int argc,
+  char const * const argv[],
+  const InitOptions & init_options = InitOptions());
 
-#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*
-  }
-}
+/// Remove ROS-specific arguments from argument vector.
+/**
+ * Some arguments may not have been intended as ROS arguments.
+ * This function populates the arguments in a vector.
+ * Since the first argument is always assumed to be a process name, the vector
+ * will always contain the process name.
+ *
+ * \param[in] argc Number of arguments.
+ * \param[in] argv Argument vector.
+ * \returns Members of the argument vector that are not ROS arguments.
+ */
+RCLCPP_PUBLIC
+std::vector<std::string>
+remove_ros_arguments(int argc, char const * const argv[]);
 
 /// Check rclcpp's status.
-// \return True if SIGINT hasn't fired yet, false otherwise.
+/**
+ * This may return false for a context which has been shutdown, or for a
+ * context that was shutdown due to SIGINT being received by the rclcpp signal
+ * handler.
+ *
+ * If nullptr is given for the context, then the global context is used, i.e.
+ * the context initialized by rclcpp::init().
+ *
+ * \param[in] context Check for shutdown of this Context.
+ * \return true if shutdown has been called, false otherwise
+ */
+RCLCPP_PUBLIC
 bool
-ok()
-{
-  return ::g_signal_status == 0;
-}
+ok(rclcpp::Context::SharedPtr context = nullptr);
 
-/// Notify the signal handler and rmw that rclcpp is shutting down.
+/// Return true if init() has already been called for the given context.
+/**
+ * If nullptr is given for the context, then the global context is used, i.e.
+ * the context initialized by rclcpp::init().
+ *
+ * Deprecated, as it is no longer different from rcl_ok().
+ *
+ * \param[in] context Check for initialization of this Context.
+ * \return true if the context is initialized, and false otherwise
+ */
+RCLCPP_PUBLIC
+bool
+is_initialized(rclcpp::Context::SharedPtr context = nullptr);
+
+/// Shutdown rclcpp context, invalidating it for derived entities.
+/**
+ * If nullptr is given for the context, then the global context is used, i.e.
+ * the context initialized by rclcpp::init().
+ *
+ * If the global context is used, then the signal handlers are also uninstalled.
+ *
+ * This will also cause the "on_shutdown" callbacks to be called.
+ *
+ * \sa rclcpp::Context::shutdown()
+ * \param[in] context to be shutdown
+ * \return true if shutdown was successful, false if context was already shutdown
+ */
+RCLCPP_PUBLIC
+bool
+shutdown(
+  rclcpp::Context::SharedPtr context = nullptr,
+  const std::string & reason = "user called rclcpp::shutdown()");
+
+/// Register a function to be called when shutdown is called on the context.
+/**
+ * If nullptr is given for the context, then the global context is used, i.e.
+ * the context initialized by rclcpp::init().
+ *
+ * These callbacks are called when the associated Context is shutdown with the
+ * Context::shutdown() method.
+ * When shutdown by the SIGINT handler, shutdown, and therefore these callbacks,
+ * is called asynchronously from the dedicated signal handling thread, at some
+ * point after the SIGINT signal is received.
+ *
+ * \sa rclcpp::Context::on_shutdown()
+ * \param[in] callback to be called when the given context is shutdown
+ * \param[in] context with which to associate the context
+ */
+RCLCPP_PUBLIC
 void
-shutdown()
-{
-  g_signal_status = SIGINT;
-  rmw_ret_t status = rmw_trigger_guard_condition(g_sigint_guard_cond_handle);
-  if (status != RMW_RET_OK) {
-    fprintf(stderr,
-      "[rclcpp::error] failed to trigger guard condition: %s\n", rmw_get_error_string_safe());
-  }
-  g_is_interrupted.store(true);
-  g_interrupt_condition_variable.notify_all();
-}
-
-
-/// Get a handle to the rmw guard condition that manages the signal handler.
-rmw_guard_condition_t *
-get_global_sigint_guard_condition()
-{
-  return ::g_sigint_guard_cond_handle;
-}
+on_shutdown(std::function<void()> callback, rclcpp::Context::SharedPtr context = nullptr);
 
 /// Use the global condition variable to block for the specified amount of time.
 /**
+ * This function can be interrupted early if the associated context becomes
+ * invalid due to shutdown() or the signal handler.
+ * \sa rclcpp::Context::sleep_for
+ *
+ * If nullptr is given for the context, then the global context is used, i.e.
+ * the context initialized by rclcpp::init().
+ *
  * \param[in] nanoseconds A std::chrono::duration representing how long to sleep for.
- * \return True if the condition variable did not timeout.
+ * \param[in] context which may interrupt this sleep
+ * \return true if the condition variable did not timeout.
  */
+RCLCPP_PUBLIC
 bool
-sleep_for(const std::chrono::nanoseconds & nanoseconds)
+sleep_for(
+  const std::chrono::nanoseconds & nanoseconds,
+  rclcpp::Context::SharedPtr context = nullptr);
+
+/// Safely check if addition will overflow.
+/**
+ * The type of the operands, T, should have defined
+ * std::numeric_limits<T>::max(), `>`, `<` and `-` operators.
+ *
+ * \param[in] x is the first addend.
+ * \param[in] y is the second addend.
+ * \tparam T is type of the operands.
+ * \return True if the x + y sum is greater than T::max value.
+ */
+template<typename T>
+bool
+add_will_overflow(const T x, const T y)
 {
-  // TODO: determine if posix's nanosleep(2) is more efficient here
-  std::chrono::nanoseconds time_left = nanoseconds;
-  {
-    std::unique_lock<std::mutex> lock(::g_interrupt_mutex);
-    auto start = std::chrono::steady_clock::now();
-    ::g_interrupt_condition_variable.wait_for(lock, nanoseconds);
-    time_left -= std::chrono::steady_clock::now() - start;
-  }
-  if (time_left > std::chrono::nanoseconds::zero() && !g_is_interrupted) {
-    return sleep_for(time_left);
-  }
-  // Return true if the timeout elapsed successfully, otherwise false.
-  return !g_is_interrupted;
+  return (y > 0) && (x > (std::numeric_limits<T>::max() - y));
 }
 
-} /* namespace utilities */
-} /* namespace rclcpp */
+/// Safely check if addition will underflow.
+/**
+ * The type of the operands, T, should have defined
+ * std::numeric_limits<T>::min(), `>`, `<` and `-` operators.
+ *
+ * \param[in] x is the first addend.
+ * \param[in] y is the second addend.
+ * \tparam T is type of the operands.
+ * \return True if the x + y sum is less than T::min value.
+ */
+template<typename T>
+bool
+add_will_underflow(const T x, const T y)
+{
+  return (y < 0) && (x < (std::numeric_limits<T>::min() - y));
+}
 
-#ifdef HAS_SIGACTION
-#undef HAS_SIGACTION
-#endif
+/// Safely check if subtraction will overflow.
+/**
+ * The type of the operands, T, should have defined
+ * std::numeric_limits<T>::max(), `>`, `<` and `+` operators.
+ *
+ * \param[in] x is the minuend.
+ * \param[in] y is the subtrahend.
+ * \tparam T is type of the operands.
+ * \return True if the difference `x - y` sum is grater than T::max value.
+ */
+template<typename T>
+bool
+sub_will_overflow(const T x, const T y)
+{
+  return (y < 0) && (x > (std::numeric_limits<T>::max() + y));
+}
 
-#endif /* RCLCPP_RCLCPP_UTILITIES_HPP_ */
+/// Safely check if subtraction will underflow.
+/**
+ * The type of the operands, T, should have defined
+ * std::numeric_limits<T>::min(), `>`, `<` and `+` operators.
+ *
+ * \param[in] x is the minuend.
+ * \param[in] y is the subtrahend.
+ * \tparam T is type of the operands.
+ * \return True if the difference `x - y` sum is less than T::min value.
+ */
+template<typename T>
+bool
+sub_will_underflow(const T x, const T y)
+{
+  return (y > 0) && (x < (std::numeric_limits<T>::min() + y));
+}
+
+/// Return the given string.
+/**
+ * This function is overloaded to transform any string to C-style string.
+ *
+ * \param[in] string_in is the string to be returned
+ * \return the given string
+ */
+RCLCPP_PUBLIC
+const char *
+get_c_string(const char * string_in);
+
+/// Return the C string from the given std::string.
+/**
+ * \param[in] string_in is a std::string
+ * \return the C string from the std::string
+ */
+RCLCPP_PUBLIC
+const char *
+get_c_string(const std::string & string_in);
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__UTILITIES_HPP_

rclcpp/include/rclcpp/visibility_control.hpp

@@ -0,0 +1,56 @@
+// Copyright 2015 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/* This header must be included by all rclcpp headers which declare symbols
+ * which are defined in the rclcpp library. When not building the rclcpp
+ * library, i.e. when using the headers in other package's code, the contents
+ * of this header change the visibility of certain symbols which the rclcpp
+ * library cannot have, but the consuming code must have inorder to link.
+ */
+
+#ifndef RCLCPP__VISIBILITY_CONTROL_HPP_
+#define RCLCPP__VISIBILITY_CONTROL_HPP_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+  #ifdef __GNUC__
+    #define RCLCPP_EXPORT __attribute__ ((dllexport))
+    #define RCLCPP_IMPORT __attribute__ ((dllimport))
+  #else
+    #define RCLCPP_EXPORT __declspec(dllexport)
+    #define RCLCPP_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef RCLCPP_BUILDING_LIBRARY
+    #define RCLCPP_PUBLIC RCLCPP_EXPORT
+  #else
+    #define RCLCPP_PUBLIC RCLCPP_IMPORT
+  #endif
+  #define RCLCPP_PUBLIC_TYPE RCLCPP_PUBLIC
+  #define RCLCPP_LOCAL
+#else
+  #define RCLCPP_EXPORT __attribute__ ((visibility("default")))
+  #define RCLCPP_IMPORT
+  #if __GNUC__ >= 4
+    #define RCLCPP_PUBLIC __attribute__ ((visibility("default")))
+    #define RCLCPP_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define RCLCPP_PUBLIC
+    #define RCLCPP_LOCAL
+  #endif
+  #define RCLCPP_PUBLIC_TYPE
+#endif
+
+#endif  // RCLCPP__VISIBILITY_CONTROL_HPP_

rclcpp/include/rclcpp/waitable.hpp

@@ -0,0 +1,141 @@
+// Copyright 2018 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__WAITABLE_HPP_
+#define RCLCPP__WAITABLE_HPP_
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcl/wait.h"
+
+namespace rclcpp
+{
+
+class Waitable
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Waitable)
+
+  /// Get the number of ready subscriptions
+  /**
+   * Returns a value of 0 by default.
+   * This should be overridden if the Waitable contains one or more subscriptions.
+   * \return The number of subscriptions associated with the Waitable.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  get_number_of_ready_subscriptions();
+
+  /// Get the number of ready timers
+  /**
+   * Returns a value of 0 by default.
+   * This should be overridden if the Waitable contains one or more timers.
+   * \return The number of timers associated with the Waitable.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  get_number_of_ready_timers();
+
+  /// Get the number of ready clients
+  /**
+   * Returns a value of 0 by default.
+   * This should be overridden if the Waitable contains one or more clients.
+   * \return The number of clients associated with the Waitable.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  get_number_of_ready_clients();
+
+  /// Get the number of ready services
+  /**
+   * Returns a value of 0 by default.
+   * This should be overridden if the Waitable contains one or more services.
+   * \return The number of services associated with the Waitable.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  get_number_of_ready_services();
+
+  /// Get the number of ready guard_conditions
+  /**
+   * Returns a value of 0 by default.
+   * This should be overridden if the Waitable contains one or more guard_conditions.
+   * \return The number of guard_conditions associated with the Waitable.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  get_number_of_ready_guard_conditions();
+
+  // TODO(jacobperron): smart pointer?
+  /// Add the Waitable to a wait set.
+  /**
+   * \param[in] wait_set A handle to the wait set to add the Waitable to.
+   * \return `true` if the Waitable is added successfully, `false` otherwise.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  add_to_wait_set(rcl_wait_set_t * wait_set) = 0;
+
+  /// Check if the Waitable is ready.
+  /**
+   * The input wait set should be the same that was used in a previously call to
+   * `add_to_wait_set()`.
+   * The wait set should also have been previously waited on with `rcl_wait()`.
+   *
+   * \param[in] wait_set A handle to the wait set the Waitable was previously added to
+   *   and that has been waited on.
+   * \return `true` if the Waitable is ready, `false` otherwise.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  is_ready(rcl_wait_set_t *) = 0;
+
+  /// Execute any entities of the Waitable that are ready.
+  /**
+   * Before calling this method, the Waitable should be added to a wait set,
+   * waited on, and then updated.
+   *
+   * Example usage:
+   *
+   * ```
+   * // ... create a wait set and a Waitable
+   * // Add the Waitable to the wait set
+   * bool add_ret = waitable.add_to_wait_set(wait_set);
+   * // ... error handling
+   * // Wait
+   * rcl_ret_t wait_ret = rcl_wait(wait_set);
+   * // ... error handling
+   * // Update the Waitable
+   * waitable.update(wait_set);
+   * // Execute any entities of the Waitable that may be ready
+   * waitable.execute();
+   * ```
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  execute() = 0;
+};  // class Waitable
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAITABLE_HPP_

rclcpp/package.xml

@@ -1,22 +1,42 @@
 <?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.6.2</version>
   <description>The ROS client library in C++.</description>
   <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
   <license>Apache License 2.0</license>
 
-  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
   <build_export_depend>rmw</build_export_depend>
 
+  <build_depend>builtin_interfaces</build_depend>
   <build_depend>rcl_interfaces</build_depend>
+  <build_depend>rosgraph_msgs</build_depend>
+  <build_depend>rosidl_generator_cpp</build_depend>
+  <build_depend>rosidl_typesupport_c</build_depend>
+  <build_depend>rosidl_typesupport_cpp</build_depend>
+  <build_export_depend>builtin_interfaces</build_export_depend>
   <build_export_depend>rcl_interfaces</build_export_depend>
+  <build_export_depend>rosgraph_msgs</build_export_depend>
+  <build_export_depend>rosidl_generator_cpp</build_export_depend>
+  <build_export_depend>rosidl_typesupport_c</build_export_depend>
+  <build_export_depend>rosidl_typesupport_cpp</build_export_depend>
 
+  <depend>rcl</depend>
+  <depend>rcl_yaml_param_parser</depend>
+  <depend>rmw_implementation</depend>
+
+  <exec_depend>ament_cmake</exec_depend>
+
+  <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>rmw</test_depend>
+  <test_depend>rmw_implementation_cmake</test_depend>
+  <test_depend>test_msgs</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

rclcpp/rclcpp-extras.cmake

@@ -14,14 +14,16 @@
 
 # copied from rclcpp/rclcpp-extras.cmake
 
-set(rclcpp_node_main_SRC "${rclcpp_DIR}/../../../src/rclcpp/node_main.cpp")
+# 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)
 
-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/resource/logging.hpp.em

@@ -0,0 +1,112 @@
+// generated from rclcpp/resource/logging.hpp.em
+
+// 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__LOGGING_HPP_
+#define RCLCPP__LOGGING_HPP_
+
+#include <type_traits>
+
+#include "rclcpp/logger.hpp"
+#include "rcutils/logging_macros.h"
+#include "rclcpp/utilities.hpp"
+
+// These are used for compiling out logging macros lower than a minimum severity.
+#define RCLCPP_LOG_MIN_SEVERITY_DEBUG 0
+#define RCLCPP_LOG_MIN_SEVERITY_INFO 1
+#define RCLCPP_LOG_MIN_SEVERITY_WARN 2
+#define RCLCPP_LOG_MIN_SEVERITY_ERROR 3
+#define RCLCPP_LOG_MIN_SEVERITY_FATAL 4
+#define RCLCPP_LOG_MIN_SEVERITY_NONE 5
+
+#define RCLCPP_FIRST_ARG(N, ...) N
+#define RCLCPP_ALL_BUT_FIRST_ARGS(N, ...) __VA_ARGS__
+
+/**
+ * \def RCLCPP_LOG_MIN_SEVERITY
+ * Define RCLCPP_LOG_MIN_SEVERITY=RCLCPP_LOG_MIN_SEVERITY_[DEBUG|INFO|WARN|ERROR|FATAL]
+ * in your build options to compile out anything below that severity.
+ * Use RCLCPP_LOG_MIN_SEVERITY_NONE to compile out all macros.
+ */
+#ifndef RCLCPP_LOG_MIN_SEVERITY
+#define RCLCPP_LOG_MIN_SEVERITY RCLCPP_LOG_MIN_SEVERITY_DEBUG
+#endif
+
+@{
+from rcutils.logging import feature_combinations
+from rcutils.logging import get_macro_parameters
+from rcutils.logging import get_suffix_from_features
+from rcutils.logging import severities
+
+# TODO(dhood): Implement the throttle macro using time sources available in rclcpp
+excluded_features = ['named', 'throttle']
+def is_supported_feature_combination(feature_combination):
+    is_excluded = any([ef in feature_combination for ef in excluded_features])
+    return not is_excluded
+}@
+@[for severity in severities]@
+/** @@name Logging macros for severity @(severity).
+ */
+///@@{
+#if (RCLCPP_LOG_MIN_SEVERITY > RCLCPP_LOG_MIN_SEVERITY_@(severity))
+// empty logging macros for severity @(severity) when being disabled at compile time
+@[ for feature_combination in [fc for fc in feature_combinations if is_supported_feature_combination(fc)]]@
+@{suffix = get_suffix_from_features(feature_combination)}@
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_@(severity)@(suffix)(...)
+@[ end for]@
+
+#else
+@[ for feature_combination in [fc for fc in feature_combinations if is_supported_feature_combination(fc)]]@
+@{suffix = get_suffix_from_features(feature_combination)}@
+/**
+ * \def RCLCPP_@(severity)@(suffix)
+ * Log a message with severity @(severity)@
+@[ if feature_combinations[feature_combination].doc_lines]@
+ with the following conditions:
+@[ else]@
+.
+@[ end if]@
+@[ for doc_line in feature_combinations[feature_combination].doc_lines]@
+ * @(doc_line)
+@[ end for]@
+ * \param logger The `rclcpp::Logger` to use
+@[ for param_name, doc_line in feature_combinations[feature_combination].params.items()]@
+ * \param @(param_name) @(doc_line)
+@[ end for]@
+ * \param ... The format string, followed by the variable arguments for the format string.
+ * It also accepts a single argument of type std::string.
+ */
+#define RCLCPP_@(severity)@(suffix)(logger, @(''.join([p + ', ' for p in get_macro_parameters(feature_combination).keys()]))...) \
+  static_assert( \
+    ::std::is_same<typename std::remove_reference<decltype(logger)>::type, \
+    typename ::rclcpp::Logger>::value, \
+    "First argument to logging macros must be an rclcpp::Logger"); \
+  RCUTILS_LOG_@(severity)@(suffix)_NAMED( \
+@{params = get_macro_parameters(feature_combination).keys()}@
+@[ if params]@
+@(''.join(['    ' + p + ', \\\n' for p in params]))@
+@[ end if]@
+    logger.get_name(), \
+    rclcpp::get_c_string(RCLCPP_FIRST_ARG(__VA_ARGS__, "")), \
+      RCLCPP_ALL_BUT_FIRST_ARGS(__VA_ARGS__,""))
+
+@[ end for]@
+#endif
+///@@}
+
+@[end for]@
+
+#endif  // RCLCPP__LOGGING_HPP_

rclcpp/src/rclcpp/any_executable.cpp

@@ -0,0 +1,37 @@
+// Copyright 2015 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/any_executable.hpp"
+
+using rclcpp::executor::AnyExecutable;
+
+AnyExecutable::AnyExecutable()
+: subscription(nullptr),
+  subscription_intra_process(nullptr),
+  timer(nullptr),
+  service(nullptr),
+  client(nullptr),
+  callback_group(nullptr),
+  node_base(nullptr)
+{}
+
+AnyExecutable::~AnyExecutable()
+{
+  // Make sure that discarded (taken but not executed) AnyExecutable's have
+  // their callback groups reset. This can happen when an executor is canceled
+  // between taking an AnyExecutable and executing it.
+  if (callback_group) {
+    callback_group->can_be_taken_from().store(true);
+  }
+}

rclcpp/src/rclcpp/callback_group.cpp

@@ -0,0 +1,120 @@
+// Copyright 2015 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/callback_group.hpp"
+
+#include <vector>
+
+using rclcpp::callback_group::CallbackGroup;
+using rclcpp::callback_group::CallbackGroupType;
+
+CallbackGroup::CallbackGroup(CallbackGroupType group_type)
+: type_(group_type), can_be_taken_from_(true)
+{}
+
+const std::vector<rclcpp::SubscriptionBase::WeakPtr> &
+CallbackGroup::get_subscription_ptrs() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  return subscription_ptrs_;
+}
+
+const std::vector<rclcpp::TimerBase::WeakPtr> &
+CallbackGroup::get_timer_ptrs() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  return timer_ptrs_;
+}
+
+const std::vector<rclcpp::ServiceBase::WeakPtr> &
+CallbackGroup::get_service_ptrs() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  return service_ptrs_;
+}
+
+const std::vector<rclcpp::ClientBase::WeakPtr> &
+CallbackGroup::get_client_ptrs() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  return client_ptrs_;
+}
+
+const std::vector<rclcpp::Waitable::WeakPtr> &
+CallbackGroup::get_waitable_ptrs() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  return waitable_ptrs_;
+}
+
+std::atomic_bool &
+CallbackGroup::can_be_taken_from()
+{
+  return can_be_taken_from_;
+}
+
+const CallbackGroupType &
+CallbackGroup::type() const
+{
+  return type_;
+}
+
+void
+CallbackGroup::add_subscription(
+  const rclcpp::SubscriptionBase::SharedPtr subscription_ptr)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  subscription_ptrs_.push_back(subscription_ptr);
+}
+
+void
+CallbackGroup::add_timer(const rclcpp::TimerBase::SharedPtr timer_ptr)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  timer_ptrs_.push_back(timer_ptr);
+}
+
+void
+CallbackGroup::add_service(const rclcpp::ServiceBase::SharedPtr service_ptr)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  service_ptrs_.push_back(service_ptr);
+}
+
+void
+CallbackGroup::add_client(const rclcpp::ClientBase::SharedPtr client_ptr)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  client_ptrs_.push_back(client_ptr);
+}
+
+void
+CallbackGroup::add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  waitable_ptrs_.push_back(waitable_ptr);
+}
+
+void
+CallbackGroup::remove_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) noexcept
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  for (auto iter = waitable_ptrs_.begin(); iter != waitable_ptrs_.end(); ++iter) {
+    const auto shared_ptr = iter->lock();
+    if (shared_ptr.get() == waitable_ptr.get()) {
+      waitable_ptrs_.erase(iter);
+      break;
+    }
+  }
+}

rclcpp/src/rclcpp/client.cpp

@@ -0,0 +1,179 @@
+// Copyright 2015 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/client.hpp"
+
+#include <algorithm>
+#include <chrono>
+#include <cstdio>
+#include <memory>
+#include <string>
+
+#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"
+#include "rclcpp/logging.hpp"
+
+using rclcpp::ClientBase;
+using rclcpp::exceptions::InvalidNodeError;
+using rclcpp::exceptions::throw_from_rcl_error;
+
+ClientBase::ClientBase(
+  rclcpp::node_interfaces::NodeBaseInterface * node_base,
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph)
+: node_graph_(node_graph),
+  node_handle_(node_base->get_shared_rcl_node_handle()),
+  context_(node_base->get_context())
+{
+  std::weak_ptr<rcl_node_t> weak_node_handle(node_handle_);
+  rcl_client_t * new_rcl_client = new rcl_client_t;
+  *new_rcl_client = rcl_get_zero_initialized_client();
+  client_handle_.reset(
+    new_rcl_client, [weak_node_handle](rcl_client_t * client)
+    {
+      auto handle = weak_node_handle.lock();
+      if (handle) {
+        if (rcl_client_fini(client, handle.get()) != RCL_RET_OK) {
+          RCLCPP_ERROR(
+            rclcpp::get_node_logger(handle.get()).get_child("rclcpp"),
+            "Error in destruction of rcl client handle: %s", rcl_get_error_string().str);
+          rcl_reset_error();
+        }
+      } else {
+        RCLCPP_ERROR(
+          rclcpp::get_logger("rclcpp"),
+          "Error in destruction of rcl client handle: "
+          "the Node Handle was destructed too early. You will leak memory");
+      }
+      delete client;
+    });
+}
+
+ClientBase::~ClientBase()
+{
+  // Make sure the client handle is destructed as early as possible and before the node handle
+  client_handle_.reset();
+}
+
+const char *
+ClientBase::get_service_name() const
+{
+  return rcl_client_get_service_name(this->get_client_handle().get());
+}
+
+std::shared_ptr<rcl_client_t>
+ClientBase::get_client_handle()
+{
+  return client_handle_;
+}
+
+std::shared_ptr<const rcl_client_t>
+ClientBase::get_client_handle() const
+{
+  return client_handle_;
+}
+
+bool
+ClientBase::service_is_ready() const
+{
+  bool is_ready;
+  rcl_ret_t ret = rcl_service_server_is_available(
+    this->get_rcl_node_handle(),
+    this->get_client_handle().get(),
+    &is_ready);
+  if (RCL_RET_NODE_INVALID == ret) {
+    const rcl_node_t * node_handle = this->get_rcl_node_handle();
+    if (node_handle && !rcl_context_is_valid(node_handle->context)) {
+      // context is shutdown, do a soft failure
+      return false;
+    }
+  }
+  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::nanoseconds(0) ?
+    timeout - (std::chrono::steady_clock::now() - start) :
+    std::chrono::nanoseconds::max();
+  if (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);
+  }
+  do {
+    if (!rclcpp::ok(this->context_)) {
+      return false;
+    }
+    // Limit each wait to 100ms to workaround an issue specific to the Connext RMW implementation.
+    // A race condition means that graph changes for services becoming available may trigger the
+    // wait set to wake up, but then not be reported as ready immediately after the wake up
+    // (see https://github.com/ros2/rmw_connext/issues/201)
+    // If no other graph events occur, the wait set will not be triggered again until the timeout
+    // has been reached, despite the service being available, so we artificially limit the wait
+    // time to limit the delay.
+    node_ptr->wait_for_graph_change(
+      event, std::min(time_to_wait, std::chrono::nanoseconds(RCL_MS_TO_NS(100))));
+    // Because of the aforementioned race condition, we check if the service is ready even if the
+    // graph event wasn't triggered.
+    event->check_and_clear();
+    if (this->service_is_ready()) {
+      return true;
+    }
+    // server is not ready, loop if there is time left
+    if (timeout > std::chrono::nanoseconds(0)) {
+      time_to_wait = timeout - (std::chrono::steady_clock::now() - start);
+    }
+    // if timeout is negative, time_to_wait will never reach zero
+  } while (time_to_wait > std::chrono::nanoseconds(0));
+  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/clock.cpp

@@ -0,0 +1,151 @@
+// 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/clock.hpp"
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "builtin_interfaces/msg/time.hpp"
+
+#include "rcl/time.h"
+
+#include "rclcpp/exceptions.hpp"
+
+#include "rcutils/logging_macros.h"
+
+namespace rclcpp
+{
+
+JumpHandler::JumpHandler(
+  std::function<void()> pre_callback,
+  std::function<void(const rcl_time_jump_t &)> post_callback,
+  const rcl_jump_threshold_t & threshold)
+: pre_callback(pre_callback),
+  post_callback(post_callback),
+  notice_threshold(threshold)
+{}
+
+Clock::Clock(rcl_clock_type_t clock_type)
+{
+  allocator_ = rcl_get_default_allocator();
+  auto ret = rcl_clock_init(clock_type, &rcl_clock_, &allocator_);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "could not get current time stamp");
+  }
+}
+
+Clock::~Clock()
+{
+  auto ret = rcl_clock_fini(&rcl_clock_);
+  if (ret != RCL_RET_OK) {
+    RCUTILS_LOG_ERROR("Failed to fini rcl clock.");
+  }
+}
+
+Time
+Clock::now()
+{
+  Time now(0, 0, rcl_clock_.type);
+
+  auto ret = rcl_clock_get_now(&rcl_clock_, &now.rcl_time_.nanoseconds);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "could not get current time stamp");
+  }
+
+  return now;
+}
+
+bool
+Clock::ros_time_is_active()
+{
+  if (!rcl_clock_valid(&rcl_clock_)) {
+    RCUTILS_LOG_ERROR("ROS time not valid!");
+    return false;
+  }
+
+  bool is_enabled;
+  auto ret = rcl_is_enabled_ros_time_override(&rcl_clock_, &is_enabled);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "Failed to check ros_time_override_status");
+  }
+  return is_enabled;
+}
+
+rcl_clock_t *
+Clock::get_clock_handle()
+{
+  return &rcl_clock_;
+}
+
+rcl_clock_type_t
+Clock::get_clock_type()
+{
+  return rcl_clock_.type;
+}
+
+void
+Clock::on_time_jump(
+  const struct rcl_time_jump_t * time_jump,
+  bool before_jump,
+  void * user_data)
+{
+  rclcpp::JumpHandler * handler = static_cast<rclcpp::JumpHandler *>(user_data);
+  if (before_jump && handler->pre_callback) {
+    handler->pre_callback();
+  } else if (!before_jump && handler->post_callback) {
+    handler->post_callback(*time_jump);
+  }
+}
+
+rclcpp::JumpHandler::SharedPtr
+Clock::create_jump_callback(
+  std::function<void()> pre_callback,
+  std::function<void(const rcl_time_jump_t &)> post_callback,
+  const rcl_jump_threshold_t & threshold)
+{
+  // Allocate a new jump handler
+  auto handler = new rclcpp::JumpHandler(pre_callback, post_callback, threshold);
+  if (nullptr == handler) {
+    rclcpp::exceptions::throw_from_rcl_error(RCL_RET_BAD_ALLOC, "Failed to allocate jump handler");
+  }
+
+  // Try to add the jump callback to the clock
+  rcl_ret_t ret = rcl_clock_add_jump_callback(&rcl_clock_, threshold,
+      rclcpp::Clock::on_time_jump, handler);
+  if (RCL_RET_OK != ret) {
+    delete handler;
+    handler = NULL;
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to add time jump callback");
+  }
+
+  // *INDENT-OFF*
+  // create shared_ptr that removes the callback automatically when all copies are destructed
+  return rclcpp::JumpHandler::SharedPtr(handler, [this](rclcpp::JumpHandler * handler) noexcept {
+    rcl_ret_t ret = rcl_clock_remove_jump_callback(&rcl_clock_, rclcpp::Clock::on_time_jump,
+        handler);
+    delete handler;
+    handler = NULL;
+    if (RCL_RET_OK != ret) {
+      RCUTILS_LOG_ERROR("Failed to remove time jump callback");
+    }
+  });
+  // *INDENT-ON*
+}
+
+}  // namespace rclcpp

rclcpp/src/rclcpp/context.cpp

@@ -0,0 +1,309 @@
+// Copyright 2015 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/context.hpp"
+
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+#include "rcl/init.h"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/logging.hpp"
+#include "rmw/impl/cpp/demangle.hpp"
+
+/// Mutex to protect initialized contexts.
+static std::mutex g_contexts_mutex;
+/// Weak list of context to be shutdown by the signal handler.
+static std::vector<std::weak_ptr<rclcpp::Context>> g_contexts;
+
+using rclcpp::Context;
+
+Context::Context()
+: rcl_context_(nullptr), shutdown_reason_("") {}
+
+Context::~Context()
+{
+  // acquire the init lock to prevent race conditions with init and shutdown
+  // this will not prevent errors, but will maybe make them easier to reproduce
+  std::lock_guard<std::recursive_mutex> lock(init_mutex_);
+  try {
+    this->shutdown("context destructor was called while still not shutdown");
+    // at this point it is shutdown and cannot reinit
+    // clean_up will finalize the rcl context
+    this->clean_up();
+  } catch (const std::exception & exc) {
+    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "unhandled exception in ~Context(): %s", exc.what());
+  } catch (...) {
+    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "unhandled exception in ~Context()");
+  }
+}
+
+RCLCPP_LOCAL
+void
+__delete_context(rcl_context_t * context)
+{
+  if (context) {
+    if (rcl_context_is_valid(context)) {
+      RCLCPP_ERROR(
+        rclcpp::get_logger("rclcpp"), "rcl context unexpectedly not shutdown during cleanup");
+    } else {
+      // if context pointer is not null and is shutdown, then it's ready for fini
+      rcl_ret_t ret = rcl_context_fini(context);
+      if (RCL_RET_OK != ret) {
+        RCLCPP_ERROR(
+          rclcpp::get_logger("rclcpp"),
+          "failed to finalize context: %s", rcl_get_error_string().str);
+        rcl_reset_error();
+      }
+    }
+    delete context;
+  }
+}
+
+void
+Context::init(
+  int argc,
+  char const * const argv[],
+  const rclcpp::InitOptions & init_options)
+{
+  std::lock_guard<std::recursive_mutex> init_lock(init_mutex_);
+  if (this->is_valid()) {
+    throw rclcpp::ContextAlreadyInitialized();
+  }
+  this->clean_up();
+  rcl_context_.reset(new rcl_context_t, __delete_context);
+  *rcl_context_.get() = rcl_get_zero_initialized_context();
+  rcl_ret_t ret = rcl_init(argc, argv, init_options.get_rcl_init_options(), rcl_context_.get());
+  if (RCL_RET_OK != ret) {
+    rcl_context_.reset();
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl");
+  }
+  init_options_ = init_options;
+
+  std::lock_guard<std::mutex> lock(g_contexts_mutex);
+  g_contexts.push_back(this->shared_from_this());
+}
+
+bool
+Context::is_valid() const
+{
+  // Take a local copy of the shared pointer to avoid it getting nulled under our feet.
+  auto local_rcl_context = rcl_context_;
+  if (!local_rcl_context) {
+    return false;
+  }
+  return rcl_context_is_valid(local_rcl_context.get());
+}
+
+const rclcpp::InitOptions &
+Context::get_init_options() const
+{
+  return init_options_;
+}
+
+rclcpp::InitOptions
+Context::get_init_options()
+{
+  return init_options_;
+}
+
+std::string
+Context::shutdown_reason()
+{
+  std::lock_guard<std::recursive_mutex> lock(init_mutex_);
+  return shutdown_reason_;
+}
+
+bool
+Context::shutdown(const std::string & reason)
+{
+  // prevent races
+  std::lock_guard<std::recursive_mutex> init_lock(init_mutex_);
+  // ensure validity
+  if (!this->is_valid()) {
+    // if it is not valid, then it cannot be shutdown
+    return false;
+  }
+  // rcl shutdown
+  rcl_ret_t ret = rcl_shutdown(rcl_context_.get());
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+  // set shutdown reason
+  shutdown_reason_ = reason;
+  // call each shutdown callback
+  for (const auto & callback : on_shutdown_callbacks_) {
+    callback();
+  }
+  // interrupt all blocking sleep_for() and all blocking executors or wait sets
+  this->interrupt_all_sleep_for();
+  this->interrupt_all_wait_sets();
+  // remove self from the global contexts
+  std::lock_guard<std::mutex> context_lock(g_contexts_mutex);
+  for (auto it = g_contexts.begin(); it != g_contexts.end(); ) {
+    auto shared_context = it->lock();
+    if (shared_context.get() == this) {
+      it = g_contexts.erase(it);
+      break;
+    } else {
+      ++it;
+    }
+  }
+  return true;
+}
+
+rclcpp::Context::OnShutdownCallback
+Context::on_shutdown(OnShutdownCallback callback)
+{
+  on_shutdown_callbacks_.push_back(callback);
+  return callback;
+}
+
+const std::vector<rclcpp::Context::OnShutdownCallback> &
+Context::get_on_shutdown_callbacks() const
+{
+  return on_shutdown_callbacks_;
+}
+
+std::vector<rclcpp::Context::OnShutdownCallback> &
+Context::get_on_shutdown_callbacks()
+{
+  return on_shutdown_callbacks_;
+}
+
+std::shared_ptr<rcl_context_t>
+Context::get_rcl_context()
+{
+  return rcl_context_;
+}
+
+bool
+Context::sleep_for(const std::chrono::nanoseconds & nanoseconds)
+{
+  std::chrono::nanoseconds time_left = nanoseconds;
+  {
+    std::unique_lock<std::mutex> lock(interrupt_mutex_);
+    auto start = std::chrono::steady_clock::now();
+    // this will release the lock while waiting
+    interrupt_condition_variable_.wait_for(lock, nanoseconds);
+    time_left -= std::chrono::steady_clock::now() - start;
+  }
+  if (time_left > std::chrono::nanoseconds::zero() && this->is_valid()) {
+    return sleep_for(time_left);
+  }
+  // Return true if the timeout elapsed successfully, otherwise false.
+  return this->is_valid();
+}
+
+void
+Context::interrupt_all_sleep_for()
+{
+  interrupt_condition_variable_.notify_all();
+}
+
+rcl_guard_condition_t *
+Context::get_interrupt_guard_condition(rcl_wait_set_t * wait_set)
+{
+  std::lock_guard<std::mutex> lock(interrupt_guard_cond_handles_mutex_);
+  auto kv = interrupt_guard_cond_handles_.find(wait_set);
+  if (kv != interrupt_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();
+    auto ret = rcl_guard_condition_init(&handle, this->get_rcl_context().get(), options);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't initialize guard condition");
+    }
+    interrupt_guard_cond_handles_.emplace(wait_set, handle);
+    return &interrupt_guard_cond_handles_[wait_set];
+  }
+}
+
+void
+Context::release_interrupt_guard_condition(rcl_wait_set_t * wait_set)
+{
+  std::lock_guard<std::mutex> lock(interrupt_guard_cond_handles_mutex_);
+  auto kv = interrupt_guard_cond_handles_.find(wait_set);
+  if (kv != interrupt_guard_cond_handles_.end()) {
+    rcl_ret_t ret = rcl_guard_condition_fini(&kv->second);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to destroy sigint guard condition");
+    }
+    interrupt_guard_cond_handles_.erase(kv);
+  } else {
+    throw std::runtime_error("Tried to release sigint guard condition for nonexistent wait set");
+  }
+}
+
+void
+Context::release_interrupt_guard_condition(
+  rcl_wait_set_t * wait_set,
+  const std::nothrow_t &) noexcept
+{
+  try {
+    this->release_interrupt_guard_condition(wait_set);
+  } catch (const std::exception & exc) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("rclcpp"),
+      "caught %s exception when releasing interrupt guard condition: %s",
+      rmw::impl::cpp::demangle(exc).c_str(), exc.what());
+  } catch (...) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("rclcpp"),
+      "caught unknown exception when releasing interrupt guard condition");
+  }
+}
+
+void
+Context::interrupt_all_wait_sets()
+{
+  std::lock_guard<std::mutex> lock(interrupt_guard_cond_handles_mutex_);
+  for (auto & kv : interrupt_guard_cond_handles_) {
+    rcl_ret_t status = rcl_trigger_guard_condition(&(kv.second));
+    if (status != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "failed to trigger guard condition in Context::interrupt_all_wait_sets(): %s",
+        rcl_get_error_string().str);
+    }
+  }
+}
+
+void
+Context::clean_up()
+{
+  shutdown_reason_ = "";
+  rcl_context_.reset();
+}
+
+std::vector<Context::SharedPtr>
+rclcpp::get_contexts()
+{
+  std::lock_guard<std::mutex> lock(g_contexts_mutex);
+  std::vector<Context::SharedPtr> shared_contexts;
+  for (auto it = g_contexts.begin(); it != g_contexts.end(); /* noop */) {
+    auto context_ptr = it->lock();
+    if (!context_ptr) {
+      // remove invalid weak context pointers
+      it = g_contexts.erase(it);
+    } else {
+      ++it;
+      shared_contexts.push_back(context_ptr);
+    }
+  }
+  return shared_contexts;
+}

rclcpp/src/rclcpp/contexts/default_context.cpp

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

rclcpp/src/rclcpp/duration.cpp

@@ -0,0 +1,229 @@
+// 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 <cmath>
+#include <cstdlib>
+#include <limits>
+#include <utility>
+
+#include "rclcpp/clock.hpp"
+#include "rclcpp/time.hpp"
+
+#include "builtin_interfaces/msg/duration.hpp"
+
+#include "rcl/time.h"
+
+#include "rclcpp/exceptions.hpp"
+
+#include "rcutils/logging_macros.h"
+
+namespace rclcpp
+{
+
+Duration::Duration(int32_t seconds, uint32_t nanoseconds)
+{
+  rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast<int64_t>(seconds));
+  rcl_duration_.nanoseconds += nanoseconds;
+}
+
+Duration::Duration(int64_t nanoseconds)
+{
+  rcl_duration_.nanoseconds = nanoseconds;
+}
+
+Duration::Duration(std::chrono::nanoseconds nanoseconds)
+{
+  rcl_duration_.nanoseconds = nanoseconds.count();
+}
+
+Duration::Duration(const Duration & rhs)
+{
+  rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds;
+}
+
+Duration::Duration(
+  const builtin_interfaces::msg::Duration & duration_msg)
+{
+  rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast<uint64_t>(duration_msg.sec));
+  rcl_duration_.nanoseconds += duration_msg.nanosec;
+}
+
+Duration::Duration(const rcl_duration_t & duration)
+: rcl_duration_(duration)
+{
+  // noop
+}
+
+Duration::~Duration()
+{
+}
+
+Duration::operator builtin_interfaces::msg::Duration() const
+{
+  builtin_interfaces::msg::Duration msg_duration;
+  msg_duration.sec = static_cast<std::int32_t>(RCL_NS_TO_S(rcl_duration_.nanoseconds));
+  msg_duration.nanosec =
+    static_cast<std::uint32_t>(rcl_duration_.nanoseconds % (1000 * 1000 * 1000));
+  return msg_duration;
+}
+
+Duration &
+Duration::operator=(const Duration & rhs)
+{
+  rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds;
+  return *this;
+}
+
+Duration &
+Duration::operator=(const builtin_interfaces::msg::Duration & duration_msg)
+{
+  rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast<int64_t>(duration_msg.sec));
+  rcl_duration_.nanoseconds += duration_msg.nanosec;
+  return *this;
+}
+
+bool
+Duration::operator==(const rclcpp::Duration & rhs) const
+{
+  return rcl_duration_.nanoseconds == rhs.rcl_duration_.nanoseconds;
+}
+
+bool
+Duration::operator<(const rclcpp::Duration & rhs) const
+{
+  return rcl_duration_.nanoseconds < rhs.rcl_duration_.nanoseconds;
+}
+
+bool
+Duration::operator<=(const rclcpp::Duration & rhs) const
+{
+  return rcl_duration_.nanoseconds <= rhs.rcl_duration_.nanoseconds;
+}
+
+bool
+Duration::operator>=(const rclcpp::Duration & rhs) const
+{
+  return rcl_duration_.nanoseconds >= rhs.rcl_duration_.nanoseconds;
+}
+
+bool
+Duration::operator>(const rclcpp::Duration & rhs) const
+{
+  return rcl_duration_.nanoseconds > rhs.rcl_duration_.nanoseconds;
+}
+
+void
+bounds_check_duration_sum(int64_t lhsns, int64_t rhsns, uint64_t max)
+{
+  auto abs_lhs = (uint64_t)std::abs(lhsns);
+  auto abs_rhs = (uint64_t)std::abs(rhsns);
+
+  if (lhsns > 0 && rhsns > 0) {
+    if (abs_lhs + abs_rhs > (uint64_t) max) {
+      throw std::overflow_error("addition leads to int64_t overflow");
+    }
+  } else if (lhsns < 0 && rhsns < 0) {
+    if (abs_lhs + abs_rhs > (uint64_t) max) {
+      throw std::underflow_error("addition leads to int64_t underflow");
+    }
+  }
+}
+
+Duration
+Duration::operator+(const rclcpp::Duration & rhs) const
+{
+  bounds_check_duration_sum(
+    this->rcl_duration_.nanoseconds,
+    rhs.rcl_duration_.nanoseconds,
+    std::numeric_limits<rcl_duration_value_t>::max());
+  return Duration(
+    rcl_duration_.nanoseconds + rhs.rcl_duration_.nanoseconds);
+}
+
+void
+bounds_check_duration_difference(int64_t lhsns, int64_t rhsns, uint64_t max)
+{
+  auto abs_lhs = (uint64_t)std::abs(lhsns);
+  auto abs_rhs = (uint64_t)std::abs(rhsns);
+
+  if (lhsns > 0 && rhsns < 0) {
+    if (abs_lhs + abs_rhs > (uint64_t) max) {
+      throw std::overflow_error("duration subtraction leads to int64_t overflow");
+    }
+  } else if (lhsns < 0 && rhsns > 0) {
+    if (abs_lhs + abs_rhs > (uint64_t) max) {
+      throw std::underflow_error("duration subtraction leads to int64_t underflow");
+    }
+  }
+}
+
+Duration
+Duration::operator-(const rclcpp::Duration & rhs) const
+{
+  bounds_check_duration_difference(
+    this->rcl_duration_.nanoseconds,
+    rhs.rcl_duration_.nanoseconds,
+    std::numeric_limits<rcl_duration_value_t>::max());
+
+  return Duration(
+    rcl_duration_.nanoseconds - rhs.rcl_duration_.nanoseconds);
+}
+
+void
+bounds_check_duration_scale(int64_t dns, double scale, uint64_t max)
+{
+  auto abs_dns = static_cast<uint64_t>(std::abs(dns));
+  auto abs_scale = std::abs(scale);
+
+  if (abs_scale > 1.0 && abs_dns > static_cast<uint64_t>(max / abs_scale)) {
+    if ((dns > 0 && scale > 0) || (dns < 0 && scale < 0)) {
+      throw std::overflow_error("duration scaling leads to int64_t overflow");
+    } else {
+      throw std::underflow_error("duration scaling leads to int64_t underflow");
+    }
+  }
+}
+
+Duration
+Duration::operator*(double scale) const
+{
+  if (!std::isfinite(scale)) {
+    throw std::runtime_error("abnormal scale in rclcpp::Duration");
+  }
+  bounds_check_duration_scale(
+    this->rcl_duration_.nanoseconds,
+    scale,
+    std::numeric_limits<rcl_duration_value_t>::max());
+  return Duration(static_cast<rcl_duration_value_t>(rcl_duration_.nanoseconds * scale));
+}
+
+rcl_duration_value_t
+Duration::nanoseconds() const
+{
+  return rcl_duration_.nanoseconds;
+}
+
+Duration
+Duration::max()
+{
+  return Duration(std::numeric_limits<int32_t>::max(), 999999999);
+}
+
+double
+Duration::seconds() const
+{
+  return std::chrono::duration<double>(std::chrono::nanoseconds(rcl_duration_.nanoseconds)).count();
+}
+
+}  // namespace rclcpp

rclcpp/src/rclcpp/event.cpp

@@ -0,0 +1,41 @@
+// 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
+{
+
+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 rclcpp