0.7.0

* Options-based create_publisher and create_subscription interfaces

Introduce new Options structs for creating publishers and subscribers. Deprecate existing interfaces for checking in CI how often they are used.

Signed-off-by: Emerson Knapp <eknapp@amazon.com>

* Remove default params that resulted in ambiguous declarations.

Signed-off-by: Emerson Knapp <eknapp@amazon.com>

* Remove one deprecation to limit upstream impact, add documentation on pub/sub options, slim down test lambdas character count

Signed-off-by: Emerson Knapp <eknapp@amazon.com>

* Un-abbreviate Allocator in new interfaces/types, define a common Options specialization that doesn't need empty brackets

Signed-off-by: Emerson Knapp <eknapp@amazon.com>

* Suppress cppcheck syntaxError for the one function

Signed-off-by: Emerson Knapp <eknapp@amazon.com>

.github/ISSUE_TEMPLATE.md

@@ -1,5 +1,6 @@
 <!--
-For general questions, please post on discourse: https://discourse.ros.org/c/ng-ros
+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
 -->

.gitignore

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

CONTRIBUTING.md

@@ -11,3 +11,8 @@ be under the Apache 2 License, as dictated by that
    the terms of any separate license agreement you may have executed
    with Licensor regarding such Contributions.
 ~~~
+
+Contributors must sign-off each commit by adding a `Signed-off-by: ...`
+line to commit messages to certify that they have the right to submit
+the code they are contributing to the project according to the
+[Developer Certificate of Origin (DCO)](https://developercertificate.org/).

rclcpp/CHANGELOG.rst

@@ -0,0 +1,126 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package rclcpp
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.7.0 (2019-04-14)
+------------------
+* Added Options-struct interfaces for creating publishers/subscribers (pre-QoS, standalone). (`#673 <https://github.com/ros2/rclcpp/issues/673>`_)
+* Replaced strncpy with memcpy. (`#684 <https://github.com/ros2/rclcpp/issues/684>`_)
+* Replaced const char * with a std::array<char, TOPIC_NAME_LENGTH> as the key of IPM IDTopicMap. (`#671 <https://github.com/ros2/rclcpp/issues/671>`_)
+* Refactored SignalHandler logger to avoid race during destruction. (`#682 <https://github.com/ros2/rclcpp/issues/682>`_)
+* Introduce rclcpp_components to implement composition. (`#665 <https://github.com/ros2/rclcpp/issues/665>`_)
+* Added QoS policy check when configuring intraprocess, skip interprocess publish when possible. (`#674 <https://github.com/ros2/rclcpp/issues/674>`_)
+* Updated to use do { .. } while(0) around content of logging macros. (`#681 <https://github.com/ros2/rclcpp/issues/681>`_)
+* Added function to get publisher's actual QoS settings. (`#667 <https://github.com/ros2/rclcpp/issues/667>`_)
+* Updated to avoid race that triggers timer too often. (`#621 <https://github.com/ros2/rclcpp/issues/621>`_)
+* Exposed get_fully_qualified_name in NodeBase API. (`#662 <https://github.com/ros2/rclcpp/issues/662>`_)
+* Updated to use ament_target_dependencies where possible. (`#659 <https://github.com/ros2/rclcpp/issues/659>`_)
+* Fixed wait for service memory leak bug. (`#656 <https://github.com/ros2/rclcpp/issues/656>`_)
+* Fixed test_time_source test. (`#639 <https://github.com/ros2/rclcpp/issues/639>`_)
+* Fixed hard-coded duration type representation so int64_t isn't assumed. (`#648 <https://github.com/ros2/rclcpp/issues/648>`_)
+* Fixed cppcheck warning. (`#646 <https://github.com/ros2/rclcpp/issues/646>`_)
+* Added count matching api and intra-process subscriber count. (`#628 <https://github.com/ros2/rclcpp/issues/628>`_)
+* Added Sub Node alternative. (`#581 <https://github.com/ros2/rclcpp/issues/581>`_)
+* Replaced 'auto' with 'const auto &'. (`#630 <https://github.com/ros2/rclcpp/issues/630>`_)
+* Set Parameter Event Publisher settings. `#591 <https://github.com/ros2/rclcpp/issues/591>`_ (`#614 <https://github.com/ros2/rclcpp/issues/614>`_)
+* Replaced node constructor arguments with NodeOptions. (`#622 <https://github.com/ros2/rclcpp/issues/622>`_)
+* Updated to pass context to wait set (`#617 <https://github.com/ros2/rclcpp/issues/617>`_)
+* Added API to get parameters in a map. (`#575 <https://github.com/ros2/rclcpp/issues/575>`_)
+* Updated Bind usage since it is is no longer in std::__1. (`#618 <https://github.com/ros2/rclcpp/issues/618>`_)
+* Fixed errors from uncrustify v0.68. (`#613 <https://github.com/ros2/rclcpp/issues/613>`_)
+* Added new constructors for SyncParameterClient. (`#612 <https://github.com/ros2/rclcpp/issues/612>`_)
+* Contributors: Alberto Soragna, Chris Lalancette, Dirk Thomas, Emerson Knapp, Francisco Martín Rico, Jacob Perron, Marko Durkovic, Michael Carroll, Peter Baughman, Shane Loretz, Wei Liu, William Woodall, Yutaka Kondo, ivanpauno, kuzai, rarvolt
+
+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

@@ -2,12 +2,14 @@ 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)
@@ -17,12 +19,7 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
 endif()
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  # we dont use add_compile_options with pedantic in message packages
-  # because the Python C extensions dont comply with it
-  # TODO(mikaelarguedas) change to add_compile_options
-  # once this is not a message package anymore
-  # https://github.com/ros2/system_tests/issues/191
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+  add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
 include_directories(include)
@@ -43,32 +40,40 @@ set(${PROJECT_NAME}_SRCS
   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_options.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
@@ -93,13 +98,16 @@ list(APPEND ${PROJECT_NAME}_SRCS
   include/rclcpp/logging.hpp)
 include_directories("${CMAKE_CURRENT_BINARY_DIR}/include")
 
-add_library(${PROJECT_NAME} SHARED
+add_library(${PROJECT_NAME}
   ${${PROJECT_NAME}_SRCS})
+# specific order: dependents before dependencies
 ament_target_dependencies(${PROJECT_NAME}
-  "builtin_interfaces"
   "rcl"
-  "rosidl_generator_cpp"
-  "rosidl_typesupport_cpp")
+  "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.
@@ -113,18 +121,21 @@ install(
   RUNTIME DESTINATION bin
 )
 
-ament_export_dependencies(ament_cmake)
-ament_export_dependencies(builtin_interfaces)
-ament_export_dependencies(rcl)
-ament_export_dependencies(rosidl_generator_cpp)
-ament_export_dependencies(rosidl_typesupport_c)
-ament_export_dependencies(rosidl_typesupport_cpp)
-
+# specific order: dependents before dependencies
 ament_export_include_directories(include)
-
 ament_export_libraries(${PROJECT_NAME})
 
+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()
 
@@ -132,120 +143,193 @@ if(BUILD_TESTING)
 
   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}
+    ament_target_dependencies(test_client
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     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}
+    ament_target_dependencies(test_expand_topic_or_service_name
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME})
   endif()
   ament_add_gtest(test_function_traits test/test_function_traits.cpp)
   if(TARGET test_function_traits)
-    target_include_directories(test_function_traits PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_function_traits
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
   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}
+    ament_target_dependencies(test_mapped_ring_buffer
+      "rcl"
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
   endif()
   ament_add_gtest(test_intra_process_manager test/test_intra_process_manager.cpp)
   if(TARGET test_intra_process_manager)
-    target_include_directories(test_intra_process_manager PUBLIC
-      ${rcl_INCLUDE_DIRS}
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_intra_process_manager
+      "rcl"
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
   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}
+    ament_target_dependencies(test_node
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     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)
+    ament_target_dependencies(test_node_global_args
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
+    )
+    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}
+    ament_target_dependencies(test_parameter_events_filter
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_parameter_events_filter ${PROJECT_NAME})
   endif()
+  ament_add_gtest(test_parameter test/test_parameter.cpp)
+  if(TARGET test_parameter)
+    ament_target_dependencies(test_parameter
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
+    )
+    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}
+    ament_target_dependencies(test_publisher
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_publisher ${PROJECT_NAME})
   endif()
-  ament_add_gtest(test_rate test/test_rate.cpp
-    ENV RCL_ASSERT_RMW_ID_MATCHES=${rmw_implementation})
+  ament_add_gtest(test_pub_sub_option_interface test/test_pub_sub_option_interface.cpp)
+  if(TARGET test_pub_sub_option_interface)
+    ament_target_dependencies(test_pub_sub_option_interface
+      test_msgs
+    )
+    target_link_libraries(test_pub_sub_option_interface
+      ${PROJECT_NAME}
+    )
+  endif()
+  ament_add_gtest(test_publisher_subscription_count_api test/test_publisher_subscription_count_api.cpp)
+  if(TARGET test_publisher_subscription_count_api)
+    ament_target_dependencies(test_publisher_subscription_count_api
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
+    )
+    target_link_libraries(test_publisher_subscription_count_api ${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}
+    ament_target_dependencies(test_rate
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     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)
+    ament_target_dependencies(test_serialized_message_allocator
+      test_msgs
+    )
+    target_link_libraries(test_serialized_message_allocator
+      ${PROJECT_NAME}
+    )
+  endif()
   ament_add_gtest(test_service test/test_service.cpp)
   if(TARGET test_service)
-    target_include_directories(test_service PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_service
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     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}
+    ament_target_dependencies(test_subscription
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_subscription ${PROJECT_NAME})
   endif()
+  ament_add_gtest(test_subscription_publisher_count_api test/test_subscription_publisher_count_api.cpp)
+  if(TARGET test_subscription_publisher_count_api)
+    ament_target_dependencies(test_subscription_publisher_count_api
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
+    )
+    target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME})
+  endif()
+  find_package(test_msgs REQUIRED)
+  ament_add_gtest(test_subscription_traits test/test_subscription_traits.cpp)
+  if(TARGET test_subscription_traits)
+    ament_target_dependencies(test_subscription_traits
+      "rcl"
+      "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}
+    ament_target_dependencies(test_find_weak_nodes
+      "rcl"
     )
     target_link_libraries(test_find_weak_nodes ${PROJECT_NAME})
   endif()
@@ -269,8 +353,8 @@ if(BUILD_TESTING)
   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}
+    ament_target_dependencies(test_externally_defined_services
+      "rcl"
     )
     target_link_libraries(test_externally_defined_services ${PROJECT_NAME})
     foreach(typesupport_impl_cpp ${typesupport_impls_cpp})
@@ -320,16 +404,44 @@ if(BUILD_TESTING)
       "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)
+    ament_target_dependencies(test_local_parameters
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
+    )
+    target_link_libraries(test_local_parameters ${PROJECT_NAME})
+  endif()
 endif()
 
-ament_package(
-  CONFIG_EXTRAS rclcpp-extras.cmake
-)
-
-install(
-  DIRECTORY cmake
-  DESTINATION share/${PROJECT_NAME}
-)
+ament_package()
 
 install(
   DIRECTORY include/ ${CMAKE_CURRENT_BINARY_DIR}/include/

rclcpp/cmake/rclcpp_create_node_main.cmake

@@ -1,26 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-
-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/include/rclcpp/allocator/allocator_deleter.hpp

@@ -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

@@ -25,6 +25,7 @@
 #include "rclcpp/subscription.hpp"
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
 
 namespace rclcpp
 {
@@ -33,8 +34,6 @@ namespace executor
 
 struct AnyExecutable
 {
-  RCLCPP_SMART_PTR_DEFINITIONS(AnyExecutable)
-
   RCLCPP_PUBLIC
   AnyExecutable();
 
@@ -47,6 +46,7 @@ struct AnyExecutable
   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_interfaces::NodeBaseInterface::SharedPtr node_base;

rclcpp/include/rclcpp/any_service_callback.hpp

@@ -32,16 +32,16 @@ class AnyServiceCallback
 {
 private:
   using SharedPtrCallback = 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 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>
-      )>;
+    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_;

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -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_;

rclcpp/include/rclcpp/callback_group.hpp

@@ -25,6 +25,7 @@
 #include "rclcpp/subscription.hpp"
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
 
 namespace rclcpp
 {
@@ -35,6 +36,7 @@ namespace node_interfaces
 class NodeServices;
 class NodeTimers;
 class NodeTopics;
+class NodeWaitables;
 }  // namespace node_interfaces
 
 namespace callback_group
@@ -51,6 +53,7 @@ class CallbackGroup
   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)
@@ -74,6 +77,10 @@ public:
   const std::vector<rclcpp::ClientBase::WeakPtr> &
   get_client_ptrs() const;
 
+  RCLCPP_PUBLIC
+  const std::vector<rclcpp::Waitable::WeakPtr> &
+  get_waitable_ptrs() const;
+
   RCLCPP_PUBLIC
   std::atomic_bool &
   can_be_taken_from();
@@ -101,6 +108,14 @@ protected:
   void
   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_;
   // Mutex to protect the subsequent vectors of pointers.
   mutable std::mutex mutex_;
@@ -108,6 +123,7 @@ protected:
   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_;
 };
 

rclcpp/include/rclcpp/client.hpp

@@ -36,6 +36,8 @@
 #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"
 
@@ -55,32 +57,31 @@ public:
   RCLCPP_PUBLIC
   ClientBase(
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
-    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
-    const std::string & service_name);
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph);
 
   RCLCPP_PUBLIC
   virtual ~ClientBase();
 
   RCLCPP_PUBLIC
-  const std::string &
+  const char *
   get_service_name() const;
 
   RCLCPP_PUBLIC
-  rcl_client_t *
+  std::shared_ptr<rcl_client_t>
   get_client_handle();
 
   RCLCPP_PUBLIC
-  const rcl_client_t *
+  std::shared_ptr<const rcl_client_t>
   get_client_handle() const;
 
   RCLCPP_PUBLIC
   bool
   service_is_ready() const;
 
-  template<typename RatioT = std::milli>
+  template<typename RepT = int64_t, typename RatioT = std::milli>
   bool
   wait_for_service(
-    std::chrono::duration<int64_t, RatioT> timeout = std::chrono::duration<int64_t, RatioT>(-1))
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
   {
     return wait_for_service_nanoseconds(
       std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
@@ -109,9 +110,9 @@ protected:
 
   rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
   std::shared_ptr<rcl_node_t> node_handle_;
+  std::shared_ptr<rclcpp::Context> context_;
 
-  rcl_client_t client_handle_ = rcl_get_zero_initialized_client();
-  std::string service_name_;
+  std::shared_ptr<rcl_client_t> client_handle_;
 };
 
 template<typename ServiceT>
@@ -130,8 +131,8 @@ public:
   using SharedFuture = std::shared_future<SharedResponse>;
   using SharedFutureWithRequest = std::shared_future<std::pair<SharedRequest, SharedResponse>>;
 
-  using CallbackType = std::function<void(SharedFuture)>;
-  using CallbackWithRequestType = std::function<void(SharedFutureWithRequest)>;
+  using CallbackType = std::function<void (SharedFuture)>;
+  using CallbackWithRequestType = std::function<void (SharedFutureWithRequest)>;
 
   RCLCPP_SMART_PTR_DEFINITIONS(Client)
 
@@ -140,13 +141,13 @@ public:
     rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
     const std::string & service_name,
     rcl_client_options_t & client_options)
-  : ClientBase(node_base, node_graph, service_name)
+  : 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(
-      &client_handle_,
+      this->get_client_handle().get(),
       this->get_rcl_node_handle(),
       service_type_support_handle,
       service_name.c_str(),
@@ -168,21 +169,16 @@ public:
 
   virtual ~Client()
   {
-    if (rcl_client_fini(&client_handle_, this->get_rcl_node_handle()) != RCL_RET_OK) {
-      fprintf(stderr,
-        "Error in destruction of rcl client handle: %s\n", rcl_get_error_string_safe());
-      rcl_reset_error();
-    }
   }
 
   std::shared_ptr<void>
-  create_response()
+  create_response() override
   {
     return std::shared_ptr<void>(new typename ServiceT::Response());
   }
 
   std::shared_ptr<rmw_request_id_t>
-  create_request_header()
+  create_request_header() override
   {
     // TODO(wjwwood): This should probably use rmw_request_id's allocator.
     //                (since it is a C type)
@@ -192,14 +188,16 @@ public:
   void
   handle_response(
     std::shared_ptr<rmw_request_id_t> request_header,
-    std::shared_ptr<void> response)
+    std::shared_ptr<void> response) override
   {
     std::unique_lock<std::mutex> lock(pending_requests_mutex_);
     auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(response);
     int64_t sequence_number = request_header->sequence_number;
     // TODO(esteve) this should throw instead since it is not expected to happen in the first place
     if (this->pending_requests_.count(sequence_number) == 0) {
-      fprintf(stderr, "Received invalid sequence number. Ignoring...\n");
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "Received invalid sequence number. Ignoring...");
       return;
     }
     auto tuple = this->pending_requests_[sequence_number];
@@ -234,7 +232,7 @@ public:
   {
     std::lock_guard<std::mutex> lock(pending_requests_mutex_);
     int64_t sequence_number;
-    rcl_ret_t ret = rcl_send_request(get_client_handle(), request.get(), &sequence_number);
+    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");
     }

rclcpp/include/rclcpp/clock.hpp

@@ -31,53 +31,18 @@ namespace rclcpp
 
 class TimeSource;
 
-/// A struct to represent a timejump.
-/**
- * It represents the time jump duration and whether it changed clock type.
- */
-struct TimeJump
-{
-  typedef enum ClockChange_t
-  {
-    ROS_TIME_NO_CHANGE,    ///< The time type before and after the jump is ROS_TIME
-    ROS_TIME_ACTIVATED,    ///< The time type switched to ROS_TIME from SYSTEM_TIME
-    ROS_TIME_DEACTIVATED,  ///< The time type switched to SYSTEM_TIME from ROS_TIME
-    SYSTEM_TIME_NO_CHANGE  ///< The time type before and after the jump is SYSTEM_TIME
-  } ClockChange_t;
-
-  ClockChange_t jump_type_;  ///< The change in clock_type if there is one.
-  rcl_duration_t delta_;     ///< The change in time value.
-};
-
-/// A class to store a threshold for a TimeJump.
-/**
- * This class can be used to evaluate a time jump's magnitude.
- */
-class JumpThreshold
-{
-public:
-  uint64_t min_forward_;   ///< The minimum jump forward to be considered exceeded..
-  uint64_t min_backward_;  ///< The minimum backwards jump to be considered exceeded.
-  bool on_clock_change_;   ///< Whether to trigger on any clock type change.
-
-  // Test if the threshold is exceeded by a TimeJump
-  RCLCPP_PUBLIC
-  bool
-  is_exceeded(const TimeJump & jump);
-};
-
 class JumpHandler
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(JumpHandler)
   JumpHandler(
     std::function<void()> pre_callback,
-    std::function<void(TimeJump)> post_callback,
-    JumpThreshold & threshold);
+    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 TimeJump &)> post_callback;
-  JumpThreshold notice_threshold;
+  std::function<void(const rcl_time_jump_t &)> post_callback;
+  rcl_jump_threshold_t notice_threshold;
 };
 
 class Clock
@@ -99,6 +64,10 @@ public:
   bool
   ros_time_is_active();
 
+  RCLCPP_PUBLIC
+  rcl_clock_t *
+  get_clock_handle();
+
   RCLCPP_PUBLIC
   rcl_clock_type_t
   get_clock_type();
@@ -112,32 +81,22 @@ public:
   JumpHandler::SharedPtr
   create_jump_callback(
     std::function<void()> pre_callback,
-    std::function<void(const TimeJump &)> post_callback,
-    JumpThreshold & threshold);
+    std::function<void(const rcl_time_jump_t &)> post_callback,
+    const rcl_jump_threshold_t & threshold);
 
 private:
-  // A method for TimeSource to get a list of callbacks to invoke while updating
+  // Invoke time jump callback
   RCLCPP_PUBLIC
-  std::vector<JumpHandler::SharedPtr>
-  get_triggered_callback_handlers(const TimeJump & jump);
-
-  // Invoke callbacks that are valid and outside threshold.
-  RCLCPP_PUBLIC
-  static void invoke_prejump_callbacks(
-    const std::vector<JumpHandler::SharedPtr> & callbacks);
-
-  RCLCPP_PUBLIC
-  static void invoke_postjump_callbacks(
-    const std::vector<JumpHandler::SharedPtr> & callbacks,
-    const TimeJump & jump);
+  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_;
-
-  std::mutex callback_list_mutex_;
-  std::vector<std::weak_ptr<JumpHandler>> active_jump_handlers_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/context.hpp

@@ -15,34 +15,292 @@
 #ifndef RCLCPP__CONTEXT_HPP_
 #define RCLCPP__CONTEXT_HPP_
 
-#include <iostream>
+#include <condition_variable>
+#include <functional>
 #include <memory>
 #include <mutex>
+#include <string>
 #include <typeindex>
 #include <typeinfo>
 #include <unordered_map>
 #include <utility>
+#include <vector>
 
+#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"
-#include "rmw/rmw.h"
 
 namespace rclcpp
 {
 
-class Context
+/// Thrown when init is called on an already initialized context.
+class ContextAlreadyInitialized : public std::runtime_error
+{
+public:
+  ContextAlreadyInitialized()
+  : std::runtime_error("context is already initialized") {}
+};
+
+/// 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;
@@ -62,13 +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)
 
+  // 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_;
 };
 
+/// Return a copy of the list of context shared pointers.
+/**
+ * This function is thread-safe.
+ */
+RCLCPP_PUBLIC
+std::vector<Context::SharedPtr>
+get_contexts();
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__CONTEXT_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

@@ -26,8 +26,13 @@
 namespace rclcpp
 {
 
-template<typename MessageT, typename CallbackT, typename AllocatorT, typename SubscriptionT>
-typename rclcpp::Subscription<MessageT, AllocatorT>::SharedPtr
+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,
@@ -36,7 +41,8 @@ create_subscription(
   rclcpp::callback_group::CallbackGroup::SharedPtr group,
   bool ignore_local_publications,
   bool use_intra_process_comms,
-  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, AllocatorT>::SharedPtr
+  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
+    CallbackMessageT, AllocatorT>::SharedPtr
   msg_mem_strat,
   typename std::shared_ptr<AllocatorT> allocator)
 {
@@ -44,8 +50,8 @@ create_subscription(
   subscription_options.qos = qos_profile;
   subscription_options.ignore_local_publications = ignore_local_publications;
 
-  auto factory =
-    rclcpp::create_subscription_factory<MessageT, CallbackT, AllocatorT, SubscriptionT>(
+  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(

rclcpp/include/rclcpp/duration.hpp

@@ -89,10 +89,25 @@ 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_;
 };

rclcpp/include/rclcpp/exceptions.hpp

@@ -116,7 +116,7 @@ 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);
+  void (* reset_error)() = rcl_reset_error);
 
 class RCLErrorBase
 {
@@ -181,6 +181,21 @@ public:
   : 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
 

rclcpp/include/rclcpp/executor.hpp

@@ -28,9 +28,10 @@
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
 
-#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/memory_strategies.hpp"
 #include "rclcpp/memory_strategy.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -65,16 +66,20 @@ to_string(const FutureReturnCode & future_return_code);
  */
 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;
-  size_t max_conditions = 0;
+  std::shared_ptr<rclcpp::Context> context;
+  size_t max_conditions;
 };
 
 static inline ExecutorArgs create_default_executor_arguments()
 {
-  ExecutorArgs args;
-  args.memory_strategy = memory_strategies::create_default_strategy();
-  args.max_conditions = 0;
-  return args;
+  return ExecutorArgs();
 }
 
 /// Coordinate the order and timing of available communication tasks.
@@ -95,7 +100,7 @@ public:
   /// Default constructor.
   // \param[in] ms The memory strategy to be used with this executor.
   RCLCPP_PUBLIC
-  explicit Executor(const ExecutorArgs & args = create_default_executor_arguments());
+  explicit Executor(const ExecutorArgs & args = ExecutorArgs());
 
   /// Default destructor.
   RCLCPP_PUBLIC
@@ -146,11 +151,11 @@ public:
    * spin_node_once to block indefinitely (the default behavior). A timeout of 0 causes this
    * function to be non-blocking.
    */
-  template<typename T = std::milli>
+  template<typename RepT = int64_t, typename T = std::milli>
   void
   spin_node_once(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
-    std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
+    std::chrono::duration<RepT, T> timeout = std::chrono::duration<RepT, T>(-1))
   {
     return spin_node_once_nanoseconds(
       node,
@@ -159,11 +164,11 @@ public:
   }
 
   /// Convenience function which takes Node and forwards NodeBaseInterface.
-  template<typename NodeT = rclcpp::Node, typename T = std::milli>
+  template<typename NodeT = rclcpp::Node, typename RepT = int64_t, 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))
+    std::chrono::duration<RepT, T> timeout = std::chrono::duration<RepT, T>(-1))
   {
     return spin_node_once_nanoseconds(
       node->get_node_base_interface(),
@@ -190,10 +195,14 @@ 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.
    */
   RCLCPP_PUBLIC
   virtual void
-  spin_some();
+  spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0));
 
   RCLCPP_PUBLIC
   virtual void
@@ -209,11 +218,11 @@ public:
    *   code.
    * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
    */
-  template<typename ResponseT, typename TimeT = std::milli>
+  template<typename ResponseT, typename TimeRepT = int64_t, 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))
+    std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
   {
     // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
     // inside a callback executed by an executor.
@@ -233,7 +242,7 @@ public:
     }
     std::chrono::nanoseconds timeout_left = timeout_ns;
 
-    while (rclcpp::ok()) {
+    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.
@@ -287,7 +296,7 @@ protected:
    */
   RCLCPP_PUBLIC
   void
-  execute_any_executable(AnyExecutable::SharedPtr any_exec);
+  execute_any_executable(AnyExecutable & any_exec);
 
   RCLCPP_PUBLIC
   static void
@@ -325,15 +334,17 @@ protected:
 
   RCLCPP_PUBLIC
   void
-  get_next_timer(AnyExecutable::SharedPtr any_exec);
+  get_next_timer(AnyExecutable & any_exec);
 
   RCLCPP_PUBLIC
-  AnyExecutable::SharedPtr
-  get_next_ready_executable();
+  bool
+  get_next_ready_executable(AnyExecutable & any_executable);
 
   RCLCPP_PUBLIC
-  AnyExecutable::SharedPtr
-  get_next_executable(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+  bool
+  get_next_executable(
+    AnyExecutable & any_executable,
+    std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
   /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins.
   std::atomic_bool spinning;
@@ -347,6 +358,9 @@ protected:
   /// The memory strategy: an interface for handling user-defined memory allocation strategies.
   memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;
 
+  /// The context associated with this executor.
+  std::shared_ptr<rclcpp::Context> context_;
+
 private:
   RCLCPP_DISABLE_COPY(Executor)
 

rclcpp/include/rclcpp/executors.hpp

@@ -65,13 +65,13 @@ using rclcpp::executors::SingleThreadedExecutor;
  *   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>
+template<typename ResponseT, typename TimeRepT = int64_t, typename TimeT = std::milli>
 rclcpp::executor::FutureReturnCode
 spin_node_until_future_complete(
   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))
+  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
   // inside a callback executed by an executor.
@@ -81,13 +81,14 @@ spin_node_until_future_complete(
   return retcode;
 }
 
-template<typename NodeT = rclcpp::Node, typename ResponseT, typename TimeT = std::milli>
+template<typename NodeT = rclcpp::Node, typename ResponseT, typename TimeRepT = int64_t,
+  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))
+  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   return rclcpp::executors::spin_node_until_future_complete(
     executor,
@@ -98,23 +99,24 @@ spin_node_until_future_complete(
 
 }  // namespace executors
 
-template<typename FutureT, typename TimeT = std::milli>
+template<typename FutureT, typename TimeRepT = int64_t, 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))
+  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, 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>
+template<typename NodeT = rclcpp::Node, typename FutureT, typename TimeRepT = int64_t,
+  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))
+  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout);
 }

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -15,7 +15,9 @@
 #ifndef RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
 #define RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
 
+#include <memory>
 #include <mutex>
+#include <set>
 #include <thread>
 #include <unordered_map>
 
@@ -34,9 +36,24 @@ class MultiThreadedExecutor : public executor::Executor
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(MultiThreadedExecutor)
 
+  /// 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 = rclcpp::executor::create_default_executor_arguments());
+    const executor::ExecutorArgs & args = executor::ExecutorArgs(),
+    size_t number_of_threads = 0,
+    bool yield_before_execute = false);
 
   RCLCPP_PUBLIC
   virtual ~MultiThreadedExecutor();
@@ -59,6 +76,9 @@ private:
 
   std::mutex wait_mutex_;
   size_t number_of_threads_;
+  bool yield_before_execute_;
+
+  std::set<TimerBase::SharedPtr> scheduled_timers_;
 };
 
 }  // namespace executors

rclcpp/include/rclcpp/executors/single_threaded_executor.hpp

@@ -45,7 +45,7 @@ public:
   /// Default constructor. See the default constructor for Executor.
   RCLCPP_PUBLIC
   SingleThreadedExecutor(
-    const executor::ExecutorArgs & args = rclcpp::executor::create_default_executor_arguments());
+    const executor::ExecutorArgs & args = executor::ExecutorArgs());
 
   /// Default destrcutor.
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/function_traits.hpp

@@ -49,14 +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;
+  using return_type = typename function_traits<decltype( &FunctionT::operator())>::return_type;
 };
 
 // Free functions
@@ -81,9 +81,9 @@ struct function_traits<ReturnTypeT (*)(Args ...)>: function_traits<ReturnTypeT(A
 // std::bind for object methods
 template<typename ClassT, typename ReturnTypeT, typename ... Args, typename ... FArgs>
 #if defined _LIBCPP_VERSION  // libc++ (Clang)
-struct function_traits<std::__1::__bind<ReturnTypeT (ClassT::*)(Args ...), FArgs ...>>
+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::*(ClassT *, FArgs ...))(Args ...)>>
+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
@@ -99,7 +99,7 @@ struct function_traits<
 // std::bind for free functions
 template<typename ReturnTypeT, typename ... Args, typename ... FArgs>
 #if defined _LIBCPP_VERSION  // libc++ (Clang)
-struct function_traits<std::__1::__bind<ReturnTypeT( &)(Args ...), FArgs ...>>
+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
@@ -130,20 +130,20 @@ struct function_traits<FunctionT &&>: function_traits<FunctionT>
  */
 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 function_traits

rclcpp/include/rclcpp/graph_listener.hpp

@@ -23,6 +23,7 @@
 
 #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"
@@ -62,7 +63,7 @@ class GraphListener : public std::enable_shared_from_this<GraphListener>
 {
 public:
   RCLCPP_PUBLIC
-  GraphListener();
+  explicit GraphListener(std::shared_ptr<rclcpp::Context> parent_context);
 
   RCLCPP_PUBLIC
   virtual ~GraphListener();
@@ -133,6 +134,12 @@ public:
   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
@@ -154,6 +161,12 @@ protected:
 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_;
@@ -164,6 +177,7 @@ private:
   std::vector<rclcpp::node_interfaces::NodeGraphInterface *> node_graph_interfaces_;
 
   rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
+  std::shared_ptr<rcl_context_t> interrupt_guard_condition_context_;
   rcl_guard_condition_t * shutdown_guard_condition_;
   rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
 };

rclcpp/include/rclcpp/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

@@ -326,7 +326,7 @@ public:
       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;
@@ -346,6 +346,11 @@ public:
   bool
   matches_any_publishers(const rmw_gid_t * id) const;
 
+  /// Return the number of intraprocess subscriptions to a topic, given the publisher id.
+  RCLCPP_PUBLIC
+  size_t
+  get_subscription_count(uint64_t intra_process_publisher_id) const;
+
 private:
   RCLCPP_PUBLIC
   static uint64_t

rclcpp/include/rclcpp/intra_process_manager_impl.hpp

@@ -16,6 +16,7 @@
 #define RCLCPP__INTRA_PROCESS_MANAGER_IMPL_HPP_
 
 #include <algorithm>
+#include <array>
 #include <atomic>
 #include <cstring>
 #include <functional>
@@ -24,10 +25,13 @@
 #include <memory>
 #include <mutex>
 #include <set>
+#include <stdexcept>
 #include <string>
 #include <unordered_map>
 #include <utility>
 
+#include "rmw/validate_full_topic_name.h"
+
 #include "rclcpp/macros.hpp"
 #include "rclcpp/mapped_ring_buffer.hpp"
 #include "rclcpp/publisher.hpp"
@@ -45,7 +49,7 @@ public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(IntraProcessManagerImplBase)
 
   IntraProcessManagerImplBase() = default;
-  ~IntraProcessManagerImplBase() = default;
+  virtual ~IntraProcessManagerImplBase() = default;
 
   virtual void
   add_subscription(uint64_t id, SubscriptionBase::SharedPtr subscription) = 0;
@@ -80,6 +84,9 @@ public:
   virtual bool
   matches_any_publishers(const rmw_gid_t * id) const = 0;
 
+  virtual size_t
+  get_subscription_count(uint64_t intra_process_publisher_id) const = 0;
+
 private:
   RCLCPP_DISABLE_COPY(IntraProcessManagerImplBase)
 };
@@ -95,9 +102,7 @@ public:
   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);
+    subscription_ids_by_topic_[fixed_size_string(subscription->get_topic_name())].insert(id);
   }
 
   void
@@ -172,7 +177,8 @@ public:
     }
 
     // Figure out what subscriptions should receive the message.
-    auto & destined_subscriptions = subscription_ids_by_topic_[publisher->get_topic_name()];
+    auto & destined_subscriptions =
+      subscription_ids_by_topic_[fixed_size_string(publisher->get_topic_name())];
     // Store the list for later comparison.
     if (info.target_subscriptions_by_message_sequence.count(message_seq) == 0) {
       info.target_subscriptions_by_message_sequence.emplace(
@@ -248,9 +254,52 @@ public:
     return false;
   }
 
+  size_t
+  get_subscription_count(uint64_t intra_process_publisher_id) const
+  {
+    auto publisher_it = publishers_.find(intra_process_publisher_id);
+    if (publisher_it == publishers_.end()) {
+      // Publisher is either invalid or no longer exists.
+      return 0;
+    }
+    auto publisher = publisher_it->second.publisher.lock();
+    if (!publisher) {
+      throw std::runtime_error("publisher has unexpectedly gone out of scope");
+    }
+    auto sub_map_it =
+      subscription_ids_by_topic_.find(fixed_size_string(publisher->get_topic_name()));
+    if (sub_map_it == subscription_ids_by_topic_.end()) {
+      // No intraprocess subscribers
+      return 0;
+    }
+    return sub_map_it->second.size();
+  }
+
 private:
   RCLCPP_DISABLE_COPY(IntraProcessManagerImpl)
 
+  using FixedSizeString = std::array<char, RMW_TOPIC_MAX_NAME_LENGTH + 1>;
+
+  FixedSizeString
+  fixed_size_string(const char * str) const
+  {
+    FixedSizeString ret;
+    size_t size = std::strlen(str) + 1;
+    if (size > ret.size()) {
+      throw std::runtime_error("failed to copy topic name");
+    }
+    std::memcpy(ret.data(), str, size);
+    return ret;
+  }
+  struct strcmp_wrapper
+  {
+    bool
+    operator()(const FixedSizeString lhs, const FixedSizeString rhs) const
+    {
+      return std::strcmp(lhs.data(), rhs.data()) < 0;
+    }
+  };
+
   template<typename T>
   using RebindAlloc = typename std::allocator_traits<Allocator>::template rebind_alloc<T>;
 
@@ -258,23 +307,15 @@ private:
 
   using AllocSet = std::set<uint64_t, std::less<uint64_t>, RebindAlloc<uint64_t>>;
   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>>>;
+    uint64_t, SubscriptionBase::WeakPtr,
+    std::hash<uint64_t>, std::equal_to<uint64_t>,
+    RebindAlloc<std::pair<const uint64_t, SubscriptionBase::WeakPtr>>>;
 
-  struct strcmp_wrapper : public std::binary_function<const char *, const char *, bool>
-  {
-    bool
-    operator()(const char * lhs, const char * rhs) const
-    {
-      return std::strcmp(lhs, rhs) < 0;
-    }
-  };
   using IDTopicMap = std::map<
-      const char *,
-      AllocSet,
-      strcmp_wrapper,
-      RebindAlloc<std::pair<const char * const, AllocSet>>>;
+    FixedSizeString,
+    AllocSet,
+    strcmp_wrapper,
+    RebindAlloc<std::pair<const FixedSizeString, AllocSet>>>;
 
   SubscriptionMap subscriptions_;
 
@@ -291,16 +332,16 @@ private:
     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>>>;
+      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>>>;
+    uint64_t, PublisherInfo,
+    std::hash<uint64_t>, std::equal_to<uint64_t>,
+    RebindAlloc<std::pair<const uint64_t, PublisherInfo>>>;
 
   PublisherMap publishers_;
 

rclcpp/include/rclcpp/intra_process_setting.hpp

@@ -0,0 +1,34 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__INTRA_PROCESS_SETTING_HPP_
+#define RCLCPP__INTRA_PROCESS_SETTING_HPP_
+
+namespace rclcpp
+{
+
+/// Used as argument in create_publisher and create_subscriber.
+enum class IntraProcessSetting
+{
+  /// Explicitly enable intraprocess comm at publisher/subscription level.
+  Enable,
+  /// Explicitly disable intraprocess comm at publisher/subscription level.
+  Disable,
+  /// Take intraprocess configuration from the node.
+  NodeDefault
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__INTRA_PROCESS_SETTING_HPP_

rclcpp/include/rclcpp/logger.hpp

@@ -20,6 +20,8 @@
 
 #include "rclcpp/visibility_control.hpp"
 
+#include "rcl/node.h"
+
 /**
  * \def RCLCPP_LOGGING_ENABLED
  * When this define evaluates to true (default), logger factory functions will
@@ -60,6 +62,18 @@ 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:

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -25,6 +25,7 @@
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
 
 namespace rclcpp
 {
@@ -43,6 +44,8 @@ public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy)
   using WeakNodeVector = std::vector<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>;
 
+  virtual ~MemoryStrategy() = default;
+
   virtual bool collect_entities(const WeakNodeVector & weak_nodes) = 0;
 
   virtual size_t number_of_ready_subscriptions() const = 0;
@@ -50,32 +53,34 @@ public:
   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;
 
-  /// Provide a newly initialized AnyExecutable object.
-  // \return Shared pointer to the fresh executable.
-  virtual rclcpp::executor::AnyExecutable::SharedPtr instantiate_next_executable() = 0;
-
   virtual void add_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
 
   virtual void remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
 
   virtual void
   get_next_subscription(
-    rclcpp::executor::AnyExecutable::SharedPtr any_exec,
+    rclcpp::executor::AnyExecutable & any_exec,
     const WeakNodeVector & weak_nodes) = 0;
 
   virtual void
   get_next_service(
-    rclcpp::executor::AnyExecutable::SharedPtr any_exec,
+    rclcpp::executor::AnyExecutable & any_exec,
     const WeakNodeVector & weak_nodes) = 0;
 
   virtual void
   get_next_client(
-    rclcpp::executor::AnyExecutable::SharedPtr any_exec,
+    rclcpp::executor::AnyExecutable & any_exec,
+    const WeakNodeVector & weak_nodes) = 0;
+
+  virtual void
+  get_next_waitable(
+    rclcpp::executor::AnyExecutable & any_exec,
     const WeakNodeVector & weak_nodes) = 0;
 
   virtual rcl_allocator_t
@@ -83,14 +88,18 @@ public:
 
   static rclcpp::SubscriptionBase::SharedPtr
   get_subscription_by_handle(
-    const rcl_subscription_t * subscriber_handle,
+    std::shared_ptr<const rcl_subscription_t> subscriber_handle,
     const WeakNodeVector & weak_nodes);
 
   static rclcpp::ServiceBase::SharedPtr
-  get_service_by_handle(const rcl_service_t * service_handle, const WeakNodeVector & weak_nodes);
+  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(const rcl_client_t * client_handle, const WeakNodeVector & weak_nodes);
+  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(
@@ -111,6 +120,11 @@ public:
   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

rclcpp/include/rclcpp/message_memory_strategy.hpp

@@ -18,10 +18,17 @@
 #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
 {
 namespace message_memory_strategy
@@ -39,16 +46,33 @@ public:
   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()
   {
@@ -62,6 +86,39 @@ public:
     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] msg Shared pointer to the message we are returning. */
   virtual void return_message(std::shared_ptr<MessageT> & msg)
@@ -69,8 +126,22 @@ public:
     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

@@ -41,18 +41,24 @@
 #include "rclcpp/logger.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/message_memory_strategy.hpp"
+#include "rclcpp/node_options.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/publisher_options.hpp"
 #include "rclcpp/service.hpp"
 #include "rclcpp/subscription.hpp"
+#include "rclcpp/subscription_options.hpp"
+#include "rclcpp/subscription_traits.hpp"
 #include "rclcpp/time.hpp"
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -69,30 +75,24 @@ public:
   /// 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.
+   * \param[in] options Additional options to control creation of the node.
    */
   RCLCPP_PUBLIC
   explicit Node(
     const std::string & node_name,
-    const std::string & namespace_ = "",
-    bool use_intra_process_comms = false);
+    const NodeOptions & options = NodeOptions());
 
-  /// Create a node based on the node name and a rclcpp::Context.
+  /// 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] 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.
+   * \param[in] options Additional options to control creation of the node.
    */
   RCLCPP_PUBLIC
-  Node(
+  explicit Node(
     const std::string & node_name,
     const std::string & namespace_,
-    rclcpp::Context::SharedPtr context,
-    bool use_intra_process_comms = false);
+    const NodeOptions & options = NodeOptions());
 
   RCLCPP_PUBLIC
   virtual ~Node();
@@ -104,11 +104,27 @@ public:
   get_name() const;
 
   /// Get the namespace of the node.
-  /** \return The namespace of the node. */
+  /**
+   * This namespace is the "node's" namespace, and therefore is not affected
+   * by any sub-namespace's that may affect entities created with this instance.
+   * Use get_effective_namespace() to get the full namespace used by entities.
+   *
+   * \sa get_sub_namespace()
+   * \sa get_effective_namespace()
+   * \return The namespace of the node.
+   */
   RCLCPP_PUBLIC
   const char *
   get_namespace() const;
 
+  /// Get the fully-qualified name of the node.
+  /**
+   * The fully-qualified name includes the local namespace and name of the node.
+   */
+  RCLCPP_PUBLIC
+  const char *
+  get_fully_qualified_name() const;
+
   /// Get the logger of the node.
   /** \return The logger of the node. */
   RCLCPP_PUBLIC
@@ -125,6 +141,24 @@ 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] options Additional options for the created Publisher.
+   * \return Shared pointer to the created publisher.
+   */
+  template<
+    typename MessageT,
+    typename AllocatorT = std::allocator<void>,
+    typename PublisherT = ::rclcpp::Publisher<MessageT, AllocatorT>>
+  std::shared_ptr<PublisherT>
+  create_publisher(
+    const std::string & topic_name,
+    size_t qos_history_depth,
+    const PublisherOptionsWithAllocator<AllocatorT> &
+    options = PublisherOptionsWithAllocator<AllocatorT>());
+
   /// Create and return a Publisher.
   /**
    * \param[in] topic_name The topic for this publisher to publish on.
@@ -135,10 +169,16 @@ public:
   template<
     typename MessageT, typename Alloc = std::allocator<void>,
     typename PublisherT = ::rclcpp::Publisher<MessageT, Alloc>>
+  // cppcheck-suppress syntaxError // bug in cppcheck 1.82 for [[deprecated]] on templated function
+  [[deprecated(
+    "use the create_publisher(const std::string &, size_t, const PublisherOptions<Alloc> & = "
+    "PublisherOptions<Alloc>()) signature instead")]]
   std::shared_ptr<PublisherT>
   create_publisher(
-    const std::string & topic_name, size_t qos_history_depth,
-    std::shared_ptr<Alloc> allocator = nullptr);
+    const std::string & topic_name,
+    size_t qos_history_depth,
+    std::shared_ptr<Alloc> allocator,
+    IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault);
 
   /// Create and return a Publisher.
   /**
@@ -154,7 +194,39 @@ public:
   create_publisher(
     const std::string & topic_name,
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default,
-    std::shared_ptr<Alloc> allocator = nullptr);
+    std::shared_ptr<Alloc> allocator = nullptr,
+    IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault);
+
+  /// Create and return a Subscription.
+  /**
+   * \param[in] topic_name The topic to subscribe on.
+   * \param[in] callback The user-defined callback function to receive a message
+   * \param[in] qos_history_depth The depth of the subscription's incoming message queue.
+   * \param[in] options Additional options for the creation of the Subscription.
+   * \param[in] msg_mem_strat The message memory strategy to use for allocating messages.
+   * \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 AllocatorT = std::allocator<void>,
+    typename SubscriptionT = rclcpp::Subscription<
+      typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, AllocatorT>>
+  std::shared_ptr<SubscriptionT>
+  create_subscription(
+    const std::string & topic_name,
+    CallbackT && callback,
+    size_t qos_history_depth,
+    const SubscriptionOptionsWithAllocator<AllocatorT> &
+    options = SubscriptionOptionsWithAllocator<AllocatorT>(),
+    typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
+      typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, AllocatorT
+    >::SharedPtr
+    msg_mem_strat = nullptr);
 
   /// Create and return a Subscription.
   /**
@@ -175,7 +247,8 @@ public:
     typename MessageT,
     typename CallbackT,
     typename Alloc = std::allocator<void>,
-    typename SubscriptionT = rclcpp::Subscription<MessageT, Alloc>>
+    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,
@@ -183,9 +256,11 @@ public:
     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);
+    std::shared_ptr<Alloc> allocator = nullptr,
+    IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault);
 
   /// Create and return a Subscription.
   /**
@@ -206,17 +281,23 @@ public:
     typename MessageT,
     typename CallbackT,
     typename Alloc = std::allocator<void>,
-    typename SubscriptionT = rclcpp::Subscription<MessageT, Alloc>>
+    typename SubscriptionT = rclcpp::Subscription<
+      typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>>
+  [[deprecated(
+    "use the create_subscription(const std::string &, CallbackT &&, size_t, "
+    "const SubscriptionOptions<Alloc> & = SubscriptionOptions<Alloc>(), ...) signature instead")]]
   std::shared_ptr<SubscriptionT>
   create_subscription(
     const std::string & topic_name,
-    size_t qos_history_depth,
     CallbackT && callback,
-    rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr,
+    size_t qos_history_depth,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group,
     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);
+    std::shared_ptr<Alloc> allocator = nullptr,
+    IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault);
 
   /// Create a timer.
   /**
@@ -224,10 +305,10 @@ public:
    * \param[in] callback User-defined callback function.
    * \param[in] group Callback group to execute this timer's callback in.
    */
-  template<typename DurationT = std::milli, typename CallbackT>
+  template<typename DurationRepT = int64_t, typename DurationT = std::milli, typename CallbackT>
   typename rclcpp::WallTimer<CallbackT>::SharedPtr
   create_wall_timer(
-    std::chrono::duration<int64_t, DurationT> period,
+    std::chrono::duration<DurationRepT, DurationT> period,
     CallbackT callback,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
@@ -250,11 +331,11 @@ public:
 
   RCLCPP_PUBLIC
   std::vector<rcl_interfaces::msg::SetParametersResult>
-  set_parameters(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+  set_parameters(const std::vector<rclcpp::Parameter> & parameters);
 
   RCLCPP_PUBLIC
   rcl_interfaces::msg::SetParametersResult
-  set_parameters_atomically(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+  set_parameters_atomically(const std::vector<rclcpp::Parameter> & parameters);
 
   template<typename ParameterT>
   void
@@ -262,19 +343,33 @@ public:
     const std::string & name,
     const ParameterT & value);
 
+  /// 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);
+
   RCLCPP_PUBLIC
-  std::vector<rclcpp::parameter::ParameterVariant>
+  std::vector<rclcpp::Parameter>
   get_parameters(const std::vector<std::string> & names) const;
 
   RCLCPP_PUBLIC
-  rclcpp::parameter::ParameterVariant
+  rclcpp::Parameter
   get_parameter(const std::string & name) const;
 
   RCLCPP_PUBLIC
   bool
   get_parameter(
     const std::string & name,
-    rclcpp::parameter::ParameterVariant & parameter) const;
+    rclcpp::Parameter & parameter) const;
 
   /// Assign the value of the parameter if set into the parameter argument.
   /**
@@ -288,6 +383,24 @@ public:
   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
@@ -306,6 +419,24 @@ public:
     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;
@@ -328,6 +459,10 @@ public:
   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;
@@ -346,7 +481,7 @@ public:
 
   /// 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
+   * The Event object is scoped and therefore to return the loan just let it go
    * out of scope.
    */
   RCLCPP_PUBLIC
@@ -390,6 +525,11 @@ 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
@@ -405,11 +545,137 @@ 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();
+
+  /// Return the sub-namespace, if this is a sub-node, otherwise an empty string.
+  /**
+   * The returned sub-namespace is either the accumulated sub-namespaces which
+   * were given to one-to-many create_sub_node() calls, or an empty string if
+   * this is an original node instance, i.e. not a sub-node.
+   *
+   * For example, consider:
+   *
+   *   auto node = std::make_shared<rclcpp::Node>("my_node", "my_ns");
+   *   node->get_sub_namespace();  // -> ""
+   *   auto sub_node1 = node->create_sub_node("a");
+   *   sub_node1->get_sub_namespace();  // -> "a"
+   *   auto sub_node2 = sub_node1->create_sub_node("b");
+   *   sub_node2->get_sub_namespace();  // -> "a/b"
+   *   auto sub_node3 = node->create_sub_node("foo");
+   *   sub_node3->get_sub_namespace();  // -> "foo"
+   *   node->get_sub_namespace();  // -> ""
+   *
+   * get_namespace() will return the original node namespace, and will not
+   * include the sub-namespace if one exists.
+   * To get that you need to call the get_effective_namespace() method.
+   *
+   * \sa get_namespace()
+   * \sa get_effective_namespace()
+   * \return the sub-namespace string, not including the node's original namespace
+   */
+  RCLCPP_PUBLIC
+  const std::string &
+  get_sub_namespace() const;
+
+  /// Return the effective namespace that is used when creating entities.
+  /**
+   * The returned namespace is a concatenation of the node namespace and the
+   * accumulated sub-namespaces, which is used as the namespace when creating
+   * entities which have relative names.
+   *
+   * For example, consider:
+   *
+   *   auto node = std::make_shared<rclcpp::Node>("my_node", "my_ns");
+   *   node->get_effective_namespace();  // -> "/my_ns"
+   *   auto sub_node1 = node->create_sub_node("a");
+   *   sub_node1->get_effective_namespace();  // -> "/my_ns/a"
+   *   auto sub_node2 = sub_node1->create_sub_node("b");
+   *   sub_node2->get_effective_namespace();  // -> "/my_ns/a/b"
+   *   auto sub_node3 = node->create_sub_node("foo");
+   *   sub_node3->get_effective_namespace();  // -> "/my_ns/foo"
+   *   node->get_effective_namespace();  // -> "/my_ns"
+   *
+   * \sa get_namespace()
+   * \sa get_sub_namespace()
+   * \return the sub-namespace string, not including the node's original namespace
+   */
+  RCLCPP_PUBLIC
+  const std::string &
+  get_effective_namespace() const;
+
+  /// Create a sub-node, which will extend the namespace of all entities created with it.
+  /**
+   * A sub-node (short for subordinate node) is an instance of this class
+   * which has been created using an existing instance of this class, but which
+   * has an additional sub-namespace (short for subordinate namespace)
+   * associated with it.
+   * The sub-namespace will extend the node's namespace for the purpose of
+   * creating additional entities, such as Publishers, Subscriptions, Service
+   * Clients and Servers, and so on.
+   *
+   * By default, when an instance of this class is created using one of the
+   * public constructors, it has no sub-namespace associated with it, and
+   * therefore is not a sub-node.
+   * That "normal" node instance may, however, be used to create further
+   * instances of this class, based on the original instance, which have an
+   * additional sub-namespace associated with them.
+   * This may be done by using this method, create_sub_node().
+   *
+   * Furthermore, a sub-node may be used to create additional sub-node's, in
+   * which case the sub-namespace passed to this function will further
+   * extend the sub-namespace of the existing sub-node.
+   * See get_sub_namespace() and get_effective_namespace() for examples.
+   *
+   * Note that entities which use absolute names are not affected by any
+   * namespaces, neither the normal node namespace nor any sub-namespace.
+   * Note also that the fully qualified node name is unaffected by a
+   * sub-namespace.
+   *
+   * The sub-namespace should be relative, and an exception will be thrown if
+   * the sub-namespace is absolute, i.e. if it starts with a leading '/'.
+   *
+   * \sa get_sub_namespace()
+   * \sa get_effective_namespace()
+   * \param[in] sub_namespace sub-namespace of the sub-node.
+   * \return newly created sub-node
+   * \throws NameValidationError if the sub-namespace is absolute, i.e. starts
+   *   with a leading '/'.
+   */
+  RCLCPP_PUBLIC
+  Node::SharedPtr
+  create_sub_node(const std::string & sub_namespace);
+
+  /// Return the NodeOptions used when creating this node.
+  RCLCPP_PUBLIC
+  const NodeOptions &
+  get_node_options() const;
+
+protected:
+  /// Construct a sub-node, which will extend the namespace of all entities created with it.
+  /**
+   * \sa create_sub_node()
+   *
+   * \param[in] other The node from which a new sub-node is created.
+   * \param[in] sub_namespace The sub-namespace of the sub-node.
+   */
+  RCLCPP_PUBLIC
+  Node(
+    const Node & other,
+    const std::string & sub_namespace);
+
 private:
   RCLCPP_DISABLE_COPY(Node)
 
@@ -423,10 +689,14 @@ private:
   rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_;
   rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
-  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
   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_;
+  const NodeOptions node_options_;
+  const std::string sub_namespace_;
+  const std::string effective_namespace_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/node_impl.hpp

@@ -39,6 +39,7 @@
 #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"
@@ -50,38 +51,148 @@
 namespace rclcpp
 {
 
+RCLCPP_LOCAL
+inline
+std::string
+extend_name_with_sub_namespace(const std::string & name, const std::string & sub_namespace)
+{
+  std::string name_with_sub_namespace(name);
+  if (sub_namespace != "" && name.front() != '/' && name.front() != '~') {
+    name_with_sub_namespace = sub_namespace + "/" + name;
+  }
+  return name_with_sub_namespace;
+}
+
+template<typename MessageT, typename AllocatorT, typename PublisherT>
+std::shared_ptr<PublisherT>
+Node::create_publisher(
+  const std::string & topic_name,
+  size_t qos_history_depth,
+  const PublisherOptionsWithAllocator<AllocatorT> & options)
+{
+  std::shared_ptr<AllocatorT> allocator = options.allocator;
+  if (!allocator) {
+    allocator = std::make_shared<AllocatorT>();
+  }
+  rmw_qos_profile_t qos_profile = options.qos_profile;
+  qos_profile.depth = qos_history_depth;
+
+  bool use_intra_process;
+  switch (options.use_intra_process_comm) {
+    case IntraProcessSetting::Enable:
+      use_intra_process = true;
+      break;
+    case IntraProcessSetting::Disable:
+      use_intra_process = false;
+      break;
+    case IntraProcessSetting::NodeDefault:
+      use_intra_process = this->get_node_options().use_intra_process_comms();
+      break;
+    default:
+      throw std::runtime_error("Unrecognized IntraProcessSetting value");
+      break;
+  }
+
+  return rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
+    this->node_topics_.get(),
+    extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()),
+    qos_profile,
+    use_intra_process,
+    allocator);
+}
+
 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)
+  std::shared_ptr<Alloc> allocator,
+  IntraProcessSetting use_intra_process_comm)
 {
-  if (!allocator) {
-    allocator = std::make_shared<Alloc>();
-  }
-  rmw_qos_profile_t qos = rmw_qos_profile_default;
-  qos.depth = qos_history_depth;
-  return this->create_publisher<MessageT, Alloc, PublisherT>(topic_name, qos, allocator);
+  PublisherOptionsWithAllocator<Alloc> pub_options;
+  pub_options.allocator = allocator;
+  pub_options.use_intra_process_comm = use_intra_process_comm;
+  return this->create_publisher<MessageT, Alloc, PublisherT>(
+    topic_name, qos_history_depth, pub_options);
 }
 
 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)
+  std::shared_ptr<Alloc> allocator, IntraProcessSetting use_intra_process_comm)
 {
+  PublisherOptionsWithAllocator<Alloc> pub_options;
+  pub_options.qos_profile = qos_profile;
+  pub_options.allocator = allocator;
+  pub_options.use_intra_process_comm = use_intra_process_comm;
+  return this->create_publisher<MessageT, Alloc, PublisherT>(
+    topic_name, qos_profile.depth, pub_options);
+}
+
+template<
+  typename MessageT,
+  typename CallbackT,
+  typename AllocatorT,
+  typename SubscriptionT>
+std::shared_ptr<SubscriptionT>
+Node::create_subscription(
+  const std::string & topic_name,
+  CallbackT && callback,
+  size_t qos_history_depth,
+  const SubscriptionOptionsWithAllocator<AllocatorT> & options,
+  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
+    typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, AllocatorT>::SharedPtr
+  msg_mem_strat)
+{
+  using CallbackMessageT = typename rclcpp::subscription_traits::has_message_type<CallbackT>::type;
+
+  std::shared_ptr<AllocatorT> allocator = options.allocator;
   if (!allocator) {
-    allocator = std::make_shared<Alloc>();
+    allocator = std::make_shared<AllocatorT>();
   }
-  return rclcpp::create_publisher<MessageT, Alloc, PublisherT>(
+
+  rmw_qos_profile_t qos_profile = options.qos_profile;
+  qos_profile.depth = qos_history_depth;
+
+  if (!msg_mem_strat) {
+    using rclcpp::message_memory_strategy::MessageMemoryStrategy;
+    msg_mem_strat = MessageMemoryStrategy<CallbackMessageT, AllocatorT>::create_default();
+  }
+
+  bool use_intra_process;
+  switch (options.use_intra_process_comm) {
+    case IntraProcessSetting::Enable:
+      use_intra_process = true;
+      break;
+    case IntraProcessSetting::Disable:
+      use_intra_process = false;
+      break;
+    case IntraProcessSetting::NodeDefault:
+      use_intra_process = this->get_node_options().use_intra_process_comms();
+      break;
+    default:
+      throw std::runtime_error("Unrecognized IntraProcessSetting value");
+      break;
+  }
+
+  return rclcpp::create_subscription<
+    MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT>(
     this->node_topics_.get(),
-    topic_name,
+    extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()),
+    std::forward<CallbackT>(callback),
     qos_profile,
-    use_intra_process_comms_,
+    options.callback_group,
+    options.ignore_local_publications,
+    use_intra_process,
+    msg_mem_strat,
     allocator);
 }
 
-template<typename MessageT, typename CallbackT, typename Alloc, typename SubscriptionT>
+template<
+  typename MessageT,
+  typename CallbackT,
+  typename Alloc,
+  typename SubscriptionT>
 std::shared_ptr<SubscriptionT>
 Node::create_subscription(
   const std::string & topic_name,
@@ -89,65 +200,62 @@ Node::create_subscription(
   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,
-  std::shared_ptr<Alloc> allocator)
+  std::shared_ptr<Alloc> allocator,
+  IntraProcessSetting use_intra_process_comm)
 {
-  if (!allocator) {
-    allocator = std::make_shared<Alloc>();
-  }
+  SubscriptionOptionsWithAllocator<Alloc> sub_options;
+  sub_options.qos_profile = qos_profile;
+  sub_options.callback_group = group;
+  sub_options.ignore_local_publications = ignore_local_publications;
+  sub_options.allocator = allocator;
+  sub_options.use_intra_process_comm = use_intra_process_comm;
 
-  if (!msg_mem_strat) {
-    using rclcpp::message_memory_strategy::MessageMemoryStrategy;
-    msg_mem_strat = MessageMemoryStrategy<MessageT, Alloc>::create_default();
-  }
-
-  return rclcpp::create_subscription<MessageT, CallbackT, Alloc, SubscriptionT>(
-    this->node_topics_.get(),
-    topic_name,
-    std::forward<CallbackT>(callback),
-    qos_profile,
-    group,
-    ignore_local_publications,
-    use_intra_process_comms_,
-    msg_mem_strat,
-    allocator);
+  return this->create_subscription<MessageT, CallbackT, Alloc, SubscriptionT>(
+    topic_name, std::forward<CallbackT>(callback), qos_profile.depth, sub_options, msg_mem_strat);
 }
 
-template<typename MessageT, typename CallbackT, typename Alloc, typename SubscriptionT>
+template<
+  typename MessageT,
+  typename CallbackT,
+  typename Alloc,
+  typename SubscriptionT>
 std::shared_ptr<SubscriptionT>
 Node::create_subscription(
   const std::string & topic_name,
-  size_t qos_history_depth,
   CallbackT && callback,
+  size_t qos_history_depth,
   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)
+  std::shared_ptr<Alloc> allocator,
+  IntraProcessSetting use_intra_process_comm)
 {
-  rmw_qos_profile_t qos = rmw_qos_profile_default;
-  qos.depth = qos_history_depth;
+  SubscriptionOptionsWithAllocator<Alloc> sub_options;
+  sub_options.callback_group = group;
+  sub_options.ignore_local_publications = ignore_local_publications;
+  sub_options.allocator = allocator;
+  sub_options.use_intra_process_comm = use_intra_process_comm;
+
   return this->create_subscription<MessageT, CallbackT, Alloc, SubscriptionT>(
-    topic_name,
-    std::forward<CallbackT>(callback),
-    qos,
-    group,
-    ignore_local_publications,
-    msg_mem_strat,
-    allocator);
+    topic_name, std::forward<CallbackT>(callback), qos_history_depth, sub_options, msg_mem_strat);
 }
 
-template<typename DurationT, typename CallbackT>
+template<typename DurationRepT, typename DurationT, typename CallbackT>
 typename rclcpp::WallTimer<CallbackT>::SharedPtr
 Node::create_wall_timer(
-  std::chrono::duration<int64_t, DurationT> period,
+  std::chrono::duration<DurationRepT, DurationT> period,
   CallbackT callback,
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
   auto timer = rclcpp::WallTimer<CallbackT>::make_shared(
     std::chrono::duration_cast<std::chrono::nanoseconds>(period),
-    std::move(callback));
+    std::move(callback),
+    this->node_base_->get_context());
   node_timers_->add_timer(timer, group);
   return timer;
 }
@@ -168,7 +276,7 @@ Node::create_client(
   auto cli = Client<ServiceT>::make_shared(
     node_base_.get(),
     node_graph_,
-    service_name,
+    extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
     options);
 
   auto cli_base_ptr = std::dynamic_pointer_cast<ClientBase>(cli);
@@ -184,18 +292,13 @@ Node::create_service(
   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;
+  return rclcpp::create_service<ServiceT, CallbackT>(
+    node_base_,
+    node_services_,
+    extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
+    std::forward<CallbackT>(callback),
+    qos_profile,
+    group);
 }
 
 template<typename CallbackT>
@@ -211,22 +314,70 @@ Node::set_parameter_if_not_set(
   const std::string & name,
   const ParameterT & value)
 {
-  rclcpp::parameter::ParameterVariant parameter_variant;
-  if (!this->get_parameter(name, parameter_variant)) {
+  std::string parameter_name_with_sub_namespace =
+    extend_name_with_sub_namespace(name, this->get_sub_namespace());
+
+  rclcpp::Parameter parameter;
+  if (!this->get_parameter(parameter_name_with_sub_namespace, parameter)) {
     this->set_parameters({
-        rclcpp::parameter::ParameterVariant(name, value),
+        rclcpp::Parameter(parameter_name_with_sub_namespace, value),
       });
   }
 }
 
+// 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;
+
+  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::ParameterVariant parameter_variant;
-  bool result = get_parameter(name, parameter_variant);
+  std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace());
+
+  rclcpp::Parameter parameter;
+
+  bool result = get_parameter(sub_name, parameter);
   if (result) {
-    value = parameter_variant.get_value<ParameterT>();
+    value = parameter.get_value<ParameterT>();
+  }
+
+  return result;
+}
+
+// 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::string & name,
+  std::map<std::string, MapValueT> & values) const
+{
+  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 result;
@@ -239,13 +390,33 @@ Node::get_parameter_or(
   ParameterT & value,
   const ParameterT & alternative_value) const
 {
-  bool got_parameter = get_parameter(name, value);
+  std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace());
+
+  bool got_parameter = get_parameter(sub_name, value);
   if (!got_parameter) {
     value = alternative_value;
   }
   return got_parameter;
 }
 
+template<typename ParameterT>
+void
+Node::get_parameter_or_set(
+  const std::string & name,
+  ParameterT & value,
+  const ParameterT & alternative_value)
+{
+  std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace());
+
+  bool got_parameter = get_parameter(sub_name, value);
+  if (!got_parameter) {
+    this->set_parameters({
+        rclcpp::Parameter(sub_name, alternative_value),
+      });
+    value = alternative_value;
+  }
+}
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__NODE_IMPL_HPP_

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -21,6 +21,7 @@
 
 #include "rclcpp/context.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_options.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -39,7 +40,7 @@ public:
   NodeBase(
     const std::string & node_name,
     const std::string & namespace_,
-    rclcpp::Context::SharedPtr context);
+    const NodeOptions & options);
 
   RCLCPP_PUBLIC
   virtual
@@ -55,6 +56,11 @@ public:
   const char *
   get_namespace() const;
 
+  RCLCPP_PUBLIC
+  virtual
+  const char *
+  get_fully_qualified_name() const;
+
   RCLCPP_PUBLIC
   virtual
   rclcpp::Context::SharedPtr

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -38,6 +38,10 @@ 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
@@ -52,6 +56,13 @@ public:
   const char *
   get_namespace() const = 0;
 
+  /// Return the fully qualified name of the node.
+  /** \return The fully qualified name of the node. */
+  RCLCPP_PUBLIC
+  virtual
+  const char *
+  get_fully_qualified_name() const = 0;
+
   /// Return the context of the node.
   /** \return SharedPtr to the node's context. */
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/node_interfaces/node_clock.hpp

@@ -41,7 +41,8 @@ public:
     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::NodeServicesInterface::SharedPtr node_services,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging);
 
   RCLCPP_PUBLIC
   virtual
@@ -60,9 +61,9 @@ private:
   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::Clock::SharedPtr ros_clock_;
-  rclcpp::TimeSource time_source_;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp

@@ -31,6 +31,10 @@ 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/include/rclcpp/node_interfaces/node_graph.hpp

@@ -20,6 +20,7 @@
 #include <memory>
 #include <mutex>
 #include <string>
+#include <utility>
 #include <vector>
 
 #include "rcl/guard_condition.h"
@@ -69,6 +70,11 @@ public:
   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

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

@@ -18,6 +18,7 @@
 #include <chrono>
 #include <map>
 #include <string>
+#include <utility>
 #include <vector>
 
 #include "rcl/guard_condition.h"
@@ -37,6 +38,10 @@ 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
@@ -66,6 +71,12 @@ public:
   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

rclcpp/include/rclcpp/node_interfaces/node_logging.hpp

@@ -46,6 +46,11 @@ public:
   rclcpp::Logger
   get_logger() const;
 
+  RCLCPP_PUBLIC
+  virtual
+  const char *
+  get_logger_name() const;
+
 private:
   RCLCPP_DISABLE_COPY(NodeLogging)
 

rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp

@@ -33,12 +33,23 @@ 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

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -16,6 +16,7 @@
 #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_
 
 #include <map>
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -30,6 +31,7 @@
 #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"
 
@@ -46,8 +48,15 @@ public:
 
   RCLCPP_PUBLIC
   NodeParameters(
-    rclcpp::node_interfaces::NodeTopicsInterface * node_topics,
-    bool use_intra_process);
+    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 start_parameter_event_publisher,
+    const rmw_qos_profile_t & parameter_event_qos_profile);
 
   RCLCPP_PUBLIC
   virtual
@@ -57,22 +66,22 @@ public:
   virtual
   std::vector<rcl_interfaces::msg::SetParametersResult>
   set_parameters(
-    const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+    const std::vector<rclcpp::Parameter> & parameters);
 
   RCLCPP_PUBLIC
   virtual
   rcl_interfaces::msg::SetParametersResult
   set_parameters_atomically(
-    const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+    const std::vector<rclcpp::Parameter> & parameters);
 
   RCLCPP_PUBLIC
   virtual
-  std::vector<rclcpp::parameter::ParameterVariant>
+  std::vector<rclcpp::Parameter>
   get_parameters(const std::vector<std::string> & names) const;
 
   RCLCPP_PUBLIC
   virtual
-  rclcpp::parameter::ParameterVariant
+  rclcpp::Parameter
   get_parameter(const std::string & name) const;
 
   RCLCPP_PUBLIC
@@ -80,7 +89,14 @@ public:
   bool
   get_parameter(
     const std::string & name,
-    rclcpp::parameter::ParameterVariant & parameter) const;
+    rclcpp::Parameter & parameter) const;
+
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  get_parameters_by_prefix(
+    const std::string & prefix,
+    std::map<std::string, rclcpp::Parameter> & parameters) const;
 
   RCLCPP_PUBLIC
   virtual
@@ -105,15 +121,19 @@ public:
 private:
   RCLCPP_DISABLE_COPY(NodeParameters)
 
-  rclcpp::node_interfaces::NodeTopicsInterface * node_topics_;
-
   mutable std::mutex mutex_;
 
   ParametersCallbackFunction parameters_callback_ = nullptr;
 
-  std::map<std::string, rclcpp::parameter::ParameterVariant> parameters_;
+  std::map<std::string, rclcpp::Parameter> parameters_;
 
   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

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
 
+#include <map>
 #include <string>
 #include <vector>
 
@@ -37,34 +38,71 @@ class NodeParametersInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeParametersInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeParametersInterface() = default;
+
   RCLCPP_PUBLIC
   virtual
   std::vector<rcl_interfaces::msg::SetParametersResult>
   set_parameters(
-    const std::vector<rclcpp::parameter::ParameterVariant> & parameters) = 0;
+    const std::vector<rclcpp::Parameter> & parameters) = 0;
 
   RCLCPP_PUBLIC
   virtual
   rcl_interfaces::msg::SetParametersResult
   set_parameters_atomically(
-    const std::vector<rclcpp::parameter::ParameterVariant> & parameters) = 0;
+    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::ParameterVariant>
+  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::ParameterVariant
+  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::ParameterVariant & parameter) const = 0;
+    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
@@ -82,8 +120,8 @@ public:
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const = 0;
 
   using ParametersCallbackFunction = std::function<
-      rcl_interfaces::msg::SetParametersResult(
-        const std::vector<rclcpp::parameter::ParameterVariant> &)>;
+    rcl_interfaces::msg::SetParametersResult(
+      const std::vector<rclcpp::Parameter> &)>;
 
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp

@@ -32,6 +32,10 @@ class NodeServicesInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeServicesInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeServicesInterface() = default;
+
   RCLCPP_PUBLIC
   virtual
   void

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_interface.hpp

@@ -31,6 +31,10 @@ class NodeTimersInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimersInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTimersInterface() = default;
+
   /// Add a timer to the node.
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp

@@ -40,6 +40,10 @@ class NodeTopicsInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTopicsInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTopicsInterface() = default;
+
   RCLCPP_PUBLIC
   virtual
   rclcpp::PublisherBase::SharedPtr

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/node_options.hpp

@@ -0,0 +1,264 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_OPTIONS_HPP_
+#define RCLCPP__NODE_OPTIONS_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rcl/node_options.h"
+#include "rclcpp/context.hpp"
+#include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Encapsulation of options for node initialization.
+class NodeOptions
+{
+public:
+  /// Create NodeOptions with default values, optionally specifying the allocator to use.
+  /**
+   * Default values for the node options:
+   *
+   *   - context = rclcpp::contexts::default_context::get_global_default_context()
+   *   - arguments = {}
+   *   - initial_parameters = {}
+   *   - use_global_arguments = true
+   *   - use_intra_process_comms = false
+   *   - start_parameter_services = true
+   *   - start_parameter_event_publisher = true
+   *   - parameter_event_qos_profile = rmw_qos_profile_parameter_events
+   *   - allocator = rcl_get_default_allocator()
+   *
+   * \param[in] allocator allocator to use in construction of NodeOptions.
+   */
+  RCLCPP_PUBLIC
+  explicit NodeOptions(rcl_allocator_t allocator = rcl_get_default_allocator());
+
+  /// Destructor.
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeOptions() = default;
+
+  /// Copy constructor.
+  RCLCPP_PUBLIC
+  NodeOptions(const NodeOptions & other);
+
+  /// Assignment operator.
+  RCLCPP_PUBLIC
+  NodeOptions &
+  operator=(const NodeOptions & other);
+
+  /// Return the rcl_node_options used by the node.
+  /**
+   * This data structure is created lazily, on the first call to this function.
+   * Repeated calls will not regenerate it unless one of the input settings
+   * changed, like arguments, use_global_arguments, or the rcl allocator.
+   */
+  RCLCPP_PUBLIC
+  const rcl_node_options_t *
+  get_rcl_node_options() const;
+
+  /// Return the context to be used by the node.
+  RCLCPP_PUBLIC
+  rclcpp::Context::SharedPtr
+  context() const;
+
+  /// Set the context, return this for parameter idiom.
+  RCLCPP_PUBLIC
+  NodeOptions &
+  context(rclcpp::Context::SharedPtr context);
+
+  /// Return a reference to the list of arguments for the node.
+  RCLCPP_PUBLIC
+  const std::vector<std::string> &
+  arguments() const;
+
+  /// Set the arguments, return this for parameter idiom.
+  /**
+   * These arguments are used to extract remappings used by the node and other
+   * settings.
+   *
+   * This will cause the internal rcl_node_options_t struct to be invalidated.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  arguments(const std::vector<std::string> & arguments);
+
+  /// Return a reference to the list of initial parameters.
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::Parameter> &
+  initial_parameters();
+
+  RCLCPP_PUBLIC
+  const std::vector<rclcpp::Parameter> &
+  initial_parameters() const;
+
+  /// Set the initial parameters, return this for parameter idiom.
+  /**
+   * These initial parameter values are used to change the initial value
+   * of declared parameters within the node, overriding hard coded default
+   * values if necessary.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  initial_parameters(const std::vector<rclcpp::Parameter> & initial_parameters);
+
+  /// Append a single initial parameter, parameter idiom style.
+  template<typename ParameterT>
+  NodeOptions &
+  append_initial_parameter(const std::string & name, const ParameterT & value)
+  {
+    this->initial_parameters().emplace_back(name, rclcpp::ParameterValue(value));
+    return *this;
+  }
+
+  /// Return a reference to the use_global_arguments flag.
+  RCLCPP_PUBLIC
+  const bool &
+  use_global_arguments() const;
+
+  /// Set the use_global_arguments flag, return this for parameter idiom.
+  /**
+   * If true this will cause the node's behavior to be influenced by "global"
+   * arguments, i.e. arguments not targeted at specific nodes, as well as the
+   * arguments targeted at the current node.
+   *
+   * This will cause the internal rcl_node_options_t struct to be invalidated.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  use_global_arguments(const bool & use_global_arguments);
+
+  /// Return a reference to the use_intra_process_comms flag
+  RCLCPP_PUBLIC
+  const bool &
+  use_intra_process_comms() const;
+
+  /// Set the use_intra_process_comms flag, return this for parameter idiom.
+  /**
+   * If true, messages on topics which are published and subscribed to within
+   * this context will go through a special intra-process communication code
+   * code path which can avoid serialization and deserialization, unnecessary
+   * copies, and achieve lower latencies in some cases.
+   *
+   * Defaults to false for now, as there are still some cases where it is not
+   * desirable.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  use_intra_process_comms(const bool & use_intra_process_comms);
+
+  /// Return a reference to the start_parameter_services flag
+  RCLCPP_PUBLIC
+  const bool &
+  start_parameter_services() const;
+
+  /// Set the start_parameter_services flag, return this for parameter idiom.
+  /**
+   * If true, ROS services are created to allow external nodes to list, get,
+   * and request to set parameters of this node.
+   *
+   * If false, parameters will still work locally, but will not be accessible
+   * remotely.
+   *
+   * \sa start_parameter_event_publisher()
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  start_parameter_services(const bool & start_parameter_services);
+
+  /// Return a reference to the start_parameter_event_publisher flag.
+  RCLCPP_PUBLIC
+  const bool &
+  start_parameter_event_publisher() const;
+
+  /// Set the start_parameter_event_publisher flag, return this for parameter idiom.
+  /**
+   * If true, a publisher is created on which an event message is published
+   * each time a parameter's state changes.
+   * This is used for recording and introspection, but is configurable
+   * separately from the other parameter services.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  start_parameter_event_publisher(const bool & start_parameter_event_publisher);
+
+  /// Return a reference to the parameter_event_qos_profile QoS.
+  RCLCPP_PUBLIC
+  const rmw_qos_profile_t &
+  parameter_event_qos_profile() const;
+
+  /// Set the parameter_event_qos_profile QoS, return this for parameter idiom.
+  /**
+   * The QoS settings to be used for the parameter event publisher, if enabled.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  parameter_event_qos_profile(const rmw_qos_profile_t & parameter_event_qos_profile);
+
+  /// Return the rcl_allocator_t to be used.
+  RCLCPP_PUBLIC
+  const rcl_allocator_t &
+  allocator() const;
+
+  /// Set the rcl_allocator_t to be used, may cause deallocation of existing rcl_node_options_t.
+  /**
+   * This will cause the internal rcl_node_options_t struct to be invalidated.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  allocator(rcl_allocator_t allocator);
+
+protected:
+  /// Retrieve the ROS_DOMAIN_ID environment variable and populate options.
+  size_t
+  get_domain_id_from_env() const;
+
+private:
+  // This is mutable to allow for a const accessor which lazily creates the node options instance.
+  /// Underlying rcl_node_options structure.
+  mutable std::unique_ptr<rcl_node_options_t, void (*)(rcl_node_options_t *)> node_options_;
+
+  // IMPORTANT: if any of these default values are changed, please update the
+  // documentation in this class.
+
+  rclcpp::Context::SharedPtr context_ {
+    rclcpp::contexts::default_context::get_global_default_context()};
+
+  std::vector<std::string> arguments_ {};
+
+  std::vector<rclcpp::Parameter> initial_parameters_ {};
+
+  bool use_global_arguments_ {true};
+
+  bool use_intra_process_comms_ {false};
+
+  bool start_parameter_services_ {true};
+
+  bool start_parameter_event_publisher_ {true};
+
+  rmw_qos_profile_t parameter_event_qos_profile_ {rmw_qos_profile_parameter_events};
+
+  rcl_allocator_t allocator_ {rcl_get_default_allocator()};
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_OPTIONS_HPP_

rclcpp/include/rclcpp/parameter.hpp

@@ -15,54 +15,34 @@
 #ifndef RCLCPP__PARAMETER_HPP_
 #define RCLCPP__PARAMETER_HPP_
 
+#include <iostream>
 #include <ostream>
 #include <sstream>
 #include <string>
 #include <vector>
 
 #include "rcl_interfaces/msg/parameter.hpp"
-#include "rcl_interfaces/msg/parameter_type.hpp"
-#include "rcl_interfaces/msg/parameter_value.hpp"
+#include "rclcpp/parameter_value.hpp"
 #include "rclcpp/visibility_control.hpp"
-#include "rmw/rmw.h"
 
 namespace rclcpp
 {
-namespace parameter
-{
 
-enum ParameterType
-{
-  PARAMETER_NOT_SET = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET,
-  PARAMETER_BOOL = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL,
-  PARAMETER_INTEGER = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER,
-  PARAMETER_DOUBLE = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE,
-  PARAMETER_STRING = rcl_interfaces::msg::ParameterType::PARAMETER_STRING,
-  PARAMETER_BYTES = rcl_interfaces::msg::ParameterType::PARAMETER_BYTES,
-};
-
-// Structure to store an arbitrary parameter with templated get/set methods
-class ParameterVariant
+/// Structure to store an arbitrary parameter with templated get/set methods.
+class Parameter
 {
 public:
   RCLCPP_PUBLIC
-  ParameterVariant();
+  Parameter();
+
   RCLCPP_PUBLIC
-  explicit ParameterVariant(const std::string & name, const bool bool_value);
-  RCLCPP_PUBLIC
-  explicit ParameterVariant(const std::string & name, const int int_value);
-  RCLCPP_PUBLIC
-  explicit ParameterVariant(const std::string & name, const int64_t int_value);
-  RCLCPP_PUBLIC
-  explicit ParameterVariant(const std::string & name, const float double_value);
-  RCLCPP_PUBLIC
-  explicit ParameterVariant(const std::string & name, const double double_value);
-  RCLCPP_PUBLIC
-  explicit ParameterVariant(const std::string & name, const std::string & string_value);
-  RCLCPP_PUBLIC
-  explicit ParameterVariant(const std::string & name, const char * string_value);
-  RCLCPP_PUBLIC
-  explicit ParameterVariant(const std::string & name, const std::vector<uint8_t> & bytes_value);
+  Parameter(const std::string & name, const ParameterValue & value);
+
+  template<typename ValueTypeT>
+  explicit Parameter(const std::string & name, ValueTypeT value)
+  : Parameter(name, ParameterValue(value))
+  {
+  }
 
   RCLCPP_PUBLIC
   ParameterType
@@ -78,105 +58,27 @@ public:
 
   RCLCPP_PUBLIC
   rcl_interfaces::msg::ParameterValue
-  get_parameter_value() const;
+  get_value_message() const;
 
-  // The following get_value() variants require the use of ParameterType
-
-  template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_INTEGER, int64_t>::type
+  /// Get value 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(wjwwood): use custom exception
-      throw std::runtime_error("Invalid type");
-    }
-    return value_.integer_value;
+    return value_.get<ParamT>();
   }
 
-  template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_DOUBLE, double>::type
+  /// Get value of parameter using c++ types as template argument.
+  template<typename T>
+  decltype(auto)
   get_value() const
   {
-    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) {
-      // TODO(wjwwood): use custom exception
-      throw std::runtime_error("Invalid type");
-    }
-    return value_.double_value;
+    return value_.get<T>();
   }
 
-  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(wjwwood): use custom exception
-      throw std::runtime_error("Invalid type");
-    }
-    return value_.string_value;
-  }
-
-  template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_BOOL, bool>::type
-  get_value() const
-  {
-    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) {
-      // TODO(wjwwood): use custom exception
-      throw std::runtime_error("Invalid type");
-    }
-    return value_.bool_value;
-  }
-
-  template<ParameterType type>
-  typename std::enable_if<
-    type == ParameterType::PARAMETER_BYTES, const std::vector<uint8_t> &>::type
-  get_value() const
-  {
-    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BYTES) {
-      // TODO(wjwwood): use custom exception
-      throw std::runtime_error("Invalid type");
-    }
-    return value_.bytes_value;
-  }
-
-  // The following get_value() variants allow the use of primitive types
-
-  template<typename type>
-  typename std::enable_if<
-    std::is_integral<type>::value && !std::is_same<type, bool>::value, int64_t>::type
-  get_value() const
-  {
-    return get_value<ParameterType::PARAMETER_INTEGER>();
-  }
-
-  template<typename type>
-  typename std::enable_if<std::is_floating_point<type>::value, double>::type
-  get_value() const
-  {
-    return get_value<ParameterType::PARAMETER_DOUBLE>();
-  }
-
-  template<typename type>
-  typename std::enable_if<std::is_convertible<type, std::string>::value, const std::string &>::type
-  get_value() const
-  {
-    return get_value<ParameterType::PARAMETER_STRING>();
-  }
-
-  template<typename type>
-  typename std::enable_if<std::is_same<type, bool>::value, bool>::type
-  get_value() const
-  {
-    return get_value<ParameterType::PARAMETER_BOOL>();
-  }
-
-  template<typename type>
-  typename std::enable_if<
-    std::is_convertible<
-      type, const std::vector<uint8_t> &>::value, const std::vector<uint8_t> &>::type
-  get_value() const
-  {
-    return get_value<ParameterType::PARAMETER_BYTES>();
-  }
+  RCLCPP_PUBLIC
+  bool
+  as_bool() const;
 
   RCLCPP_PUBLIC
   int64_t
@@ -190,21 +92,33 @@ public:
   const std::string &
   as_string() const;
 
-  RCLCPP_PUBLIC
-  bool
-  as_bool() const;
-
   RCLCPP_PUBLIC
   const std::vector<uint8_t> &
-  as_bytes() const;
+  as_byte_array() const;
 
   RCLCPP_PUBLIC
-  static ParameterVariant
-  from_parameter(const rcl_interfaces::msg::Parameter & parameter);
+  const std::vector<bool> &
+  as_bool_array() const;
+
+  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();
+  to_parameter_msg() const;
 
   RCLCPP_PUBLIC
   std::string
@@ -212,24 +126,22 @@ public:
 
 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 ParameterVariant & param);
+_to_json_dict_entry(const Parameter & param);
 
 RCLCPP_PUBLIC
 std::ostream &
-operator<<(std::ostream & os, const rclcpp::parameter::ParameterVariant & pv);
+operator<<(std::ostream & os, const rclcpp::Parameter & pv);
 
 RCLCPP_PUBLIC
 std::ostream &
-operator<<(std::ostream & os, const std::vector<ParameterVariant> & parameters);
+operator<<(std::ostream & os, const std::vector<Parameter> & parameters);
 
-}  // namespace parameter
 }  // namespace rclcpp
 
 namespace std
@@ -238,12 +150,12 @@ namespace std
 /// Return a json encoded version of the parameter intended for a list.
 RCLCPP_PUBLIC
 std::string
-to_string(const rclcpp::parameter::ParameterVariant & param);
+to_string(const rclcpp::Parameter & param);
 
 /// Return a json encoded version of a vector of parameters, as a string.
 RCLCPP_PUBLIC
 std::string
-to_string(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+to_string(const std::vector<rclcpp::Parameter> & parameters);
 
 }  // namespace std
 

rclcpp/include/rclcpp/parameter_client.hpp

@@ -68,25 +68,25 @@ public:
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
   RCLCPP_PUBLIC
-  std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>
+  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>>)
+      void(std::shared_future<std::vector<rclcpp::Parameter>>)
     > callback = nullptr);
 
   RCLCPP_PUBLIC
-  std::shared_future<std::vector<rclcpp::parameter::ParameterType>>
+  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>>)
+      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,
+    const std::vector<rclcpp::Parameter> & parameters,
     std::function<
       void(std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>)
     > callback = nullptr);
@@ -94,7 +94,7 @@ public:
   RCLCPP_PUBLIC
   std::shared_future<rcl_interfaces::msg::SetParametersResult>
   set_parameters_atomically(
-    const std::vector<rclcpp::parameter::ParameterVariant> & parameters,
+    const std::vector<rclcpp::Parameter> & parameters,
     std::function<
       void(std::shared_future<rcl_interfaces::msg::SetParametersResult>)
     > callback = nullptr);
@@ -120,8 +120,9 @@ public:
     auto msg_mem_strat =
       MessageMemoryStrategy<rcl_interfaces::msg::ParameterEvent, Alloc>::create_default();
 
+    using rcl_interfaces::msg::ParameterEvent;
     return rclcpp::create_subscription<
-      rcl_interfaces::msg::ParameterEvent, CallbackT, Alloc, SubscriptionT>(
+      ParameterEvent, CallbackT, Alloc, ParameterEvent, SubscriptionT>(
       this->node_topics_interface_.get(),
       "parameter_events",
       std::forward<CallbackT>(callback),
@@ -137,10 +138,10 @@ public:
   bool
   service_is_ready() const;
 
-  template<typename RatioT = std::milli>
+  template<typename RepT = int64_t, typename RatioT = std::milli>
   bool
   wait_for_service(
-    std::chrono::duration<int64_t, RatioT> timeout = std::chrono::duration<int64_t, RatioT>(-1))
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
   {
     return wait_for_service_nanoseconds(
       std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
@@ -185,7 +186,30 @@ public:
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
   RCLCPP_PUBLIC
-  std::vector<rclcpp::parameter::ParameterVariant>
+  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
@@ -200,7 +224,7 @@ public:
     std::vector<std::string> names;
     names.push_back(parameter_name);
     auto vars = get_parameters(names);
-    if ((vars.size() != 1) || (vars[0].get_type() == rclcpp::parameter::PARAMETER_NOT_SET)) {
+    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>());
@@ -226,16 +250,16 @@ public:
   }
 
   RCLCPP_PUBLIC
-  std::vector<rclcpp::parameter::ParameterType>
+  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);
+  set_parameters(const std::vector<rclcpp::Parameter> & parameters);
 
   RCLCPP_PUBLIC
   rcl_interfaces::msg::SetParametersResult
-  set_parameters_atomically(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+  set_parameters_atomically(const std::vector<rclcpp::Parameter> & parameters);
 
   RCLCPP_PUBLIC
   rcl_interfaces::msg::ListParametersResult
@@ -257,17 +281,17 @@ public:
     return async_parameters_client_->service_is_ready();
   }
 
-  template<typename RatioT = std::milli>
+  template<typename RepT = int64_t, typename RatioT = std::milli>
   bool
   wait_for_service(
-    std::chrono::duration<int64_t, RatioT> timeout = std::chrono::duration<int64_t, RatioT>(-1))
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
   {
     return async_parameters_client_->wait_for_service(timeout);
   }
 
 private:
   rclcpp::executor::Executor::SharedPtr executor_;
-  rclcpp::Node::SharedPtr node_;
+  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_;
   AsyncParametersClient::SharedPtr async_parameters_client_;
 };
 

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

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__PARAMETER_SERVICE_HPP_
 #define RCLCPP__PARAMETER_SERVICE_HPP_
 
+#include <memory>
 #include <string>
 
 #include "rcl_interfaces/srv/describe_parameters.hpp"
@@ -40,11 +41,12 @@ public:
 
   RCLCPP_PUBLIC
   explicit ParameterService(
-    const rclcpp::Node::SharedPtr node,
+    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::SharedPtr node_;
   rclcpp::Service<rcl_interfaces::srv::GetParameters>::SharedPtr get_parameters_service_;
   rclcpp::Service<rcl_interfaces::srv::GetParameterTypes>::SharedPtr
     get_parameter_types_service_;

rclcpp/include/rclcpp/parameter_value.hpp

@@ -0,0 +1,316 @@
+// 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
+{
+  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(const ParameterType type);
+
+RCLCPP_PUBLIC
+std::ostream &
+operator<<(std::ostream & os, const 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

@@ -45,6 +45,15 @@ namespace node_interfaces
 class NodeTopicsInterface;
 }
 
+namespace intra_process_manager
+{
+/**
+ * NOTE(ivanpauno): IntraProcessManager is forward declared here, avoiding a circular inclusion between intra_process_manager.hpp and publisher.hpp.
+ * SharedPtr and WeakPtr of the IntraProcessManager are defined again here, to avoid a warning for accessing a member of a forward declared class.
+ */
+class IntraProcessManager;
+}
+
 class PublisherBase
 {
   friend ::rclcpp::node_interfaces::NodeTopicsInterface;
@@ -107,6 +116,32 @@ public:
   const rcl_publisher_t *
   get_publisher_handle() const;
 
+  /// Get subscription count
+  /** \return The number of subscriptions. */
+  RCLCPP_PUBLIC
+  size_t
+  get_subscription_count() const;
+
+  /// Get intraprocess subscription count
+  /** \return The number of intraprocess subscriptions. */
+  RCLCPP_PUBLIC
+  size_t
+  get_intra_process_subscription_count() const;
+
+  /// Get the actual QoS settings, after the defaults have been determined.
+  /**
+   * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT
+   * can only be resolved after the creation of the publisher, and it
+   * depends on the underlying rmw implementation.
+   * If the underlying setting in use can't be represented in ROS terms,
+   * it will be set to RMW_QOS_POLICY_*_UNKNOWN.
+   * May throw runtime_error when an unexpected error occurs.
+   * \return The actual qos settings.
+   */
+  RCLCPP_PUBLIC
+  rmw_qos_profile_t
+  get_actual_qos() const;
+
   /// Compare this publisher to a gid.
   /**
    * Note that this function calls the next function.
@@ -128,13 +163,16 @@ public:
   operator==(const rmw_gid_t * gid) const;
 
   using StoreMessageCallbackT = std::function<uint64_t(uint64_t, void *, const std::type_info &)>;
+  using IntraProcessManagerSharedPtr =
+    std::shared_ptr<rclcpp::intra_process_manager::IntraProcessManager>;
 
   /// 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,
+    StoreMessageCallbackT store_callback,
+    IntraProcessManagerSharedPtr ipm,
     const rcl_publisher_options_t & intra_process_options);
 
 protected:
@@ -143,6 +181,10 @@ protected:
   rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher();
   rcl_publisher_t intra_process_publisher_handle_ = rcl_get_zero_initialized_publisher();
 
+  using IntraProcessManagerWeakPtr =
+    std::weak_ptr<rclcpp::intra_process_manager::IntraProcessManager>;
+  bool intra_process_is_enabled_;
+  IntraProcessManagerWeakPtr weak_ipm_;
   uint64_t intra_process_publisher_id_;
   StoreMessageCallbackT store_intra_process_message_;
 
@@ -188,7 +230,11 @@ public:
   virtual void
   publish(std::unique_ptr<MessageT, MessageDeleter> & msg)
   {
-    this->do_inter_process_publish(msg.get());
+    bool inter_process_subscriptions_exist =
+      get_subscription_count() > get_intra_process_subscription_count();
+    if (!intra_process_is_enabled_ || inter_process_subscriptions_exist) {
+      this->do_inter_process_publish(msg.get());
+    }
     if (store_intra_process_message_) {
       // Take the pointer from the unique_msg, release it and pass as a void *
       // to the ipm. The ipm should then capture it again as a unique_ptr of
@@ -205,11 +251,18 @@ public:
       ipm.publisher_id = intra_process_publisher_id_;
       ipm.message_sequence = message_seq;
       auto status = rcl_publish(&intra_process_publisher_handle_, &ipm);
-      if (status != RCL_RET_OK) {
-        // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
-        throw std::runtime_error(
-          std::string("failed to publish intra process message: ") + rcl_get_error_string_safe());
-        // *INDENT-ON*
+      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.
@@ -279,6 +332,25 @@ public:
     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_;
@@ -289,11 +361,18 @@ protected:
   do_inter_process_publish(const MessageT * msg)
   {
     auto status = rcl_publish(&publisher_handle_, msg);
-    if (status != RCL_RET_OK) {
-      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
-      throw std::runtime_error(
-        std::string("failed to publish message: ") + rcl_get_error_string_safe());
-      // *INDENT-ON*
+    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");
     }
   }
 

rclcpp/include/rclcpp/publisher_factory.hpp

@@ -46,19 +46,19 @@ 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)>;
+    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)>;
+    uint64_t(
+      rclcpp::intra_process_manager::IntraProcessManager * ipm,
+      rclcpp::PublisherBase::SharedPtr publisher)>;
 
   AddPublisherToIntraProcessManagerFunction add_publisher_to_intra_process_manager;
 
@@ -66,8 +66,8 @@ struct PublisherFactory
   // 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)>;
+    rclcpp::PublisherBase::StoreMessageCallbackT(
+      rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm)>;
 
   SharedPublishCallbackFactoryFunction create_shared_publish_callback;
 };

rclcpp/include/rclcpp/publisher_options.hpp

@@ -0,0 +1,44 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__PUBLISHER_OPTIONS_HPP_
+#define RCLCPP__PUBLISHER_OPTIONS_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/intra_process_setting.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Structure containing optional configuration for Publishers.
+template<typename Allocator>
+struct PublisherOptionsWithAllocator
+{
+  /// The quality of service profile to pass on to the rmw implementation.
+  rmw_qos_profile_t qos_profile = rmw_qos_profile_default;
+  /// Optional custom allocator.
+  std::shared_ptr<Allocator> allocator = nullptr;
+  /// Setting to explicitly set intraprocess communications.
+  IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault;
+};
+
+using PublisherOptions = PublisherOptionsWithAllocator<std::allocator<void>>;
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__PUBLISHER_OPTIONS_HPP_

rclcpp/include/rclcpp/rclcpp.hpp

@@ -49,10 +49,12 @@
  *   - rclcpp::Node::describe_parameters()
  *   - rclcpp::Node::list_parameters()
  *   - rclcpp::Node::register_param_change_callback()
- *   - rclcpp::parameter::ParameterVariant
+ *   - rclcpp::Parameter
+ *   - rclcpp::ParameterValue
  *   - rclcpp::AsyncParametersClient
  *   - rclcpp::SyncParametersClient
  *   - rclcpp/parameter.hpp
+ *   - rclcpp/parameter_value.hpp
  *   - rclcpp/parameter_client.hpp
  *   - rclcpp/parameter_service.hpp
  * - Rate:
@@ -149,5 +151,6 @@
 #include "rclcpp/time.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
 
 #endif  // RCLCPP__RCLCPP_HPP_

rclcpp/include/rclcpp/service.hpp

@@ -30,6 +30,7 @@
 #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"
 
@@ -41,11 +42,6 @@ class ServiceBase
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase)
 
-  RCLCPP_PUBLIC
-  ServiceBase(
-    std::shared_ptr<rcl_node_t> node_handle,
-    const std::string & service_name);
-
   RCLCPP_PUBLIC
   explicit ServiceBase(
     std::shared_ptr<rcl_node_t> node_handle);
@@ -54,15 +50,15 @@ public:
   virtual ~ServiceBase();
 
   RCLCPP_PUBLIC
-  std::string
+  const char *
   get_service_name();
 
   RCLCPP_PUBLIC
-  rcl_service_t *
+  std::shared_ptr<rcl_service_t>
   get_service_handle();
 
   RCLCPP_PUBLIC
-  const rcl_service_t *
+  std::shared_ptr<const rcl_service_t>
   get_service_handle() const;
 
   virtual std::shared_ptr<void> create_request() = 0;
@@ -84,8 +80,7 @@ protected:
 
   std::shared_ptr<rcl_node_t> node_handle_;
 
-  rcl_service_t * service_handle_ = nullptr;
-  std::string service_name_;
+  std::shared_ptr<rcl_service_t> service_handle_;
   bool owns_rcl_handle_ = true;
 };
 
@@ -94,15 +89,15 @@ 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>)>;
+    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(
@@ -110,17 +105,37 @@ public:
     const std::string & service_name,
     AnyServiceCallback<ServiceT> any_callback,
     rcl_service_options_t & service_options)
-  : ServiceBase(node_handle, service_name), any_callback_(any_callback)
+  : 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_ = new rcl_service_t;
-    *service_handle_ = rcl_get_zero_initialized_service();
+    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_,
+      service_handle_.get(),
       node_handle.get(),
       service_type_support_handle,
       service_name.c_str(),
@@ -141,6 +156,24 @@ public:
     }
   }
 
+  Service(
+    std::shared_ptr<rcl_node_t> node_handle,
+    std::shared_ptr<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.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,
@@ -149,35 +182,22 @@ public:
     any_callback_(any_callback)
   {
     // check if service handle was initialized
-    // TODO(karsten1987): Take this verification
-    // directly in rcl_*_t
-    // see: https://github.com/ros2/rcl/issues/81
-    if (!service_handle->impl) {
+    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*
     }
-    service_handle_ = service_handle;
-    service_name_ = std::string(rcl_service_get_service_name(service_handle));
-    owns_rcl_handle_ = false;
+
+    // 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()
   {
-    // check if you have ownership of the handle
-    if (owns_rcl_handle_) {
-      if (rcl_service_fini(service_handle_, node_handle_.get()) != RCL_RET_OK) {
-        std::stringstream ss;
-        ss << "Error in destruction of rcl service_handle_ handle: " <<
-          rcl_get_error_string_safe() << '\n';
-        (std::cerr << ss.str()).flush();
-        rcl_reset_error();
-      }
-      delete service_handle_;
-    }
   }
 
   std::shared_ptr<void> create_request()
@@ -206,7 +226,7 @@ public:
     std::shared_ptr<rmw_request_id_t> req_id,
     std::shared_ptr<typename ServiceT::Response> response)
   {
-    rcl_ret_t status = rcl_send_response(get_service_handle(), req_id.get(), response.get());
+    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");

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -25,6 +25,8 @@
 #include "rclcpp/node.hpp"
 #include "rclcpp/visibility_control.hpp"
 
+#include "rcutils/logging_macros.h"
+
 #include "rmw/types.h"
 
 namespace rclcpp
@@ -46,21 +48,16 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
 public:
   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;
 
   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>();
   }
 
@@ -90,28 +87,40 @@ public:
     service_handles_.clear();
     client_handles_.clear();
     timer_handles_.clear();
+    waitable_handles_.clear();
   }
 
   virtual void remove_null_handles(rcl_wait_set_t * wait_set)
   {
-    for (size_t i = 0; i < wait_set->size_of_subscriptions; ++i) {
+    // 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] = nullptr;
+        subscription_handles_[i].reset();
       }
     }
-    for (size_t i = 0; i < wait_set->size_of_services; ++i) {
+    for (size_t i = 0; i < service_handles_.size(); ++i) {
       if (!wait_set->services[i]) {
-        service_handles_[i] = nullptr;
+        service_handles_[i].reset();
       }
     }
-    for (size_t i = 0; i < wait_set->size_of_clients; ++i) {
+    for (size_t i = 0; i < client_handles_.size(); ++i) {
       if (!wait_set->clients[i]) {
-        client_handles_[i] = nullptr;
+        client_handles_[i].reset();
       }
     }
-    for (size_t i = 0; i < wait_set->size_of_timers; ++i) {
+    for (size_t i = 0; i < timer_handles_.size(); ++i) {
       if (!wait_set->timers[i]) {
-        timer_handles_[i] = nullptr;
+        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();
       }
     }
 
@@ -134,6 +143,11 @@ public:
       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)
@@ -178,6 +192,12 @@ public:
             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);
+          }
+        }
       }
     }
     return has_invalid_weak_nodes;
@@ -186,53 +206,65 @@ public:
   bool add_handles_to_wait_set(rcl_wait_set_t * wait_set)
   {
     for (auto subscription : subscription_handles_) {
-      if (rcl_wait_set_add_subscription(wait_set, subscription) != RCL_RET_OK) {
-        fprintf(stderr, "Couldn't add subscription to wait set: %s\n", rcl_get_error_string_safe());
+      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) != RCL_RET_OK) {
-        fprintf(stderr, "Couldn't add client to wait set: %s\n", rcl_get_error_string_safe());
+      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) != RCL_RET_OK) {
-        fprintf(stderr, "Couldn't add service to wait set: %s\n", rcl_get_error_string_safe());
+      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) != RCL_RET_OK) {
-        fprintf(stderr, "Couldn't add timer to wait set: %s\n", rcl_get_error_string_safe());
+      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) != RCL_RET_OK) {
-        fprintf(stderr, "Couldn't add guard_condition to wait set: %s\n",
-          rcl_get_error_string_safe());
+      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;
   }
 
-  /// Provide a newly initialized AnyExecutable object.
-  // \return Shared pointer to the fresh executable.
-  executor::AnyExecutable::SharedPtr instantiate_next_executable()
-  {
-    return std::allocate_shared<executor::AnyExecutable>(*executable_allocator_.get());
-  }
-
   virtual void
   get_next_subscription(
-    executor::AnyExecutable::SharedPtr any_exec,
+    executor::AnyExecutable & any_exec,
     const WeakNodeVector & weak_nodes)
   {
     auto it = subscription_handles_.begin();
@@ -249,7 +281,7 @@ public:
         if (!group) {
           // Group was not found, meaning the subscription is not valid...
           // Remove it from the ready list and continue looking
-          subscription_handles_.erase(it);
+          it = subscription_handles_.erase(it);
           continue;
         }
         if (!group->can_be_taken_from().load()) {
@@ -260,23 +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_base = get_node_by_group(group, weak_nodes);
+        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
-      subscription_handles_.erase(it);
+      it = subscription_handles_.erase(it);
     }
   }
 
   virtual void
   get_next_service(
-    executor::AnyExecutable::SharedPtr any_exec,
+    executor::AnyExecutable & any_exec,
     const WeakNodeVector & weak_nodes)
   {
     auto it = service_handles_.begin();
@@ -288,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()) {
@@ -298,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_base = 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()) {
@@ -321,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()) {
@@ -331,14 +363,47 @@ 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_base = 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);
     }
   }
 
@@ -349,42 +414,67 @@ public:
 
   size_t number_of_ready_subscriptions() const
   {
-    return subscription_handles_.size();
+    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
   {
-    return service_handles_.size();
+    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
   {
-    return client_handles_.size();
+    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
   {
-    return guard_conditions_.size();
+    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
   {
-    return timer_handles_.size();
+    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<const rcl_guard_condition_t *> guard_conditions_;
 
-  VectorRebind<const rcl_subscription_t *> subscription_handles_;
-  VectorRebind<const rcl_service_t *> service_handles_;
-  VectorRebind<const rcl_client_t *> client_handles_;
-  VectorRebind<const rcl_timer_t *> timer_handles_;
+  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_;
 };
 

rclcpp/include/rclcpp/subscription.hpp

@@ -29,12 +29,13 @@
 
 #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/expand_topic_or_service_name.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -45,6 +46,15 @@ namespace node_interfaces
 class NodeTopicsInterface;
 }  // namespace node_interfaces
 
+namespace intra_process_manager
+{
+/**
+ * NOTE(ivanpauno): IntraProcessManager is forward declared here, avoiding a circular inclusion between intra_process_manager.hpp and publisher.hpp.
+ * SharedPtr and WeakPtr of the IntraProcessManager are defined again here, to avoid a warning for accessing a member of a forward declared class.
+ */
+class IntraProcessManager;
+}
+
 /// Virtual base class for subscriptions. This pattern allows us to iterate over different template
 /// specializations of Subscription, among other things.
 class SubscriptionBase
@@ -58,13 +68,15 @@ public:
    * \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] 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<rcl_node_t> node_handle,
     const rosidl_message_type_support_t & type_support_handle,
     const std::string & topic_name,
-    const rcl_subscription_options_t & subscription_options);
+    const rcl_subscription_options_t & subscription_options,
+    bool is_serialized = false);
 
   /// Default destructor.
   RCLCPP_PUBLIC
@@ -76,21 +88,27 @@ public:
   get_topic_name() const;
 
   RCLCPP_PUBLIC
-  rcl_subscription_t *
+  std::shared_ptr<rcl_subscription_t>
   get_subscription_handle();
 
   RCLCPP_PUBLIC
-  const rcl_subscription_t *
+  const std::shared_ptr<rcl_subscription_t>
   get_subscription_handle() const;
 
   RCLCPP_PUBLIC
-  virtual const rcl_subscription_t *
+  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;
+
+  /// 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.
@@ -104,31 +122,59 @@ public:
   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;
+
+  /// Get matching publisher count
+  /** \return The number of publishers on this topic. */
+  RCLCPP_PUBLIC
+  size_t
+  get_publisher_count() const;
+
 protected:
-  rcl_subscription_t intra_process_subscription_handle_ = rcl_get_zero_initialized_subscription();
-  rcl_subscription_t subscription_handle_ = rcl_get_zero_initialized_subscription();
+  std::shared_ptr<rcl_subscription_t> intra_process_subscription_handle_;
+  std::shared_ptr<rcl_subscription_t> subscription_handle_;
   std::shared_ptr<rcl_node_t> node_handle_;
 
+  using IntraProcessManagerWeakPtr =
+    std::weak_ptr<rclcpp::intra_process_manager::IntraProcessManager>;
+  bool use_intra_process_;
+  IntraProcessManagerWeakPtr weak_ipm_;
+  uint64_t intra_process_subscription_id_;
+
 private:
   RCLCPP_DISABLE_COPY(SubscriptionBase)
+
+  rosidl_message_type_support_t type_support_;
+  bool is_serialized_;
 };
 
 /// 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_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)
 
@@ -137,6 +183,7 @@ public:
    * The constructor for a subscription is almost never called directly. Instead, subscriptions
    * should be instantiated through Node::create_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] subscription_options options for the subscription.
    * \param[in] callback User defined callback to call when a message is received.
@@ -144,17 +191,19 @@ public:
    */
   Subscription(
     std::shared_ptr<rcl_node_t> node_handle,
+    const rosidl_message_type_support_t & type_support_handle,
     const std::string & topic_name,
     const rcl_subscription_options_t & subscription_options,
-    AnySubscriptionCallback<MessageT, Alloc> callback,
-    typename message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
-    memory_strategy = message_memory_strategy::MessageMemoryStrategy<MessageT,
+    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,
-      *rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>(),
+      type_support_handle,
       topic_name,
-      subscription_options),
+      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),
@@ -167,11 +216,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
@@ -181,6 +231,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_) {
@@ -190,7 +245,7 @@ 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);
   }
 
@@ -198,10 +253,15 @@ public:
   /** \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)
@@ -229,19 +289,22 @@ public:
   }
 
   using GetMessageCallbackType =
-      std::function<void(uint64_t, uint64_t, uint64_t, MessageUniquePtr &)>;
-  using MatchesAnyPublishersCallbackType = std::function<bool(const rmw_gid_t *)>;
+    std::function<void (uint64_t, uint64_t, uint64_t, MessageUniquePtr &)>;
+  using MatchesAnyPublishersCallbackType = std::function<bool (const rmw_gid_t *)>;
 
   /// Implemenation detail.
+  // TODO(ivanpauno): This can be moved to the base class. No reason to be here.
+  // Also get_intra_process_message_callback_ and matches_any_intra_process_publishers_.
   void setup_intra_process(
     uint64_t intra_process_subscription_id,
     GetMessageCallbackType get_message_callback,
     MatchesAnyPublishersCallbackType matches_any_publisher_callback,
+    IntraProcessManagerWeakPtr weak_ipm,
     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_,
+      intra_process_subscription_handle_.get(),
       node_handle_.get(),
       rclcpp::type_support::get_intra_process_message_msg_type_support(),
       intra_process_topic_name.c_str(),
@@ -263,28 +326,29 @@ public:
     intra_process_subscription_id_ = intra_process_subscription_id;
     get_intra_process_message_callback_ = get_message_callback;
     matches_any_intra_process_publishers_ = matches_any_publisher_callback;
+    weak_ipm_ = weak_ipm;
+    use_intra_process_ = true;
   }
 
   /// Implemenation detail.
-  const rcl_subscription_t *
+  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_;
+    return intra_process_subscription_handle_;
   }
 
 private:
   RCLCPP_DISABLE_COPY(Subscription)
 
-  AnySubscriptionCallback<MessageT, Alloc> any_callback_;
-  typename message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
+  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 rclcpp

rclcpp/include/rclcpp/subscription_factory.hpp

@@ -25,6 +25,7 @@
 #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"
@@ -47,40 +48,46 @@ 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)>;
+    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)>;
+    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 SubscriptionT>
+template<
+  typename MessageT,
+  typename CallbackT,
+  typename Alloc,
+  typename CallbackMessageT,
+  typename SubscriptionT>
 SubscriptionFactory
 create_subscription_factory(
   CallbackT && callback,
-  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
+  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
+    CallbackMessageT, Alloc>::SharedPtr
   msg_mem_strat,
   std::shared_ptr<Alloc> allocator)
 {
   SubscriptionFactory factory;
 
   using rclcpp::AnySubscriptionCallback;
-  AnySubscriptionCallback<MessageT, Alloc> any_subscription_callback(allocator);
+  AnySubscriptionCallback<CallbackMessageT, Alloc> any_subscription_callback(allocator);
   any_subscription_callback.set(std::forward<CallbackT>(callback));
 
   auto message_alloc =
-    std::make_shared<typename Subscription<MessageT, Alloc>::MessageAlloc>();
+    std::make_shared<typename Subscription<CallbackMessageT, Alloc>::MessageAlloc>();
 
   // factory function that creates a MessageT specific SubscriptionT
   factory.create_typed_subscription =
@@ -91,13 +98,14 @@ create_subscription_factory(
     ) -> rclcpp::SubscriptionBase::SharedPtr
     {
       subscription_options.allocator =
-        rclcpp::allocator::get_rcl_allocator<MessageT>(*message_alloc.get());
+        rclcpp::allocator::get_rcl_allocator<CallbackMessageT>(*message_alloc.get());
 
       using rclcpp::Subscription;
       using rclcpp::SubscriptionBase;
 
-      auto sub = Subscription<MessageT, Alloc>::make_shared(
+      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,
@@ -117,7 +125,7 @@ create_subscription_factory(
       uint64_t intra_process_subscription_id = ipm->add_subscription(subscription);
 
       auto intra_process_options = rcl_subscription_get_default_options();
-      intra_process_options.allocator = rclcpp::allocator::get_rcl_allocator<MessageT>(
+      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;
@@ -128,7 +136,7 @@ create_subscription_factory(
         uint64_t publisher_id,
         uint64_t message_sequence,
         uint64_t subscription_id,
-        typename rclcpp::Subscription<MessageT, Alloc>::MessageUniquePtr & message)
+        typename rclcpp::Subscription<CallbackMessageT, Alloc>::MessageUniquePtr & message)
         {
           auto ipm = weak_ipm.lock();
           if (!ipm) {
@@ -136,7 +144,7 @@ create_subscription_factory(
             throw std::runtime_error(
                     "intra process take called after destruction of intra process manager");
           }
-          ipm->take_intra_process_message<MessageT, Alloc>(
+          ipm->take_intra_process_message<CallbackMessageT, Alloc>(
             publisher_id, message_sequence, subscription_id, message);
         };
 
@@ -158,6 +166,7 @@ create_subscription_factory(
         intra_process_subscription_id,
         take_intra_process_message_func,
         matches_any_publisher_func,
+        weak_ipm,
         intra_process_options
       );
     };

rclcpp/include/rclcpp/subscription_options.hpp

@@ -0,0 +1,48 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__SUBSCRIPTION_OPTIONS_HPP_
+#define RCLCPP__SUBSCRIPTION_OPTIONS_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/intra_process_setting.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Structure containing optional configuration for Subscriptions.
+template<typename Allocator>
+struct SubscriptionOptionsWithAllocator
+{
+  /// The quality of service profile to pass on to the rmw implementation.
+  rmw_qos_profile_t qos_profile = rmw_qos_profile_default;
+  /// True to ignore local publications.
+  bool ignore_local_publications = false;
+  /// The callback group for this subscription. NULL to use the default callback group.
+  rclcpp::callback_group::CallbackGroup::SharedPtr callback_group = nullptr;
+  /// Optional custom allocator.
+  std::shared_ptr<Allocator> allocator = nullptr;
+  /// Setting to explicitly set intraprocess communications.
+  IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault;
+};
+
+using SubscriptionOptions = SubscriptionOptionsWithAllocator<std::allocator<void>>;
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__SUBSCRIPTION_OPTIONS_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

@@ -35,7 +35,7 @@ public:
   Time(int32_t seconds, uint32_t nanoseconds, rcl_clock_type_t clock_type = RCL_SYSTEM_TIME);
 
   RCLCPP_PUBLIC
-  explicit Time(uint64_t nanoseconds = 0, rcl_clock_type_t clock = RCL_SYSTEM_TIME);
+  explicit Time(int64_t nanoseconds = 0, rcl_clock_type_t clock = RCL_SYSTEM_TIME);
 
   RCLCPP_PUBLIC
   Time(const Time & rhs);
@@ -102,6 +102,17 @@ 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;

rclcpp/include/rclcpp/time_source.hpp

@@ -21,6 +21,7 @@
 #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"
@@ -46,10 +47,13 @@ public:
 
   RCLCPP_PUBLIC
   void attachNode(
-    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);
+    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();
@@ -73,15 +77,28 @@ private:
   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 = builtin_interfaces::msg::Time;
+  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 builtin_interfaces::msg::Time::SharedPtr msg);
+  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_;
@@ -115,7 +132,7 @@ private:
   // This is needed when new clocks are added.
   bool ros_time_active_;
   // Last set message to be passed to newly registered clocks
-  builtin_interfaces::msg::Time::SharedPtr last_msg_set_;
+  rosgraph_msgs::msg::Clock::SharedPtr last_msg_set_;
 
   // A lock to protect iterating the associated_clocks_ field.
   std::mutex clock_list_lock_;

rclcpp/include/rclcpp/timer.hpp

@@ -23,6 +23,8 @@
 #include <type_traits>
 #include <utility>
 
+#include "rclcpp/clock.hpp"
+#include "rclcpp/context.hpp"
 #include "rclcpp/function_traits.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/rate.hpp"
@@ -44,7 +46,10 @@ public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase)
 
   RCLCPP_PUBLIC
-  explicit TimerBase(std::chrono::nanoseconds period);
+  explicit TimerBase(
+    Clock::SharedPtr clock,
+    std::chrono::nanoseconds period,
+    rclcpp::Context::SharedPtr context);
 
   RCLCPP_PUBLIC
   ~TimerBase();
@@ -62,7 +67,7 @@ public:
   execute_callback() = 0;
 
   RCLCPP_PUBLIC
-  const rcl_timer_t *
+  std::shared_ptr<const rcl_timer_t>
   get_timer_handle();
 
   /// Check how long the timer has until its next scheduled callback.
@@ -85,21 +90,20 @@ public:
   bool is_ready();
 
 protected:
-  rcl_timer_t timer_handle_ = rcl_get_zero_initialized_timer();
+  Clock::SharedPtr clock_;
+  std::shared_ptr<rcl_timer_t> timer_handle_;
 };
 
 
-using VoidCallbackType = std::function<void()>;
-using TimerCallbackType = std::function<void(TimerBase &)>;
+using VoidCallbackType = std::function<void ()>;
+using TimerCallbackType = std::function<void (TimerBase &)>;
 
-/// Generic timer templated on the clock type. Periodically executes a user-specified callback.
+/// Generic timer. Periodically executes a user-specified callback.
 template<
   typename FunctorT,
-  class Clock,
   typename std::enable_if<
-    (rclcpp::function_traits::same_arguments<FunctorT, VoidCallbackType>::value ||
-    rclcpp::function_traits::same_arguments<FunctorT, TimerCallbackType>::value) &&
-    Clock::is_steady
+    rclcpp::function_traits::same_arguments<FunctorT, VoidCallbackType>::value ||
+    rclcpp::function_traits::same_arguments<FunctorT, TimerCallbackType>::value
   >::type * = nullptr
 >
 class GenericTimer : public TimerBase
@@ -109,11 +113,15 @@ public:
 
   /// 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, FunctorT && callback)
-  : TimerBase(period), callback_(std::forward<FunctorT>(callback))
+  explicit GenericTimer(
+    Clock::SharedPtr clock, std::chrono::nanoseconds period, FunctorT && callback,
+    rclcpp::Context::SharedPtr context
+  )
+  : TimerBase(clock, period, context), callback_(std::forward<FunctorT>(callback))
   {
   }
 
@@ -122,15 +130,12 @@ public:
   {
     // Stop the timer from running.
     cancel();
-    if (rcl_timer_fini(&timer_handle_) != RCL_RET_OK) {
-      fprintf(stderr, "Failed to clean up rcl timer handle: %s\n", rcl_get_error_string_safe());
-    }
   }
 
   void
-  execute_callback()
+  execute_callback() override
   {
-    rcl_ret_t ret = rcl_timer_call(&timer_handle_);
+    rcl_ret_t ret = rcl_timer_call(timer_handle_.get());
     if (ret == RCL_RET_TIMER_CANCELED) {
       return;
     }
@@ -165,10 +170,10 @@ public:
     callback_(*this);
   }
 
-  virtual bool
-  is_steady()
+  bool
+  is_steady() override
   {
-    return Clock::is_steady;
+    return clock_->get_clock_type() == RCL_STEADY_TIME;
   }
 
 protected:
@@ -177,8 +182,29 @@ protected:
   FunctorT callback_;
 };
 
-template<typename CallbackType>
-using WallTimer = GenericTimer<CallbackType, 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)
+
+  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)
+  {}
+
+protected:
+  RCLCPP_DISABLE_COPY(WallTimer)
+};
 
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/utilities.hpp

@@ -17,12 +17,13 @@
 
 #include <chrono>
 #include <functional>
+#include <limits>
+#include <vector>
 
+#include "rclcpp/context.hpp"
+#include "rclcpp/init_options.hpp"
 #include "rclcpp/visibility_control.hpp"
 
-#include "rcl/guard_condition.h"
-#include "rcl/wait.h"
-
 #include "rmw/macros.h"
 #include "rmw/rmw.h"
 
@@ -44,70 +45,262 @@ std::string to_string(T value)
 
 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());
+
+/// 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();
+
+/// 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.
  */
 RCLCPP_PUBLIC
-void
-init(int argc, char * argv[]);
+std::vector<std::string>
+init_and_remove_ros_arguments(
+  int argc,
+  char const * const argv[],
+  const InitOptions & init_options = InitOptions());
+
+/// 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();
+ok(rclcpp::Context::SharedPtr context = nullptr);
 
-/// Notify the signal handler and rmw that rclcpp is shutting down.
-RCLCPP_PUBLIC
-void
-shutdown();
-
-/// Register a function to be called when shutdown is called.
-/** Calling the callbacks is the last thing shutdown() does. */
-RCLCPP_PUBLIC
-void
-on_shutdown(std::function<void(void)> callback);
-
-/// Get a handle to the rmw guard condition that manages the signal handler.
+/// Return true if init() has already been called for the given 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). Will throw an exception if
- * initialization of the guard condition fails.
- * \param wait_set Pointer to the rcl_wait_set_t that will be using the
- * resulting guard condition.
- * \return Pointer to the guard condition.
+ * 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
-rcl_guard_condition_t *
-get_sigint_guard_condition(rcl_wait_set_t * wait_set);
+bool
+is_initialized(rclcpp::Context::SharedPtr context = nullptr);
 
-/// Release the previously allocated guard condition that manages the signal handler.
+/// Shutdown rclcpp context, invalidating it for derived entities.
 /**
- * If you previously called get_sigint_guard_condition() for a given wait set
- * to get a sigint guard condition, then you should call release_sigint_guard_condition()
- * when you're done, to free that condition.  Will throw an exception if
- * get_sigint_guard_condition() wasn't previously called for the given wait set.
- * \param wait_set Pointer to the rcl_wait_set_t that was using the
- * resulting guard condition.
+ * 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
-release_sigint_guard_condition(rcl_wait_set_t * wait_set);
+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)
+{
+  return (y > 0) && (x > (std::numeric_limits<T>::max() - y));
+}
+
+/// 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));
+}
+
+/// 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));
+}
+
+/// 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
 

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

@@ -2,27 +2,30 @@
 <?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.4.0</version>
+  <version>0.7.0</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>
@@ -33,6 +36,7 @@
   <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/resource/logging.hpp.em

@@ -21,6 +21,7 @@
 
 #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
@@ -30,6 +31,9 @@
 #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]
@@ -67,6 +71,9 @@ def is_supported_feature_combination(feature_combination):
 #else
 @[ for feature_combination in [fc for fc in feature_combinations if is_supported_feature_combination(fc)]]@
 @{suffix = get_suffix_from_features(feature_combination)}@
+// The RCLCPP_@(severity)@(suffix) macro is surrounded by do { .. } while (0)
+// to implement the standard C macro idiom to make the macro safe in all
+// contexts; see http://c-faq.com/cpp/multistmt.html for more information.
 /**
  * \def RCLCPP_@(severity)@(suffix)
  * Log a message with severity @(severity)@
@@ -82,19 +89,24 @@ def is_supported_feature_combination(feature_combination):
 @[ 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
+ * \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<std::remove_reference<decltype(logger)>::type, ::rclcpp::Logger>::value, \
-    "First argument to logging macros must be an rclcpp::Logger"); \
-  RCUTILS_LOG_@(severity)@(suffix)_NAMED( \
+  do { \
+    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]))@
+@(''.join(['      ' + p + ', \\\n' for p in params]))@
 @[ end if]@
-    logger.get_name(), \
-    __VA_ARGS__)
+      logger.get_name(), \
+      rclcpp::get_c_string(RCLCPP_FIRST_ARG(__VA_ARGS__, "")), \
+        RCLCPP_ALL_BUT_FIRST_ARGS(__VA_ARGS__,"")); \
+  } while (0)
 
 @[ end for]@
 #endif

rclcpp/src/rclcpp/callback_group.cpp

@@ -51,6 +51,13 @@ CallbackGroup::get_client_ptrs() const
   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()
 {
@@ -91,3 +98,23 @@ 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

@@ -14,6 +14,7 @@
 
 #include "rclcpp/client.hpp"
 
+#include <algorithm>
 #include <chrono>
 #include <cstdio>
 #include <memory>
@@ -26,6 +27,7 @@
 #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;
@@ -33,39 +35,74 @@ using rclcpp::exceptions::throw_from_rcl_error;
 
 ClientBase::ClientBase(
   rclcpp::node_interfaces::NodeBaseInterface * node_base,
-  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
-  const std::string & service_name)
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph)
 : node_graph_(node_graph),
   node_handle_(node_base->get_shared_rcl_node_handle()),
-  service_name_(service_name)
-{}
+  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() {}
+ClientBase::~ClientBase()
+{
+  // Make sure the client handle is destructed as early as possible and before the node handle
+  client_handle_.reset();
+}
 
-const std::string &
+const char *
 ClientBase::get_service_name() const
 {
-  return this->service_name_;
+  return rcl_client_get_service_name(this->get_client_handle().get());
 }
 
-rcl_client_t *
+std::shared_ptr<rcl_client_t>
 ClientBase::get_client_handle()
 {
-  return &client_handle_;
+  return client_handle_;
 }
 
-const rcl_client_t *
+std::shared_ptr<const rcl_client_t>
 ClientBase::get_client_handle() const
 {
-  return &client_handle_;
+  return client_handle_;
 }
 
 bool
 ClientBase::service_is_ready() const
 {
   bool is_ready;
-  rcl_ret_t ret =
-    rcl_service_server_is_available(this->get_rcl_node_handle(), &client_handle_, &is_ready);
+  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");
   }
@@ -81,7 +118,6 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
   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;
@@ -90,31 +126,43 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
     // check was non-blocking, return immediately
     return false;
   }
+  auto event = node_ptr->get_graph_event();
   // update the time even on the first loop to account for time spent in the first call
   // to this->server_is_ready()
-  std::chrono::nanoseconds time_to_wait = timeout - (std::chrono::steady_clock::now() - start);
-  if (timeout > std::chrono::nanoseconds(0) && time_to_wait < std::chrono::nanoseconds(0)) {
+  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);
   }
-  // continue forever if timeout is negative, otherwise continue until out of time_to_wait
   do {
-    if (!rclcpp::ok()) {
+    if (!rclcpp::ok(this->context_)) {
       return false;
     }
-    node_ptr->wait_for_graph_change(event, time_to_wait);
-    event->check_and_clear();  // reset the event
-
-    // always check if the service is ready, even if the graph event wasn't triggered
-    // this is needed to avoid a race condition that is specific to the Connext RMW implementation
+    // 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
-    time_to_wait = timeout - (std::chrono::steady_clock::now() - start);
-  } while (time_to_wait > std::chrono::nanoseconds(0) || timeout < std::chrono::nanoseconds(0));
+    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
 }
 

rclcpp/src/rclcpp/clock.cpp

@@ -28,27 +28,11 @@
 
 namespace rclcpp
 {
-bool
-JumpThreshold::is_exceeded(const TimeJump & jump)
-{
-  if (on_clock_change_ &&
-    (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_ACTIVATED ||
-    jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_DEACTIVATED))
-  {
-    return true;
-  }
-  if ((uint64_t)jump.delta_.nanoseconds > min_forward_ ||
-    (uint64_t)jump.delta_.nanoseconds < min_backward_)
-  {
-    return true;
-  }
-  return false;
-}
 
 JumpHandler::JumpHandler(
   std::function<void()> pre_callback,
-  std::function<void(TimeJump)> post_callback,
-  JumpThreshold & threshold)
+  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)
@@ -77,7 +61,7 @@ Clock::now()
 {
   Time now(0, 0, rcl_clock_.type);
 
-  auto ret = rcl_clock_get_now(&rcl_clock_, &now.rcl_time_);
+  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");
@@ -103,68 +87,70 @@ Clock::ros_time_is_active()
   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 TimeJump &)> post_callback,
-  JumpThreshold & threshold)
+  std::function<void(const rcl_time_jump_t &)> post_callback,
+  const rcl_jump_threshold_t & threshold)
 {
-  // JumpHandler jump_callback;
-  auto jump_callback =
-    std::make_shared<rclcpp::JumpHandler>(pre_callback, post_callback, threshold);
-  {
-    std::lock_guard<std::mutex> guard(callback_list_mutex_);
-    active_jump_handlers_.push_back(jump_callback);
+  // 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");
   }
-  return jump_callback;
-}
 
-std::vector<JumpHandler::SharedPtr>
-Clock::get_triggered_callback_handlers(const TimeJump & jump)
-{
-  std::vector<JumpHandler::SharedPtr> callbacks;
-  std::lock_guard<std::mutex> guard(callback_list_mutex_);
-  active_jump_handlers_.erase(
-    std::remove_if(
-      active_jump_handlers_.begin(),
-      active_jump_handlers_.end(),
-      [&callbacks, &jump](const std::weak_ptr<JumpHandler> & wjcb) {
-        if (auto jcb = wjcb.lock()) {
-          if (jcb->notice_threshold.is_exceeded(jump)) {
-            callbacks.push_back(jcb);
-          }
-          return false;
-        }
-        // Lock failed so clear the weak pointer.
-        return true;
-      }),
-    active_jump_handlers_.end());
-  return callbacks;
-}
-
-void
-Clock::invoke_prejump_callbacks(
-  const std::vector<JumpHandler::SharedPtr> & callbacks)
-{
-  for (const auto cb : callbacks) {
-    cb->pre_callback();
+  // 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 = nullptr;
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to add time jump callback");
   }
-}
 
-void
-Clock::invoke_postjump_callbacks(
-  const std::vector<JumpHandler::SharedPtr> & callbacks,
-  const TimeJump & jump)
-{
-  for (auto cb : callbacks) {
-    cb->post_callback(jump);
+  if (nullptr == handler) {
+    // imposible to reach here; added to make cppcheck happy
+    return nullptr;
   }
+
+  // *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

@@ -14,6 +14,296 @@
 
 #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() {}
+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/duration.cpp

@@ -12,6 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <cmath>
 #include <cstdlib>
 #include <limits>
 #include <utility>
@@ -87,9 +88,6 @@ Duration::operator=(const Duration & rhs)
 Duration &
 Duration::operator=(const builtin_interfaces::msg::Duration & duration_msg)
 {
-  if (duration_msg.sec < 0) {
-    throw std::runtime_error("cannot store a negative duration point in rclcpp::Duration");
-  }
   rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast<int64_t>(duration_msg.sec));
   rcl_duration_.nanoseconds += duration_msg.nanosec;
   return *this;
@@ -182,10 +180,50 @@ Duration::operator-(const rclcpp::Duration & rhs) const
     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/exceptions.cpp

@@ -44,7 +44,7 @@ throw_from_rcl_error(
   rcl_ret_t ret,
   const std::string & prefix,
   const rcl_error_state_t * error_state,
-  void (*reset_error)())
+  void (* reset_error)())
 {
   if (RCL_RET_OK == ret) {
     throw std::invalid_argument("ret is RCL_RET_OK");
@@ -75,7 +75,7 @@ throw_from_rcl_error(
 
 RCLErrorBase::RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state)
 : ret(ret), message(error_state->message), file(error_state->file), line(error_state->line_number),
-  formatted_message(rcl_get_error_string_safe())
+  formatted_message(rcl_get_error_string().str)
 {}
 
 RCLError::RCLError(

rclcpp/src/rclcpp/executor.cpp

@@ -28,6 +28,9 @@
 
 #include "rcl_interfaces/msg/intra_process_message.hpp"
 
+#include "rcutils/logging_macros.h"
+
+using rclcpp::exceptions::throw_from_rcl_error;
 using rclcpp::executor::AnyExecutable;
 using rclcpp::executor::Executor;
 using rclcpp::executor::ExecutorArgs;
@@ -38,33 +41,35 @@ Executor::Executor(const ExecutorArgs & args)
   memory_strategy_(args.memory_strategy)
 {
   rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options();
-  if (rcl_guard_condition_init(
-      &interrupt_guard_condition_, guard_condition_options) != RCL_RET_OK)
-  {
-    throw std::runtime_error(
-            std::string("Failed to create interrupt guard condition in Executor constructor: ") +
-            rcl_get_error_string_safe());
+  rcl_ret_t ret = rcl_guard_condition_init(
+    &interrupt_guard_condition_, args.context->get_rcl_context().get(), guard_condition_options);
+  if (RCL_RET_OK != ret) {
+    throw_from_rcl_error(ret, "Failed to create interrupt guard condition in Executor constructor");
   }
 
   // The number of guard conditions is always at least 2: 1 for the ctrl-c guard cond,
   // and one for the executor's guard cond (interrupt_guard_condition_)
 
   // Put the global ctrl-c guard condition in
-  memory_strategy_->add_guard_condition(rclcpp::get_sigint_guard_condition(&wait_set_));
+  memory_strategy_->add_guard_condition(args.context->get_interrupt_guard_condition(&wait_set_));
 
   // Put the executor's guard condition in
   memory_strategy_->add_guard_condition(&interrupt_guard_condition_);
   rcl_allocator_t allocator = memory_strategy_->get_allocator();
 
-  if (rcl_wait_set_init(
-      &wait_set_, 0, 2, 0, 0, 0, allocator) != RCL_RET_OK)
-  {
-    fprintf(stderr,
-      "[rclcpp::error] failed to create wait set: %s\n", rcl_get_error_string_safe());
+  // Store the context for later use.
+  context_ = args.context;
+
+  ret = rcl_wait_set_init(&wait_set_, 0, 2, 0, 0, 0, context_->get_rcl_context().get(), allocator);
+  if (RCL_RET_OK != ret) {
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "failed to create wait set: %s", rcl_get_error_string().str);
     rcl_reset_error();
     if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
-      fprintf(stderr,
-        "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "failed to destroy guard condition: %s", rcl_get_error_string().str);
       rcl_reset_error();
     }
     throw std::runtime_error("Failed to create wait set in Executor constructor");
@@ -85,20 +90,21 @@ Executor::~Executor()
 
   // Finalize the wait set.
   if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) {
-    fprintf(stderr,
-      "[rclcpp::error] failed to destroy wait set: %s\n", rcl_get_error_string_safe());
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "failed to destroy wait set: %s", rcl_get_error_string().str);
     rcl_reset_error();
   }
   // Finalize the interrupt guard condition.
   if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
-    fprintf(stderr,
-      "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "failed to destroy guard condition: %s", rcl_get_error_string().str);
     rcl_reset_error();
   }
   // Remove and release the sigint guard condition
-  memory_strategy_->remove_guard_condition(
-    rclcpp::get_sigint_guard_condition(&wait_set_));
-  rclcpp::release_sigint_guard_condition(&wait_set_);
+  memory_strategy_->remove_guard_condition(context_->get_interrupt_guard_condition(&wait_set_));
+  context_->release_interrupt_guard_condition(&wait_set_, std::nothrow);
 }
 
 void
@@ -121,7 +127,7 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt
   if (notify) {
     // Interrupt waiting to handle new node
     if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-      throw std::runtime_error(rcl_get_error_string_safe());
+      throw std::runtime_error(rcl_get_error_string().str);
     }
   }
   // Add the node's notify condition to the guard condition handles
@@ -155,7 +161,7 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node
     // If the node was matched and removed, interrupt waiting
     if (node_removed) {
       if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-        throw std::runtime_error(rcl_get_error_string_safe());
+        throw std::runtime_error(rcl_get_error_string().str);
       }
     }
   }
@@ -194,15 +200,32 @@ Executor::spin_node_some(std::shared_ptr<rclcpp::Node> node)
 }
 
 void
-Executor::spin_some()
+Executor::spin_some(std::chrono::nanoseconds max_duration)
 {
+  auto start = std::chrono::steady_clock::now();
+  auto max_duration_not_elapsed = [max_duration, start]() {
+      if (std::chrono::nanoseconds(0) == max_duration) {
+        // told to spin forever if need be
+        return true;
+      } else if (std::chrono::steady_clock::now() - start < max_duration) {
+        // told to spin only for some maximum amount of time
+        return true;
+      }
+      // spun too long
+      return false;
+    };
+
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin_some() called while already spinning");
   }
   RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
-  AnyExecutable::SharedPtr any_exec;
-  while ((any_exec = get_next_executable(std::chrono::milliseconds::zero())) && spinning.load()) {
-    execute_any_executable(any_exec);
+  while (spinning.load() && max_duration_not_elapsed()) {
+    AnyExecutable any_exec;
+    if (get_next_executable(any_exec, std::chrono::milliseconds::zero())) {
+      execute_any_executable(any_exec);
+    } else {
+      break;
+    }
   }
 }
 
@@ -213,8 +236,8 @@ Executor::spin_once(std::chrono::nanoseconds timeout)
     throw std::runtime_error("spin_once() called while already spinning");
   }
   RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
-  auto any_exec = get_next_executable(timeout);
-  if (any_exec) {
+  AnyExecutable any_exec;
+  if (get_next_executable(any_exec, timeout)) {
     execute_any_executable(any_exec);
   }
 }
@@ -224,7 +247,7 @@ Executor::cancel()
 {
   spinning.store(false);
   if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-    throw std::runtime_error(rcl_get_error_string_safe());
+    throw std::runtime_error(rcl_get_error_string().str);
   }
 }
 
@@ -238,32 +261,35 @@ Executor::set_memory_strategy(rclcpp::memory_strategy::MemoryStrategy::SharedPtr
 }
 
 void
-Executor::execute_any_executable(AnyExecutable::SharedPtr any_exec)
+Executor::execute_any_executable(AnyExecutable & any_exec)
 {
-  if (!any_exec || !spinning.load()) {
+  if (!spinning.load()) {
     return;
   }
-  if (any_exec->timer) {
-    execute_timer(any_exec->timer);
+  if (any_exec.timer) {
+    execute_timer(any_exec.timer);
   }
-  if (any_exec->subscription) {
-    execute_subscription(any_exec->subscription);
+  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.subscription_intra_process) {
+    execute_intra_process_subscription(any_exec.subscription_intra_process);
   }
-  if (any_exec->service) {
-    execute_service(any_exec->service);
+  if (any_exec.service) {
+    execute_service(any_exec.service);
   }
-  if (any_exec->client) {
-    execute_client(any_exec->client);
+  if (any_exec.client) {
+    execute_client(any_exec.client);
+  }
+  if (any_exec.waitable) {
+    any_exec.waitable->execute();
   }
   // Reset the callback_group, regardless of type
-  any_exec->callback_group->can_be_taken_from().store(true);
+  any_exec.callback_group->can_be_taken_from().store(true);
   // Wake the wait, because it may need to be recalculated or work that
   // was previously blocked is now available.
   if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-    throw std::runtime_error(rcl_get_error_string_safe());
+    throw std::runtime_error(rcl_get_error_string().str);
   }
 }
 
@@ -271,21 +297,41 @@ void
 Executor::execute_subscription(
   rclcpp::SubscriptionBase::SharedPtr subscription)
 {
-  std::shared_ptr<void> message = subscription->create_message();
   rmw_message_info_t message_info;
+  message_info.from_intra_process = false;
 
-  auto ret = rcl_take(subscription->get_subscription_handle(),
+  if (subscription->is_serialized()) {
+    auto serialized_msg = subscription->create_serialized_message();
+    auto ret = rcl_take_serialized_message(
+      subscription->get_subscription_handle().get(),
+      serialized_msg.get(), &message_info);
+    if (RCL_RET_OK == ret) {
+      auto void_serialized_msg = std::static_pointer_cast<void>(serialized_msg);
+      subscription->handle_message(void_serialized_msg, message_info);
+    } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "take_serialized failed for subscription on topic '%s': %s",
+        subscription->get_topic_name(), rcl_get_error_string().str);
+      rcl_reset_error();
+    }
+    subscription->return_serialized_message(serialized_msg);
+  } else {
+    std::shared_ptr<void> message = subscription->create_message();
+    auto ret = rcl_take(
+      subscription->get_subscription_handle().get(),
       message.get(), &message_info);
-  if (ret == RCL_RET_OK) {
-    message_info.from_intra_process = false;
-    subscription->handle_message(message, message_info);
-  } else if (ret != RCL_RET_SUBSCRIPTION_TAKE_FAILED) {
-    fprintf(stderr,
-      "[rclcpp::error] take failed for subscription on topic '%s': %s\n",
-      subscription->get_topic_name(), rcl_get_error_string_safe());
-    rcl_reset_error();
+    if (RCL_RET_OK == ret) {
+      subscription->handle_message(message, message_info);
+    } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "could not deserialize serialized message on topic '%s': %s",
+        subscription->get_topic_name(), rcl_get_error_string().str);
+      rcl_reset_error();
+    }
+    subscription->return_message(message);
   }
-  subscription->return_message(message);
 }
 
 void
@@ -295,7 +341,7 @@ Executor::execute_intra_process_subscription(
   rcl_interfaces::msg::IntraProcessMessage ipm;
   rmw_message_info_t message_info;
   rcl_ret_t status = rcl_take(
-    subscription->get_intra_process_subscription_handle(),
+    subscription->get_intra_process_subscription_handle().get(),
     &ipm,
     &message_info);
 
@@ -303,9 +349,10 @@ Executor::execute_intra_process_subscription(
     message_info.from_intra_process = true;
     subscription->handle_intra_process_message(ipm, message_info);
   } else if (status != RCL_RET_SUBSCRIPTION_TAKE_FAILED) {
-    fprintf(stderr,
-      "[rclcpp::error] take failed for intra process subscription on topic '%s': %s\n",
-      subscription->get_topic_name(), rcl_get_error_string_safe());
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "take failed for intra process subscription on topic '%s': %s",
+      subscription->get_topic_name(), rcl_get_error_string().str);
     rcl_reset_error();
   }
 }
@@ -324,15 +371,16 @@ Executor::execute_service(
   auto request_header = service->create_request_header();
   std::shared_ptr<void> request = service->create_request();
   rcl_ret_t status = rcl_take_request(
-    service->get_service_handle(),
+    service->get_service_handle().get(),
     request_header.get(),
     request.get());
   if (status == RCL_RET_OK) {
     service->handle_request(request_header, request);
   } else if (status != RCL_RET_SERVICE_TAKE_FAILED) {
-    fprintf(stderr,
-      "[rclcpp::error] take request failed for server of service '%s': %s\n",
-      service->get_service_name().c_str(), rcl_get_error_string_safe());
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "take request failed for server of service '%s': %s",
+      service->get_service_name(), rcl_get_error_string().str);
     rcl_reset_error();
   }
 }
@@ -344,15 +392,16 @@ Executor::execute_client(
   auto request_header = client->create_request_header();
   std::shared_ptr<void> response = client->create_response();
   rcl_ret_t status = rcl_take_response(
-    client->get_client_handle(),
+    client->get_client_handle().get(),
     request_header.get(),
     response.get());
   if (status == RCL_RET_OK) {
     client->handle_response(request_header, response);
   } else if (status != RCL_RET_CLIENT_TAKE_FAILED) {
-    fprintf(stderr,
-      "[rclcpp::error] take response failed for client of service '%s': %s\n",
-      client->get_service_name().c_str(), rcl_get_error_string_safe());
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "take response failed for client of service '%s': %s",
+      client->get_service_name(), rcl_get_error_string().str);
     rcl_reset_error();
   }
 }
@@ -376,45 +425,19 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
       )
     );
   }
-
-  if (rcl_wait_set_resize_subscriptions(
-      &wait_set_, memory_strategy_->number_of_ready_subscriptions()) != RCL_RET_OK)
-  {
-    throw std::runtime_error(
-            std::string("Couldn't resize the number of subscriptions in wait set : ") +
-            rcl_get_error_string_safe());
+  // clear wait set
+  if (rcl_wait_set_clear(&wait_set_) != RCL_RET_OK) {
+    throw std::runtime_error("Couldn't clear wait set");
   }
 
-  if (rcl_wait_set_resize_services(
-      &wait_set_, memory_strategy_->number_of_ready_services()) != RCL_RET_OK)
-  {
+  // The size of waitables are accounted for in size of the other entities
+  rcl_ret_t ret = rcl_wait_set_resize(
+    &wait_set_, memory_strategy_->number_of_ready_subscriptions(),
+    memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(),
+    memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services());
+  if (RCL_RET_OK != ret) {
     throw std::runtime_error(
-            std::string("Couldn't resize the number of services in wait set : ") +
-            rcl_get_error_string_safe());
-  }
-
-  if (rcl_wait_set_resize_clients(
-      &wait_set_, memory_strategy_->number_of_ready_clients()) != RCL_RET_OK)
-  {
-    throw std::runtime_error(
-            std::string("Couldn't resize the number of clients in wait set : ") +
-            rcl_get_error_string_safe());
-  }
-
-  if (rcl_wait_set_resize_guard_conditions(
-      &wait_set_, memory_strategy_->number_of_guard_conditions()) != RCL_RET_OK)
-  {
-    throw std::runtime_error(
-            std::string("Couldn't resize the number of guard_conditions in wait set : ") +
-            rcl_get_error_string_safe());
-  }
-
-  if (rcl_wait_set_resize_timers(
-      &wait_set_, memory_strategy_->number_of_ready_timers()) != RCL_RET_OK)
-  {
-    throw std::runtime_error(
-            std::string("Couldn't resize the number of timers in wait set : ") +
-            rcl_get_error_string_safe());
+            std::string("Couldn't resize the wait set : ") + rcl_get_error_string().str);
   }
 
   if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) {
@@ -423,7 +446,9 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
   rcl_ret_t status =
     rcl_wait(&wait_set_, std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count());
   if (status == RCL_RET_WAIT_SET_EMPTY) {
-    fprintf(stderr, "Warning: empty wait set received in rcl_wait(). This should never happen.\n");
+    RCUTILS_LOG_WARN_NAMED(
+      "rclcpp",
+      "empty wait set received in rcl_wait(). This should never happen.");
   } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) {
     using rclcpp::exceptions::throw_from_rcl_error;
     throw_from_rcl_error(status, "rcl_wait() failed");
@@ -432,21 +457,6 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
   // check the null handles in the wait set and remove them from the handles in memory strategy
   // for callback-based entities
   memory_strategy_->remove_null_handles(&wait_set_);
-  if (rcl_wait_set_clear_subscriptions(&wait_set_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear subscriptions from wait set");
-  }
-  if (rcl_wait_set_clear_services(&wait_set_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear servicess from wait set");
-  }
-  if (rcl_wait_set_clear_clients(&wait_set_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear clients from wait set");
-  }
-  if (rcl_wait_set_clear_guard_conditions(&wait_set_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear guard conditions from wait set");
-  }
-  if (rcl_wait_set_clear_timers(&wait_set_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear timers from wait set");
-  }
 }
 
 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
@@ -495,7 +505,7 @@ Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer)
 }
 
 void
-Executor::get_next_timer(AnyExecutable::SharedPtr any_exec)
+Executor::get_next_timer(AnyExecutable & any_exec)
 {
   for (auto & weak_node : weak_nodes_) {
     auto node = weak_node.lock();
@@ -510,8 +520,8 @@ Executor::get_next_timer(AnyExecutable::SharedPtr any_exec)
       for (auto & timer_ref : group->get_timer_ptrs()) {
         auto timer = timer_ref.lock();
         if (timer && timer->is_ready()) {
-          any_exec->timer = timer;
-          any_exec->callback_group = group;
+          any_exec.timer = timer;
+          any_exec.callback_group = group;
           node = get_node_by_group(group);
           return;
         }
@@ -520,67 +530,74 @@ Executor::get_next_timer(AnyExecutable::SharedPtr any_exec)
   }
 }
 
-AnyExecutable::SharedPtr
-Executor::get_next_ready_executable()
+bool
+Executor::get_next_ready_executable(AnyExecutable & any_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;
+  get_next_timer(any_executable);
+  if (any_executable.timer) {
+    return true;
   }
   // 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;
+  memory_strategy_->get_next_subscription(any_executable, weak_nodes_);
+  if (any_executable.subscription || any_executable.subscription_intra_process) {
+    return true;
   }
   // 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;
+  memory_strategy_->get_next_service(any_executable, weak_nodes_);
+  if (any_executable.service) {
+    return true;
   }
   // Check the clients to see if there are any that are ready
-  memory_strategy_->get_next_client(any_exec, weak_nodes_);
-  if (any_exec->client) {
-    return any_exec;
+  memory_strategy_->get_next_client(any_executable, weak_nodes_);
+  if (any_executable.client) {
+    return true;
+  }
+  // Check the waitables to see if there are any that are ready
+  memory_strategy_->get_next_waitable(any_executable, weak_nodes_);
+  if (any_executable.waitable) {
+    return true;
   }
   // If there is no ready executable, return a null ptr
-  return nullptr;
+  return false;
 }
 
-AnyExecutable::SharedPtr
-Executor::get_next_executable(std::chrono::nanoseconds timeout)
+bool
+Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanoseconds timeout)
 {
+  bool success = false;
   // Check to see if there are any subscriptions or timers needing service
   // TODO(wjwwood): improve run to run efficiency of this function
-  auto any_exec = get_next_ready_executable();
+  success = get_next_ready_executable(any_executable);
   // If there are none
-  if (!any_exec) {
+  if (!success) {
     // Wait for subscriptions or timers to work on
     wait_for_work(timeout);
     if (!spinning.load()) {
-      return nullptr;
+      return false;
     }
     // Try again
-    any_exec = get_next_ready_executable();
+    success = get_next_ready_executable(any_executable);
   }
   // At this point any_exec should be valid with either a valid subscription
   // or a valid timer, or it should be a null shared_ptr
-  if (any_exec) {
+  if (success) {
     // 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)
+    using callback_group::CallbackGroupType;
+    if (
+      any_executable.callback_group &&
+      any_executable.callback_group->type() == CallbackGroupType::MutuallyExclusive)
     {
       // It should not have been taken otherwise
-      assert(any_exec->callback_group->can_be_taken_from().load());
+      assert(any_executable.callback_group->can_be_taken_from().load());
       // Set to false to indicate something is being run from this group
       // This is reset to true either when the any_exec is executed or when the
       // any_exec is destructued
-      any_exec->callback_group->can_be_taken_from().store(false);
+      any_executable.callback_group->can_be_taken_from().store(false);
     }
   }
-  return any_exec;
+  return success;
 }
 
 std::ostream &

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -16,6 +16,7 @@
 
 #include <chrono>
 #include <functional>
+#include <memory>
 #include <vector>
 
 #include "rclcpp/utilities.hpp"
@@ -23,10 +24,13 @@
 
 using rclcpp::executors::MultiThreadedExecutor;
 
-MultiThreadedExecutor::MultiThreadedExecutor(const rclcpp::executor::ExecutorArgs & args)
-: executor::Executor(args)
+MultiThreadedExecutor::MultiThreadedExecutor(
+  const rclcpp::executor::ExecutorArgs & args,
+  size_t number_of_threads,
+  bool yield_before_execute)
+: executor::Executor(args), yield_before_execute_(yield_before_execute)
 {
-  number_of_threads_ = std::thread::hardware_concurrency();
+  number_of_threads_ = number_of_threads ? number_of_threads : std::thread::hardware_concurrency();
   if (number_of_threads_ == 0) {
     number_of_threads_ = 1;
   }
@@ -66,15 +70,36 @@ MultiThreadedExecutor::get_number_of_threads()
 void
 MultiThreadedExecutor::run(size_t)
 {
-  while (rclcpp::ok() && spinning.load()) {
-    executor::AnyExecutable::SharedPtr any_exec;
+  while (rclcpp::ok(this->context_) && spinning.load()) {
+    executor::AnyExecutable any_exec;
     {
       std::lock_guard<std::mutex> wait_lock(wait_mutex_);
-      if (!rclcpp::ok() || !spinning.load()) {
+      if (!rclcpp::ok(this->context_) || !spinning.load()) {
         return;
       }
-      any_exec = get_next_executable();
+      if (!get_next_executable(any_exec)) {
+        continue;
+      }
+      if (any_exec.timer) {
+        // Guard against multiple threads getting the same timer.
+        if (scheduled_timers_.count(any_exec.timer) != 0) {
+          continue;
+        }
+        scheduled_timers_.insert(any_exec.timer);
+      }
     }
+    if (yield_before_execute_) {
+      std::this_thread::yield();
+    }
+
     execute_any_executable(any_exec);
+
+    if (any_exec.timer) {
+      std::lock_guard<std::mutex> wait_lock(wait_mutex_);
+      auto it = scheduled_timers_.find(any_exec.timer);
+      if (it != scheduled_timers_.end()) {
+        scheduled_timers_.erase(it);
+      }
+    }
   }
 }

rclcpp/src/rclcpp/executors/single_threaded_executor.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include "rclcpp/executors/single_threaded_executor.hpp"
+#include "rclcpp/any_executable.hpp"
 #include "rclcpp/scope_exit.hpp"
 
 using rclcpp::executors::SingleThreadedExecutor;
@@ -29,8 +30,10 @@ SingleThreadedExecutor::spin()
     throw std::runtime_error("spin() called while already spinning");
   }
   RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
-  while (rclcpp::ok() && spinning.load()) {
-    auto any_exec = get_next_executable();
-    execute_any_executable(any_exec);
+  while (rclcpp::ok(this->context_) && spinning.load()) {
+    rclcpp::executor::AnyExecutable any_executable;
+    if (get_next_executable(any_executable)) {
+      execute_any_executable(any_executable);
+    }
   }
 }

rclcpp/src/rclcpp/expand_topic_or_service_name.cpp

@@ -20,6 +20,7 @@
 #include "rcl/validate_topic_name.h"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/scope_exit.hpp"
+#include "rcutils/logging_macros.h"
 #include "rcutils/types/string_map.h"
 #include "rmw/error_handling.h"
 #include "rmw/validate_namespace.h"
@@ -50,25 +51,18 @@ rclcpp::expand_topic_or_service_name(
   }
   rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&substitutions_map);
   if (ret != RCL_RET_OK) {
-    rcutils_error_state_t error_state;
-    if (rcutils_error_state_copy(rcl_get_error_state(), &error_state) != RCUTILS_RET_OK) {
-      throw std::bad_alloc();
-    }
-    auto error_state_scope_exit = rclcpp::make_scope_exit(
-      [&error_state]() {
-        rcutils_error_state_fini(&error_state);
-      });
+    const rcutils_error_state_t * error_state = rcl_get_error_state();
     // finalize the string map before throwing
     rcutils_ret = rcutils_string_map_fini(&substitutions_map);
     if (rcutils_ret != RCUTILS_RET_OK) {
-      fprintf(stderr,
-        "[rclcpp|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: "
-        "failed to fini string_map (%d) during error handling: %s\n",
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "failed to fini string_map (%d) during error handling: %s",
         rcutils_ret,
-        rcutils_get_error_string_safe());
+        rcutils_get_error_string().str);
       rcutils_reset_error();
     }
-    throw_from_rcl_error(ret, "", &error_state);
+    throw_from_rcl_error(ret, "", error_state);
   }
 
   ret = rcl_expand_topic_name(
@@ -102,20 +96,20 @@ rclcpp::expand_topic_or_service_name(
         throw_from_rcl_error(ret);
       }
 
-      if (validation_result == RCL_TOPIC_NAME_VALID) {
+      if (validation_result != RCL_TOPIC_NAME_VALID) {
+        const char * validation_message =
+          rcl_topic_name_validation_result_string(validation_result);
+        if (is_service) {
+          using rclcpp::exceptions::InvalidServiceNameError;
+          throw InvalidServiceNameError(name.c_str(), validation_message, invalid_index);
+        } else {
+          using rclcpp::exceptions::InvalidTopicNameError;
+          throw InvalidTopicNameError(name.c_str(), validation_message, invalid_index);
+        }
+      } else {
         throw std::runtime_error("topic name unexpectedly valid");
       }
-      const char * validation_message = rcl_topic_name_validation_result_string(validation_result);
-      if (!validation_message) {
-        throw std::runtime_error("unable to get validation error message");
-      }
-      if (is_service) {
-        using rclcpp::exceptions::InvalidServiceNameError;
-        throw InvalidServiceNameError(name.c_str(), validation_message, invalid_index);
-      } else {
-        using rclcpp::exceptions::InvalidTopicNameError;
-        throw InvalidTopicNameError(name.c_str(), validation_message, invalid_index);
-      }
+
       // if invalid node name
     } else if (ret == RCL_RET_NODE_INVALID_NAME) {
       rcl_reset_error();  // explicitly discard error from rcl_expand_topic_name()
@@ -133,10 +127,16 @@ rclcpp::expand_topic_or_service_name(
           RCL_RET_ERROR, "failed to validate node name",
           rmw_get_error_state(), rmw_reset_error);
       }
-      throw rclcpp::exceptions::InvalidNodeNameError(
-              node_name.c_str(),
-              rmw_node_name_validation_result_string(validation_result),
-              invalid_index);
+
+      if (validation_result != RMW_NODE_NAME_VALID) {
+        throw rclcpp::exceptions::InvalidNodeNameError(
+                node_name.c_str(),
+                rmw_node_name_validation_result_string(validation_result),
+                invalid_index);
+      } else {
+        throw std::runtime_error("invalid rcl node name but valid rmw node name");
+      }
+
       // if invalid namespace
     } else if (ret == RCL_RET_NODE_INVALID_NAMESPACE) {
       rcl_reset_error();  // explicitly discard error from rcl_expand_topic_name()
@@ -154,10 +154,15 @@ rclcpp::expand_topic_or_service_name(
           RCL_RET_ERROR, "failed to validate namespace",
           rmw_get_error_state(), rmw_reset_error);
       }
-      throw rclcpp::exceptions::InvalidNamespaceError(
-              namespace_.c_str(),
-              rmw_namespace_validation_result_string(validation_result),
-              invalid_index);
+
+      if (validation_result != RMW_NAMESPACE_VALID) {
+        throw rclcpp::exceptions::InvalidNamespaceError(
+                namespace_.c_str(),
+                rmw_namespace_validation_result_string(validation_result),
+                invalid_index);
+      } else {
+        throw std::runtime_error("invalid rcl namespace but valid rmw namespace");
+      }
       // something else happened
     } else {
       throw_from_rcl_error(ret);
@@ -179,6 +184,7 @@ rclcpp::expand_topic_or_service_name(
       RCL_RET_ERROR, "failed to validate full topic name",
       rmw_get_error_state(), rmw_reset_error);
   }
+
   if (validation_result != RMW_TOPIC_VALID) {
     if (is_service) {
       throw rclcpp::exceptions::InvalidServiceNameError(

rclcpp/src/rclcpp/graph_listener.cpp

@@ -26,6 +26,8 @@
 #include "rclcpp/node.hpp"
 #include "rmw/impl/cpp/demangle.hpp"
 
+#include "rcutils/logging_macros.h"
+
 using rclcpp::exceptions::throw_from_rcl_error;
 
 namespace rclcpp
@@ -33,22 +35,32 @@ namespace rclcpp
 namespace graph_listener
 {
 
-GraphListener::GraphListener()
-: is_started_(false), is_shutdown_(false), shutdown_guard_condition_(nullptr)
+GraphListener::GraphListener(std::shared_ptr<rclcpp::Context> parent_context)
+: parent_context_(parent_context),
+  is_started_(false),
+  is_shutdown_(false),
+  interrupt_guard_condition_context_(nullptr),
+  shutdown_guard_condition_(nullptr)
 {
+  // TODO(wjwwood): make a guard condition class in rclcpp so this can be tracked
+  //   automatically with the rcl guard condition
+  // hold on to this context to prevent it from going out of scope while this
+  // guard condition is using it.
+  interrupt_guard_condition_context_ = parent_context->get_rcl_context();
   rcl_ret_t ret = rcl_guard_condition_init(
     &interrupt_guard_condition_,
+    interrupt_guard_condition_context_.get(),
     rcl_guard_condition_get_default_options());
   if (RCL_RET_OK != ret) {
     throw_from_rcl_error(ret, "failed to create interrupt guard condition");
   }
 
-  shutdown_guard_condition_ = rclcpp::get_sigint_guard_condition(&wait_set_);
+  shutdown_guard_condition_ = parent_context->get_interrupt_guard_condition(&wait_set_);
 }
 
 GraphListener::~GraphListener()
 {
-  this->shutdown();
+  this->shutdown(std::nothrow);
 }
 
 void
@@ -67,6 +79,7 @@ GraphListener::start_if_not_started()
       0,  // number_of_timers
       0,  // number_of_clients
       0,  // number_of_services
+      this->parent_context_->get_rcl_context().get(),
       rcl_get_default_allocator());
     if (RCL_RET_OK != ret) {
       throw_from_rcl_error(ret, "failed to initialize wait set");
@@ -79,7 +92,8 @@ GraphListener::start_if_not_started()
       [weak_this]() {
         auto shared_this = weak_this.lock();
         if (shared_this) {
-          shared_this->shutdown();
+          // should not throw from on_shutdown if it can be avoided
+          shared_this->shutdown(std::nothrow);
         }
       });
     // Start the listener thread.
@@ -94,13 +108,16 @@ GraphListener::run()
   try {
     run_loop();
   } catch (const std::exception & exc) {
-    fprintf(stderr,
-      "[rclcpp] caught %s exception in GraphListener thread: %s\n",
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "caught %s exception in GraphListener thread: %s",
       rmw::impl::cpp::demangle(exc).c_str(),
       exc.what());
     std::rethrow_exception(std::current_exception());
   } catch (...) {
-    fprintf(stderr, "[rclcpp] unknown error in GraphListener thread\n");
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "unknown error in GraphListener thread");
     std::rethrow_exception(std::current_exception());
   }
 }
@@ -125,29 +142,35 @@ GraphListener::run_loop()
     std::lock_guard<std::mutex> nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock);
 
     // Resize the wait set if necessary.
-    if (wait_set_.size_of_guard_conditions < (node_graph_interfaces_.size() + 2)) {
-      ret = rcl_wait_set_resize_guard_conditions(&wait_set_, node_graph_interfaces_.size() + 2);
+    const size_t node_graph_interfaces_size = node_graph_interfaces_.size();
+    // Add 2 for the interrupt and shutdown guard conditions
+    if (wait_set_.size_of_guard_conditions < (node_graph_interfaces_size + 2)) {
+      ret = rcl_wait_set_resize(&wait_set_, 0, node_graph_interfaces_size + 2, 0, 0, 0);
       if (RCL_RET_OK != ret) {
         throw_from_rcl_error(ret, "failed to resize wait set");
       }
     }
-    // Clear the wait set's guard conditions.
-    ret = rcl_wait_set_clear_guard_conditions(&wait_set_);
+    // Clear the wait set.
+    ret = rcl_wait_set_clear(&wait_set_);
     if (RCL_RET_OK != ret) {
       throw_from_rcl_error(ret, "failed to clear wait set");
     }
     // Put the interrupt guard condition in the wait set.
-    ret = rcl_wait_set_add_guard_condition(&wait_set_, &interrupt_guard_condition_);
+    ret = rcl_wait_set_add_guard_condition(&wait_set_, &interrupt_guard_condition_, NULL);
     if (RCL_RET_OK != ret) {
       throw_from_rcl_error(ret, "failed to add interrupt guard condition to wait set");
     }
     // Put the shutdown guard condition in the wait set.
-    ret = rcl_wait_set_add_guard_condition(&wait_set_, shutdown_guard_condition_);
+    size_t shutdown_guard_condition_index = 0u;
+    ret = rcl_wait_set_add_guard_condition(
+      &wait_set_, shutdown_guard_condition_, &shutdown_guard_condition_index);
     if (RCL_RET_OK != ret) {
       throw_from_rcl_error(ret, "failed to add shutdown guard condition to wait set");
     }
     // Put graph guard conditions for each node into the wait set.
-    for (const auto node_ptr : node_graph_interfaces_) {
+    std::vector<size_t> graph_gc_indexes(node_graph_interfaces_size, 0u);
+    for (size_t i = 0u; i < node_graph_interfaces_size; ++i) {
+      auto node_ptr = node_graph_interfaces_[i];
       // Only wait on graph changes if some user of the node is watching.
       if (node_ptr->count_graph_users() == 0) {
         continue;
@@ -157,7 +180,7 @@ GraphListener::run_loop()
       if (!graph_gc) {
         throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition");
       }
-      ret = rcl_wait_set_add_guard_condition(&wait_set_, graph_gc);
+      ret = rcl_wait_set_add_guard_condition(&wait_set_, graph_gc, &graph_gc_indexes[i]);
       if (RCL_RET_OK != ret) {
         throw_from_rcl_error(ret, "failed to add graph guard condition to wait set");
       }
@@ -172,23 +195,18 @@ GraphListener::run_loop()
       throw_from_rcl_error(ret, "failed to wait on wait set");
     }
 
-    bool shutdown_guard_condition_triggered = false;
     // Check to see if the shutdown guard condition has been triggered.
-    for (size_t i = 0; i < wait_set_.size_of_guard_conditions; ++i) {
-      if (shutdown_guard_condition_ == wait_set_.guard_conditions[i]) {
-        shutdown_guard_condition_triggered = true;
-      }
-    }
+    bool shutdown_guard_condition_triggered =
+      (shutdown_guard_condition_ == wait_set_.guard_conditions[shutdown_guard_condition_index]);
     // Notify nodes who's guard conditions are set (triggered).
-    for (const auto node_ptr : node_graph_interfaces_) {
+    for (size_t i = 0u; i < node_graph_interfaces_size; ++i) {
+      const auto node_ptr = node_graph_interfaces_[i];
       auto graph_gc = node_ptr->get_graph_guard_condition();
       if (!graph_gc) {
         throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition");
       }
-      for (size_t i = 0; i < wait_set_.size_of_guard_conditions; ++i) {
-        if (graph_gc == wait_set_.guard_conditions[i]) {
-          node_ptr->notify_graph_change();
-        }
+      if (graph_gc == wait_set_.guard_conditions[graph_gc_indexes[i]]) {
+        node_ptr->notify_graph_change();
       }
       if (shutdown_guard_condition_triggered) {
         // If shutdown, then notify the node of this as well.
@@ -322,7 +340,7 @@ GraphListener::remove_node(rclcpp::node_interfaces::NodeGraphInterface * node_gr
 }
 
 void
-GraphListener::shutdown()
+GraphListener::__shutdown(bool should_throw)
 {
   std::lock_guard<std::mutex> shutdown_lock(shutdown_mutex_);
   if (!is_shutdown_.exchange(true)) {
@@ -331,11 +349,16 @@ GraphListener::shutdown()
       listener_thread_.join();
     }
     rcl_ret_t ret = rcl_guard_condition_fini(&interrupt_guard_condition_);
+    interrupt_guard_condition_context_.reset();  // release context guard condition was using
     if (RCL_RET_OK != ret) {
       throw_from_rcl_error(ret, "failed to finalize interrupt guard condition");
     }
     if (shutdown_guard_condition_) {
-      rclcpp::release_sigint_guard_condition(&wait_set_);
+      if (should_throw) {
+        parent_context_->release_interrupt_guard_condition(&wait_set_);
+      } else {
+        parent_context_->release_interrupt_guard_condition(&wait_set_, std::nothrow);
+      }
       shutdown_guard_condition_ = nullptr;
     }
     if (is_started_) {
@@ -347,6 +370,29 @@ GraphListener::shutdown()
   }
 }
 
+void
+GraphListener::shutdown()
+{
+  this->__shutdown(true);
+}
+
+void
+GraphListener::shutdown(const std::nothrow_t &) noexcept
+{
+  try {
+    this->__shutdown(false);
+  } catch (const std::exception & exc) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("rclcpp"),
+      "caught %s exception when shutting down GraphListener: %s",
+      rmw::impl::cpp::demangle(exc).c_str(), exc.what());
+  } catch (...) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("rclcpp"),
+      "caught unknown exception when shutting down GraphListener");
+  }
+}
+
 bool
 GraphListener::is_shutdown()
 {

rclcpp/src/rclcpp/init_options.cpp

@@ -0,0 +1,86 @@
+// 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.
+
+#include "rclcpp/init_options.hpp"
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/logging.hpp"
+
+namespace rclcpp
+{
+
+InitOptions::InitOptions(rcl_allocator_t allocator)
+: init_options_(new rcl_init_options_t)
+{
+  *init_options_ = rcl_get_zero_initialized_init_options();
+  rcl_ret_t ret = rcl_init_options_init(init_options_.get(), allocator);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialized rcl init options");
+  }
+}
+
+InitOptions::InitOptions(const rcl_init_options_t & init_options)
+: init_options_(new rcl_init_options_t)
+{
+  *init_options_ = rcl_get_zero_initialized_init_options();
+  rcl_ret_t ret = rcl_init_options_copy(&init_options, init_options_.get());
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to copy rcl init options");
+  }
+}
+
+InitOptions::InitOptions(const InitOptions & other)
+: InitOptions(*other.get_rcl_init_options())
+{}
+
+InitOptions &
+InitOptions::operator=(const InitOptions & other)
+{
+  if (this != &other) {
+    this->finalize_init_options();
+    rcl_ret_t ret = rcl_init_options_copy(other.get_rcl_init_options(), init_options_.get());
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to copy rcl init options");
+    }
+  }
+  return *this;
+}
+
+InitOptions::~InitOptions()
+{
+  this->finalize_init_options();
+}
+
+void
+InitOptions::finalize_init_options()
+{
+  if (init_options_) {
+    rcl_ret_t ret = rcl_init_options_fini(init_options_.get());
+    if (RCL_RET_OK != ret) {
+      RCLCPP_ERROR(
+        rclcpp::get_logger("rclcpp"),
+        "failed to finalize rcl init options: %s", rcl_get_error_string().str);
+      rcl_reset_error();
+    }
+    *init_options_ = rcl_get_zero_initialized_init_options();
+  }
+}
+
+const rcl_init_options_t *
+InitOptions::get_rcl_init_options() const
+{
+  return init_options_.get();
+}
+
+}  // namespace rclcpp

rclcpp/src/rclcpp/intra_process_manager.cpp

@@ -56,6 +56,12 @@ IntraProcessManager::matches_any_publishers(const rmw_gid_t * id) const
   return impl_->matches_any_publishers(id);
 }
 
+size_t
+IntraProcessManager::get_subscription_count(uint64_t intra_process_publisher_id) const
+{
+  return impl_->get_subscription_count(intra_process_publisher_id);
+}
+
 uint64_t
 IntraProcessManager::get_next_unique_id()
 {

rclcpp/src/rclcpp/logger.cpp

@@ -16,10 +16,12 @@
 
 #include "rclcpp/logger.hpp"
 
+#include "rclcpp/logging.hpp"
+
 namespace rclcpp
 {
 
-rclcpp::Logger
+Logger
 get_logger(const std::string & name)
 {
 #if RCLCPP_LOGGING_ENABLED
@@ -30,4 +32,16 @@ get_logger(const std::string & name)
 #endif
 }
 
+Logger
+get_node_logger(const rcl_node_t * node)
+{
+  const char * logger_name = rcl_node_get_logger_name(node);
+  if (nullptr == logger_name) {
+    auto logger = rclcpp::get_logger("rclcpp");
+    RCLCPP_ERROR(logger, "failed to get logger name from node at address %p", node);
+    return logger;
+  }
+  return rclcpp::get_logger(logger_name);
+}
+
 }  // namespace rclcpp

rclcpp/src/rclcpp/memory_strategy.cpp

@@ -13,12 +13,14 @@
 // limitations under the License.
 
 #include "rclcpp/memory_strategy.hpp"
+#include <memory>
 
 using rclcpp::memory_strategy::MemoryStrategy;
 
 rclcpp::SubscriptionBase::SharedPtr
 MemoryStrategy::get_subscription_by_handle(
-  const rcl_subscription_t * subscriber_handle, const WeakNodeVector & weak_nodes)
+  std::shared_ptr<const rcl_subscription_t> subscriber_handle,
+  const WeakNodeVector & weak_nodes)
 {
   for (auto & weak_node : weak_nodes) {
     auto node = weak_node.lock();
@@ -48,7 +50,7 @@ MemoryStrategy::get_subscription_by_handle(
 
 rclcpp::ServiceBase::SharedPtr
 MemoryStrategy::get_service_by_handle(
-  const rcl_service_t * service_handle,
+  std::shared_ptr<const rcl_service_t> service_handle,
   const WeakNodeVector & weak_nodes)
 {
   for (auto & weak_node : weak_nodes) {
@@ -74,7 +76,7 @@ MemoryStrategy::get_service_by_handle(
 
 rclcpp::ClientBase::SharedPtr
 MemoryStrategy::get_client_by_handle(
-  const rcl_client_t * client_handle,
+  std::shared_ptr<const rcl_client_t> client_handle,
   const WeakNodeVector & weak_nodes)
 {
   for (auto & weak_node : weak_nodes) {
@@ -198,3 +200,29 @@ MemoryStrategy::get_group_by_client(
   }
   return nullptr;
 }
+
+rclcpp::callback_group::CallbackGroup::SharedPtr
+MemoryStrategy::get_group_by_waitable(
+  rclcpp::Waitable::SharedPtr waitable,
+  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_waitable : group->get_waitable_ptrs()) {
+        auto group_waitable = weak_waitable.lock();
+        if (group_waitable && group_waitable == waitable) {
+          return group;
+        }
+      }
+    }
+  }
+  return nullptr;
+}

rclcpp/src/rclcpp/node.cpp

@@ -29,48 +29,152 @@
 #include "rclcpp/node_interfaces/node_logging.hpp"
 #include "rclcpp/node_interfaces/node_parameters.hpp"
 #include "rclcpp/node_interfaces/node_services.hpp"
+#include "rclcpp/node_interfaces/node_time_source.hpp"
 #include "rclcpp/node_interfaces/node_timers.hpp"
 #include "rclcpp/node_interfaces/node_topics.hpp"
+#include "rclcpp/node_interfaces/node_waitables.hpp"
+
+#include "rmw/validate_namespace.h"
 
 using rclcpp::Node;
+using rclcpp::NodeOptions;
 using rclcpp::exceptions::throw_from_rcl_error;
 
+RCLCPP_LOCAL
+std::string
+extend_sub_namespace(const std::string & existing_sub_namespace, const std::string & extension)
+{
+  // Assumption is that the existing_sub_namespace does not need checking
+  // because it would be checked already when it was set with this function.
+
+  // check if the new sub-namespace extension is absolute
+  if (extension.front() == '/') {
+    throw rclcpp::exceptions::NameValidationError(
+            "sub_namespace",
+            extension.c_str(),
+            "a sub-namespace should not have a leading /",
+            0);
+  }
+
+  std::string new_sub_namespace;
+  if (existing_sub_namespace.empty()) {
+    new_sub_namespace = extension;
+  } else {
+    new_sub_namespace = existing_sub_namespace + "/" + extension;
+  }
+
+  // remove any trailing `/` so that new extensions do no result in `//`
+  if (new_sub_namespace.back() == '/') {
+    new_sub_namespace = new_sub_namespace.substr(0, new_sub_namespace.size() - 1);
+  }
+
+  return new_sub_namespace;
+}
+
+RCLCPP_LOCAL
+std::string
+create_effective_namespace(const std::string & node_namespace, const std::string & sub_namespace)
+{
+  // Assumption is that both the node_namespace and sub_namespace are conforming
+  // and do not need trimming of `/` and other things, as they were validated
+  // in other functions already.
+
+  if (node_namespace.back() == '/') {
+    // this is the special case where node_namespace is just `/`
+    return node_namespace + sub_namespace;
+  } else {
+    return node_namespace + "/" + sub_namespace;
+  }
+}
+
 Node::Node(
   const std::string & node_name,
-  const std::string & namespace_,
-  bool use_intra_process_comms)
-: Node(
-    node_name,
-    namespace_,
-    rclcpp::contexts::default_context::get_global_default_context(),
-    use_intra_process_comms)
-{}
+  const NodeOptions & options)
+: Node(node_name, "", options)
+{
+}
 
 Node::Node(
   const std::string & node_name,
   const std::string & namespace_,
-  rclcpp::Context::SharedPtr context,
-  bool use_intra_process_comms)
-: node_base_(new rclcpp::node_interfaces::NodeBase(node_name, namespace_, context)),
+  const NodeOptions & options)
+: node_base_(new rclcpp::node_interfaces::NodeBase(
+      node_name, namespace_, options)),
   node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())),
   node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_.get())),
   node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())),
   node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())),
   node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())),
-  node_parameters_(new rclcpp::node_interfaces::NodeParameters(
-      node_topics_.get(),
-      use_intra_process_comms
-    )),
   node_clock_(new rclcpp::node_interfaces::NodeClock(
       node_base_,
       node_topics_,
       node_graph_,
-      node_services_
+      node_services_,
+      node_logging_
     )),
-  use_intra_process_comms_(use_intra_process_comms)
+  node_parameters_(new rclcpp::node_interfaces::NodeParameters(
+      node_base_,
+      node_topics_,
+      node_services_,
+      node_clock_,
+      options.initial_parameters(),
+      options.use_intra_process_comms(),
+      options.start_parameter_services(),
+      options.start_parameter_event_publisher(),
+      options.parameter_event_qos_profile()
+    )),
+  node_time_source_(new rclcpp::node_interfaces::NodeTimeSource(
+      node_base_,
+      node_topics_,
+      node_graph_,
+      node_services_,
+      node_logging_,
+      node_clock_,
+      node_parameters_
+    )),
+  node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())),
+  node_options_(options),
+  sub_namespace_(""),
+  effective_namespace_(create_effective_namespace(this->get_namespace(), sub_namespace_))
 {
 }
 
+Node::Node(
+  const Node & other,
+  const std::string & sub_namespace)
+: node_base_(other.node_base_),
+  node_graph_(other.node_graph_),
+  node_logging_(other.node_logging_),
+  node_timers_(other.node_timers_),
+  node_topics_(other.node_topics_),
+  node_services_(other.node_services_),
+  node_clock_(other.node_clock_),
+  node_parameters_(other.node_parameters_),
+  node_options_(other.node_options_),
+  sub_namespace_(extend_sub_namespace(other.get_sub_namespace(), sub_namespace)),
+  effective_namespace_(create_effective_namespace(other.get_namespace(), sub_namespace_))
+{
+  // Validate new effective namespace.
+  int validation_result;
+  size_t invalid_index;
+  rmw_ret_t rmw_ret =
+    rmw_validate_namespace(effective_namespace_.c_str(), &validation_result, &invalid_index);
+
+  if (rmw_ret != RMW_RET_OK) {
+    if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
+      throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "failed to validate subnode namespace");
+    }
+    throw_from_rcl_error(RCL_RET_ERROR, "failed to validate subnode namespace");
+  }
+
+  if (validation_result != RMW_NAMESPACE_VALID) {
+    throw rclcpp::exceptions::InvalidNamespaceError(
+            effective_namespace_.c_str(),
+            rmw_namespace_validation_result_string(validation_result),
+            invalid_index);
+  }
+}
+
 Node::~Node()
 {}
 
@@ -86,6 +190,12 @@ Node::get_namespace() const
   return node_base_->get_namespace();
 }
 
+const char *
+Node::get_fully_qualified_name() const
+{
+  return node_base_->get_fully_qualified_name();
+}
+
 rclcpp::Logger
 Node::get_logger() const
 {
@@ -107,26 +217,26 @@ Node::group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group)
 
 std::vector<rcl_interfaces::msg::SetParametersResult>
 Node::set_parameters(
-  const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
+  const std::vector<rclcpp::Parameter> & parameters)
 {
   return node_parameters_->set_parameters(parameters);
 }
 
 rcl_interfaces::msg::SetParametersResult
 Node::set_parameters_atomically(
-  const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
+  const std::vector<rclcpp::Parameter> & parameters)
 {
   return node_parameters_->set_parameters_atomically(parameters);
 }
 
-std::vector<rclcpp::parameter::ParameterVariant>
+std::vector<rclcpp::Parameter>
 Node::get_parameters(
   const std::vector<std::string> & names) const
 {
   return node_parameters_->get_parameters(names);
 }
 
-rclcpp::parameter::ParameterVariant
+rclcpp::Parameter
 Node::get_parameter(const std::string & name) const
 {
   return node_parameters_->get_parameter(name);
@@ -134,7 +244,7 @@ Node::get_parameter(const std::string & name) const
 
 bool Node::get_parameter(
   const std::string & name,
-  rclcpp::parameter::ParameterVariant & parameter) const
+  rclcpp::Parameter & parameter) const
 {
   return node_parameters_->get_parameter(name, parameter);
 }
@@ -160,6 +270,12 @@ Node::list_parameters(
   return node_parameters_->list_parameters(prefixes, depth);
 }
 
+std::vector<std::string>
+Node::get_node_names() const
+{
+  return node_graph_->get_node_names();
+}
+
 std::map<std::string, std::vector<std::string>>
 Node::get_topic_names_and_types() const
 {
@@ -234,6 +350,18 @@ Node::get_node_graph_interface()
   return node_graph_;
 }
 
+rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr
+Node::get_node_logging_interface()
+{
+  return node_logging_;
+}
+
+rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr
+Node::get_node_time_source_interface()
+{
+  return node_time_source_;
+}
+
 rclcpp::node_interfaces::NodeTimersInterface::SharedPtr
 Node::get_node_timers_interface()
 {
@@ -257,3 +385,35 @@ Node::get_node_parameters_interface()
 {
   return node_parameters_;
 }
+
+rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr
+Node::get_node_waitables_interface()
+{
+  return node_waitables_;
+}
+
+const std::string &
+Node::get_sub_namespace() const
+{
+  return this->sub_namespace_;
+}
+
+const std::string &
+Node::get_effective_namespace() const
+{
+  return this->effective_namespace_;
+}
+
+Node::SharedPtr
+Node::create_sub_node(const std::string & sub_namespace)
+{
+  // Cannot use make_shared<Node>() here as it requires the constructor to be
+  // public, and this constructor is intentionally protected instead.
+  return std::shared_ptr<Node>(new Node(*this, sub_namespace));
+}
+
+const NodeOptions &
+Node::get_node_options() const
+{
+  return this->node_options_;
+}

rclcpp/src/rclcpp/node_interfaces/node_base.cpp

@@ -19,9 +19,11 @@
 
 #include "rclcpp/node_interfaces/node_base.hpp"
 
+#include "rcl/arguments.h"
 #include "rclcpp/exceptions.hpp"
-#include "rmw/validate_node_name.h"
+#include "rcutils/logging_macros.h"
 #include "rmw/validate_namespace.h"
+#include "rmw/validate_node_name.h"
 
 using rclcpp::exceptions::throw_from_rcl_error;
 
@@ -30,8 +32,8 @@ using rclcpp::node_interfaces::NodeBase;
 NodeBase::NodeBase(
   const std::string & node_name,
   const std::string & namespace_,
-  rclcpp::Context::SharedPtr context)
-: context_(context),
+  const rclcpp::NodeOptions & options)
+: context_(options.context()),
   node_handle_(nullptr),
   default_callback_group_(nullptr),
   associated_with_executor_(false),
@@ -39,7 +41,8 @@ NodeBase::NodeBase(
 {
   // Setup the guard condition that is notified when changes occur in the graph.
   rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options();
-  rcl_ret_t ret = rcl_guard_condition_init(&notify_guard_condition_, guard_condition_options);
+  rcl_ret_t ret = rcl_guard_condition_init(
+    &notify_guard_condition_, options.context()->get_rcl_context().get(), guard_condition_options);
   if (ret != RCL_RET_OK) {
     throw_from_rcl_error(ret, "failed to create interrupt guard condition");
   }
@@ -48,52 +51,23 @@ NodeBase::NodeBase(
   auto finalize_notify_guard_condition = [this]() {
       // Finalize the interrupt guard condition.
       if (rcl_guard_condition_fini(&notify_guard_condition_) != RCL_RET_OK) {
-        fprintf(stderr,
-          "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "failed to destroy guard condition: %s", rcl_get_error_string().str);
       }
     };
 
-  // Determine the domain id based on the options and the ROS_DOMAIN_ID env variable.
-  size_t domain_id = 0;
-  char * ros_domain_id = nullptr;
-  const char * env_var = "ROS_DOMAIN_ID";
-#ifndef _WIN32
-  ros_domain_id = getenv(env_var);
-#else
-  size_t ros_domain_id_size;
-  _dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var);
-#endif
-  if (ros_domain_id) {
-    uint32_t number = strtoul(ros_domain_id, NULL, 0);
-    if (number == (std::numeric_limits<uint32_t>::max)()) {
-      // Finalize the interrupt guard condition.
-      finalize_notify_guard_condition();
-#ifdef _WIN32
-      // free the ros_domain_id before throwing, if getenv was used on Windows
-      free(ros_domain_id);
-#endif
-
-      throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number");
-    }
-    domain_id = static_cast<size_t>(number);
-#ifdef _WIN32
-    free(ros_domain_id);
-#endif
-  }
-
   // Create the rcl node and store it in a shared_ptr with a custom destructor.
-  rcl_node_t * rcl_node = new rcl_node_t(rcl_get_zero_initialized_node());
+  std::unique_ptr<rcl_node_t> rcl_node(new rcl_node_t(rcl_get_zero_initialized_node()));
 
-  rcl_node_options_t options = rcl_node_get_default_options();
-  // TODO(wjwwood): pass the Allocator to the options
-  options.domain_id = domain_id;
-  ret = rcl_node_init(rcl_node, node_name.c_str(), namespace_.c_str(), &options);
+  ret = rcl_node_init(
+    rcl_node.get(),
+    node_name.c_str(), namespace_.c_str(),
+    options.context()->get_rcl_context().get(), options.get_rcl_node_options());
   if (ret != RCL_RET_OK) {
     // Finalize the interrupt guard condition.
     finalize_notify_guard_condition();
 
-    delete rcl_node;
-
     if (ret == RCL_RET_NODE_INVALID_NAME) {
       rcl_reset_error();  // discard rcl_node_init error
       int validation_result;
@@ -106,10 +80,15 @@ NodeBase::NodeBase(
         }
         throw_from_rcl_error(RCL_RET_ERROR, "failed to validate node name");
       }
-      throw rclcpp::exceptions::InvalidNodeNameError(
-              node_name.c_str(),
-              rmw_node_name_validation_result_string(validation_result),
-              invalid_index);
+
+      if (validation_result != RMW_NODE_NAME_VALID) {
+        throw rclcpp::exceptions::InvalidNodeNameError(
+                node_name.c_str(),
+                rmw_node_name_validation_result_string(validation_result),
+                invalid_index);
+      } else {
+        throw std::runtime_error("valid rmw node name but invalid rcl node name");
+      }
     }
 
     if (ret == RCL_RET_NODE_INVALID_NAMESPACE) {
@@ -124,21 +103,26 @@ NodeBase::NodeBase(
         }
         throw_from_rcl_error(RCL_RET_ERROR, "failed to validate namespace");
       }
-      throw rclcpp::exceptions::InvalidNamespaceError(
-              namespace_.c_str(),
-              rmw_namespace_validation_result_string(validation_result),
-              invalid_index);
-    }
 
+      if (validation_result != RMW_NAMESPACE_VALID) {
+        throw rclcpp::exceptions::InvalidNamespaceError(
+                namespace_.c_str(),
+                rmw_namespace_validation_result_string(validation_result),
+                invalid_index);
+      } else {
+        throw std::runtime_error("valid rmw node namespace but invalid rcl node namespace");
+      }
+    }
     throw_from_rcl_error(ret, "failed to initialize rcl node");
   }
 
   node_handle_.reset(
-    rcl_node,
+    rcl_node.release(),
     [](rcl_node_t * node) -> void {
       if (rcl_node_fini(node) != RCL_RET_OK) {
-        fprintf(
-          stderr, "Error in destruction of rcl node handle: %s\n", rcl_get_error_string_safe());
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Error in destruction of rcl node handle: %s", rcl_get_error_string().str);
       }
       delete node;
     });
@@ -158,8 +142,9 @@ NodeBase::~NodeBase()
     std::lock_guard<std::recursive_mutex> notify_condition_lock(notify_guard_condition_mutex_);
     notify_guard_condition_is_valid_ = false;
     if (rcl_guard_condition_fini(&notify_guard_condition_) != RCL_RET_OK) {
-      fprintf(stderr,
-        "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "failed to destroy guard condition: %s", rcl_get_error_string().str);
     }
   }
 }
@@ -176,6 +161,12 @@ NodeBase::get_namespace() const
   return rcl_node_get_namespace(node_handle_.get());
 }
 
+const char *
+NodeBase::get_fully_qualified_name() const
+{
+  return rcl_node_get_fully_qualified_name(node_handle_.get());
+}
+
 rclcpp::Context::SharedPtr
 NodeBase::get_context()
 {

rclcpp/src/rclcpp/node_interfaces/node_clock.cpp

@@ -23,20 +23,15 @@ NodeClock::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::NodeServicesInterface::SharedPtr node_services,
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging)
 : node_base_(node_base),
   node_topics_(node_topics),
   node_graph_(node_graph),
   node_services_(node_services),
+  node_logging_(node_logging),
   ros_clock_(std::make_shared<rclcpp::Clock>(RCL_ROS_TIME))
-{
-  time_source_.attachNode(
-    node_base_,
-    node_topics_,
-    node_graph_,
-    node_services_);
-  time_source_.attachClock(ros_clock_);
-}
+{}
 
 NodeClock::~NodeClock()
 {}

rclcpp/src/rclcpp/node_interfaces/node_graph.cpp

@@ -16,6 +16,7 @@
 
 #include <map>
 #include <string>
+#include <utility>
 #include <vector>
 
 #include "rcl/graph.h"
@@ -29,7 +30,9 @@ using rclcpp::graph_listener::GraphListener;
 
 NodeGraph::NodeGraph(rclcpp::node_interfaces::NodeBaseInterface * node_base)
 : node_base_(node_base),
-  graph_listener_(node_base->get_context()->get_sub_context<GraphListener>()),
+  graph_listener_(
+    node_base->get_context()->get_sub_context<GraphListener>(node_base->get_context())
+  ),
   should_add_to_graph_listener_(true),
   graph_users_count_(0)
 {}
@@ -58,13 +61,13 @@ NodeGraph::get_topic_names_and_types(bool no_demangle) const
     &topic_names_and_types);
   if (ret != RCL_RET_OK) {
     auto error_msg = std::string("failed to get topic names and types: ") +
-      rcl_get_error_string_safe();
+      rcl_get_error_string().str;
     rcl_reset_error();
     if (rcl_names_and_types_fini(&topic_names_and_types) != RCL_RET_OK) {
       error_msg += std::string(", failed also to cleanup topic names and types, leaking memory: ") +
-        rcl_get_error_string_safe();
+        rcl_get_error_string().str;
     }
-    throw std::runtime_error(error_msg + rcl_get_error_string_safe());
+    throw std::runtime_error(error_msg + rcl_get_error_string().str);
   }
 
   std::map<std::string, std::vector<std::string>> topics_and_types;
@@ -79,7 +82,7 @@ NodeGraph::get_topic_names_and_types(bool no_demangle) const
   if (ret != RCL_RET_OK) {
     // *INDENT-OFF*
     throw std::runtime_error(
-      std::string("could not destroy topic names and types: ") + rcl_get_error_string_safe());
+      std::string("could not destroy topic names and types: ") + rcl_get_error_string().str);
     // *INDENT-ON*
   }
 
@@ -98,14 +101,14 @@ NodeGraph::get_service_names_and_types() const
     &service_names_and_types);
   if (ret != RCL_RET_OK) {
     auto error_msg = std::string("failed to get service names and types: ") +
-      rcl_get_error_string_safe();
+      rcl_get_error_string().str;
     rcl_reset_error();
     if (rcl_names_and_types_fini(&service_names_and_types) != RCL_RET_OK) {
       error_msg +=
         std::string(", failed also to cleanup service names and types, leaking memory: ") +
-        rcl_get_error_string_safe();
+        rcl_get_error_string().str;
     }
-    throw std::runtime_error(error_msg + rcl_get_error_string_safe());
+    throw std::runtime_error(error_msg + rcl_get_error_string().str);
   }
 
   std::map<std::string, std::vector<std::string>> services_and_types;
@@ -120,7 +123,7 @@ NodeGraph::get_service_names_and_types() const
   if (ret != RCL_RET_OK) {
     // *INDENT-OFF*
     throw std::runtime_error(
-      std::string("could not destroy service names and types: ") + rcl_get_error_string_safe());
+      std::string("could not destroy service names and types: ") + rcl_get_error_string().str);
     // *INDENT-ON*
   }
 
@@ -129,36 +132,73 @@ NodeGraph::get_service_names_and_types() const
 
 std::vector<std::string>
 NodeGraph::get_node_names() const
+{
+  std::vector<std::string> nodes;
+  auto names_and_namespaces = get_node_names_and_namespaces();
+
+  for (const auto & it : names_and_namespaces) {
+    nodes.push_back(it.first);
+  }
+  return nodes;
+}
+
+std::vector<std::pair<std::string, std::string>>
+NodeGraph::get_node_names_and_namespaces() const
 {
   rcutils_string_array_t node_names_c =
     rcutils_get_zero_initialized_string_array();
+  rcutils_string_array_t node_namespaces_c =
+    rcutils_get_zero_initialized_string_array();
 
   auto allocator = rcl_get_default_allocator();
   auto ret = rcl_get_node_names(
     node_base_->get_rcl_node_handle(),
     allocator,
-    &node_names_c);
+    &node_names_c,
+    &node_namespaces_c);
   if (ret != RCL_RET_OK) {
-    auto error_msg = std::string("failed to get node names: ") + rcl_get_error_string_safe();
+    auto error_msg = std::string("failed to get node names: ") + rcl_get_error_string().str;
     rcl_reset_error();
     if (rcutils_string_array_fini(&node_names_c) != RCUTILS_RET_OK) {
       error_msg += std::string(", failed also to cleanup node names, leaking memory: ") +
-        rcl_get_error_string_safe();
+        rcl_get_error_string().str;
+      rcl_reset_error();
+    }
+    if (rcutils_string_array_fini(&node_namespaces_c) != RCUTILS_RET_OK) {
+      error_msg += std::string(", failed also to cleanup node namespaces, leaking memory: ") +
+        rcl_get_error_string().str;
+      rcl_reset_error();
     }
     // TODO(karsten1987): Append rcutils_error_message once it's in master
     throw std::runtime_error(error_msg);
   }
 
-  std::vector<std::string> node_names(&node_names_c.data[0],
-    &node_names_c.data[0 + node_names_c.size]);
-  ret = rcutils_string_array_fini(&node_names_c);
-  if (ret != RCUTILS_RET_OK) {
+  std::vector<std::pair<std::string, std::string>> node_names(node_names_c.size);
+  for (size_t i = 0; i < node_names_c.size; ++i) {
+    if (node_names_c.data[i] && node_namespaces_c.data[i]) {
+      node_names.push_back(std::make_pair(node_names_c.data[i], node_namespaces_c.data[i]));
+    }
+  }
+
+  std::string error;
+  rcl_ret_t ret_names = rcutils_string_array_fini(&node_names_c);
+  if (ret_names != RCUTILS_RET_OK) {
     // *INDENT-OFF*
     // TODO(karsten1987): Append rcutils_error_message once it's in master
-    throw std::runtime_error(
-      std::string("could not destroy node names: "));
+    error = "could not destroy node names";
     // *INDENT-ON*
   }
+  rcl_ret_t ret_ns = rcutils_string_array_fini(&node_namespaces_c);
+  if (ret_ns != RCUTILS_RET_OK) {
+    // *INDENT-OFF*
+    // TODO(karsten1987): Append rcutils_error_message once it's in master
+    error += ", could not destroy node namespaces";
+    // *INDENT-ON*
+  }
+
+  if (ret_names != RCUTILS_RET_OK || ret_ns != RCUTILS_RET_OK) {
+    throw std::runtime_error(error);
+  }
 
   return node_names;
 }
@@ -166,20 +206,20 @@ NodeGraph::get_node_names() const
 size_t
 NodeGraph::count_publishers(const std::string & topic_name) const
 {
-  auto rmw_node_handle = rcl_node_get_rmw_handle(node_base_->get_rcl_node_handle());
+  auto rcl_node_handle = node_base_->get_rcl_node_handle();
+
   auto fqdn = rclcpp::expand_topic_or_service_name(
     topic_name,
-    rmw_node_handle->name,
-    rmw_node_handle->namespace_,
+    rcl_node_get_name(rcl_node_handle),
+    rcl_node_get_namespace(rcl_node_handle),
     false);    // false = not a service
 
   size_t count;
-  // TODO(wjwwood): use the rcl equivalent methods
-  auto ret = rmw_count_publishers(rmw_node_handle, fqdn.c_str(), &count);
+  auto ret = rcl_count_publishers(rcl_node_handle, fqdn.c_str(), &count);
   if (ret != RMW_RET_OK) {
     // *INDENT-OFF*
     throw std::runtime_error(
-      std::string("could not count publishers: ") + rmw_get_error_string_safe());
+      std::string("could not count publishers: ") + rmw_get_error_string().str);
     // *INDENT-ON*
   }
   return count;
@@ -188,20 +228,20 @@ NodeGraph::count_publishers(const std::string & topic_name) const
 size_t
 NodeGraph::count_subscribers(const std::string & topic_name) const
 {
-  auto rmw_node_handle = rcl_node_get_rmw_handle(node_base_->get_rcl_node_handle());
+  auto rcl_node_handle = node_base_->get_rcl_node_handle();
+
   auto fqdn = rclcpp::expand_topic_or_service_name(
     topic_name,
-    rmw_node_handle->name,
-    rmw_node_handle->namespace_,
+    rcl_node_get_name(rcl_node_handle),
+    rcl_node_get_namespace(rcl_node_handle),
     false);    // false = not a service
 
   size_t count;
-  // TODO(wjwwood): use the rcl equivalent methods
-  auto ret = rmw_count_subscribers(rmw_node_handle, fqdn.c_str(), &count);
+  auto ret = rcl_count_subscribers(rcl_node_handle, fqdn.c_str(), &count);
   if (ret != RMW_RET_OK) {
     // *INDENT-OFF*
     throw std::runtime_error(
-      std::string("could not count subscribers: ") + rmw_get_error_string_safe());
+      std::string("could not count subscribers: ") + rmw_get_error_string().str);
     // *INDENT-ON*
   }
   return count;
@@ -296,8 +336,8 @@ NodeGraph::wait_for_graph_change(
       throw EventNotRegisteredError();
     }
   }
-  auto pred = [&event]() {
-      return event->check() || !rclcpp::ok();
+  auto pred = [&event, context = node_base_->get_context()]() {
+      return event->check() || !rclcpp::ok(context);
     };
   std::unique_lock<std::mutex> graph_lock(graph_mutex_);
   if (!pred()) {

rclcpp/src/rclcpp/node_interfaces/node_logging.cpp

@@ -19,8 +19,7 @@ using rclcpp::node_interfaces::NodeLogging;
 NodeLogging::NodeLogging(rclcpp::node_interfaces::NodeBaseInterface * node_base)
 : node_base_(node_base)
 {
-  // TODO(dhood): use the namespace (slashes converted to dots)
-  logger_ = rclcpp::get_logger(node_base_->get_name());
+  logger_ = rclcpp::get_logger(this->get_logger_name());
 }
 
 NodeLogging::~NodeLogging()
@@ -32,3 +31,9 @@ NodeLogging::get_logger() const
 {
   return logger_;
 }
+
+const char *
+NodeLogging::get_logger_name() const
+{
+  return rcl_node_get_logger_name(node_base_->get_rcl_node_handle());
+}

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -14,23 +14,35 @@
 
 #include "rclcpp/node_interfaces/node_parameters.hpp"
 
+#include <rcl_yaml_param_parser/parser.h>
+
 #include <map>
 #include <memory>
+#include <sstream>
 #include <string>
 #include <utility>
 #include <vector>
 
 #include "rcl_interfaces/srv/list_parameters.hpp"
 #include "rclcpp/create_publisher.hpp"
+#include "rclcpp/parameter_map.hpp"
+#include "rclcpp/scope_exit.hpp"
 #include "rcutils/logging_macros.h"
 #include "rmw/qos_profiles.h"
 
 using rclcpp::node_interfaces::NodeParameters;
 
 NodeParameters::NodeParameters(
-  rclcpp::node_interfaces::NodeTopicsInterface * node_topics,
-  bool use_intra_process)
-: node_topics_(node_topics)
+  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+  const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+  const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
+  const rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
+  const std::vector<rclcpp::Parameter> & initial_parameters,
+  bool use_intra_process,
+  bool start_parameter_services,
+  bool start_parameter_event_publisher,
+  const rmw_qos_profile_t & parameter_event_qos_profile)
+: events_publisher_(nullptr), node_clock_(node_clock)
 {
   using MessageT = rcl_interfaces::msg::ParameterEvent;
   using PublisherT = rclcpp::Publisher<MessageT>;
@@ -38,12 +50,123 @@ NodeParameters::NodeParameters(
   // TODO(wjwwood): expose this allocator through the Parameter interface.
   auto allocator = std::make_shared<AllocatorT>();
 
-  events_publisher_ = rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
-    node_topics_,
-    "parameter_events",
-    rmw_qos_profile_parameter_events,
-    use_intra_process,
-    allocator);
+  if (start_parameter_services) {
+    parameter_service_ = std::make_shared<ParameterService>(node_base, node_services, this);
+  }
+
+  if (start_parameter_event_publisher) {
+    events_publisher_ = rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
+      node_topics.get(),
+      "parameter_events",
+      parameter_event_qos_profile,
+      use_intra_process,
+      allocator);
+  }
+
+  // Get the node options
+  const rcl_node_t * node = node_base->get_rcl_node_handle();
+  if (nullptr == node) {
+    throw std::runtime_error("Need valid node handle in NodeParameters");
+  }
+  const rcl_node_options_t * options = rcl_node_get_options(node);
+  if (nullptr == options) {
+    throw std::runtime_error("Need valid node options in NodeParameters");
+  }
+
+  // Get paths to yaml files containing initial parameter values
+  std::vector<std::string> yaml_paths;
+
+  auto get_yaml_paths = [&yaml_paths, &options](const rcl_arguments_t * args) {
+      int num_yaml_files = rcl_arguments_get_param_files_count(args);
+      if (num_yaml_files > 0) {
+        char ** param_files;
+        rcl_ret_t ret = rcl_arguments_get_param_files(args, options->allocator, &param_files);
+        if (RCL_RET_OK != ret) {
+          rclcpp::exceptions::throw_from_rcl_error(ret);
+        }
+        auto cleanup_param_files = make_scope_exit(
+          [&param_files, &num_yaml_files, &options]() {
+            for (int i = 0; i < num_yaml_files; ++i) {
+              options->allocator.deallocate(param_files[i], options->allocator.state);
+            }
+            options->allocator.deallocate(param_files, options->allocator.state);
+          });
+        for (int i = 0; i < num_yaml_files; ++i) {
+          yaml_paths.emplace_back(param_files[i]);
+        }
+      }
+    };
+
+  // global before local so that local overwrites global
+  if (options->use_global_arguments) {
+    auto context_ptr = node_base->get_context()->get_rcl_context();
+    get_yaml_paths(&(context_ptr->global_arguments));
+  }
+  get_yaml_paths(&(options->arguments));
+
+  // Get fully qualified node name post-remapping to use to find node's params in yaml files
+  const std::string node_name = node_base->get_name();
+  const std::string node_namespace = node_base->get_namespace();
+  if (0u == node_namespace.size() || 0u == node_name.size()) {
+    // Should never happen
+    throw std::runtime_error("Node name and namespace were not set");
+  }
+
+  if ('/' == node_namespace.at(node_namespace.size() - 1)) {
+    combined_name_ = node_namespace + node_name;
+  } else {
+    combined_name_ = node_namespace + '/' + node_name;
+  }
+
+  std::map<std::string, rclcpp::Parameter> parameters;
+
+  // TODO(sloretz) use rcl to parse yaml when circular dependency is solved
+  // See https://github.com/ros2/rcl/issues/252
+  for (const std::string & yaml_path : yaml_paths) {
+    rcl_params_t * yaml_params = rcl_yaml_node_struct_init(options->allocator);
+    if (nullptr == yaml_params) {
+      throw std::bad_alloc();
+    }
+    if (!rcl_parse_yaml_file(yaml_path.c_str(), yaml_params)) {
+      std::ostringstream ss;
+      ss << "Failed to parse parameters from file '" << yaml_path << "': " <<
+        rcl_get_error_string().str;
+      rcl_reset_error();
+      throw std::runtime_error(ss.str());
+    }
+
+    rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(yaml_params);
+    rcl_yaml_node_struct_fini(yaml_params);
+    auto iter = initial_map.find(combined_name_);
+    if (initial_map.end() == iter) {
+      continue;
+    }
+
+    // Combine parameter yaml files, overwriting values in older ones
+    for (auto & param : iter->second) {
+      parameters[param.get_name()] = param;
+    }
+  }
+
+  // initial values passed to constructor overwrite yaml file sources
+  for (auto & param : initial_parameters) {
+    parameters[param.get_name()] = param;
+  }
+
+  std::vector<rclcpp::Parameter> combined_values;
+  combined_values.reserve(parameters.size());
+  for (auto & kv : parameters) {
+    combined_values.emplace_back(kv.second);
+  }
+
+  // TODO(sloretz) store initial values and use them when a parameter is created ros2/rclcpp#475
+  // Set initial parameter values
+  if (!combined_values.empty()) {
+    rcl_interfaces::msg::SetParametersResult result = set_parameters_atomically(combined_values);
+    if (!result.successful) {
+      throw std::runtime_error("Failed to set initial parameters");
+    }
+  }
 }
 
 NodeParameters::~NodeParameters()
@@ -51,10 +174,10 @@ NodeParameters::~NodeParameters()
 
 std::vector<rcl_interfaces::msg::SetParametersResult>
 NodeParameters::set_parameters(
-  const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
+  const std::vector<rclcpp::Parameter> & parameters)
 {
   std::vector<rcl_interfaces::msg::SetParametersResult> results;
-  for (auto p : parameters) {
+  for (const auto & p : parameters) {
     auto result = set_parameters_atomically({{p}});
     results.push_back(result);
   }
@@ -63,12 +186,14 @@ NodeParameters::set_parameters(
 
 rcl_interfaces::msg::SetParametersResult
 NodeParameters::set_parameters_atomically(
-  const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
+  const std::vector<rclcpp::Parameter> & parameters)
 {
   std::lock_guard<std::mutex> lock(mutex_);
-  std::map<std::string, rclcpp::parameter::ParameterVariant> tmp_map;
+  std::map<std::string, rclcpp::Parameter> tmp_map;
   auto parameter_event = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
 
+  parameter_event->node = combined_name_;
+
   // TODO(jacquelinekay): handle parameter constraints
   rcl_interfaces::msg::SetParametersResult result;
   if (parameters_callback_) {
@@ -81,44 +206,55 @@ NodeParameters::set_parameters_atomically(
     return result;
   }
 
-  for (auto p : parameters) {
-    if (parameters_.find(p.get_name()) == parameters_.end()) {
-      if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
-        // case: parameter not set before, and input is something other than "NOT_SET"
-        parameter_event->new_parameters.push_back(p.to_parameter());
+  for (const auto & p : parameters) {
+    if (p.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) {
+      if (parameters_.find(p.get_name()) != parameters_.end()) {
+        // case: parameter was set before, and input is "NOT_SET"
+        // therefore we will erase the parameter from parameters_ later
+        parameter_event->deleted_parameters.push_back(p.to_parameter_msg());
       }
-    } else if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
-      // case: parameter was set before, and input is something other than "NOT_SET"
-      parameter_event->changed_parameters.push_back(p.to_parameter());
     } else {
-      // case: parameter was set before, and input is "NOT_SET"
-      // therefore we will "unset" the previously set parameter
-      // it is not necessary to erase the parameter from parameters_
-      // because the new value for this key (p.get_name()) will be a
-      // ParameterVariant with type "NOT_SET"
-      parameter_event->deleted_parameters.push_back(p.to_parameter());
+      if (parameters_.find(p.get_name()) == parameters_.end()) {
+        // case: parameter not set before, and input is something other than "NOT_SET"
+        parameter_event->new_parameters.push_back(p.to_parameter_msg());
+      } else {
+        // case: parameter was set before, and input is something other than "NOT_SET"
+        parameter_event->changed_parameters.push_back(p.to_parameter_msg());
+      }
+      tmp_map[p.get_name()] = p;
     }
-    tmp_map[p.get_name()] = p;
   }
   // std::map::insert will not overwrite elements, so we'll keep the new
   // ones and add only those that already exist in the Node's internal map
   tmp_map.insert(parameters_.begin(), parameters_.end());
+
+  // remove explicitly deleted parameters
+  for (const auto & p : parameters) {
+    if (p.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) {
+      tmp_map.erase(p.get_name());
+    }
+  }
+
   std::swap(tmp_map, parameters_);
 
-  events_publisher_->publish(parameter_event);
+  // events_publisher_ may be nullptr if it was disabled in constructor
+  if (nullptr != events_publisher_) {
+    parameter_event->stamp = node_clock_->get_clock()->now();
+    events_publisher_->publish(parameter_event);
+  }
 
   return result;
 }
 
-std::vector<rclcpp::parameter::ParameterVariant>
+std::vector<rclcpp::Parameter>
 NodeParameters::get_parameters(const std::vector<std::string> & names) const
 {
   std::lock_guard<std::mutex> lock(mutex_);
-  std::vector<rclcpp::parameter::ParameterVariant> results;
+  std::vector<rclcpp::Parameter> results;
 
   for (auto & name : names) {
     if (std::any_of(parameters_.cbegin(), parameters_.cend(),
-      [&name](const std::pair<std::string, rclcpp::parameter::ParameterVariant> & kv) {
+      [&name](const std::pair<std::string, rclcpp::Parameter> & kv) {
         return name == kv.first;
       }))
     {
@@ -128,10 +264,10 @@ NodeParameters::get_parameters(const std::vector<std::string> & names) const
   return results;
 }
 
-rclcpp::parameter::ParameterVariant
+rclcpp::Parameter
 NodeParameters::get_parameter(const std::string & name) const
 {
-  rclcpp::parameter::ParameterVariant parameter;
+  rclcpp::Parameter parameter;
 
   if (get_parameter(name, parameter)) {
     return parameter;
@@ -143,7 +279,7 @@ NodeParameters::get_parameter(const std::string & name) const
 bool
 NodeParameters::get_parameter(
   const std::string & name,
-  rclcpp::parameter::ParameterVariant & parameter) const
+  rclcpp::Parameter & parameter) const
 {
   std::lock_guard<std::mutex> lock(mutex_);
 
@@ -155,6 +291,27 @@ NodeParameters::get_parameter(
   }
 }
 
+bool
+NodeParameters::get_parameters_by_prefix(
+  const std::string & prefix,
+  std::map<std::string, rclcpp::Parameter> & parameters) const
+{
+  std::string prefix_with_dot = prefix + ".";
+  bool ret = false;
+
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  for (const auto & param : parameters_) {
+    if (param.first.find(prefix_with_dot) == 0 && param.first.length() > prefix_with_dot.length()) {
+      // Found one!
+      parameters[param.first.substr(prefix_with_dot.length())] = param.second;
+      ret = true;
+    }
+  }
+
+  return ret;
+}
+
 std::vector<rcl_interfaces::msg::ParameterDescriptor>
 NodeParameters::describe_parameters(const std::vector<std::string> & names) const
 {
@@ -239,7 +396,7 @@ NodeParameters::register_param_change_callback(ParametersCallbackFunction callba
 {
   if (parameters_callback_) {
     RCUTILS_LOG_WARN("param_change_callback already registered, "
-      "overwriting previous callback")
+      "overwriting previous callback");
   }
   parameters_callback_ = callback;
 }

rclcpp/src/rclcpp/node_interfaces/node_services.cpp

@@ -45,8 +45,8 @@ NodeServices::add_service(
     auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
     if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
       throw std::runtime_error(
-              std::string(
-                "Failed to notify wait set on service creation: ") + rmw_get_error_string()
+              std::string("Failed to notify wait set on service creation: ") +
+              rmw_get_error_string().str
       );
     }
   }
@@ -72,7 +72,8 @@ NodeServices::add_client(
     auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
     if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
       throw std::runtime_error(
-              std::string("Failed to notify wait set on client creation: ") + rmw_get_error_string()
+              std::string("Failed to notify wait set on client creation: ") +
+              rmw_get_error_string().str
       );
     }
   }

rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp

@@ -0,0 +1,50 @@
+// 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.
+
+#include "rclcpp/node_interfaces/node_time_source.hpp"
+
+#include <memory>
+#include <string>
+
+using rclcpp::node_interfaces::NodeTimeSource;
+
+NodeTimeSource::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)
+: node_base_(node_base),
+  node_topics_(node_topics),
+  node_graph_(node_graph),
+  node_services_(node_services),
+  node_logging_(node_logging),
+  node_clock_(node_clock),
+  node_parameters_(node_parameters)
+{
+  time_source_.attachNode(
+    node_base_,
+    node_topics_,
+    node_graph_,
+    node_services_,
+    node_logging_,
+    node_clock_,
+    node_parameters_);
+  time_source_.attachClock(node_clock_->get_clock());
+}
+
+NodeTimeSource::~NodeTimeSource()
+{}

rclcpp/src/rclcpp/node_interfaces/node_timers.cpp

@@ -41,7 +41,7 @@ NodeTimers::add_timer(
   }
   if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
     throw std::runtime_error(
-            std::string(
-              "Failed to notify wait set on timer creation: ") + rmw_get_error_string());
+            std::string("Failed to notify wait set on timer creation: ") +
+            rmw_get_error_string().str);
   }
 }

rclcpp/src/rclcpp/node_interfaces/node_topics.cpp

@@ -54,6 +54,7 @@ NodeTopics::create_publisher(
     publisher->setup_intra_process(
       intra_process_publisher_id,
       shared_publish_callback,
+      ipm,
       publisher_options);
   }
 
@@ -73,8 +74,8 @@ NodeTopics::add_publisher(
     auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
     if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
       throw std::runtime_error(
-              std::string(
-                "Failed to notify wait set on publisher creation: ") + rmw_get_error_string());
+              std::string("Failed to notify wait set on publisher creation: ") +
+              rmw_get_error_string().str);
     }
   }
 }
@@ -123,8 +124,8 @@ NodeTopics::add_subscription(
     auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
     if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
       throw std::runtime_error(
-              std::string(
-                "Failed to notify wait set on subscription creation: ") + rmw_get_error_string()
+              std::string("Failed to notify wait set on subscription creation: ") +
+              rmw_get_error_string().str
       );
     }
   }

rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp

@@ -0,0 +1,68 @@
+// 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.
+
+#include "rclcpp/node_interfaces/node_waitables.hpp"
+
+#include <string>
+
+using rclcpp::node_interfaces::NodeWaitables;
+
+NodeWaitables::NodeWaitables(rclcpp::node_interfaces::NodeBaseInterface * node_base)
+: node_base_(node_base)
+{}
+
+NodeWaitables::~NodeWaitables()
+{}
+
+void
+NodeWaitables::add_waitable(
+  rclcpp::Waitable::SharedPtr waitable_ptr,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group)
+{
+  if (group) {
+    if (!node_base_->callback_group_in_node(group)) {
+      // TODO(jacobperron): use custom exception
+      throw std::runtime_error("Cannot create waitable, group not in node.");
+    }
+    group->add_waitable(waitable_ptr);
+  } else {
+    node_base_->get_default_callback_group()->add_waitable(waitable_ptr);
+  }
+
+  // Notify the executor that a new waitable was created using the parent Node.
+  {
+    auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
+    if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
+      throw std::runtime_error(
+              std::string("Failed to notify wait set on waitable creation: ") +
+              rmw_get_error_string().str
+      );
+    }
+  }
+}
+
+void
+NodeWaitables::remove_waitable(
+  rclcpp::Waitable::SharedPtr waitable_ptr,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group) noexcept
+{
+  if (group) {
+    if (!node_base_->callback_group_in_node(group)) {
+      return;
+    }
+    group->remove_waitable(waitable_ptr);
+  } else {
+    node_base_->get_default_callback_group()->remove_waitable(waitable_ptr);
+  }
+}

rclcpp/src/rclcpp/node_options.cpp

@@ -0,0 +1,267 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/node_options.hpp"
+
+#include <limits>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/logging.hpp"
+
+using rclcpp::exceptions::throw_from_rcl_error;
+
+namespace rclcpp
+{
+
+namespace detail
+{
+static
+void
+rcl_node_options_t_destructor(rcl_node_options_t * node_options)
+{
+  if (node_options) {
+    rcl_ret_t ret = rcl_node_options_fini(node_options);
+    if (RCL_RET_OK != ret) {
+      // Cannot throw here, as it may be called in the destructor.
+      RCLCPP_ERROR(
+        rclcpp::get_logger("rclcpp"),
+        "failed to finalize rcl node options: %s", rcl_get_error_string().str);
+      rcl_reset_error();
+    }
+  }
+}
+}  // namespace detail
+
+NodeOptions::NodeOptions(rcl_allocator_t allocator)
+: node_options_(nullptr, detail::rcl_node_options_t_destructor), allocator_(allocator)
+{}
+
+NodeOptions::NodeOptions(const NodeOptions & other)
+: node_options_(nullptr, detail::rcl_node_options_t_destructor)
+{
+  *this = other;
+}
+
+NodeOptions &
+NodeOptions::operator=(const NodeOptions & other)
+{
+  if (this != &other) {
+    this->context_ = other.context_;
+    this->arguments_ = other.arguments_;
+    this->initial_parameters_ = other.initial_parameters_;
+    this->use_global_arguments_ = other.use_global_arguments_;
+    this->use_intra_process_comms_ = other.use_intra_process_comms_;
+    this->start_parameter_services_ = other.start_parameter_services_;
+    this->allocator_ = other.allocator_;
+  }
+  return *this;
+}
+
+const rcl_node_options_t *
+NodeOptions::get_rcl_node_options() const
+{
+  // If it is nullptr, create it on demand.
+  if (!node_options_) {
+    node_options_.reset(new rcl_node_options_t);
+    *node_options_ = rcl_node_get_default_options();
+    node_options_->allocator = this->allocator_;
+    node_options_->use_global_arguments = this->use_global_arguments_;
+    node_options_->domain_id = this->get_domain_id_from_env();
+
+    std::unique_ptr<const char *[]> c_args;
+    if (!this->arguments_.empty()) {
+      c_args.reset(new const char *[this->arguments_.size()]);
+      for (std::size_t i = 0; i < this->arguments_.size(); ++i) {
+        c_args[i] = this->arguments_[i].c_str();
+      }
+    }
+
+    if (this->arguments_.size() > std::numeric_limits<int>::max()) {
+      throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "Too many args");
+    }
+
+    rmw_ret_t ret = rcl_parse_arguments(
+      static_cast<int>(this->arguments_.size()), c_args.get(), this->allocator_,
+      &(node_options_->arguments));
+
+    if (RCL_RET_OK != ret) {
+      throw_from_rcl_error(ret, "failed to parse arguments");
+    }
+  }
+
+  return node_options_.get();
+}
+
+rclcpp::Context::SharedPtr
+NodeOptions::context() const
+{
+  return this->context_;
+}
+
+NodeOptions &
+NodeOptions::context(rclcpp::Context::SharedPtr context)
+{
+  this->context_ = context;
+  return *this;
+}
+
+const std::vector<std::string> &
+NodeOptions::arguments() const
+{
+  return this->arguments_;
+}
+
+NodeOptions &
+NodeOptions::arguments(const std::vector<std::string> & arguments)
+{
+  this->node_options_.reset();  // reset node options to make it be recreated on next access.
+  this->arguments_ = arguments;
+  return *this;
+}
+
+std::vector<rclcpp::Parameter> &
+NodeOptions::initial_parameters()
+{
+  return this->initial_parameters_;
+}
+
+const std::vector<rclcpp::Parameter> &
+NodeOptions::initial_parameters() const
+{
+  return this->initial_parameters_;
+}
+
+NodeOptions &
+NodeOptions::initial_parameters(const std::vector<rclcpp::Parameter> & initial_parameters)
+{
+  this->initial_parameters_ = initial_parameters;
+  return *this;
+}
+
+const bool &
+NodeOptions::use_global_arguments() const
+{
+  return this->node_options_->use_global_arguments;
+}
+
+NodeOptions &
+NodeOptions::use_global_arguments(const bool & use_global_arguments)
+{
+  this->node_options_.reset();  // reset node options to make it be recreated on next access.
+  this->use_global_arguments_ = use_global_arguments;
+  return *this;
+}
+
+const bool &
+NodeOptions::use_intra_process_comms() const
+{
+  return this->use_intra_process_comms_;
+}
+
+NodeOptions &
+NodeOptions::use_intra_process_comms(const bool & use_intra_process_comms)
+{
+  this->use_intra_process_comms_ = use_intra_process_comms;
+  return *this;
+}
+
+const bool &
+NodeOptions::start_parameter_services() const
+{
+  return this->start_parameter_services_;
+}
+
+NodeOptions &
+NodeOptions::start_parameter_services(const bool & start_parameter_services)
+{
+  this->start_parameter_services_ = start_parameter_services;
+  return *this;
+}
+
+const bool &
+NodeOptions::start_parameter_event_publisher() const
+{
+  return this->start_parameter_event_publisher_;
+}
+
+NodeOptions &
+NodeOptions::start_parameter_event_publisher(const bool & start_parameter_event_publisher)
+{
+  this->start_parameter_event_publisher_ = start_parameter_event_publisher;
+  return *this;
+}
+
+const rmw_qos_profile_t &
+NodeOptions::parameter_event_qos_profile() const
+{
+  return this->parameter_event_qos_profile_;
+}
+
+NodeOptions &
+NodeOptions::parameter_event_qos_profile(const rmw_qos_profile_t & parameter_event_qos_profile)
+{
+  this->parameter_event_qos_profile_ = parameter_event_qos_profile;
+  return *this;
+}
+
+const rcl_allocator_t &
+NodeOptions::allocator() const
+{
+  return this->allocator_;
+}
+
+NodeOptions &
+NodeOptions::allocator(rcl_allocator_t allocator)
+{
+  this->node_options_.reset();  // reset node options to make it be recreated on next access.
+  this->allocator_ = allocator;
+  return *this;
+}
+
+// TODO(wjwwood): reuse rcutils_get_env() to avoid code duplication.
+//   See also: https://github.com/ros2/rcl/issues/119
+size_t
+NodeOptions::get_domain_id_from_env() const
+{
+  // Determine the domain id based on the options and the ROS_DOMAIN_ID env variable.
+  size_t domain_id = std::numeric_limits<size_t>::max();
+  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)()) {
+#ifdef _WIN32
+      // free the ros_domain_id before throwing, if getenv was used on Windows
+      free(ros_domain_id);
+#endif
+      throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number");
+    }
+    domain_id = static_cast<size_t>(number);
+#ifdef _WIN32
+    free(ros_domain_id);
+#endif
+  }
+  return domain_id;
+}
+
+}  // namespace rclcpp