0.5.1

* Further expand test tolerance to address flakiness.

* Remove newline.

.github/ISSUE_TEMPLATE.md

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

rclcpp/.gitignore

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

rclcpp/CHANGELOG.rst

@@ -0,0 +1,46 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package rclcpp
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+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,18 +2,29 @@ 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(rmw_implementation_cmake REQUIRED)
+find_package(rosgraph_msgs REQUIRED)
 find_package(rosidl_generator_cpp REQUIRED)
+find_package(rosidl_typesupport_c REQUIRED)
 find_package(rosidl_typesupport_cpp REQUIRED)
 
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  set(CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} "-std=c++14")
-  add_compile_options(-Wall -Wextra -Wpedantic)
+  # we dont use add_compile_options with pedantic in message packages
+  # because the Python C extensions dont comply with it
+  # TODO(mikaelarguedas) change to add_compile_options
+  # once this is not a message package anymore
+  # https://github.com/ros2/system_tests/issues/191
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
 endif()
 
 include_directories(include)
@@ -22,76 +33,133 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/any_executable.cpp
   src/rclcpp/callback_group.cpp
   src/rclcpp/client.cpp
+  src/rclcpp/clock.cpp
   src/rclcpp/context.cpp
   src/rclcpp/contexts/default_context.cpp
+  src/rclcpp/duration.cpp
   src/rclcpp/event.cpp
   src/rclcpp/exceptions.cpp
   src/rclcpp/executor.cpp
   src/rclcpp/executors.cpp
+  src/rclcpp/expand_topic_or_service_name.cpp
   src/rclcpp/executors/multi_threaded_executor.cpp
   src/rclcpp/executors/single_threaded_executor.cpp
   src/rclcpp/graph_listener.cpp
   src/rclcpp/intra_process_manager.cpp
   src/rclcpp/intra_process_manager_impl.cpp
+  src/rclcpp/logger.cpp
   src/rclcpp/memory_strategies.cpp
   src/rclcpp/memory_strategy.cpp
   src/rclcpp/node.cpp
   src/rclcpp/node_interfaces/node_base.cpp
+  src/rclcpp/node_interfaces/node_clock.cpp
   src/rclcpp/node_interfaces/node_graph.cpp
+  src/rclcpp/node_interfaces/node_logging.cpp
   src/rclcpp/node_interfaces/node_parameters.cpp
   src/rclcpp/node_interfaces/node_services.cpp
   src/rclcpp/node_interfaces/node_timers.cpp
   src/rclcpp/node_interfaces/node_topics.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/subscription.cpp
+  src/rclcpp/time.cpp
+  src/rclcpp/time_source.cpp
   src/rclcpp/timer.cpp
   src/rclcpp/type_support.cpp
   src/rclcpp/utilities.cpp
 )
 
-macro(target)
-  if(NOT target_suffix STREQUAL "")
-    get_rcl_information("${rmw_implementation}" "rcl${target_suffix}")
-  endif()
-  add_library(${PROJECT_NAME}${target_suffix} SHARED
-    ${${PROJECT_NAME}_SRCS})
-  ament_target_dependencies(${PROJECT_NAME}${target_suffix}
-    "rcl${target_suffix}"
-    "rosidl_generator_cpp"
-    "rosidl_typesupport_cpp")
+# "watch" template for changes
+configure_file(
+  "resource/logging.hpp.em"
+  "logging.hpp.em.watch"
+  COPYONLY
+)
+# generate header with logging macros
+set(python_code
+  "import em"
+  "em.invoke(['-o', 'include/rclcpp/logging.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/logging.hpp.em'])")
+string(REPLACE ";" "$<SEMICOLON>" python_code "${python_code}")
+add_custom_command(OUTPUT include/rclcpp/logging.hpp
+  COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp"
+  COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_code}"
+  DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/logging.hpp.em.watch"
+  COMMENT "Expanding logging.hpp.em"
+  VERBATIM
+)
+list(APPEND ${PROJECT_NAME}_SRCS
+  include/rclcpp/logging.hpp)
+include_directories("${CMAKE_CURRENT_BINARY_DIR}/include")
 
-  # Causes the visibility macros to use dllexport rather than dllimport,
-  # which is appropriate when building the dll but not consuming it.
-  target_compile_definitions(${PROJECT_NAME}${target_suffix}
-    PRIVATE "RCLCPP_BUILDING_LIBRARY")
+add_library(${PROJECT_NAME}
+  ${${PROJECT_NAME}_SRCS})
+# specific order: dependents before dependencies
+ament_target_dependencies(${PROJECT_NAME}
+  "rcl"
+  "rcl_yaml_param_parser"
+  "builtin_interfaces"
+  "rosgraph_msgs"
+  "rosidl_typesupport_cpp"
+  "rosidl_generator_cpp")
 
-  install(
-    TARGETS ${PROJECT_NAME}${target_suffix}
-    ARCHIVE DESTINATION lib
-    LIBRARY DESTINATION lib
-    RUNTIME DESTINATION bin
-  )
-endmacro()
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(${PROJECT_NAME}
+  PRIVATE "RCLCPP_BUILDING_LIBRARY")
 
-call_for_each_rmw_implementation(target GENERATE_DEFAULT)
+install(
+  TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+# specific order: dependents before dependencies
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
 
 ament_export_dependencies(ament_cmake)
 ament_export_dependencies(rcl)
-ament_export_dependencies(rosidl_generator_cpp)
+ament_export_dependencies(builtin_interfaces)
+ament_export_dependencies(rosgraph_msgs)
 ament_export_dependencies(rosidl_typesupport_cpp)
-
-ament_export_include_directories(include)
-
-ament_export_libraries(${PROJECT_NAME})
+ament_export_dependencies(rosidl_typesupport_c)
+ament_export_dependencies(rosidl_generator_cpp)
+ament_export_dependencies(rcl_yaml_param_parser)
 
 if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 
+  find_package(rmw_implementation_cmake REQUIRED)
+
+  ament_add_gtest(test_client test/test_client.cpp)
+  if(TARGET test_client)
+    target_include_directories(test_client PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_client ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_expand_topic_or_service_name test/test_expand_topic_or_service_name.cpp)
+  if(TARGET test_expand_topic_or_service_name)
+    target_include_directories(test_expand_topic_or_service_name PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME})
+  endif()
   ament_add_gtest(test_function_traits test/test_function_traits.cpp)
   if(TARGET test_function_traits)
     target_include_directories(test_function_traits PUBLIC
@@ -121,6 +189,64 @@ if(BUILD_TESTING)
       ${rosidl_typesupport_cpp_INCLUDE_DIRS}
     )
   endif()
+  ament_add_gtest(test_node test/test_node.cpp)
+  if(TARGET test_node)
+    target_include_directories(test_node PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_node ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_node_global_args test/test_node_global_args.cpp)
+  if(TARGET test_node_global_args)
+    target_include_directories(test_node_global_args PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_node_global_args ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_node_initial_parameters test/test_node_initial_parameters.cpp)
+  if(TARGET test_node_initial_parameters)
+    target_link_libraries(test_node_initial_parameters ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_parameter_events_filter test/test_parameter_events_filter.cpp)
+  if(TARGET test_parameter_events_filter)
+    target_include_directories(test_parameter_events_filter PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_parameter_events_filter ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_parameter test/test_parameter.cpp)
+  if(TARGET test_parameter)
+    target_include_directories(test_parameter PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_parameter ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_parameter_map test/test_parameter_map.cpp)
+  if(TARGET test_parameter_map)
+    target_link_libraries(test_parameter_map ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_publisher test/test_publisher.cpp)
+  if(TARGET test_publisher)
+    target_include_directories(test_publisher PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_publisher ${PROJECT_NAME})
+  endif()
   ament_add_gtest(test_rate test/test_rate.cpp
     ENV RCL_ASSERT_RMW_ID_MATCHES=${rmw_implementation})
   if(TARGET test_rate)
@@ -134,6 +260,47 @@ if(BUILD_TESTING)
       ${PROJECT_NAME}
     )
   endif()
+  ament_add_gtest(test_serialized_message_allocator test/test_serialized_message_allocator.cpp
+    ENV RCL_ASSERT_RMW_ID_MATCHES=${rmw_implementation})
+  if(TARGET test_serialized_message_allocator)
+    target_include_directories(test_serialized_message_allocator PUBLIC
+      ${test_msgs_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_serialized_message_allocator
+      ${PROJECT_NAME}
+      ${test_msgs_LIBRARIES}
+    )
+  endif()
+  ament_add_gtest(test_service test/test_service.cpp)
+  if(TARGET test_service)
+    target_include_directories(test_service PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_service ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_subscription test/test_subscription.cpp)
+  if(TARGET test_subscription)
+    target_include_directories(test_subscription PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_subscription ${PROJECT_NAME})
+  endif()
+  find_package(test_msgs REQUIRED)
+  ament_add_gtest(test_subscription_traits test/test_subscription_traits.cpp)
+  if(TARGET test_subscription_traits)
+    target_include_directories(test_subscription_traits PUBLIC
+      ${rcl_INCLUDE_DIRS}
+    )
+  ament_target_dependencies(test_subscription_traits
+    "test_msgs"
+  )
+  endif()
   ament_add_gtest(test_find_weak_nodes test/test_find_weak_nodes.cpp)
   if(TARGET test_find_weak_nodes)
     target_include_directories(test_find_weak_nodes PUBLIC
@@ -155,7 +322,7 @@ if(BUILD_TESTING)
 
   set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}")
   if(WIN32)
-    set(append_library_dirs "${append_library_dirs}/$<CONFIGURATION>")
+    set(append_library_dirs "${append_library_dirs}/$<CONFIG>")
   endif()
 
   ament_add_gtest(test_externally_defined_services test/test_externally_defined_services.cpp
@@ -174,6 +341,60 @@ if(BUILD_TESTING)
         mock_msgs ${typesupport_impl_c})
     endforeach()
   endif()
+
+  ament_add_gtest(test_duration test/test_duration.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_duration)
+    ament_target_dependencies(test_duration
+      "rcl")
+    target_link_libraries(test_duration ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_executor test/test_executor.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_executor)
+    ament_target_dependencies(test_executor
+      "rcl")
+    target_link_libraries(test_executor ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_logger test/test_logger.cpp)
+  target_link_libraries(test_logger ${PROJECT_NAME})
+
+  ament_add_gmock(test_logging test/test_logging.cpp)
+  target_link_libraries(test_logging ${PROJECT_NAME})
+
+  ament_add_gtest(test_time test/test_time.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_time)
+    ament_target_dependencies(test_time
+      "rcl")
+    target_link_libraries(test_time ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_time_source test/test_time_source.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_time_source)
+    ament_target_dependencies(test_time_source
+      "rcl")
+    target_link_libraries(test_time_source ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_utilities test/test_utilities.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_utilities)
+    ament_target_dependencies(test_utilities
+      "rcl")
+    target_link_libraries(test_utilities ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_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()
 endif()
 
 ament_package(
@@ -186,6 +407,6 @@ install(
 )
 
 install(
-  DIRECTORY include/
+  DIRECTORY include/ ${CMAKE_CURRENT_BINARY_DIR}/include/
   DESTINATION include
 )

rclcpp/Doxyfile

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

rclcpp/cmake/get_rclcpp_information.cmake

@@ -1,93 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-#
-# Get all information about rclcpp for a specific RMW implementation.
-#
-# It sets the common variables _DEFINITIONS, _INCLUDE_DIRS and _LIBRARIES
-# with the given prefix.
-#
-# :param rmw_implementation: the RMW implementation name
-# :type rmw_implementation: string
-# :param var_prefix: the prefix of all output variable names
-# :type var_prefix: string
-#
-macro(get_rclcpp_information rmw_implementation var_prefix)
-  # pretend to be a "package"
-  # so that the variables can be used by various functions / macros
-  set(${var_prefix}_FOUND TRUE)
-
-  # Get rcl using the existing macro
-  if(NOT target_suffix STREQUAL "")
-    get_rcl_information("${rmw_implementation}" "rcl${target_suffix}")
-  endif()
-
-  # include directories
-  normalize_path(${var_prefix}_INCLUDE_DIRS
-    "${rclcpp_DIR}/../../../include")
-
-  # libraries
-  set(_libs)
-  # search for library relative to this CMake file
-  set(_library_target "rclcpp")
-  get_available_rmw_implementations(_rmw_impls)
-  list(LENGTH _rmw_impls _rmw_impls_length)
-  if(_rmw_impls_length GREATER 1)
-    set(_library_target "${_library_target}__${rmw_implementation}")
-  endif()
-  set(_lib "NOTFOUND")
-  find_library(
-    _lib NAMES "${_library_target}"
-    PATHS "${rclcpp_DIR}/../../../lib"
-    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH
-  )
-  if(NOT _lib)
-    # warn about not existing library and ignore it
-    message(WARNING "Package 'rclcpp' doesn't contain the library '${_library_target}'")
-  elseif(NOT IS_ABSOLUTE "${_lib}")
-    # the found library must be an absolute path
-    message(FATAL_ERROR "Package 'rclcpp' found the library '${_library_target}' at '${_lib}' which is not an absolute path")
-  elseif(NOT EXISTS "${_lib}")
-    # the found library must exist
-    message(FATAL_ERROR "Package 'rclcpp' found the library '${_lib}' which doesn't exist")
-  else()
-    list(APPEND _libs "${_lib}")
-  endif()
-
-  # dependencies
-  set(_exported_dependencies
-    "rcl_interfaces"
-    "rcl${target_suffix}"
-    "rosidl_generator_cpp"
-    "rosidl_typesupport_cpp")
-  set(${var_prefix}_DEFINITIONS)
-  foreach(_dep ${_exported_dependencies})
-    if(NOT ${_dep}_FOUND)
-      find_package("${_dep}" QUIET REQUIRED)
-    endif()
-    if(${_dep}_DEFINITIONS)
-      list_append_unique(${var_prefix}_DEFINITIONS "${${_dep}_DEFINITIONS}")
-    endif()
-    if(${_dep}_INCLUDE_DIRS)
-      list_append_unique(${var_prefix}_INCLUDE_DIRS "${${_dep}_INCLUDE_DIRS}")
-    endif()
-    if(${_dep}_LIBRARIES)
-      list(APPEND _libs "${${_dep}_LIBRARIES}")
-    endif()
-  endforeach()
-  if(_libs)
-    ament_libraries_deduplicate(_libs "${_libs}")
-  endif()
-  set(${var_prefix}_LIBRARIES "${_libs}")
-endmacro()

rclcpp/include/rclcpp/allocator/allocator_common.hpp

@@ -64,8 +64,9 @@ void * retyped_reallocate(void * untyped_pointer, size_t size, void * untyped_al
 
 
 // Convert a std::allocator_traits-formatted Allocator into an rcl allocator
-template<typename T, typename Alloc,
-typename std::enable_if<!std::is_same<Alloc, std::allocator<void>>::value>::type * = nullptr>
+template<
+  typename T, typename Alloc,
+  typename std::enable_if<!std::is_same<Alloc, std::allocator<void>>::value>::type * = nullptr>
 rcl_allocator_t get_rcl_allocator(Alloc & allocator)
 {
   rcl_allocator_t rcl_allocator = rcl_get_default_allocator();
@@ -81,8 +82,9 @@ rcl_allocator_t get_rcl_allocator(Alloc & allocator)
 }
 
 // TODO(jacquelinekay) Workaround for an incomplete implementation of std::allocator<void>
-template<typename T, typename Alloc,
-typename std::enable_if<std::is_same<Alloc, std::allocator<void>>::value>::type * = nullptr>
+template<
+  typename T, typename Alloc,
+  typename std::enable_if<std::is_same<Alloc, std::allocator<void>>::value>::type * = nullptr>
 rcl_allocator_t get_rcl_allocator(Alloc & allocator)
 {
   (void)allocator;

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

@@ -33,8 +33,6 @@ namespace executor
 
 struct AnyExecutable
 {
-  RCLCPP_SMART_PTR_DEFINITIONS(AnyExecutable)
-
   RCLCPP_PUBLIC
   AnyExecutable();
 
@@ -42,11 +40,11 @@ struct AnyExecutable
   virtual ~AnyExecutable();
 
   // Only one of the following pointers will be set.
-  rclcpp::subscription::SubscriptionBase::SharedPtr subscription;
-  rclcpp::subscription::SubscriptionBase::SharedPtr subscription_intra_process;
-  rclcpp::timer::TimerBase::SharedPtr timer;
-  rclcpp::service::ServiceBase::SharedPtr service;
-  rclcpp::client::ClientBase::SharedPtr client;
+  rclcpp::SubscriptionBase::SharedPtr subscription;
+  rclcpp::SubscriptionBase::SharedPtr subscription_intra_process;
+  rclcpp::TimerBase::SharedPtr timer;
+  rclcpp::ServiceBase::SharedPtr service;
+  rclcpp::ClientBase::SharedPtr client;
   // 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

@@ -27,22 +27,21 @@
 namespace rclcpp
 {
 
-namespace any_service_callback
-{
-
 template<typename ServiceT>
 class AnyServiceCallback
 {
 private:
-  using SharedPtrCallback = std::function<void(
-        const std::shared_ptr<typename ServiceT::Request>,
-        std::shared_ptr<typename ServiceT::Response>
-      )>;
-  using SharedPtrWithRequestHeaderCallback = std::function<void(
-        const std::shared_ptr<rmw_request_id_t>,
-        const std::shared_ptr<typename ServiceT::Request>,
-        std::shared_ptr<typename ServiceT::Response>
-      )>;
+  using SharedPtrCallback = std::function<
+    void(
+      const std::shared_ptr<typename ServiceT::Request>,
+      std::shared_ptr<typename ServiceT::Response>
+    )>;
+  using SharedPtrWithRequestHeaderCallback = std::function<
+    void(
+      const std::shared_ptr<rmw_request_id_t>,
+      const std::shared_ptr<typename ServiceT::Request>,
+      std::shared_ptr<typename ServiceT::Response>
+    )>;
 
   SharedPtrCallback shared_ptr_callback_;
   SharedPtrWithRequestHeaderCallback shared_ptr_with_request_header_callback_;
@@ -98,7 +97,6 @@ public:
   }
 };
 
-}  // namespace any_service_callback
 }  // namespace rclcpp
 
 #endif  // RCLCPP__ANY_SERVICE_CALLBACK_HPP_

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -30,9 +30,6 @@
 namespace rclcpp
 {
 
-namespace any_subscription_callback
-{
-
 template<typename MessageT, typename Alloc>
 class AnySubscriptionCallback
 {
@@ -43,13 +40,13 @@ class AnySubscriptionCallback
 
   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 &)>;
+    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 &)>;
+    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_;
@@ -209,7 +206,6 @@ private:
   MessageDeleter message_deleter_;
 };
 
-}  // namespace any_subscription_callback
 }  // namespace rclcpp
 
 #endif  // RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_

rclcpp/include/rclcpp/callback_group.hpp

@@ -59,19 +59,19 @@ public:
   explicit CallbackGroup(CallbackGroupType group_type);
 
   RCLCPP_PUBLIC
-  const std::vector<rclcpp::subscription::SubscriptionBase::WeakPtr> &
+  const std::vector<rclcpp::SubscriptionBase::WeakPtr> &
   get_subscription_ptrs() const;
 
   RCLCPP_PUBLIC
-  const std::vector<rclcpp::timer::TimerBase::WeakPtr> &
+  const std::vector<rclcpp::TimerBase::WeakPtr> &
   get_timer_ptrs() const;
 
   RCLCPP_PUBLIC
-  const std::vector<rclcpp::service::ServiceBase::WeakPtr> &
+  const std::vector<rclcpp::ServiceBase::WeakPtr> &
   get_service_ptrs() const;
 
   RCLCPP_PUBLIC
-  const std::vector<rclcpp::client::ClientBase::WeakPtr> &
+  const std::vector<rclcpp::ClientBase::WeakPtr> &
   get_client_ptrs() const;
 
   RCLCPP_PUBLIC
@@ -87,27 +87,27 @@ protected:
 
   RCLCPP_PUBLIC
   void
-  add_subscription(const rclcpp::subscription::SubscriptionBase::SharedPtr subscription_ptr);
+  add_subscription(const rclcpp::SubscriptionBase::SharedPtr subscription_ptr);
 
   RCLCPP_PUBLIC
   void
-  add_timer(const rclcpp::timer::TimerBase::SharedPtr timer_ptr);
+  add_timer(const rclcpp::TimerBase::SharedPtr timer_ptr);
 
   RCLCPP_PUBLIC
   void
-  add_service(const rclcpp::service::ServiceBase::SharedPtr service_ptr);
+  add_service(const rclcpp::ServiceBase::SharedPtr service_ptr);
 
   RCLCPP_PUBLIC
   void
-  add_client(const rclcpp::client::ClientBase::SharedPtr client_ptr);
+  add_client(const rclcpp::ClientBase::SharedPtr client_ptr);
 
   CallbackGroupType type_;
   // Mutex to protect the subsequent vectors of pointers.
   mutable std::mutex mutex_;
-  std::vector<rclcpp::subscription::SubscriptionBase::WeakPtr> subscription_ptrs_;
-  std::vector<rclcpp::timer::TimerBase::WeakPtr> timer_ptrs_;
-  std::vector<rclcpp::service::ServiceBase::WeakPtr> service_ptrs_;
-  std::vector<rclcpp::client::ClientBase::WeakPtr> client_ptrs_;
+  std::vector<rclcpp::SubscriptionBase::WeakPtr> subscription_ptrs_;
+  std::vector<rclcpp::TimerBase::WeakPtr> timer_ptrs_;
+  std::vector<rclcpp::ServiceBase::WeakPtr> service_ptrs_;
+  std::vector<rclcpp::ClientBase::WeakPtr> client_ptrs_;
   std::atomic_bool can_be_taken_from_;
 };
 

rclcpp/include/rclcpp/client.hpp

@@ -27,13 +27,17 @@
 #include "rcl/error_handling.h"
 #include "rcl/wait.h"
 
+#include "rclcpp/exceptions.hpp"
 #include "rclcpp/function_traits.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/utilities.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/visibility_control.hpp"
 
+#include "rcutils/logging_macros.h"
+
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 
@@ -45,9 +49,6 @@ namespace node_interfaces
 class NodeBaseInterface;
 }  // namespace node_interfaces
 
-namespace client
-{
-
 class ClientBase
 {
 public:
@@ -56,18 +57,21 @@ 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
-  const rcl_client_t *
+  std::shared_ptr<rcl_client_t>
+  get_client_handle();
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<const rcl_client_t>
   get_client_handle() const;
 
   RCLCPP_PUBLIC
@@ -98,13 +102,16 @@ protected:
 
   RCLCPP_PUBLIC
   rcl_node_t *
+  get_rcl_node_handle();
+
+  RCLCPP_PUBLIC
+  const rcl_node_t *
   get_rcl_node_handle() const;
 
   rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
   std::shared_ptr<rcl_node_t> node_handle_;
 
-  rcl_client_t client_handle_ = rcl_get_zero_initialized_client();
-  std::string service_name_;
+  std::shared_ptr<rcl_client_t> client_handle_;
 };
 
 template<typename ServiceT>
@@ -133,28 +140,34 @@ 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>();
-    if (rcl_client_init(&client_handle_, this->get_rcl_node_handle(),
-      service_type_support_handle, service_name.c_str(), &client_options) != RCL_RET_OK)
-    {
-      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
-      throw std::runtime_error(
-        std::string("could not create client: ") +
-        rcl_get_error_string_safe());
-      // *INDENT-ON*
+    rcl_ret_t ret = rcl_client_init(
+      this->get_client_handle().get(),
+      this->get_rcl_node_handle(),
+      service_type_support_handle,
+      service_name.c_str(),
+      &client_options);
+    if (ret != RCL_RET_OK) {
+      if (ret == RCL_RET_SERVICE_NAME_INVALID) {
+        auto rcl_node_handle = this->get_rcl_node_handle();
+        // this will throw on any validation problem
+        rcl_reset_error();
+        expand_topic_or_service_name(
+          service_name,
+          rcl_node_get_name(rcl_node_handle),
+          rcl_node_get_namespace(rcl_node_handle),
+          true);
+      }
+      rclcpp::exceptions::throw_from_rcl_error(ret, "could not create client");
     }
   }
 
   virtual ~Client()
   {
-    if (rcl_client_fini(&client_handle_, this->get_rcl_node_handle()) != RCL_RET_OK) {
-      fprintf(stderr,
-        "Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe());
-    }
   }
 
   std::shared_ptr<void>
@@ -172,15 +185,18 @@ public:
   }
 
   void
-  handle_response(std::shared_ptr<rmw_request_id_t> request_header,
+  handle_response(
+    std::shared_ptr<rmw_request_id_t> request_header,
     std::shared_ptr<void> response)
   {
-    std::lock_guard<std::mutex> lock(pending_requests_mutex_);
+    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];
@@ -188,6 +204,9 @@ public:
     auto callback = std::get<1>(tuple);
     auto future = std::get<2>(tuple);
     this->pending_requests_.erase(sequence_number);
+    // Unlock here to allow the service to be called recursively from one of its callbacks.
+    lock.unlock();
+
     call_promise->set_value(typed_response);
     callback(future);
   }
@@ -212,11 +231,9 @@ public:
   {
     std::lock_guard<std::mutex> lock(pending_requests_mutex_);
     int64_t sequence_number;
-    if (RCL_RET_OK != rcl_send_request(get_client_handle(), request.get(), &sequence_number)) {
-      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
-      throw std::runtime_error(
-        std::string("failed to send request: ") + rcl_get_error_string_safe());
-      // *INDENT-ON*
+    rcl_ret_t ret = rcl_send_request(get_client_handle().get(), request.get(), &sequence_number);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");
     }
 
     SharedPromise call_promise = std::make_shared<Promise>();
@@ -259,7 +276,6 @@ private:
   std::mutex pending_requests_mutex_;
 };
 
-}  // namespace client
 }  // namespace rclcpp
 
 #endif  // RCLCPP__CLIENT_HPP_

rclcpp/include/rclcpp/clock.hpp

@@ -0,0 +1,145 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__CLOCK_HPP_
+#define RCLCPP__CLOCK_HPP_
+
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <vector>
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/time.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcl/time.h"
+
+namespace rclcpp
+{
+
+class TimeSource;
+
+/// 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()> pre_callback;
+  std::function<void(const TimeJump &)> post_callback;
+  JumpThreshold notice_threshold;
+};
+
+class Clock
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(Clock)
+
+  RCLCPP_PUBLIC
+  explicit Clock(rcl_clock_type_t clock_type = RCL_SYSTEM_TIME);
+
+  RCLCPP_PUBLIC
+  ~Clock();
+
+  RCLCPP_PUBLIC
+  Time
+  now();
+
+  RCLCPP_PUBLIC
+  bool
+  ros_time_is_active();
+
+  RCLCPP_PUBLIC
+  rcl_clock_type_t
+  get_clock_type();
+
+  // Add a callback to invoke if the jump threshold is exceeded.
+  /**
+   * These callback functions must remain valid as long as the
+   * returned shared pointer is valid.
+   */
+  RCLCPP_PUBLIC
+  JumpHandler::SharedPtr
+  create_jump_callback(
+    std::function<void()> pre_callback,
+    std::function<void(const TimeJump &)> post_callback,
+    JumpThreshold & threshold);
+
+private:
+  // A method for TimeSource to get a list of callbacks to invoke while updating
+  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);
+
+  /// 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
+
+#endif  // RCLCPP__CLOCK_HPP_

rclcpp/include/rclcpp/context.hpp

@@ -30,9 +30,6 @@
 namespace rclcpp
 {
 
-namespace context
-{
-
 class Context
 {
 public:
@@ -52,13 +49,11 @@ public:
     auto it = sub_contexts_.find(type_i);
     if (it == sub_contexts_.end()) {
       // It doesn't exist yet, make it
-      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
       sub_context = std::shared_ptr<SubContext>(
         new SubContext(std::forward<Args>(args) ...),
-        [] (SubContext * sub_context_ptr) {
+        [](SubContext * sub_context_ptr) {
           delete sub_context_ptr;
         });
-      // *INDENT-ON*
       sub_contexts_[type_i] = sub_context;
     } else {
       // It exists, get it out and cast it.
@@ -74,7 +69,6 @@ private:
   std::mutex mutex_;
 };
 
-}  // namespace context
 }  // namespace rclcpp
 
 #endif  // RCLCPP__CONTEXT_HPP_

rclcpp/include/rclcpp/contexts/default_context.hpp

@@ -25,7 +25,7 @@ namespace contexts
 namespace default_context
 {
 
-class DefaultContext : public rclcpp::context::Context
+class DefaultContext : public rclcpp::Context
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(DefaultContext)

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

@@ -0,0 +1,106 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__DURATION_HPP_
+#define RCLCPP__DURATION_HPP_
+
+#include <chrono>
+
+#include "builtin_interfaces/msg/duration.hpp"
+#include "rcl/time.h"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+class Duration
+{
+public:
+  RCLCPP_PUBLIC
+  Duration(int32_t seconds, uint32_t nanoseconds);
+
+  RCLCPP_PUBLIC
+  explicit Duration(
+    rcl_duration_value_t nanoseconds);
+
+  RCLCPP_PUBLIC
+  explicit Duration(
+    std::chrono::nanoseconds nanoseconds);
+
+  RCLCPP_PUBLIC
+  Duration(
+    const builtin_interfaces::msg::Duration & duration_msg);
+
+  RCLCPP_PUBLIC
+  explicit Duration(const rcl_duration_t & duration);
+
+  RCLCPP_PUBLIC
+  Duration(const Duration & rhs);
+
+  RCLCPP_PUBLIC
+  virtual ~Duration();
+
+  RCLCPP_PUBLIC
+  operator builtin_interfaces::msg::Duration() const;
+
+  RCLCPP_PUBLIC
+  Duration &
+  operator=(const Duration & rhs);
+
+  RCLCPP_PUBLIC
+  Duration &
+  operator=(const builtin_interfaces::msg::Duration & Duration_msg);
+
+  RCLCPP_PUBLIC
+  bool
+  operator==(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator<(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator<=(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator>=(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator>(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  Duration
+  operator+(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  Duration
+  operator-(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  Duration
+  operator*(double scale) const;
+
+  RCLCPP_PUBLIC
+  rcl_duration_value_t
+  nanoseconds() const;
+
+private:
+  rcl_duration_t rcl_duration_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DURATION_HPP_

rclcpp/include/rclcpp/event.hpp

@@ -23,8 +23,6 @@
 
 namespace rclcpp
 {
-namespace event
-{
 
 class Event
 {
@@ -52,7 +50,6 @@ private:
   std::atomic_bool state_;
 };
 
-}  // namespace event
 }  // namespace rclcpp
 
 #endif  // RCLCPP__EVENT_HPP_

rclcpp/include/rclcpp/exceptions.hpp

@@ -35,18 +35,88 @@ public:
   : std::runtime_error("node is invalid") {}
 };
 
+/// Thrown when a any kind of name (node, namespace, topic, etc.) is invalid.
+class NameValidationError : public std::invalid_argument
+{
+public:
+  NameValidationError(
+    const char * name_type_,
+    const char * name_,
+    const char * error_msg_,
+    size_t invalid_index_)
+  : std::invalid_argument(format_error(name_type_, name_, error_msg_, invalid_index_)),
+    name_type(name_type_), name(name_), error_msg(error_msg_), invalid_index(invalid_index_)
+  {}
+
+  static std::string
+  format_error(
+    const char * name_type,
+    const char * name,
+    const char * error_msg,
+    size_t invalid_index);
+
+  const std::string name_type;
+  const std::string name;
+  const std::string error_msg;
+  const size_t invalid_index;
+};
+
+/// Thrown when a node name is invalid.
+class InvalidNodeNameError : public NameValidationError
+{
+public:
+  InvalidNodeNameError(const char * node_name, const char * error_msg, size_t invalid_index)
+  : NameValidationError("node name", node_name, error_msg, invalid_index)
+  {}
+};
+
+/// Thrown when a node namespace is invalid.
+class InvalidNamespaceError : public NameValidationError
+{
+public:
+  InvalidNamespaceError(const char * namespace_, const char * error_msg, size_t invalid_index)
+  : NameValidationError("namespace", namespace_, error_msg, invalid_index)
+  {}
+};
+
+/// Thrown when a topic name is invalid.
+class InvalidTopicNameError : public NameValidationError
+{
+public:
+  InvalidTopicNameError(const char * namespace_, const char * error_msg, size_t invalid_index)
+  : NameValidationError("topic name", namespace_, error_msg, invalid_index)
+  {}
+};
+
+/// Thrown when a service name is invalid.
+class InvalidServiceNameError : public NameValidationError
+{
+public:
+  InvalidServiceNameError(const char * namespace_, const char * error_msg, size_t invalid_index)
+  : NameValidationError("service name", namespace_, error_msg, invalid_index)
+  {}
+};
+
 /// Throw a C++ std::exception which was created based on an rcl error.
-/*
+/**
+ * Passing nullptr for reset_error is safe and will avoid calling any function
+ * to reset the error.
+ *
  * \param ret the return code for the current error state
  * \param prefix string to prefix to the error if applicable (not all errors have custom messages)
- * \param reset_error if true rcl_reset_error() is called before returning
+ * \param error_state error state to create exception from, if nullptr rcl_get_error_state is used
+ * \param reset_error function to be called before throwing which whill clear the error state
  * \throws std::invalid_argument if ret is RCL_RET_OK
  * \throws std::runtime_error if the rcl_get_error_state returns 0
  * \throws RCLErrorBase some child class exception based on ret
  */
 RCLCPP_PUBLIC
 void
-throw_from_rcl_error(rcl_ret_t ret, const std::string & prefix = "", bool reset_error = true);
+throw_from_rcl_error(
+  rcl_ret_t ret,
+  const std::string & prefix = "",
+  const rcl_error_state_t * error_state = nullptr,
+  void (*reset_error)() = rcl_reset_error);
 
 class RCLErrorBase
 {
@@ -111,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

@@ -38,10 +38,7 @@ namespace rclcpp
 {
 
 // Forward declaration is used in convenience method signature.
-namespace node
-{
 class Node;
-}  // namespace node
 
 namespace executor
 {
@@ -124,7 +121,7 @@ public:
   /// Convenience function which takes Node and forwards NodeBaseInterface.
   RCLCPP_PUBLIC
   virtual void
-  add_node(std::shared_ptr<rclcpp::node::Node> node_ptr, bool notify = true);
+  add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true);
 
   /// Remove a node from the executor.
   /**
@@ -140,7 +137,7 @@ public:
   /// Convenience function which takes Node and forwards NodeBaseInterface.
   RCLCPP_PUBLIC
   virtual void
-  remove_node(std::shared_ptr<rclcpp::node::Node> node_ptr, bool notify = true);
+  remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true);
 
   /// Add a node to executor, execute the next available unit of work, and remove the node.
   /**
@@ -151,7 +148,8 @@ public:
    */
   template<typename T = std::milli>
   void
-  spin_node_once(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
+  spin_node_once(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
     std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
   {
     return spin_node_once_nanoseconds(
@@ -161,9 +159,10 @@ public:
   }
 
   /// Convenience function which takes Node and forwards NodeBaseInterface.
-  template<typename NodeT = rclcpp::node::Node, typename T = std::milli>
+  template<typename NodeT = rclcpp::Node, typename T = std::milli>
   void
-  spin_node_once(std::shared_ptr<NodeT> node,
+  spin_node_once(
+    std::shared_ptr<NodeT> node,
     std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
   {
     return spin_node_once_nanoseconds(
@@ -183,7 +182,7 @@ public:
   /// Convenience function which takes Node and forwards NodeBaseInterface.
   RCLCPP_PUBLIC
   void
-  spin_node_some(std::shared_ptr<rclcpp::node::Node> node);
+  spin_node_some(std::shared_ptr<rclcpp::Node> node);
 
   /// Complete all available queued work without blocking.
   /**
@@ -202,14 +201,13 @@ public:
 
   /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
   /**
-   * \param[in] executor The executor which will spin the node.
-   * \param[in] node_ptr The node to spin.
    * \param[in] future The future to wait on. If SUCCESS, the future is safe to access after this
-     function.
+   *   function.
    * \param[in] timeout Optional timeout parameter, which gets passed to Executor::spin_node_once.
-     -1 is block forever, 0 is non-blocking.
-     If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return code.
-   * \return The return code, one of SUCCESS, INTERRUPTED, or TIMEOUT.
+   *   `-1` is block forever, `0` is non-blocking.
+   *   If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return
+   *   code.
+   * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
    */
   template<typename ResponseT, typename TimeT = std::milli>
   FutureReturnCode
@@ -235,7 +233,7 @@ public:
     }
     std::chrono::nanoseconds timeout_left = timeout_ns;
 
-    while (rclcpp::utilities::ok()) {
+    while (rclcpp::ok()) {
       // Do one item of work.
       spin_once(timeout_left);
       // Check if the future is set, return SUCCESS if it is.
@@ -289,29 +287,29 @@ protected:
    */
   RCLCPP_PUBLIC
   void
-  execute_any_executable(AnyExecutable::SharedPtr any_exec);
+  execute_any_executable(AnyExecutable & any_exec);
 
   RCLCPP_PUBLIC
   static void
   execute_subscription(
-    rclcpp::subscription::SubscriptionBase::SharedPtr subscription);
+    rclcpp::SubscriptionBase::SharedPtr subscription);
 
   RCLCPP_PUBLIC
   static void
   execute_intra_process_subscription(
-    rclcpp::subscription::SubscriptionBase::SharedPtr subscription);
+    rclcpp::SubscriptionBase::SharedPtr subscription);
 
   RCLCPP_PUBLIC
   static void
-  execute_timer(rclcpp::timer::TimerBase::SharedPtr timer);
+  execute_timer(rclcpp::TimerBase::SharedPtr timer);
 
   RCLCPP_PUBLIC
   static void
-  execute_service(rclcpp::service::ServiceBase::SharedPtr service);
+  execute_service(rclcpp::ServiceBase::SharedPtr service);
 
   RCLCPP_PUBLIC
   static void
-  execute_client(rclcpp::client::ClientBase::SharedPtr client);
+  execute_client(rclcpp::ClientBase::SharedPtr client);
 
   RCLCPP_PUBLIC
   void
@@ -323,19 +321,21 @@ protected:
 
   RCLCPP_PUBLIC
   rclcpp::callback_group::CallbackGroup::SharedPtr
-  get_group_by_timer(rclcpp::timer::TimerBase::SharedPtr timer);
+  get_group_by_timer(rclcpp::TimerBase::SharedPtr timer);
 
   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;
@@ -343,8 +343,8 @@ protected:
   /// Guard condition for signaling the rmw layer to wake up for special events.
   rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
 
-  /// Waitset for managing entities that the rmw layer waits on.
-  rcl_wait_set_t waitset_ = rcl_get_zero_initialized_wait_set();
+  /// Wait set for managing entities that the rmw layer waits on.
+  rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
 
   /// The memory strategy: an interface for handling user-defined memory allocation strategies.
   memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;

rclcpp/include/rclcpp/executors.hpp

@@ -28,40 +28,42 @@ namespace rclcpp
 {
 
 /// Create a default single-threaded executor and execute any immediately available work.
-// \param[in] node_ptr Shared pointer to the node to spin.
+/** \param[in] node_ptr Shared pointer to the node to spin. */
 RCLCPP_PUBLIC
 void
 spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
 
 RCLCPP_PUBLIC
 void
-spin_some(rclcpp::node::Node::SharedPtr node_ptr);
+spin_some(rclcpp::Node::SharedPtr node_ptr);
 
 /// Create a default single-threaded executor and spin the specified node.
-// \param[in] node_ptr Shared pointer to the node to spin.
+/** \param[in] node_ptr Shared pointer to the node to spin. */
 RCLCPP_PUBLIC
 void
 spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
 
 RCLCPP_PUBLIC
 void
-spin(rclcpp::node::Node::SharedPtr node_ptr);
+spin(rclcpp::Node::SharedPtr node_ptr);
 
 namespace executors
 {
 
-using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor;
-using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor;
+using rclcpp::executors::MultiThreadedExecutor;
+using rclcpp::executors::SingleThreadedExecutor;
 
 /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
 /**
  * \param[in] executor The executor which will spin the node.
  * \param[in] node_ptr The node to spin.
- * \param[in] future The future to wait on. If SUCCESS, the future is safe to access after this function
- * \param[in] timeout Optional timeout parameter, which gets passed to Executor::spin_node_once.
-   -1 is block forever, 0 is non-blocking.
-   If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return code.
- * \return The return code, one of SUCCESS, INTERRUPTED, or TIMEOUT.
+ * \param[in] future The future to wait on. If `SUCCESS`, the future is safe to
+ *   access after this function
+ * \param[in] timeout Optional timeout parameter, which gets passed to
+ *   Executor::spin_node_once.
+ *   `-1` is block forever, `0` is non-blocking.
+ *   If the time spent inside the blocking loop exceeds this timeout, return a `TIMEOUT` return code.
+ * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
  */
 template<typename ResponseT, typename TimeT = std::milli>
 rclcpp::executor::FutureReturnCode
@@ -79,7 +81,7 @@ spin_node_until_future_complete(
   return retcode;
 }
 
-template<typename NodeT = rclcpp::node::Node, typename ResponseT, typename TimeT = std::milli>
+template<typename NodeT = rclcpp::Node, typename ResponseT, typename TimeT = std::milli>
 rclcpp::executor::FutureReturnCode
 spin_node_until_future_complete(
   rclcpp::executor::Executor & executor,
@@ -107,7 +109,7 @@ spin_until_future_complete(
   return executors::spin_node_until_future_complete<FutureT>(executor, node_ptr, future, timeout);
 }
 
-template<typename NodeT = rclcpp::node::Node, typename FutureT, typename TimeT = std::milli>
+template<typename NodeT = rclcpp::Node, typename FutureT, typename TimeT = std::milli>
 rclcpp::executor::FutureReturnCode
 spin_until_future_complete(
   std::shared_ptr<NodeT> node_ptr,

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>
 
@@ -28,17 +30,28 @@ namespace rclcpp
 {
 namespace executors
 {
-namespace multi_threaded_executor
-{
 
 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 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 = rclcpp::executor::create_default_executor_arguments(),
+    size_t number_of_threads = 0,
+    bool yield_before_execute = false);
 
   RCLCPP_PUBLIC
   virtual ~MultiThreadedExecutor();
@@ -61,9 +74,12 @@ private:
 
   std::mutex wait_mutex_;
   size_t number_of_threads_;
+  bool yield_before_execute_;
+
+  std::mutex scheduled_timers_mutex_;
+  std::set<TimerBase::SharedPtr> scheduled_timers_;
 };
 
-}  // namespace multi_threaded_executor
 }  // namespace executors
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/executors/single_threaded_executor.hpp

@@ -34,8 +34,6 @@ namespace rclcpp
 {
 namespace executors
 {
-namespace single_threaded_executor
-{
 
 /// Single-threaded executor implementation
 // This is the default executor created by rclcpp::spin.
@@ -64,7 +62,6 @@ private:
   RCLCPP_DISABLE_COPY(SingleThreadedExecutor)
 };
 
-}  // namespace single_threaded_executor
 }  // namespace executors
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/expand_topic_or_service_name.hpp

@@ -0,0 +1,63 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__EXPAND_TOPIC_OR_SERVICE_NAME_HPP_
+#define RCLCPP__EXPAND_TOPIC_OR_SERVICE_NAME_HPP_
+
+#include <string>
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Expand a topic or service name and throw if it is not valid.
+/**
+ * This function can be used to "just" validate a topic or service name too,
+ * since expanding the topic name is required to fully validate a name.
+ *
+ * If the name is invalid, then InvalidTopicNameError is thrown or
+ * InvalidServiceNameError if is_service is true.
+ *
+ * This function can take any form of a topic or service name, i.e. it does not
+ * have to be a fully qualified name.
+ * The node name and namespace are used to expand it if necessary while
+ * validating it.
+ *
+ * Expansion is done with rcl_expand_topic_name.
+ * The validation is doen with rcl_validate_topic_name and
+ * rmw_validate_full_topic_name, so details about failures can be found in the
+ * documentation for those functions.
+ *
+ * \param name the topic or service name to be validated
+ * \param node_name the name of the node associated with the name
+ * \param namespace_ the namespace of the node associated with the name
+ * \param is_service if true InvalidServiceNameError is thrown instead
+ * \returns expanded (and validated) topic name
+ * \throws InvalidTopicNameError if name is invalid and is_service is false
+ * \throws InvalidServiceNameError if name is invalid and is_service is true
+ * \throws std::bad_alloc if memory cannot be allocated
+ * \throws RCLError if an unexpect error occurs
+ */
+RCLCPP_PUBLIC
+std::string
+expand_topic_or_service_name(
+  const std::string & name,
+  const std::string & node_name,
+  const std::string & namespace_,
+  bool is_service = false);
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPAND_TOPIC_OR_SERVICE_NAME_HPP_

rclcpp/include/rclcpp/function_traits.hpp

@@ -49,7 +49,7 @@ 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;
 
@@ -82,6 +82,8 @@ struct function_traits<ReturnTypeT (*)(Args ...)>: function_traits<ReturnTypeT(A
 template<typename ClassT, typename ReturnTypeT, typename ... Args, typename ... FArgs>
 #if defined _LIBCPP_VERSION  // libc++ (Clang)
 struct function_traits<std::__1::__bind<ReturnTypeT (ClassT::*)(Args ...), FArgs ...>>
+#elif defined _GLIBCXX_RELEASE  // glibc++ (GNU C++ >= 7.1)
+struct function_traits<std::_Bind<ReturnTypeT(ClassT::*(FArgs ...))(Args ...)>>
 #elif defined __GLIBCXX__  // glibc++ (GNU C++)
 struct function_traits<std::_Bind<std::_Mem_fn<ReturnTypeT (ClassT::*)(Args ...)>(FArgs ...)>>
 #elif defined _MSC_VER  // MS Visual Studio

rclcpp/include/rclcpp/graph_listener.hpp

@@ -68,7 +68,8 @@ public:
   virtual ~GraphListener();
 
   /// Start the graph listener's listen thread if it hasn't been started.
-  /* This function is thread-safe.
+  /**
+   * This function is thread-safe.
    *
    * \throws GraphListenerShutdownError if the GraphListener is shutdown
    */
@@ -78,7 +79,7 @@ public:
   start_if_not_started();
 
   /// Add a node to the graph listener's list of nodes.
-  /*
+  /**
    * \throws GraphListenerShutdownError if the GraphListener is shutdown
    * \throws NodeAlreadyAddedError if the given node is already in the list
    * \throws std::invalid_argument if node is nullptr
@@ -90,7 +91,8 @@ public:
   add_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph);
 
   /// Return true if the given node is in the graph listener's list of nodes.
-  /* Also return false if given nullptr.
+  /**
+   * Also return false if given nullptr.
    *
    * \throws std::system_error anything std::mutex::lock() throws
    */
@@ -100,7 +102,8 @@ public:
   has_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph);
 
   /// Remove a node from the graph listener's list of nodes.
-  /*
+  /**
+   *
    * \throws NodeNotFoundError if the given node is not in the list
    * \throws std::invalid_argument if node is nullptr
    * \throws std::system_error anything std::mutex::lock() throws
@@ -111,7 +114,8 @@ public:
   remove_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph);
 
   /// Stop the listening thread.
-  /* The thread cannot be restarted, and the class is defunct after calling.
+  /**
+   * The thread cannot be restarted, and the class is defunct after calling.
    * This function is called by the ~GraphListener() and does nothing if
    * shutdown() was already called.
    * This function exists separately from the ~GraphListener() so that it can

rclcpp/include/rclcpp/intra_process_manager.hpp

@@ -23,6 +23,7 @@
 #include <exception>
 #include <map>
 #include <memory>
+#include <mutex>
 #include <unordered_map>
 #include <set>
 
@@ -40,7 +41,8 @@ namespace intra_process_manager
 {
 
 /// This class facilitates intra process communication between nodes.
-/* This class is used in the creation of publishers and subscriptions.
+/**
+ * This class is used in the creation of publishers and subscriptions.
  * A singleton instance of this class is owned by a rclcpp::Context and a
  * rclcpp::Node can use an associated Context to get an instance of this class.
  * Nodes which do not have a common Context will not exchange intra process
@@ -54,7 +56,7 @@ namespace intra_process_manager
  * When a publisher is created, it advertises on the topic the user provided,
  * as well as a "shadowing" topic of type rcl_interfaces/IntraProcessMessage.
  * For instance, if the user specified the topic '/namespace/chatter', then the
- * corresponding intra process topic might be '/namespace/chatter__intra'.
+ * corresponding intra process topic might be '/namespace/chatter/_intra'.
  * The publisher is also allocated an id which is unique among all publishers
  * and subscriptions in this process.
  * Additionally, when registered with this class a ring buffer is created and
@@ -133,7 +135,8 @@ public:
   virtual ~IntraProcessManager();
 
   /// Register a subscription with the manager, returns subscriptions unique id.
-  /* In addition to generating a unique intra process id for the subscription,
+  /**
+   * In addition to generating a unique intra process id for the subscription,
    * this method also stores the topic name of the subscription.
    *
    * This method is normally called during the creation of a subscription,
@@ -146,10 +149,11 @@ public:
    */
   RCLCPP_PUBLIC
   uint64_t
-  add_subscription(subscription::SubscriptionBase::SharedPtr subscription);
+  add_subscription(SubscriptionBase::SharedPtr subscription);
 
   /// Unregister a subscription using the subscription's unique id.
-  /* This method does not allocate memory.
+  /**
+   * This method does not allocate memory.
    *
    * \param intra_process_subscription_id id of the subscription to remove.
    */
@@ -158,7 +162,8 @@ public:
   remove_subscription(uint64_t intra_process_subscription_id);
 
   /// Register a publisher with the manager, returns the publisher unique id.
-  /* In addition to generating and returning a unique id for the publisher,
+  /**
+   * In addition to generating and returning a unique id for the publisher,
    * this method creates internal ring buffer storage for "in-flight" intra
    * process messages which are stored when store_intra_process_message is
    * called with this publisher's unique id.
@@ -181,20 +186,23 @@ public:
    */
   template<typename MessageT, typename Alloc>
   uint64_t
-  add_publisher(typename publisher::Publisher<MessageT, Alloc>::SharedPtr publisher,
+  add_publisher(
+    typename Publisher<MessageT, Alloc>::SharedPtr publisher,
     size_t buffer_size = 0)
   {
     auto id = IntraProcessManager::get_next_unique_id();
     size_t size = buffer_size > 0 ? buffer_size : publisher->get_queue_size();
-    auto mrb = mapped_ring_buffer::MappedRingBuffer<MessageT,
-      typename publisher::Publisher<MessageT, Alloc>::MessageAlloc>::make_shared(
-      size, publisher->get_allocator());
+    auto mrb = mapped_ring_buffer::MappedRingBuffer<
+      MessageT,
+      typename Publisher<MessageT, Alloc>::MessageAlloc
+      >::make_shared(size, publisher->get_allocator());
     impl_->add_publisher(id, publisher, mrb, size);
     return id;
   }
 
   /// Unregister a publisher using the publisher's unique id.
-  /* This method does not allocate memory.
+  /**
+   * This method does not allocate memory.
    *
    * \param intra_process_publisher_id id of the publisher to remove.
    */
@@ -203,7 +211,8 @@ public:
   remove_publisher(uint64_t intra_process_publisher_id);
 
   /// Store a message in the manager, and return the message sequence number.
-  /* The given message is stored in internal storage using the given publisher
+  /**
+   * The given message is stored in internal storage using the given publisher
    * id and the newly generated message sequence, which is also returned.
    * The combination of publisher id and message sequence number can later
    * be used with a subscription id to retrieve the message by calling
@@ -232,8 +241,9 @@ public:
    * \param message the message that is being stored.
    * \return the message sequence number.
    */
-  template<typename MessageT, typename Alloc = std::allocator<void>,
-  typename Deleter = std::default_delete<MessageT>>
+  template<
+    typename MessageT, typename Alloc = std::allocator<void>,
+    typename Deleter = std::default_delete<MessageT>>
   uint64_t
   store_intra_process_message(
     uint64_t intra_process_publisher_id,
@@ -261,7 +271,8 @@ public:
   }
 
   /// Take an intra process message.
-  /* The intra_process_publisher_id and message_sequence_number parameters
+  /**
+   * The intra_process_publisher_id and message_sequence_number parameters
    * uniquely identify a message instance, which should be taken.
    *
    * The requesting_subscriptions_intra_process_id parameter is used to make
@@ -294,8 +305,9 @@ public:
    * \param requesting_subscriptions_intra_process_id the subscription's id.
    * \param message the message typed unique_ptr used to return the message.
    */
-  template<typename MessageT, typename Alloc = std::allocator<void>,
-  typename Deleter = std::default_delete<MessageT>>
+  template<
+    typename MessageT, typename Alloc = std::allocator<void>,
+    typename Deleter = std::default_delete<MessageT>>
   void
   take_intra_process_message(
     uint64_t intra_process_publisher_id,
@@ -308,6 +320,7 @@ public:
     message = nullptr;
 
     size_t target_subs_size = 0;
+    std::lock_guard<std::mutex> lock(take_mutex_);
     mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = impl_->take_intra_process_message(
       intra_process_publisher_id,
       message_sequence_number,
@@ -339,6 +352,7 @@ private:
   get_next_unique_id();
 
   IntraProcessManagerImplBase::SharedPtr impl_;
+  std::mutex take_mutex_;
 };
 
 }  // namespace intra_process_manager

rclcpp/include/rclcpp/intra_process_manager_impl.hpp

@@ -48,13 +48,14 @@ public:
   ~IntraProcessManagerImplBase() = default;
 
   virtual void
-  add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription) = 0;
+  add_subscription(uint64_t id, SubscriptionBase::SharedPtr subscription) = 0;
 
   virtual void
   remove_subscription(uint64_t intra_process_subscription_id) = 0;
 
-  virtual void add_publisher(uint64_t id,
-    publisher::PublisherBase::WeakPtr publisher,
+  virtual void add_publisher(
+    uint64_t id,
+    PublisherBase::WeakPtr publisher,
     mapped_ring_buffer::MappedRingBufferBase::SharedPtr mrb,
     size_t size) = 0;
 
@@ -70,7 +71,8 @@ public:
   store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq) = 0;
 
   virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr
-  take_intra_process_message(uint64_t intra_process_publisher_id,
+  take_intra_process_message(
+    uint64_t intra_process_publisher_id,
     uint64_t message_sequence_number,
     uint64_t requesting_subscriptions_intra_process_id,
     size_t & size) = 0;
@@ -90,7 +92,7 @@ public:
   ~IntraProcessManagerImpl() = default;
 
   void
-  add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription)
+  add_subscription(uint64_t id, SubscriptionBase::SharedPtr subscription)
   {
     subscriptions_[id] = subscription;
     // subscription->get_topic_name() -> const char * can be used as the key,
@@ -114,8 +116,9 @@ public:
     }
   }
 
-  void add_publisher(uint64_t id,
-    publisher::PublisherBase::WeakPtr publisher,
+  void add_publisher(
+    uint64_t id,
+    PublisherBase::WeakPtr publisher,
     mapped_ring_buffer::MappedRingBufferBase::SharedPtr mrb,
     size_t size)
   {
@@ -189,7 +192,8 @@ public:
   }
 
   mapped_ring_buffer::MappedRingBufferBase::SharedPtr
-  take_intra_process_message(uint64_t intra_process_publisher_id,
+  take_intra_process_message(
+    uint64_t intra_process_publisher_id,
     uint64_t message_sequence_number,
     uint64_t requesting_subscriptions_intra_process_id,
     size_t & size
@@ -253,9 +257,10 @@ private:
   RebindAlloc<uint64_t> uint64_allocator;
 
   using AllocSet = std::set<uint64_t, std::less<uint64_t>, RebindAlloc<uint64_t>>;
-  using SubscriptionMap = std::unordered_map<uint64_t, subscription::SubscriptionBase::WeakPtr,
-      std::hash<uint64_t>, std::equal_to<uint64_t>,
-      RebindAlloc<std::pair<const uint64_t, subscription::SubscriptionBase::WeakPtr>>>;
+  using SubscriptionMap = std::unordered_map<
+    uint64_t, SubscriptionBase::WeakPtr,
+    std::hash<uint64_t>, std::equal_to<uint64_t>,
+    RebindAlloc<std::pair<const uint64_t, SubscriptionBase::WeakPtr>>>;
 
   struct strcmp_wrapper : public std::binary_function<const char *, const char *, bool>
   {
@@ -266,10 +271,10 @@ private:
     }
   };
   using IDTopicMap = std::map<
-      const char *,
-      AllocSet,
-      strcmp_wrapper,
-      RebindAlloc<std::pair<const std::string, AllocSet>>>;
+    const char *,
+    AllocSet,
+    strcmp_wrapper,
+    RebindAlloc<std::pair<const char * const, AllocSet>>>;
 
   SubscriptionMap subscriptions_;
 
@@ -281,19 +286,21 @@ private:
 
     PublisherInfo() = default;
 
-    publisher::PublisherBase::WeakPtr publisher;
+    PublisherBase::WeakPtr publisher;
     std::atomic<uint64_t> sequence_number;
     mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer;
 
-    using TargetSubscriptionsMap = std::unordered_map<uint64_t, AllocSet,
-        std::hash<uint64_t>, std::equal_to<uint64_t>,
-        RebindAlloc<std::pair<const uint64_t, AllocSet>>>;
+    using TargetSubscriptionsMap = std::unordered_map<
+      uint64_t, AllocSet,
+      std::hash<uint64_t>, std::equal_to<uint64_t>,
+      RebindAlloc<std::pair<const uint64_t, AllocSet>>>;
     TargetSubscriptionsMap target_subscriptions_by_message_sequence;
   };
 
-  using PublisherMap = std::unordered_map<uint64_t, PublisherInfo,
-      std::hash<uint64_t>, std::equal_to<uint64_t>,
-      RebindAlloc<std::pair<const uint64_t, PublisherInfo>>>;
+  using PublisherMap = std::unordered_map<
+    uint64_t, PublisherInfo,
+    std::hash<uint64_t>, std::equal_to<uint64_t>,
+    RebindAlloc<std::pair<const uint64_t, PublisherInfo>>>;
 
   PublisherMap publishers_;
 

rclcpp/include/rclcpp/logger.hpp

@@ -0,0 +1,131 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__LOGGER_HPP_
+#define RCLCPP__LOGGER_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/visibility_control.hpp"
+
+/**
+ * \def RCLCPP_LOGGING_ENABLED
+ * When this define evaluates to true (default), logger factory functions will
+ * behave normally.
+ * When false, logger factory functions will create dummy loggers to avoid
+ * computational expense in manipulating objects.
+ * This should be used in combination with `RCLCPP_LOG_MIN_SEVERITY` to compile
+ * out logging macros.
+ */
+// TODO(dhood): determine this automatically from `RCLCPP_LOG_MIN_SEVERITY`
+#ifndef RCLCPP_LOGGING_ENABLED
+#define RCLCPP_LOGGING_ENABLED 1
+#endif
+
+namespace rclcpp
+{
+
+// Forward declaration is used for friend statement.
+namespace node_interfaces
+{
+class NodeLogging;
+}
+
+class Logger;
+
+/// Return a named logger.
+/**
+ * The returned logger's name will include any naming conventions, such as a
+ * name prefix.
+ * Currently there are no such naming conventions but they may be introduced in
+ * the future.
+ *
+ * \param[in] name the name of the logger
+ * \return a logger with the fully-qualified name including naming conventions, or
+ * \return a dummy logger if logging is disabled.
+ */
+RCLCPP_PUBLIC
+Logger
+get_logger(const std::string & name);
+
+class Logger
+{
+private:
+  friend Logger rclcpp::get_logger(const std::string & name);
+  friend ::rclcpp::node_interfaces::NodeLogging;
+
+  /// Constructor of a dummy logger.
+  /**
+   * This is used when logging is disabled: see `RCLCPP_LOGGING_ENABLED`.
+   * This cannot be called directly, see `rclcpp::get_logger` instead.
+   */
+  Logger()
+  : name_(nullptr) {}
+
+  /// Constructor of a named logger.
+  /**
+   * This cannot be called directly, see `rclcpp::get_logger` instead.
+   */
+  explicit Logger(const std::string & name)
+  : name_(new std::string(name)) {}
+
+  std::shared_ptr<const std::string> name_;
+
+public:
+  RCLCPP_PUBLIC
+  Logger(const Logger &) = default;
+
+  /// Get the name of this logger.
+  /**
+   * \return the full name of the logger including any prefixes, or
+   * \return `nullptr` if this logger is invalid (e.g. because logging is
+   *   disabled).
+   */
+  RCLCPP_PUBLIC
+  const char *
+  get_name() const
+  {
+    if (!name_) {
+      return nullptr;
+    }
+    return name_->c_str();
+  }
+
+  /// Return a logger that is a descendant of this logger.
+  /**
+   * The child logger's full name will include any hierarchy conventions that
+   * indicate it is a descendant of this logger.
+   * For example, ```get_logger('abc').get_child('def')``` will return a logger
+   * with name `abc.def`.
+   *
+   * \param[in] suffix the child logger's suffix
+   * \return a logger with the fully-qualified name including the suffix, or
+   * \return a dummy logger if this logger is invalid (e.g. because logging is
+   *   disabled).
+   */
+  RCLCPP_PUBLIC
+  Logger
+  get_child(const std::string & suffix)
+  {
+    if (!name_) {
+      return Logger();
+    }
+    return Logger(*name_ + "." + suffix);
+  }
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__LOGGER_HPP_

rclcpp/include/rclcpp/macros.hpp

@@ -18,7 +18,8 @@
 #ifndef RCLCPP__MACROS_HPP_
 #define RCLCPP__MACROS_HPP_
 
-/* Disables the copy constructor and operator= for the given class.
+/**
+ * Disables the copy constructor and operator= for the given class.
  *
  * Use in the private section of the class.
  */
@@ -26,38 +27,41 @@
   __VA_ARGS__(const __VA_ARGS__ &) = delete; \
   __VA_ARGS__ & operator=(const __VA_ARGS__ &) = delete;
 
-/* Defines aliases and static functions for using the Class with smart pointers.
+/**
+ * Defines aliases and static functions for using the Class with smart pointers.
  *
  * Use in the public section of the class.
- * Make sure to include <memory> in the header when using this.
+ * Make sure to include `<memory>` in the header when using this.
  */
 #define RCLCPP_SMART_PTR_DEFINITIONS(...) \
   RCLCPP_SHARED_PTR_DEFINITIONS(__VA_ARGS__) \
   RCLCPP_WEAK_PTR_DEFINITIONS(__VA_ARGS__) \
   RCLCPP_UNIQUE_PTR_DEFINITIONS(__VA_ARGS__)
 
-/* Defines aliases and static functions for using the Class with smart pointers.
+/**
+ * Defines aliases and static functions for using the Class with smart pointers.
  *
- * Same as RCLCPP_SMART_PTR_DEFINITIONS expect it excludes the static
+ * Same as RCLCPP_SMART_PTR_DEFINITIONS except it excludes the static
  * Class::make_unique() method definition which does not work on classes which
  * are not CopyConstructable.
  *
  * Use in the public section of the class.
- * Make sure to include <memory> in the header when using this.
+ * Make sure to include `<memory>` in the header when using this.
  */
 #define RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(...) \
   RCLCPP_SHARED_PTR_DEFINITIONS(__VA_ARGS__) \
   RCLCPP_WEAK_PTR_DEFINITIONS(__VA_ARGS__) \
   __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__)
 
-/* Defines aliases only for using the Class with smart pointers.
+/**
+ * Defines aliases only for using the Class with smart pointers.
  *
- * Same as RCLCPP_SMART_PTR_DEFINITIONS expect it excludes the static
+ * Same as RCLCPP_SMART_PTR_DEFINITIONS except it excludes the static
  * method definitions which do not work on pure virtual classes and classes
  * which are not CopyConstructable.
  *
  * Use in the public section of the class.
- * Make sure to include <memory> in the header when using this.
+ * Make sure to include `<memory>` in the header when using this.
  */
 #define RCLCPP_SMART_PTR_ALIASES_ONLY(...) \
   __RCLCPP_SHARED_PTR_ALIAS(__VA_ARGS__) \
@@ -106,6 +110,4 @@
 #define RCLCPP_STRING_JOIN(arg1, arg2) RCLCPP_DO_STRING_JOIN(arg1, arg2)
 #define RCLCPP_DO_STRING_JOIN(arg1, arg2) arg1 ## arg2
 
-#define RCLCPP_INFO(Args) std::cout << Args << std::endl;
-
 #endif  // RCLCPP__MACROS_HPP_

rclcpp/include/rclcpp/mapped_ring_buffer.hpp

@@ -39,7 +39,8 @@ public:
 };
 
 /// Ring buffer container of unique_ptr's of T, which can be accessed by a key.
-/* T must be a CopyConstructable and CopyAssignable.
+/**
+ * T must be a CopyConstructable and CopyAssignable.
  * This class can be used in a container by using the base class MappedRingBufferBase.
  * This class must have a positive, non-zero size.
  * This class cannot be resized nor can it reserve additional space after construction.
@@ -66,9 +67,11 @@ public:
   using ElemUniquePtr = std::unique_ptr<T, ElemDeleter>;
 
   /// Constructor.
-  /* The constructor will allocate memory while reserving space.
+  /**
+   * The constructor will allocate memory while reserving space.
    *
    * \param size size of the ring buffer; must be positive and non-zero.
+   * \param allocator optional custom allocator
    */
   explicit MappedRingBuffer(size_t size, std::shared_ptr<Alloc> allocator = nullptr)
   : elements_(size), head_(0)
@@ -86,7 +89,8 @@ public:
   virtual ~MappedRingBuffer() {}
 
   /// Return a copy of the value stored in the ring buffer at the given key.
-  /* The key is matched if an element in the ring buffer has a matching key.
+  /**
+   * The key is matched if an element in the ring buffer has a matching key.
    * This method will allocate in order to return a copy.
    *
    * The key is not guaranteed to be unique, see the class docs for more.
@@ -110,7 +114,8 @@ public:
   }
 
   /// Return ownership of the value stored in the ring buffer, leaving a copy.
-  /* The key is matched if an element in the ring bufer has a matching key.
+  /**
+   * The key is matched if an element in the ring bufer has a matching key.
    * This method will allocate in order to store a copy.
    *
    * The key is not guaranteed to be unique, see the class docs for more.
@@ -147,7 +152,8 @@ public:
   }
 
   /// Return ownership of the value stored in the ring buffer at the given key.
-  /* The key is matched if an element in the ring buffer has a matching key.
+  /**
+   * The key is matched if an element in the ring buffer has a matching key.
    *
    * The key is not guaranteed to be unique, see the class docs for more.
    *
@@ -169,7 +175,8 @@ public:
   }
 
   /// Insert a key-value pair, displacing an existing pair if necessary.
-  /* The key's uniqueness is not checked on insertion.
+  /**
+   * The key's uniqueness is not checked on insertion.
    * It is up to the user to ensure the key is unique.
    * This method should not allocate memory.
    *
@@ -221,11 +228,11 @@ private:
   typename std::vector<element, VectorAlloc>::iterator
   get_iterator_of_key(uint64_t key)
   {
-    // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
-    auto it = std::find_if(elements_.begin(), elements_.end(), [key](element & e) -> bool {
-      return e.key == key && e.in_use;
-    });
-    // *INDENT-ON*
+    auto it = std::find_if(
+      elements_.begin(), elements_.end(),
+      [key](element & e) -> bool {
+        return e.key == key && e.in_use;
+      });
     return it;
   }
 

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -51,59 +51,65 @@ public:
   virtual size_t number_of_ready_timers() const = 0;
   virtual size_t number_of_guard_conditions() const = 0;
 
-  virtual bool add_handles_to_waitset(rcl_wait_set_t * wait_set) = 0;
+  virtual 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,
+  get_next_subscription(
+    rclcpp::executor::AnyExecutable & any_exec,
     const WeakNodeVector & weak_nodes) = 0;
 
   virtual void
-  get_next_service(rclcpp::executor::AnyExecutable::SharedPtr any_exec,
+  get_next_service(
+    rclcpp::executor::AnyExecutable & any_exec,
     const WeakNodeVector & weak_nodes) = 0;
 
   virtual void
-  get_next_client(rclcpp::executor::AnyExecutable::SharedPtr any_exec,
+  get_next_client(
+    rclcpp::executor::AnyExecutable & any_exec,
     const WeakNodeVector & weak_nodes) = 0;
 
   virtual rcl_allocator_t
   get_allocator() = 0;
 
-  static rclcpp::subscription::SubscriptionBase::SharedPtr
-  get_subscription_by_handle(const rcl_subscription_t * subscriber_handle,
+  static rclcpp::SubscriptionBase::SharedPtr
+  get_subscription_by_handle(
+    std::shared_ptr<const rcl_subscription_t> subscriber_handle,
     const WeakNodeVector & weak_nodes);
 
-  static rclcpp::service::ServiceBase::SharedPtr
-  get_service_by_handle(const rcl_service_t * service_handle, const WeakNodeVector & weak_nodes);
+  static rclcpp::ServiceBase::SharedPtr
+  get_service_by_handle(
+    std::shared_ptr<const rcl_service_t> service_handle,
+    const WeakNodeVector & weak_nodes);
 
-  static rclcpp::client::ClientBase::SharedPtr
-  get_client_by_handle(const rcl_client_t * client_handle, const WeakNodeVector & weak_nodes);
+  static rclcpp::ClientBase::SharedPtr
+  get_client_by_handle(
+    std::shared_ptr<const rcl_client_t> client_handle,
+    const WeakNodeVector & weak_nodes);
 
   static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
-  get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group,
+  get_node_by_group(
+    rclcpp::callback_group::CallbackGroup::SharedPtr group,
     const WeakNodeVector & weak_nodes);
 
   static rclcpp::callback_group::CallbackGroup::SharedPtr
   get_group_by_subscription(
-    rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
+    rclcpp::SubscriptionBase::SharedPtr subscription,
     const WeakNodeVector & weak_nodes);
 
   static rclcpp::callback_group::CallbackGroup::SharedPtr
   get_group_by_service(
-    rclcpp::service::ServiceBase::SharedPtr service,
+    rclcpp::ServiceBase::SharedPtr service,
     const WeakNodeVector & weak_nodes);
 
   static rclcpp::callback_group::CallbackGroup::SharedPtr
-  get_group_by_client(rclcpp::client::ClientBase::SharedPtr client,
+  get_group_by_client(
+    rclcpp::ClientBase::SharedPtr client,
     const WeakNodeVector & weak_nodes);
 };
 

rclcpp/include/rclcpp/message_memory_strategy.hpp

@@ -18,17 +18,22 @@
 #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 "rmw/serialized_message.h"
+
 namespace rclcpp
 {
 namespace message_memory_strategy
 {
 
 /// Default allocation strategy for messages received by subscriptions.
-// A message memory strategy must be templated on the type of the subscription it belongs to.
+/** A message memory strategy must be templated on the type of the subscription it belongs to. */
 template<typename MessageT, typename Alloc = std::allocator<void>>
 class MessageMemoryStrategy
 {
@@ -39,14 +44,29 @@ 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());
   }
 
   /// Default factory method
@@ -56,21 +76,66 @@ public:
   }
 
   /// By default, dynamically allocate a new message.
-  // \return Shared pointer to the new message.
+  /** \return Shared pointer to the new message. */
   virtual std::shared_ptr<MessageT> borrow_message()
   {
     return std::allocate_shared<MessageT, MessageAlloc>(*message_allocator_.get());
   }
 
+  virtual std::shared_ptr<rcl_serialized_message_t> borrow_serialized_message(size_t capacity)
+  {
+    auto msg = new rcl_serialized_message_t;
+    *msg = rmw_get_zero_initialized_serialized_message();
+    auto ret = rmw_serialized_message_init(msg, capacity, &rcutils_allocator_);
+    if (ret != RCL_RET_OK) {
+      rclcpp::exceptions::throw_from_rcl_error(ret);
+    }
+
+    auto serialized_msg = std::shared_ptr<rcl_serialized_message_t>(msg,
+        [](rmw_serialized_message_t * msg) {
+          auto ret = rmw_serialized_message_fini(msg);
+          delete msg;
+          if (ret != RCL_RET_OK) {
+            rclcpp::exceptions::throw_from_rcl_error(ret, "leaking memory");
+          }
+        });
+
+    return serialized_msg;
+  }
+
+  virtual std::shared_ptr<rcl_serialized_message_t> borrow_serialized_message()
+  {
+    return borrow_serialized_message(default_buffer_capacity_);
+  }
+
+  virtual void set_default_buffer_capacity(size_t capacity)
+  {
+    default_buffer_capacity_ = capacity;
+  }
+
   /// Release ownership of the message, which will deallocate it if it has no more owners.
-  // \param[in] Shared pointer to the message we are returning.
+  /** \param[in] msg Shared pointer to the message we are returning. */
   virtual void return_message(std::shared_ptr<MessageT> & msg)
   {
     msg.reset();
   }
 
+  virtual void return_serialized_message(std::shared_ptr<rcl_serialized_message_t> & serialized_msg)
+  {
+    serialized_msg.reset();
+  }
+
   std::shared_ptr<MessageAlloc> message_allocator_;
   MessageDeleter message_deleter_;
+
+  std::shared_ptr<SerializedMessageAlloc> serialized_message_allocator_;
+  SerializedMessageDeleter serialized_message_deleter_;
+
+  std::shared_ptr<BufferAlloc> buffer_allocator_;
+  BufferDeleter buffer_deleter_;
+  size_t default_buffer_capacity_ = 0;
+
+  rcutils_allocator_t rcutils_allocator_;
 };
 
 }  // namespace message_memory_strategy

rclcpp/include/rclcpp/node.hpp

@@ -35,12 +35,16 @@
 
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/client.hpp"
+#include "rclcpp/clock.hpp"
 #include "rclcpp/context.hpp"
 #include "rclcpp/event.hpp"
+#include "rclcpp/logger.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/message_memory_strategy.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_clock_interface.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
+#include "rclcpp/node_interfaces/node_logging_interface.hpp"
 #include "rclcpp/node_interfaces/node_parameters_interface.hpp"
 #include "rclcpp/node_interfaces/node_services_interface.hpp"
 #include "rclcpp/node_interfaces/node_timers_interface.hpp"
@@ -49,15 +53,14 @@
 #include "rclcpp/publisher.hpp"
 #include "rclcpp/service.hpp"
 #include "rclcpp/subscription.hpp"
+#include "rclcpp/subscription_traits.hpp"
+#include "rclcpp/time.hpp"
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
 
-namespace node
-{
-
 /// Node is the single point of entry for creating publishers and subscribers.
 class Node : public std::enable_shared_from_this<Node>
 {
@@ -67,33 +70,60 @@ 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.
    */
   RCLCPP_PUBLIC
-  explicit Node(const std::string & node_name, bool use_intra_process_comms = false);
+  explicit Node(
+    const std::string & node_name,
+    const std::string & namespace_ = "",
+    bool use_intra_process_comms = false);
 
-  /// Create a node based on the node name and a rclcpp::context::Context.
+  /// Create a node based on the node name and a rclcpp::Context.
   /**
    * \param[in] node_name Name of the node.
+   * \param[in] namespace_ Namespace of the node.
    * \param[in] context The context for the node (usually represents the state of a process).
+   * \param[in] arguments Command line arguments that should apply only to this node.
+   * \param[in] initial_parameters a list of initial values for parameters on the node.
+   * This can be used to provide remapping rules that only affect one instance.
+   * \param[in] use_global_arguments False to prevent node using arguments passed to the process.
    * \param[in] use_intra_process_comms True to use the optimized intra-process communication
    * pipeline to pass messages between nodes in the same process using shared memory.
    */
   RCLCPP_PUBLIC
   Node(
-    const std::string & node_name, rclcpp::context::Context::SharedPtr context,
-    bool use_intra_process_comms = false);
+    const std::string & node_name,
+    const std::string & namespace_,
+    rclcpp::Context::SharedPtr context,
+    const std::vector<std::string> & arguments,
+    const std::vector<Parameter> & initial_parameters,
+    bool use_global_arguments = true,
+    bool use_intra_process_comms = false,
+    bool start_parameter_services = true);
 
   RCLCPP_PUBLIC
   virtual ~Node();
 
   /// Get the name of the node.
-  // \return The name of the node.
+  /** \return The name of the node. */
   RCLCPP_PUBLIC
   const char *
   get_name() const;
 
+  /// Get the namespace of the node.
+  /** \return The namespace of the node. */
+  RCLCPP_PUBLIC
+  const char *
+  get_namespace() const;
+
+  /// Get the logger of the node.
+  /** \return The logger of the node. */
+  RCLCPP_PUBLIC
+  rclcpp::Logger
+  get_logger() const;
+
   /// Create and return a callback group.
   RCLCPP_PUBLIC
   rclcpp::callback_group::CallbackGroup::SharedPtr
@@ -108,11 +138,12 @@ public:
   /**
    * \param[in] topic_name The topic for this publisher to publish on.
    * \param[in] qos_history_depth The depth of the publisher message queue.
+   * \param[in] allocator Optional custom allocator.
    * \return Shared pointer to the created publisher.
    */
   template<
     typename MessageT, typename Alloc = std::allocator<void>,
-    typename PublisherT = ::rclcpp::publisher::Publisher<MessageT, Alloc>>
+    typename PublisherT = ::rclcpp::Publisher<MessageT, Alloc>>
   std::shared_ptr<PublisherT>
   create_publisher(
     const std::string & topic_name, size_t qos_history_depth,
@@ -122,11 +153,12 @@ public:
   /**
    * \param[in] topic_name The topic for this publisher to publish on.
    * \param[in] qos_profile The quality of service profile to pass on to the rmw implementation.
+   * \param[in] allocator Optional custom allocator.
    * \return Shared pointer to the created publisher.
    */
   template<
     typename MessageT, typename Alloc = std::allocator<void>,
-    typename PublisherT = ::rclcpp::publisher::Publisher<MessageT, Alloc>>
+    typename PublisherT = ::rclcpp::Publisher<MessageT, Alloc>>
   std::shared_ptr<PublisherT>
   create_publisher(
     const std::string & topic_name,
@@ -141,6 +173,7 @@ public:
    * \param[in] group The callback group for this subscription. NULL for no callback group.
    * \param[in] ignore_local_publications True to ignore local publications.
    * \param[in] msg_mem_strat The message memory strategy to use for allocating messages.
+   * \param[in] allocator Optional custom allocator.
    * \return Shared pointer to the created subscription.
    */
   /* TODO(jacquelinekay):
@@ -151,7 +184,8 @@ public:
     typename MessageT,
     typename CallbackT,
     typename Alloc = std::allocator<void>,
-    typename SubscriptionT = rclcpp::subscription::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,
@@ -159,7 +193,8 @@ 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);
 
@@ -171,6 +206,7 @@ public:
    * \param[in] group The callback group for this subscription. NULL for no callback group.
    * \param[in] ignore_local_publications True to ignore local publications.
    * \param[in] msg_mem_strat The message memory strategy to use for allocating messages.
+   * \param[in] allocator Optional custom allocator.
    * \return Shared pointer to the created subscription.
    */
   /* TODO(jacquelinekay):
@@ -181,15 +217,17 @@ public:
     typename MessageT,
     typename CallbackT,
     typename Alloc = std::allocator<void>,
-    typename SubscriptionT = rclcpp::subscription::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,
-    size_t qos_history_depth,
     CallbackT && callback,
+    size_t qos_history_depth,
     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);
 
@@ -200,7 +238,7 @@ public:
    * \param[in] group Callback group to execute this timer's callback in.
    */
   template<typename DurationT = std::milli, typename CallbackT>
-  typename rclcpp::timer::WallTimer<CallbackT>::SharedPtr
+  typename rclcpp::WallTimer<CallbackT>::SharedPtr
   create_wall_timer(
     std::chrono::duration<int64_t, DurationT> period,
     CallbackT callback,
@@ -208,7 +246,7 @@ public:
 
   /* Create and return a Client. */
   template<typename ServiceT>
-  typename rclcpp::client::Client<ServiceT>::SharedPtr
+  typename rclcpp::Client<ServiceT>::SharedPtr
   create_client(
     const std::string & service_name,
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default,
@@ -216,7 +254,7 @@ public:
 
   /* Create and return a Service. */
   template<typename ServiceT, typename CallbackT>
-  typename rclcpp::service::Service<ServiceT>::SharedPtr
+  typename rclcpp::Service<ServiceT>::SharedPtr
   create_service(
     const std::string & service_name,
     CallbackT && callback,
@@ -225,30 +263,62 @@ 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
+  set_parameter_if_not_set(
+    const std::string & name,
+    const ParameterT & value);
 
   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.
+  /**
+   * If the parameter was not set, then the "parameter" argument is never assigned a value.
+   *
+   * \param[in] name The name of the parameter to get.
+   * \param[out] parameter The output where the value of the parameter should be assigned.
+   * \returns true if the parameter was set, false otherwise
+   */
   template<typename ParameterT>
   bool
   get_parameter(const std::string & name, ParameterT & parameter) const;
 
+  /// Get the parameter value, or the "alternative value" if not set, and assign it to "value".
+  /**
+   * If the parameter was not set, then the "value" argument is assigned
+   * the "alternative_value".
+   * In all cases, the parameter remains not set after this function is called.
+   *
+   * \param[in] name The name of the parameter to get.
+   * \param[out] value The output where the value of the parameter should be assigned.
+   * \param[in] alternative_value Value to be stored in output if the parameter was not set.
+   * \returns true if the parameter was set, false otherwise
+   */
+  template<typename ParameterT>
+  bool
+  get_parameter_or(
+    const std::string & name,
+    ParameterT & value,
+    const ParameterT & alternative_value) const;
+
   RCLCPP_PUBLIC
   std::vector<rcl_interfaces::msg::ParameterDescriptor>
   describe_parameters(const std::vector<std::string> & names) const;
@@ -263,7 +333,8 @@ public:
 
   /// Register the callback for parameter changes
   /**
-   * \param[in] User defined callback function, It is expected to atomically set parameters.
+   * \param[in] callback User defined callback function.
+   *   It is expected to atomically set parameters.
    * \note Repeated invocations of this function will overwrite previous callbacks
    */
   template<typename CallbackT>
@@ -271,9 +342,13 @@ public:
   register_param_change_callback(CallbackT && callback);
 
   RCLCPP_PUBLIC
-  std::map<std::string, std::string>
+  std::map<std::string, std::vector<std::string>>
   get_topic_names_and_types() const;
 
+  RCLCPP_PUBLIC
+  std::map<std::string, std::vector<std::string>>
+  get_service_names_and_types() const;
+
   RCLCPP_PUBLIC
   size_t
   count_publishers(const std::string & topic_name) const;
@@ -284,15 +359,16 @@ 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
-  rclcpp::event::Event::SharedPtr
+  rclcpp::Event::SharedPtr
   get_graph_event();
 
   /// Wait for a graph event to occur by waiting on an Event to become set.
-  /* The given Event must be acquire through the get_graph_event() method.
+  /**
+   * The given Event must be acquire through the get_graph_event() method.
    *
    * \throws InvalidEventError if the given event is nullptr
    * \throws EventNotRegisteredError if the given event was not acquired with
@@ -301,14 +377,27 @@ public:
   RCLCPP_PUBLIC
   void
   wait_for_graph_change(
-    rclcpp::event::Event::SharedPtr event,
+    rclcpp::Event::SharedPtr event,
     std::chrono::nanoseconds timeout);
 
+  RCLCPP_PUBLIC
+  rclcpp::Clock::SharedPtr
+  get_clock();
+
+  RCLCPP_PUBLIC
+  Time
+  now();
+
   /// Return the Node's internal NodeBaseInterface implementation.
   RCLCPP_PUBLIC
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
   get_node_base_interface();
 
+  /// Return the Node's internal NodeClockInterface implementation.
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr
+  get_node_clock_interface();
+
   /// Return the Node's internal NodeGraphInterface implementation.
   RCLCPP_PUBLIC
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr
@@ -343,15 +432,16 @@ private:
 
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
   rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_;
   rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
   rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
 
   bool use_intra_process_comms_;
 };
 
-}  // namespace node
 }  // namespace rclcpp
 
 #ifndef RCLCPP__NODE_IMPL_HPP_

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"
@@ -49,8 +50,6 @@
 
 namespace rclcpp
 {
-namespace node
-{
 
 template<typename MessageT, typename Alloc, typename PublisherT>
 std::shared_ptr<PublisherT>
@@ -83,7 +82,11 @@ Node::create_publisher(
     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,
@@ -91,20 +94,23 @@ 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)
 {
+  using CallbackMessageT = typename rclcpp::subscription_traits::has_message_type<CallbackT>::type;
+
   if (!allocator) {
     allocator = std::make_shared<Alloc>();
   }
 
   if (!msg_mem_strat) {
     using rclcpp::message_memory_strategy::MessageMemoryStrategy;
-    msg_mem_strat = MessageMemoryStrategy<MessageT, Alloc>::create_default();
+    msg_mem_strat = MessageMemoryStrategy<CallbackMessageT, Alloc>::create_default();
   }
 
-  return rclcpp::create_subscription<MessageT, CallbackT, Alloc, SubscriptionT>(
+  return rclcpp::create_subscription<MessageT, CallbackT, Alloc, CallbackMessageT, SubscriptionT>(
     this->node_topics_.get(),
     topic_name,
     std::forward<CallbackT>(callback),
@@ -116,21 +122,26 @@ Node::create_subscription(
     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,
-  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)
 {
   rmw_qos_profile_t qos = rmw_qos_profile_default;
   qos.depth = qos_history_depth;
-  return this->create_subscription<MessageT, CallbackT, Alloc, SubscriptionT>(
+  return this->create_subscription<MessageT>(
     topic_name,
     std::forward<CallbackT>(callback),
     qos,
@@ -141,13 +152,13 @@ Node::create_subscription(
 }
 
 template<typename DurationT, typename CallbackT>
-typename rclcpp::timer::WallTimer<CallbackT>::SharedPtr
+typename rclcpp::WallTimer<CallbackT>::SharedPtr
 Node::create_wall_timer(
   std::chrono::duration<int64_t, DurationT> period,
   CallbackT callback,
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
-  auto timer = rclcpp::timer::WallTimer<CallbackT>::make_shared(
+  auto timer = rclcpp::WallTimer<CallbackT>::make_shared(
     std::chrono::duration_cast<std::chrono::nanoseconds>(period),
     std::move(callback));
   node_timers_->add_timer(timer, group);
@@ -155,7 +166,7 @@ Node::create_wall_timer(
 }
 
 template<typename ServiceT>
-typename client::Client<ServiceT>::SharedPtr
+typename Client<ServiceT>::SharedPtr
 Node::create_client(
   const std::string & service_name,
   const rmw_qos_profile_t & qos_profile,
@@ -164,8 +175,8 @@ Node::create_client(
   rcl_client_options_t options = rcl_client_get_default_options();
   options.qos = qos_profile;
 
-  using rclcpp::client::Client;
-  using rclcpp::client::ClientBase;
+  using rclcpp::Client;
+  using rclcpp::ClientBase;
 
   auto cli = Client<ServiceT>::make_shared(
     node_base_.get(),
@@ -179,25 +190,16 @@ Node::create_client(
 }
 
 template<typename ServiceT, typename CallbackT>
-typename rclcpp::service::Service<ServiceT>::SharedPtr
+typename rclcpp::Service<ServiceT>::SharedPtr
 Node::create_service(
   const std::string & service_name,
   CallbackT && callback,
   const rmw_qos_profile_t & qos_profile,
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
-  rclcpp::service::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::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<service::ServiceBase>(serv);
-  node_services_->add_service(serv_base_ptr, group);
-  return serv;
+  return rclcpp::create_service<ServiceT, CallbackT>(
+    node_base_, node_services_,
+    service_name, std::forward<CallbackT>(callback), qos_profile, group);
 }
 
 template<typename CallbackT>
@@ -208,17 +210,46 @@ Node::register_param_change_callback(CallbackT && callback)
 }
 
 template<typename ParameterT>
-bool
-Node::get_parameter(const std::string & name, ParameterT & parameter) const
+void
+Node::set_parameter_if_not_set(
+  const std::string & name,
+  const ParameterT & value)
 {
-  rclcpp::parameter::ParameterVariant parameter_variant(name, parameter);
-  bool result = get_parameter(name, parameter_variant);
-  parameter = parameter_variant.get_value<ParameterT>();
+  rclcpp::Parameter parameter;
+  if (!this->get_parameter(name, parameter)) {
+    this->set_parameters({
+        rclcpp::Parameter(name, value),
+      });
+  }
+}
+
+template<typename ParameterT>
+bool
+Node::get_parameter(const std::string & name, ParameterT & value) const
+{
+  rclcpp::Parameter parameter;
+  bool result = get_parameter(name, parameter);
+  if (result) {
+    value = parameter.get_value<ParameterT>();
+  }
 
   return result;
 }
 
-}  // namespace node
+template<typename ParameterT>
+bool
+Node::get_parameter_or(
+  const std::string & name,
+  ParameterT & value,
+  const ParameterT & alternative_value) const
+{
+  bool got_parameter = get_parameter(name, value);
+  if (!got_parameter) {
+    value = alternative_value;
+  }
+  return got_parameter;
+}
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__NODE_IMPL_HPP_

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -36,7 +36,12 @@ public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBaseInterface)
 
   RCLCPP_PUBLIC
-  NodeBase(const std::string & node_name, rclcpp::context::Context::SharedPtr context);
+  NodeBase(
+    const std::string & node_name,
+    const std::string & namespace_,
+    rclcpp::Context::SharedPtr context,
+    const std::vector<std::string> & arguments,
+    bool use_global_arguments);
 
   RCLCPP_PUBLIC
   virtual
@@ -49,7 +54,12 @@ public:
 
   RCLCPP_PUBLIC
   virtual
-  rclcpp::context::Context::SharedPtr
+  const char *
+  get_namespace() const;
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::Context::SharedPtr
   get_context();
 
   RCLCPP_PUBLIC
@@ -110,7 +120,7 @@ public:
 private:
   RCLCPP_DISABLE_COPY(NodeBase)
 
-  rclcpp::context::Context::SharedPtr context_;
+  rclcpp::Context::SharedPtr context_;
 
   std::shared_ptr<rcl_node_t> node_handle_;
 

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -39,17 +39,24 @@ public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBaseInterface)
 
   /// Return the name of the node.
-  // \return The name of the node.
+  /** \return The name of the node. */
   RCLCPP_PUBLIC
   virtual
   const char *
   get_name() const = 0;
 
-  /// Return the context of the node.
-  // \return SharedPtr to the node's context.
+  /// Return the namespace of the node.
+  /** \return The namespace of the node. */
   RCLCPP_PUBLIC
   virtual
-  rclcpp::context::Context::SharedPtr
+  const char *
+  get_namespace() const = 0;
+
+  /// Return the context of the node.
+  /** \return SharedPtr to the node's context. */
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::Context::SharedPtr
   get_context() = 0;
 
   /// Return the rcl_node_t node handle (non-const version).
@@ -65,7 +72,8 @@ public:
   get_rcl_node_handle() const = 0;
 
   /// Return the rcl_node_t node handle in a std::shared_ptr.
-  /* This handle remains valid after the Node is destroyed.
+  /**
+   * This handle remains valid after the Node is destroyed.
    * The actual rcl node is not finalized until it is out of scope everywhere.
    */
   RCLCPP_PUBLIC
@@ -74,7 +82,8 @@ public:
   get_shared_rcl_node_handle() = 0;
 
   /// Return the rcl_node_t node handle in a std::shared_ptr.
-  /* This handle remains valid after the Node is destroyed.
+  /**
+   * This handle remains valid after the Node is destroyed.
    * The actual rcl node is not finalized until it is out of scope everywhere.
    */
   RCLCPP_PUBLIC
@@ -113,7 +122,8 @@ public:
   get_associated_with_executor_atomic() = 0;
 
   /// Return guard condition that should be notified when the internal node state changes.
-  /* For example, this should be notified when a publisher is added or removed.
+  /**
+   * For example, this should be notified when a publisher is added or removed.
    *
    * \return the rcl_guard_condition_t if it is valid, else nullptr
    */
@@ -123,7 +133,7 @@ public:
   get_notify_guard_condition() = 0;
 
   /// Acquire and return a scoped lock that protects the notify guard condition.
-  /* This should be used when triggering the notify guard condition. */
+  /** This should be used when triggering the notify guard condition. */
   RCLCPP_PUBLIC
   virtual
   std::unique_lock<std::recursive_mutex>

rclcpp/include/rclcpp/node_interfaces/node_clock.hpp

@@ -0,0 +1,71 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_clock_interface.hpp"
+#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/time_source.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeClock part of the Node API.
+class NodeClock : public NodeClockInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeClock)
+
+  RCLCPP_PUBLIC
+  explicit NodeClock(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
+    rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeClock();
+
+  /// Get a clock which will be kept up to date by the node.
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::Clock::SharedPtr
+  get_clock();
+
+private:
+  RCLCPP_DISABLE_COPY(NodeClock)
+
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
+  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
+
+  rclcpp::Clock::SharedPtr ros_clock_;
+  rclcpp::TimeSource time_source_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_

rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp

@@ -0,0 +1,44 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeClock part of the Node API.
+class NodeClockInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeClockInterface)
+
+  /// Get a ROS clock which will be kept up to date by the node.
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::Clock::SharedPtr
+  get_clock() = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

@@ -1,4 +1,4 @@
-// Copyright 2016 Open Source Robotics Foundation, Inc.
+// Copyright 2016-2017 Open Source Robotics Foundation, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -56,8 +56,18 @@ public:
 
   RCLCPP_PUBLIC
   virtual
-  std::map<std::string, std::string>
-  get_topic_names_and_types() const;
+  std::map<std::string, std::vector<std::string>>
+  get_topic_names_and_types(bool no_demangle = false) const;
+
+  RCLCPP_PUBLIC
+  virtual
+  std::map<std::string, std::vector<std::string>>
+  get_service_names_and_types() const;
+
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<std::string>
+  get_node_names() const;
 
   RCLCPP_PUBLIC
   virtual
@@ -86,14 +96,14 @@ public:
 
   RCLCPP_PUBLIC
   virtual
-  rclcpp::event::Event::SharedPtr
+  rclcpp::Event::SharedPtr
   get_graph_event();
 
   RCLCPP_PUBLIC
   virtual
   void
   wait_for_graph_change(
-    rclcpp::event::Event::SharedPtr event,
+    rclcpp::Event::SharedPtr event,
     std::chrono::nanoseconds timeout);
 
   RCLCPP_PUBLIC
@@ -117,9 +127,9 @@ private:
   /// For notifying waiting threads (wait_for_graph_change()) on changes (notify_graph_change()).
   std::condition_variable graph_cv_;
   /// Weak references to graph events out on loan.
-  std::vector<rclcpp::event::Event::WeakPtr> graph_events_;
+  std::vector<rclcpp::Event::WeakPtr> graph_events_;
   /// Number of graph events out on loan, used to determine if the graph should be monitored.
-  /* graph_users_count_ is atomic so that it can be accessed without acquiring the graph_mutex_ */
+  /** graph_users_count_ is atomic so that it can be accessed without acquiring the graph_mutex_ */
   std::atomic_size_t graph_users_count_;
 };
 

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

@@ -1,4 +1,4 @@
-// Copyright 2016 Open Source Robotics Foundation, Inc.
+// Copyright 2016-2017 Open Source Robotics Foundation, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -18,6 +18,7 @@
 #include <chrono>
 #include <map>
 #include <string>
+#include <vector>
 
 #include "rcl/guard_condition.h"
 
@@ -36,14 +37,34 @@ class NodeGraphInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeGraphInterface)
 
-  /// Return a map of existing topic names (string) to topic types (string).
-  /* A topic is considered to exist when at least one publisher or subscriber
+  /// Return a map of existing topic names to list of topic types.
+  /**
+   * A topic is considered to exist when at least one publisher or subscriber
    * exists for it, whether they be local or remote to this process.
+   *
+   * \param[in] no_demangle if true, topic names and types are not demangled
    */
   RCLCPP_PUBLIC
   virtual
-  std::map<std::string, std::string>
-  get_topic_names_and_types() const = 0;
+  std::map<std::string, std::vector<std::string>>
+  get_topic_names_and_types(bool no_demangle = false) const = 0;
+
+  /// Return a map of existing service names to list of service types.
+  /**
+   * A service is considered to exist when at least one service server or
+   * service client exists for it, whether they be local or remote to this
+   * process.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::map<std::string, std::vector<std::string>>
+  get_service_names_and_types() const = 0;
+
+  /// Return a vector of existing node names (string).
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<std::string>
+  get_node_names() const = 0;
 
   /// Return the number of publishers that are advertised on a given topic.
   RCLCPP_PUBLIC
@@ -64,7 +85,8 @@ public:
   get_graph_guard_condition() const = 0;
 
   /// Notify threads waiting on graph changes.
-  /* Affects threads waiting on the notify guard condition, see:
+  /**
+   * Affects threads waiting on the notify guard condition, see:
    * get_notify_guard_condition(), as well as the threads waiting on graph
    * changes using a graph Event, see: wait_for_graph_change().
    *
@@ -84,17 +106,19 @@ public:
   notify_shutdown() = 0;
 
   /// Return a graph event, which will be set anytime a graph change occurs.
-  /* The graph Event object is a loan which must be returned.
+  /**
+   * The graph Event object is a loan which must be returned.
    * The Event object is scoped and therefore to return the load just let it go
    * out of scope.
    */
   RCLCPP_PUBLIC
   virtual
-  rclcpp::event::Event::SharedPtr
+  rclcpp::Event::SharedPtr
   get_graph_event() = 0;
 
   /// Wait for a graph event to occur by waiting on an Event to become set.
-  /* The given Event must be acquire through the get_graph_event() method.
+  /**
+   * The given Event must be acquire through the get_graph_event() method.
    *
    * \throws InvalidEventError if the given event is nullptr
    * \throws EventNotRegisteredError if the given event was not acquired with
@@ -104,11 +128,12 @@ public:
   virtual
   void
   wait_for_graph_change(
-    rclcpp::event::Event::SharedPtr event,
+    rclcpp::Event::SharedPtr event,
     std::chrono::nanoseconds timeout) = 0;
 
   /// Return the number of on loan graph events, see get_graph_event().
-  /* This is typically only used by the rclcpp::graph_listener::GraphListener.
+  /**
+   * This is typically only used by the rclcpp::graph_listener::GraphListener.
    */
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/node_interfaces/node_logging.hpp

@@ -0,0 +1,66 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_LOGGING_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_LOGGING_HPP_
+
+#include <memory>
+
+#include "rclcpp/logger.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_logging_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeLogging part of the Node API.
+class NodeLogging : public NodeLoggingInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeLoggingInterface)
+
+  RCLCPP_PUBLIC
+  explicit NodeLogging(rclcpp::node_interfaces::NodeBaseInterface * node_base);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeLogging();
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::Logger
+  get_logger() const;
+
+  RCLCPP_PUBLIC
+  virtual
+  const char *
+  get_logger_name() const;
+
+private:
+  RCLCPP_DISABLE_COPY(NodeLogging)
+
+  /// Handle to the NodeBaseInterface given in the constructor.
+  rclcpp::node_interfaces::NodeBaseInterface * node_base_;
+
+  rclcpp::Logger logger_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_LOGGING_HPP_

rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp

@@ -0,0 +1,54 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_LOGGING_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_LOGGING_INTERFACE_HPP_
+
+#include <memory>
+
+#include "rclcpp/logger.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeLogging part of the Node API.
+class NodeLoggingInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeLoggingInterface)
+
+  /// Return the logger of the node.
+  /** \return The logger of the node. */
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::Logger
+  get_logger() const = 0;
+
+  /// Return the logger name associated with the node.
+  /** \return The logger name associated with the node. */
+  RCLCPP_PUBLIC
+  virtual
+  const char *
+  get_logger_name() const = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_LOGGING_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -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,12 @@ 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 std::vector<Parameter> & initial_parameters,
+    bool use_intra_process,
+    bool start_parameter_services);
 
   RCLCPP_PUBLIC
   virtual
@@ -57,22 +63,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 +86,7 @@ public:
   bool
   get_parameter(
     const std::string & name,
-    rclcpp::parameter::ParameterVariant & parameter) const;
+    rclcpp::Parameter & parameter) const;
 
   RCLCPP_PUBLIC
   virtual
@@ -105,15 +111,15 @@ 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::Publisher<rcl_interfaces::msg::ParameterEvent>::SharedPtr events_publisher_;
+  Publisher<rcl_interfaces::msg::ParameterEvent>::SharedPtr events_publisher_;
+
+  std::shared_ptr<ParameterService> parameter_service_;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -41,30 +41,49 @@ 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;
 
   RCLCPP_PUBLIC
   virtual
@@ -81,9 +100,9 @@ public:
   rcl_interfaces::msg::ListParametersResult
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const = 0;
 
-  using ParametersCallbackFunction =
-      std::function<rcl_interfaces::msg::SetParametersResult(
-        const std::vector<rclcpp::parameter::ParameterVariant> &)>;
+  using ParametersCallbackFunction = std::function<
+    rcl_interfaces::msg::SetParametersResult(
+      const std::vector<rclcpp::Parameter> &)>;
 
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/node_interfaces/node_services.hpp

@@ -45,14 +45,14 @@ public:
   virtual
   void
   add_client(
-    rclcpp::client::ClientBase::SharedPtr client_base_ptr,
+    rclcpp::ClientBase::SharedPtr client_base_ptr,
     rclcpp::callback_group::CallbackGroup::SharedPtr group);
 
   RCLCPP_PUBLIC
   virtual
   void
   add_service(
-    rclcpp::service::ServiceBase::SharedPtr service_base_ptr,
+    rclcpp::ServiceBase::SharedPtr service_base_ptr,
     rclcpp::callback_group::CallbackGroup::SharedPtr group);
 
 private:

rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp

@@ -36,14 +36,14 @@ public:
   virtual
   void
   add_client(
-    rclcpp::client::ClientBase::SharedPtr client_base_ptr,
+    rclcpp::ClientBase::SharedPtr client_base_ptr,
     rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0;
 
   RCLCPP_PUBLIC
   virtual
   void
   add_service(
-    rclcpp::service::ServiceBase::SharedPtr service_base_ptr,
+    rclcpp::ServiceBase::SharedPtr service_base_ptr,
     rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0;
 };
 

rclcpp/include/rclcpp/node_interfaces/node_timers.hpp

@@ -45,7 +45,7 @@ public:
   virtual
   void
   add_timer(
-    rclcpp::timer::TimerBase::SharedPtr timer,
+    rclcpp::TimerBase::SharedPtr timer,
     rclcpp::callback_group::CallbackGroup::SharedPtr callback_group);
 
 private:

rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp

@@ -36,7 +36,7 @@ public:
   virtual
   void
   add_timer(
-    rclcpp::timer::TimerBase::SharedPtr timer,
+    rclcpp::TimerBase::SharedPtr timer,
     rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0;
 };
 

rclcpp/include/rclcpp/node_interfaces/node_topics.hpp

@@ -47,7 +47,7 @@ public:
 
   RCLCPP_PUBLIC
   virtual
-  rclcpp::publisher::PublisherBase::SharedPtr
+  rclcpp::PublisherBase::SharedPtr
   create_publisher(
     const std::string & topic_name,
     const rclcpp::PublisherFactory & publisher_factory,
@@ -58,11 +58,11 @@ public:
   virtual
   void
   add_publisher(
-    rclcpp::publisher::PublisherBase::SharedPtr publisher);
+    rclcpp::PublisherBase::SharedPtr publisher);
 
   RCLCPP_PUBLIC
   virtual
-  rclcpp::subscription::SubscriptionBase::SharedPtr
+  rclcpp::SubscriptionBase::SharedPtr
   create_subscription(
     const std::string & topic_name,
     const rclcpp::SubscriptionFactory & subscription_factory,
@@ -73,7 +73,7 @@ public:
   virtual
   void
   add_subscription(
-    rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
+    rclcpp::SubscriptionBase::SharedPtr subscription,
     rclcpp::callback_group::CallbackGroup::SharedPtr callback_group);
 
 private:

rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp

@@ -42,7 +42,7 @@ public:
 
   RCLCPP_PUBLIC
   virtual
-  rclcpp::publisher::PublisherBase::SharedPtr
+  rclcpp::PublisherBase::SharedPtr
   create_publisher(
     const std::string & topic_name,
     const rclcpp::PublisherFactory & publisher_factory,
@@ -53,11 +53,11 @@ public:
   virtual
   void
   add_publisher(
-    rclcpp::publisher::PublisherBase::SharedPtr publisher) = 0;
+    rclcpp::PublisherBase::SharedPtr publisher) = 0;
 
   RCLCPP_PUBLIC
   virtual
-  rclcpp::subscription::SubscriptionBase::SharedPtr
+  rclcpp::SubscriptionBase::SharedPtr
   create_subscription(
     const std::string & topic_name,
     const rclcpp::SubscriptionFactory & subscription_factory,
@@ -68,7 +68,7 @@ public:
   virtual
   void
   add_subscription(
-    rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
+    rclcpp::SubscriptionBase::SharedPtr subscription,
     rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0;
 };
 

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,38 +126,36 @@ public:
 
 private:
   std::string name_;
-  rcl_interfaces::msg::ParameterValue value_;
+  ParameterValue value_;
 };
 
-
-/* Return a json encoded version of the parameter intended for a dict. */
+/// Return a json encoded version of the parameter intended for a dict.
 RCLCPP_PUBLIC
 std::string
-_to_json_dict_entry(const ParameterVariant & param);
+_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
 {
 
-/* Return a json encoded version of the parameter intended for a list. */
+/// Return a json encoded version of the parameter intended for a list.
 RCLCPP_PUBLIC
 std::string
-to_string(const rclcpp::parameter::ParameterVariant & param);
+to_string(const rclcpp::Parameter & param);
 
-/* Return a json encoded version of a vector of parameters, as a string*/
+/// Return a json encoded version of a vector of parameters, as a string.
 RCLCPP_PUBLIC
 std::string
-to_string(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+to_string(const std::vector<rclcpp::Parameter> & parameters);
 
 }  // namespace std
 

rclcpp/include/rclcpp/parameter_client.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__PARAMETER_CLIENT_HPP_
 #define RCLCPP__PARAMETER_CLIENT_HPP_
 
+#include <memory>
 #include <string>
 #include <utility>
 #include <vector>
@@ -29,6 +30,7 @@
 #include "rcl_interfaces/srv/set_parameters.hpp"
 #include "rcl_interfaces/srv/set_parameters_atomically.hpp"
 #include "rclcpp/executors.hpp"
+#include "rclcpp/create_subscription.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/parameter.hpp"
@@ -38,8 +40,6 @@
 
 namespace rclcpp
 {
-namespace parameter_client
-{
 
 class AsyncParametersClient
 {
@@ -48,30 +48,45 @@ public:
 
   RCLCPP_PUBLIC
   AsyncParametersClient(
-    const rclcpp::node::Node::SharedPtr node,
+    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+    const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
+    const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
+    const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
     const std::string & remote_node_name = "",
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
   RCLCPP_PUBLIC
-  std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>
+  AsyncParametersClient(
+    const rclcpp::Node::SharedPtr node,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+
+  RCLCPP_PUBLIC
+  AsyncParametersClient(
+    rclcpp::Node * node,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+
+  RCLCPP_PUBLIC
+  std::shared_future<std::vector<rclcpp::Parameter>>
   get_parameters(
     const std::vector<std::string> & names,
     std::function<
-      void(std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>)
+      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);
@@ -79,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);
@@ -93,24 +108,61 @@ public:
       void(std::shared_future<rcl_interfaces::msg::ListParametersResult>)
     > callback = nullptr);
 
-  template<typename CallbackT>
-  typename rclcpp::subscription::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
+  template<
+    typename CallbackT,
+    typename Alloc = std::allocator<void>,
+    typename SubscriptionT =
+    rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent, Alloc>>
+  typename rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
   on_parameter_event(CallbackT && callback)
   {
-    return node_->create_subscription<rcl_interfaces::msg::ParameterEvent>(
-      "parameter_events", std::forward<CallbackT>(callback), rmw_qos_profile_parameter_events);
+    using rclcpp::message_memory_strategy::MessageMemoryStrategy;
+    auto msg_mem_strat =
+      MessageMemoryStrategy<rcl_interfaces::msg::ParameterEvent, Alloc>::create_default();
+
+    using rcl_interfaces::msg::ParameterEvent;
+    return rclcpp::create_subscription<
+      ParameterEvent, CallbackT, Alloc, ParameterEvent, SubscriptionT>(
+      this->node_topics_interface_.get(),
+      "parameter_events",
+      std::forward<CallbackT>(callback),
+      rmw_qos_profile_default,
+      nullptr,  // group,
+      false,  // ignore_local_publications,
+      false,  // use_intra_process_comms_,
+      msg_mem_strat,
+      std::make_shared<Alloc>());
   }
 
+  RCLCPP_PUBLIC
+  bool
+  service_is_ready() const;
+
+  template<typename RatioT = std::milli>
+  bool
+  wait_for_service(
+    std::chrono::duration<int64_t, RatioT> timeout = std::chrono::duration<int64_t, RatioT>(-1))
+  {
+    return wait_for_service_nanoseconds(
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
+  }
+
+protected:
+  RCLCPP_PUBLIC
+  bool
+  wait_for_service_nanoseconds(std::chrono::nanoseconds timeout);
+
 private:
-  const rclcpp::node::Node::SharedPtr node_;
-  rclcpp::client::Client<rcl_interfaces::srv::GetParameters>::SharedPtr get_parameters_client_;
-  rclcpp::client::Client<rcl_interfaces::srv::GetParameterTypes>::SharedPtr
+  const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface_;
+  rclcpp::Client<rcl_interfaces::srv::GetParameters>::SharedPtr get_parameters_client_;
+  rclcpp::Client<rcl_interfaces::srv::GetParameterTypes>::SharedPtr
     get_parameter_types_client_;
-  rclcpp::client::Client<rcl_interfaces::srv::SetParameters>::SharedPtr set_parameters_client_;
-  rclcpp::client::Client<rcl_interfaces::srv::SetParametersAtomically>::SharedPtr
+  rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr set_parameters_client_;
+  rclcpp::Client<rcl_interfaces::srv::SetParametersAtomically>::SharedPtr
     set_parameters_atomically_client_;
-  rclcpp::client::Client<rcl_interfaces::srv::ListParameters>::SharedPtr list_parameters_client_;
-  rclcpp::client::Client<rcl_interfaces::srv::DescribeParameters>::SharedPtr
+  rclcpp::Client<rcl_interfaces::srv::ListParameters>::SharedPtr list_parameters_client_;
+  rclcpp::Client<rcl_interfaces::srv::DescribeParameters>::SharedPtr
     describe_parameters_client_;
   std::string remote_node_name_;
 };
@@ -122,17 +174,19 @@ public:
 
   RCLCPP_PUBLIC
   explicit SyncParametersClient(
-    rclcpp::node::Node::SharedPtr node,
+    rclcpp::Node::SharedPtr node,
+    const std::string & remote_node_name = "",
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
   RCLCPP_PUBLIC
   SyncParametersClient(
     rclcpp::executor::Executor::SharedPtr executor,
-    rclcpp::node::Node::SharedPtr node,
+    rclcpp::Node::SharedPtr node,
+    const std::string & remote_node_name = "",
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
   RCLCPP_PUBLIC
-  std::vector<rclcpp::parameter::ParameterVariant>
+  std::vector<rclcpp::Parameter>
   get_parameters(const std::vector<std::string> & parameter_names);
 
   RCLCPP_PUBLIC
@@ -147,7 +201,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>());
@@ -158,33 +212,31 @@ public:
   T
   get_parameter(const std::string & parameter_name, const T & default_value)
   {
-    // *INDENT-OFF*
-    return get_parameter_impl(parameter_name,
-      std::function<T()>([&default_value]() -> T {return default_value; }));
-    // *INDENT-ON*
+    return get_parameter_impl(
+      parameter_name,
+      std::function<T()>([&default_value]() -> T {return default_value;}));
   }
 
   template<typename T>
   T
   get_parameter(const std::string & parameter_name)
   {
-    // *INDENT-OFF*
-    return get_parameter_impl(parameter_name,
-      std::function<T()>([]() -> T {throw std::runtime_error("Parameter not set"); }));
-    // *INDENT-ON*
+    return get_parameter_impl(
+      parameter_name,
+      std::function<T()>([]() -> T {throw std::runtime_error("Parameter not set");}));
   }
 
   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
@@ -193,19 +245,33 @@ public:
     uint64_t depth);
 
   template<typename CallbackT>
-  typename rclcpp::subscription::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
+  typename rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
   on_parameter_event(CallbackT && callback)
   {
     return async_parameters_client_->on_parameter_event(std::forward<CallbackT>(callback));
   }
 
+  RCLCPP_PUBLIC
+  bool
+  service_is_ready() const
+  {
+    return async_parameters_client_->service_is_ready();
+  }
+
+  template<typename RatioT = std::milli>
+  bool
+  wait_for_service(
+    std::chrono::duration<int64_t, RatioT> timeout = std::chrono::duration<int64_t, RatioT>(-1))
+  {
+    return async_parameters_client_->wait_for_service(timeout);
+  }
+
 private:
   rclcpp::executor::Executor::SharedPtr executor_;
-  rclcpp::node::Node::SharedPtr node_;
+  rclcpp::Node::SharedPtr node_;
   AsyncParametersClient::SharedPtr async_parameters_client_;
 };
 
-}  // namespace parameter_client
 }  // namespace rclcpp
 
 #endif  // RCLCPP__PARAMETER_CLIENT_HPP_

rclcpp/include/rclcpp/parameter_events_filter.hpp

@@ -0,0 +1,78 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__PARAMETER_EVENTS_FILTER_HPP_
+#define RCLCPP__PARAMETER_EVENTS_FILTER_HPP_
+
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "rcl_interfaces/msg/parameter.hpp"
+#include "rcl_interfaces/msg/parameter_event.hpp"
+#include "rcl_interfaces/msg/parameter_value.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rmw/rmw.h"
+
+namespace rclcpp
+{
+
+class ParameterEventsFilter
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventsFilter)
+  enum class EventType {NEW, DELETED, CHANGED};  ///< An enum for the type of event.
+  /// Used for the listed results
+  using EventPair = std::pair<EventType, rcl_interfaces::msg::Parameter *>;
+
+  /// Construct a filtered view of a parameter event.
+  /**
+   * \param[in] event The parameter event message to filter.
+   * \param[in] names A list of parameter names of interest.
+   * \param[in] types A list of the types of parameter events of iterest.
+  *    EventType NEW, DELETED, or CHANGED
+  *
+  * Example Usage:
+  * If you have recieved a parameter event and are only interested in parameters foo and
+  * bar being added or changed but don't care about deletion.
+  *    auto res = rclcpp::ParameterEventsFilter(
+  *      event_shared_ptr,
+  *      {"foo", "bar"},
+  *      {rclcpp::ParameterEventsFilter::EventType::NEW, rclcpp::ParameterEventsFilter::EventType::CHANGED});
+  */
+  RCLCPP_PUBLIC
+  ParameterEventsFilter(
+    rcl_interfaces::msg::ParameterEvent::SharedPtr event,
+    const std::vector<std::string> & names,
+    const std::vector<EventType> & types);
+
+  /// Get the result of the filter
+  /**
+   * \return A std::vector<EventPair> of all matching parameter changes in this event.
+   */
+  RCLCPP_PUBLIC
+  const std::vector<EventPair> & get_events() const;
+
+private:
+  // access only allowed via const accessor.
+  std::vector<EventPair> result_;  ///< Storage of the resultant vector
+  rcl_interfaces::msg::ParameterEvent::SharedPtr event_;  ///< Keep event in scope
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__PARAMETER_EVENTS_FILTER_HPP_

rclcpp/include/rclcpp/parameter_map.hpp

@@ -0,0 +1,53 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__PARAMETER_MAP_HPP_
+#define RCLCPP__PARAMETER_MAP_HPP_
+
+#include <rcl_yaml_param_parser/types.h>
+
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/parameter_value.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// A map of fully qualified node names to a list of parameters
+using ParameterMap = std::unordered_map<std::string, std::vector<Parameter>>;
+
+/// Convert parameters from rcl_yaml_param_parser into C++ class instances.
+/// \param[in] c_params C structures containing parameters for multiple nodes.
+/// \returns a map where the keys are fully qualified node names and values a list of parameters.
+/// \throws InvalidParametersException if the `rcl_params_t` is inconsistent or invalid.
+RCLCPP_PUBLIC
+ParameterMap
+parameter_map_from(const rcl_params_t * const c_params);
+
+/// Convert parameter value from rcl_yaml_param_parser into a C++ class instance.
+/// \param[in] c_value C structure containing a value of a parameter.
+/// \returns an instance of a parameter value
+/// \throws InvalidParameterValueException if the `rcl_variant_t` is inconsistent or invalid.
+RCLCPP_PUBLIC
+ParameterValue
+parameter_value_from(const rcl_variant_t * const c_value);
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__PARAMETER_MAP_HPP_

rclcpp/include/rclcpp/parameter_service.hpp

@@ -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"
@@ -32,8 +33,6 @@
 
 namespace rclcpp
 {
-namespace parameter_service
-{
 
 class ParameterService
 {
@@ -42,23 +41,23 @@ public:
 
   RCLCPP_PUBLIC
   explicit ParameterService(
-    const rclcpp::node::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::Node::SharedPtr node_;
-  rclcpp::service::Service<rcl_interfaces::srv::GetParameters>::SharedPtr get_parameters_service_;
-  rclcpp::service::Service<rcl_interfaces::srv::GetParameterTypes>::SharedPtr
+  rclcpp::Service<rcl_interfaces::srv::GetParameters>::SharedPtr get_parameters_service_;
+  rclcpp::Service<rcl_interfaces::srv::GetParameterTypes>::SharedPtr
     get_parameter_types_service_;
-  rclcpp::service::Service<rcl_interfaces::srv::SetParameters>::SharedPtr set_parameters_service_;
-  rclcpp::service::Service<rcl_interfaces::srv::SetParametersAtomically>::SharedPtr
+  rclcpp::Service<rcl_interfaces::srv::SetParameters>::SharedPtr set_parameters_service_;
+  rclcpp::Service<rcl_interfaces::srv::SetParametersAtomically>::SharedPtr
     set_parameters_atomically_service_;
-  rclcpp::service::Service<rcl_interfaces::srv::DescribeParameters>::SharedPtr
+  rclcpp::Service<rcl_interfaces::srv::DescribeParameters>::SharedPtr
     describe_parameters_service_;
-  rclcpp::service::Service<rcl_interfaces::srv::ListParameters>::SharedPtr list_parameters_service_;
+  rclcpp::Service<rcl_interfaces::srv::ListParameters>::SharedPtr list_parameters_service_;
 };
 
-}  // namespace parameter_service
 }  // namespace rclcpp
 
 #endif  // RCLCPP__PARAMETER_SERVICE_HPP_

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,12 +45,9 @@ namespace node_interfaces
 class NodeTopicsInterface;
 }
 
-namespace publisher
-{
-
 class PublisherBase
 {
-  friend rclcpp::node_interfaces::NodeTopicsInterface;
+  friend ::rclcpp::node_interfaces::NodeTopicsInterface;
 
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase)
@@ -75,29 +72,41 @@ public:
   virtual ~PublisherBase();
 
   /// Get the topic that this publisher publishes on.
-  // \return The topic name.
+  /** \return The topic name. */
   RCLCPP_PUBLIC
   const char *
   get_topic_name() const;
 
   /// Get the queue size for this publisher.
-  // \return The queue size.
+  /** \return The queue size. */
   RCLCPP_PUBLIC
   size_t
   get_queue_size() const;
 
   /// Get the global identifier for this publisher (used in rmw and by DDS).
-  // \return The gid.
+  /** \return The gid. */
   RCLCPP_PUBLIC
   const rmw_gid_t &
   get_gid() const;
 
   /// Get the global identifier for this publisher used by intra-process communication.
-  // \return The intra-process gid.
+  /** \return The intra-process gid. */
   RCLCPP_PUBLIC
   const rmw_gid_t &
   get_intra_process_gid() const;
 
+  /// Get the rcl publisher handle.
+  /** \return The rcl publisher handle. */
+  RCLCPP_PUBLIC
+  rcl_publisher_t *
+  get_publisher_handle();
+
+  /// Get the rcl publisher handle.
+  /** \return The rcl publisher handle. */
+  RCLCPP_PUBLIC
+  const rcl_publisher_t *
+  get_publisher_handle() const;
+
   /// Compare this publisher to a gid.
   /**
    * Note that this function calls the next function.
@@ -196,11 +205,8 @@ 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_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.
@@ -261,6 +267,28 @@ public:
     return this->publish(unique_msg);
   }
 
+  virtual void
+  publish(const MessageT * msg)
+  {
+    if (!msg) {
+      throw std::runtime_error("msg argument is nullptr");
+    }
+    return this->publish(*msg);
+  }
+
+  void
+  publish(const rcl_serialized_message_t * serialized_msg)
+  {
+    if (store_intra_process_message_) {
+      // TODO(Karsten1987): support serialized message passed by intraprocess
+      throw std::runtime_error("storing serialized messages in intra process is not supported yet");
+    }
+    auto status = rcl_publish_serialized_message(&publisher_handle_, serialized_msg);
+    if (RCL_RET_OK != status) {
+      rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish serialized message");
+    }
+  }
+
   std::shared_ptr<MessageAlloc> get_allocator() const
   {
     return message_allocator_;
@@ -271,11 +299,8 @@ 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_OK != status) {
+      rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish message");
     }
   }
 
@@ -284,8 +309,6 @@ protected:
   MessageDeleter message_deleter_;
 };
 
-}  // namespace publisher
-
 }  // namespace rclcpp
 
 #endif  // RCLCPP__PUBLISHER_HPP_

rclcpp/include/rclcpp/publisher_factory.hpp

@@ -32,7 +32,8 @@ namespace rclcpp
 {
 
 /// Factory with functions used to create a MessageT specific PublisherT.
-/* This factory class is used to encapsulate the template generated functions
+/**
+ * This factory class is used to encapsulate the template generated functions
  * which are used during the creation of a Message type specific publisher
  * within a non-templated class.
  *
@@ -45,19 +46,19 @@ struct PublisherFactory
 {
   // Creates a PublisherT<MessageT, ...> publisher object and returns it as a PublisherBase.
   using PublisherFactoryFunction = std::function<
-      rclcpp::publisher::PublisherBase::SharedPtr(
-        rclcpp::node_interfaces::NodeBaseInterface * node_base,
-        const std::string & topic_name,
-        rcl_publisher_options_t & publisher_options)>;
+    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::publisher::PublisherBase::SharedPtr publisher)>;
+    uint64_t(
+      rclcpp::intra_process_manager::IntraProcessManager * ipm,
+      rclcpp::PublisherBase::SharedPtr publisher)>;
 
   AddPublisherToIntraProcessManagerFunction add_publisher_to_intra_process_manager;
 
@@ -65,8 +66,8 @@ struct PublisherFactory
   // PublisherT::publish() and which handles the intra process transmission of
   // the message being published.
   using SharedPublishCallbackFactoryFunction = std::function<
-      rclcpp::publisher::PublisherBase::StoreMessageCallbackT(
-        rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm)>;
+    rclcpp::PublisherBase::StoreMessageCallbackT(
+      rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm)>;
 
   SharedPublishCallbackFactoryFunction create_shared_publish_callback;
 };
@@ -95,13 +96,13 @@ create_publisher_factory(std::shared_ptr<Alloc> allocator)
   factory.add_publisher_to_intra_process_manager =
     [](
     rclcpp::intra_process_manager::IntraProcessManager * ipm,
-    rclcpp::publisher::PublisherBase::SharedPtr publisher) -> uint64_t
+    rclcpp::PublisherBase::SharedPtr publisher) -> uint64_t
     {
       return ipm->add_publisher<MessageT, Alloc>(std::dynamic_pointer_cast<PublisherT>(publisher));
     };
 
   // function to create a shared publish callback std::function
-  using StoreMessageCallbackT = rclcpp::publisher::PublisherBase::StoreMessageCallbackT;
+  using StoreMessageCallbackT = rclcpp::PublisherBase::StoreMessageCallbackT;
   factory.create_shared_publish_callback =
     [](rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm) -> StoreMessageCallbackT
     {
@@ -128,7 +129,7 @@ create_publisher_factory(std::shared_ptr<Alloc> allocator)
                     "' is incompatible from the publisher type '" + message_type_info.name() + "'");
           }
           MessageT * typed_message_ptr = static_cast<MessageT *>(msg);
-          using MessageDeleter = typename publisher::Publisher<MessageT, Alloc>::MessageDeleter;
+          using MessageDeleter = typename Publisher<MessageT, Alloc>::MessageDeleter;
           std::unique_ptr<MessageT, MessageDeleter> unique_msg(typed_message_ptr);
           uint64_t message_seq =
             ipm->store_intra_process_message<MessageT, Alloc>(publisher_id, unique_msg);

rclcpp/include/rclcpp/rate.hpp

@@ -25,8 +25,6 @@
 
 namespace rclcpp
 {
-namespace rate
-{
 
 class RateBase
 {
@@ -84,7 +82,7 @@ public:
       return false;
     }
     // Sleep (will get interrupted by ctrl-c, may not sleep full time)
-    rclcpp::utilities::sleep_for(time_to_sleep);
+    rclcpp::sleep_for(time_to_sleep);
     return true;
   }
 
@@ -116,7 +114,6 @@ private:
 using Rate = GenericRate<std::chrono::system_clock>;
 using WallRate = GenericRate<std::chrono::steady_clock>;
 
-}  // namespace rate
 }  // namespace rclcpp
 
 #endif  // RCLCPP__RATE_HPP_

rclcpp/include/rclcpp/rclcpp.hpp

@@ -12,6 +12,129 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+/** \mainpage rclcpp: ROS Client Library for C++
+ *
+ * `rclcpp` provides the canonical C++ API for interacting with ROS.
+ * It consists of these main components:
+ *
+ * - Node
+ *   - rclcpp::Node
+ *   - rclcpp/node.hpp
+ * - Publisher
+ *   - rclcpp::Node::create_publisher()
+ *   - rclcpp::Publisher
+ *   - rclcpp::Publisher::publish()
+ *   - rclcpp/publisher.hpp
+ * - Subscription
+ *   - rclcpp::Node::create_subscription()
+ *   - rclcpp::Subscription
+ *   - rclcpp/subscription.hpp
+ * - Service Client
+ *   - rclcpp::Node::create_client()
+ *   - rclcpp::Client
+ *   - rclcpp/client.hpp
+ * - Service Server
+ *   - rclcpp::Node::create_service()
+ *   - rclcpp::Service
+ *   - rclcpp/service.hpp
+ * - Timer
+ *   - rclcpp::Node::create_wall_timer()
+ *   - rclcpp::WallTimer
+ *   - rclcpp::TimerBase
+ *   - rclcpp/timer.hpp
+ * - Parameters:
+ *   - rclcpp::Node::set_parameters()
+ *   - rclcpp::Node::get_parameters()
+ *   - rclcpp::Node::get_parameter()
+ *   - rclcpp::Node::describe_parameters()
+ *   - rclcpp::Node::list_parameters()
+ *   - rclcpp::Node::register_param_change_callback()
+ *   - rclcpp::Parameter
+ *   - rclcpp::ParameterValue
+ *   - rclcpp::AsyncParametersClient
+ *   - rclcpp::SyncParametersClient
+ *   - rclcpp/parameter.hpp
+ *   - rclcpp/parameter_value.hpp
+ *   - rclcpp/parameter_client.hpp
+ *   - rclcpp/parameter_service.hpp
+ * - Rate:
+ *   - rclcpp::Rate
+ *   - rclcpp::WallRate
+ *   - rclcpp/rate.hpp
+ *
+ * There are also some components which help control the execution of callbacks:
+ *
+ * - Executors (responsible for execution of callbacks through a blocking spin):
+ *   - rclcpp::spin()
+ *   - rclcpp::spin_some()
+ *   - rclcpp::spin_until_future_complete()
+ *   - rclcpp::executors::SingleThreadedExecutor
+ *   - rclcpp::executors::SingleThreadedExecutor::add_node()
+ *   - rclcpp::executors::SingleThreadedExecutor::spin()
+ *   - rclcpp::executors::MultiThreadedExecutor
+ *   - rclcpp::executors::MultiThreadedExecutor::add_node()
+ *   - rclcpp::executors::MultiThreadedExecutor::spin()
+ *   - rclcpp/executor.hpp
+ *   - rclcpp/executors.hpp
+ *   - rclcpp/executors/single_threaded_executor.hpp
+ *   - rclcpp/executors/multi_threaded_executor.hpp
+ * - CallbackGroups (mechanism for enforcing concurrency rules for callbacks):
+ *   - rclcpp::Node::create_callback_group()
+ *   - rclcpp::callback_group::CallbackGroup
+ *   - rclcpp/callback_group.hpp
+ *
+ * Additionally, there are some methods for introspecting the ROS graph:
+ *
+ * - Graph Events (a waitable event object that wakes up when the graph changes):
+ *   - rclcpp::Node::get_graph_event()
+ *   - rclcpp::Node::wait_for_graph_change()
+ *   - rclcpp::Event
+ * - List topic names and types:
+ *   - rclcpp::Node::get_topic_names_and_types()
+ * - Get the number of publishers or subscribers on a topic:
+ *   - rclcpp::Node::count_publishers()
+ *   - rclcpp::Node::count_subscribers()
+ *
+ * And components related to logging:
+ *
+ * - Logging macros:
+ *   - Some examples (not exhaustive):
+ *     - RCLCPP_DEBUG()
+ *     - RCLCPP_INFO()
+ *     - RCLCPP_WARN_ONCE()
+ *     - RCLCPP_ERROR_SKIPFIRST()
+ *   - rclcpp/logging.hpp
+ * - Logger:
+ *   - rclcpp::Logger
+ *   - rclcpp/logger.hpp
+ *   - rclcpp::Node::get_logger()
+ *
+ * Finally, there are many internal API's and utilities:
+ *
+ * - Exceptions:
+ *   - rclcpp/exceptions.hpp
+ * - Allocator related items:
+ *   - rclcpp/allocator/allocator_common.hpp
+ *   - rclcpp/allocator/allocator_deleter.hpp
+ * - Memory management tools:
+ *   - rclcpp/memory_strategies.hpp
+ *   - rclcpp/memory_strategy.hpp
+ *   - rclcpp/message_memory_strategy.hpp
+ *   - rclcpp/strategies/allocator_memory_strategy.hpp
+ *   - rclcpp/strategies/message_pool_memory_strategy.hpp
+ * - Context object which is shared amongst multiple Nodes:
+ *   - rclcpp::Context
+ *   - rclcpp/context.hpp
+ *   - rclcpp/contexts/default_context.hpp
+ * - Various utilities:
+ *   - rclcpp/function_traits.hpp
+ *   - rclcpp/macros.hpp
+ *   - rclcpp/scope_exit.hpp
+ *   - rclcpp/time.hpp
+ *   - rclcpp/utilities.hpp
+ *   - rclcpp/visibility_control.hpp
+ */
+
 #ifndef RCLCPP__RCLCPP_HPP_
 #define RCLCPP__RCLCPP_HPP_
 
@@ -19,34 +142,14 @@
 #include <memory>
 
 #include "rclcpp/executors.hpp"
+#include "rclcpp/logging.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/parameter_client.hpp"
 #include "rclcpp/parameter_service.hpp"
 #include "rclcpp/rate.hpp"
+#include "rclcpp/time.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
 
-namespace rclcpp
-{
-
-// Namespace escalations.
-// For example, this next line escalates type "rclcpp:node::Node" to "rclcpp::Node"
-using rclcpp::node::Node;
-using rclcpp::publisher::Publisher;
-using rclcpp::subscription::SubscriptionBase;
-using rclcpp::subscription::Subscription;
-using rclcpp::rate::GenericRate;
-using rclcpp::rate::WallRate;
-using rclcpp::timer::GenericTimer;
-using rclcpp::timer::TimerBase;
-using rclcpp::timer::WallTimer;
-using ContextSharedPtr = rclcpp::context::Context::SharedPtr;
-using rclcpp::utilities::ok;
-using rclcpp::utilities::shutdown;
-using rclcpp::utilities::init;
-using rclcpp::utilities::sleep_for;
-
-}  // namespace rclcpp
-
 #endif  // RCLCPP__RCLCPP_HPP_

rclcpp/include/rclcpp/scope_exit.hpp

@@ -31,7 +31,7 @@ struct ScopeExit
 {
   explicit ScopeExit(Callable callable)
   : callable_(callable) {}
-  ~ScopeExit() {callable_(); }
+  ~ScopeExit() {callable_();}
 
 private:
   Callable callable_;
@@ -47,6 +47,6 @@ make_scope_exit(Callable callable)
 }  // namespace rclcpp
 
 #define RCLCPP_SCOPE_EXIT(code) \
-  auto RCLCPP_STRING_JOIN(scope_exit_, __LINE__) = rclcpp::make_scope_exit([&]() {code; })
+  auto RCLCPP_STRING_JOIN(scope_exit_, __LINE__) = rclcpp::make_scope_exit([&]() {code;})
 
 #endif  // RCLCPP__SCOPE_EXIT_HPP_

rclcpp/include/rclcpp/service.hpp

@@ -25,27 +25,23 @@
 #include "rcl/service.h"
 
 #include "rclcpp/any_service_callback.hpp"
+#include "rclcpp/exceptions.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/logging.hpp"
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 
 namespace rclcpp
 {
-namespace service
-{
 
 class ServiceBase
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase)
 
-  RCLCPP_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,13 +50,17 @@ public:
   virtual ~ServiceBase();
 
   RCLCPP_PUBLIC
-  std::string
+  const char *
   get_service_name();
 
   RCLCPP_PUBLIC
-  const rcl_service_t *
+  std::shared_ptr<rcl_service_t>
   get_service_handle();
 
+  RCLCPP_PUBLIC
+  std::shared_ptr<const rcl_service_t>
+  get_service_handle() const;
+
   virtual std::shared_ptr<void> create_request() = 0;
   virtual std::shared_ptr<rmw_request_id_t> create_request_header() = 0;
   virtual void handle_request(
@@ -70,29 +70,34 @@ public:
 protected:
   RCLCPP_DISABLE_COPY(ServiceBase)
 
+  RCLCPP_PUBLIC
+  rcl_node_t *
+  get_rcl_node_handle();
+
+  RCLCPP_PUBLIC
+  const rcl_node_t *
+  get_rcl_node_handle() const;
+
   std::shared_ptr<rcl_node_t> node_handle_;
 
-  rcl_service_t * service_handle_ = nullptr;
-  std::string service_name_;
+  std::shared_ptr<rcl_service_t> service_handle_;
   bool owns_rcl_handle_ = true;
 };
 
-using any_service_callback::AnyServiceCallback;
-
 template<typename ServiceT>
 class Service : public ServiceBase
 {
 public:
   using CallbackType = std::function<
-      void(
-        const std::shared_ptr<typename ServiceT::Request>,
-        std::shared_ptr<typename ServiceT::Response>)>;
+    void(
+      const std::shared_ptr<typename ServiceT::Request>,
+      std::shared_ptr<typename ServiceT::Response>)>;
 
   using CallbackWithHeaderType = std::function<
-      void(
-        const std::shared_ptr<rmw_request_id_t>,
-        const std::shared_ptr<typename ServiceT::Request>,
-        std::shared_ptr<typename ServiceT::Response>)>;
+    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(
@@ -100,24 +105,75 @@ 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();
-
-    if (rcl_service_init(
-        service_handle_, node_handle.get(), service_type_support_handle, service_name.c_str(),
-        &service_options) != RCL_RET_OK)
-    {
-      throw std::runtime_error(std::string("could not create service: ") +
+    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_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp"),
+              "Error in destruction of rcl service handle: %s",
               rcl_get_error_string_safe());
+            rcl_reset_error();
+          }
+        } else {
+          RCLCPP_ERROR(
+            rclcpp::get_logger("rclcpp"),
+            "Error in destruction of rcl service handle: "
+            "the Node Handle was destructed too early. You will leak memory");
+        }
+        delete service;
+      });
+    *service_handle_.get() = rcl_get_zero_initialized_service();
+
+    rcl_ret_t ret = rcl_service_init(
+      service_handle_.get(),
+      node_handle.get(),
+      service_type_support_handle,
+      service_name.c_str(),
+      &service_options);
+    if (ret != RCL_RET_OK) {
+      if (ret == RCL_RET_SERVICE_NAME_INVALID) {
+        auto rcl_node_handle = get_rcl_node_handle();
+        // this will throw on any validation problem
+        rcl_reset_error();
+        expand_topic_or_service_name(
+          service_name,
+          rcl_node_get_name(rcl_node_handle),
+          rcl_node_get_namespace(rcl_node_handle),
+          true);
+      }
+
+      rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service");
     }
   }
 
+  Service(
+    std::shared_ptr<rcl_node_t> node_handle,
+    std::shared_ptr<rcl_service_t> service_handle,
+    AnyServiceCallback<ServiceT> any_callback)
+  : ServiceBase(node_handle),
+    any_callback_(any_callback)
+  {
+    // check if service handle was initialized
+    if (!rcl_service_is_valid(service_handle.get(), nullptr)) {
+      // *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,
@@ -126,33 +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, nullptr)) {
       // *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();
-      }
-    }
   }
 
   std::shared_ptr<void> create_request()
@@ -167,7 +212,8 @@ public:
     return std::shared_ptr<rmw_request_id_t>(new rmw_request_id_t);
   }
 
-  void handle_request(std::shared_ptr<rmw_request_id_t> request_header,
+  void handle_request(
+    std::shared_ptr<rmw_request_id_t> request_header,
     std::shared_ptr<void> request)
   {
     auto typed_request = std::static_pointer_cast<typename ServiceT::Request>(request);
@@ -180,13 +226,10 @@ 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) {
-      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
-      throw std::runtime_error(
-        std::string("failed to send response: ") + rcl_get_error_string_safe());
-      // *INDENT-ON*
+      rclcpp::exceptions::throw_from_rcl_error(status, "failed to send response");
     }
   }
 
@@ -196,7 +239,6 @@ private:
   AnyServiceCallback<ServiceT> any_callback_;
 };
 
-}  // namespace service
 }  // namespace rclcpp
 
 #endif  // RCLCPP__SERVICE_HPP_

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>();
   }
 
@@ -96,22 +93,22 @@ public:
   {
     for (size_t i = 0; i < wait_set->size_of_subscriptions; ++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) {
       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) {
       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) {
       if (!wait_set->timers[i]) {
-        timer_handles_[i] = nullptr;
+        timer_handles_[i].reset();
       }
     }
 
@@ -183,39 +180,49 @@ public:
     return has_invalid_weak_nodes;
   }
 
-  bool add_handles_to_waitset(rcl_wait_set_t * wait_set)
+  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 waitset: %s\n", rcl_get_error_string_safe());
+      if (rcl_wait_set_add_subscription(wait_set, subscription.get()) != RCL_RET_OK) {
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Couldn't add subscription to wait set: %s", rcl_get_error_string_safe());
         return false;
       }
     }
 
     for (auto client : client_handles_) {
-      if (rcl_wait_set_add_client(wait_set, client) != RCL_RET_OK) {
-        fprintf(stderr, "Couldn't add client to waitset: %s\n", rcl_get_error_string_safe());
+      if (rcl_wait_set_add_client(wait_set, client.get()) != RCL_RET_OK) {
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Couldn't add client to wait set: %s", rcl_get_error_string_safe());
         return false;
       }
     }
 
     for (auto service : service_handles_) {
-      if (rcl_wait_set_add_service(wait_set, service) != RCL_RET_OK) {
-        fprintf(stderr, "Couldn't add service to waitset: %s\n", rcl_get_error_string_safe());
+      if (rcl_wait_set_add_service(wait_set, service.get()) != RCL_RET_OK) {
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Couldn't add service to wait set: %s", rcl_get_error_string_safe());
         return false;
       }
     }
 
     for (auto timer : timer_handles_) {
-      if (rcl_wait_set_add_timer(wait_set, timer) != RCL_RET_OK) {
-        fprintf(stderr, "Couldn't add timer to waitset: %s\n", rcl_get_error_string_safe());
+      if (rcl_wait_set_add_timer(wait_set, timer.get()) != RCL_RET_OK) {
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Couldn't add timer to wait set: %s", rcl_get_error_string_safe());
         return false;
       }
     }
 
     for (auto guard_condition : guard_conditions_) {
       if (rcl_wait_set_add_guard_condition(wait_set, guard_condition) != RCL_RET_OK) {
-        fprintf(stderr, "Couldn't add guard_condition to waitset: %s\n",
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Couldn't add guard_condition to wait set: %s",
           rcl_get_error_string_safe());
         return false;
       }
@@ -223,15 +230,9 @@ public:
     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,
+  get_next_subscription(
+    executor::AnyExecutable & any_exec,
     const WeakNodeVector & weak_nodes)
   {
     auto it = subscription_handles_.begin();
@@ -248,7 +249,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()) {
@@ -259,22 +260,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,
+  get_next_service(
+    executor::AnyExecutable & any_exec,
     const WeakNodeVector & weak_nodes)
   {
     auto it = service_handles_.begin();
@@ -286,7 +288,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()) {
@@ -296,19 +298,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()) {
@@ -319,7 +321,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()) {
@@ -329,14 +331,14 @@ 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);
     }
   }
 
@@ -373,16 +375,15 @@ public:
 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_;
 
-  std::shared_ptr<ExecAlloc> executable_allocator_;
   std::shared_ptr<VoidAlloc> allocator_;
 };
 

rclcpp/include/rclcpp/subscription.hpp

@@ -29,9 +29,12 @@
 
 #include "rcl_interfaces/msg/intra_process_message.hpp"
 
+#include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/message_memory_strategy.hpp"
-#include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/subscription_traits.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -43,9 +46,6 @@ namespace node_interfaces
 class NodeTopicsInterface;
 }  // namespace node_interfaces
 
-namespace subscription
-{
-
 /// Virtual base class for subscriptions. This pattern allows us to iterate over different template
 /// specializations of Subscription, among other things.
 class SubscriptionBase
@@ -65,7 +65,8 @@ public:
     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
@@ -77,17 +78,27 @@ public:
   get_topic_name() const;
 
   RCLCPP_PUBLIC
-  const rcl_subscription_t *
+  std::shared_ptr<rcl_subscription_t>
+  get_subscription_handle();
+
+  RCLCPP_PUBLIC
+  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.
+  /** \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.
@@ -97,36 +108,51 @@ public:
   handle_message(std::shared_ptr<void> & message, const rmw_message_info_t & message_info) = 0;
 
   /// Return the message borrowed in create_message.
-  // \param[in] Shared pointer to the returned message.
+  /** \param[in] message Shared pointer to the returned message. */
   virtual void
   return_message(std::shared_ptr<void> & message) = 0;
+
+  /// Return the message borrowed in create_serialized_message.
+  /** \param[in] message Shared pointer to the returned message. */
+  virtual void
+  return_serialized_message(std::shared_ptr<rcl_serialized_message_t> & message) = 0;
+
   virtual void
   handle_intra_process_message(
     rcl_interfaces::msg::IntraProcessMessage & ipm,
     const rmw_message_info_t & message_info) = 0;
 
+  const rosidl_message_type_support_t &
+  get_message_type_support_handle() const;
+
+  bool
+  is_serialized() const;
+
 protected:
-  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_;
 
 private:
   RCLCPP_DISABLE_COPY(SubscriptionBase)
+
+  rosidl_message_type_support_t type_support_;
+  bool is_serialized_;
 };
 
-using any_subscription_callback::AnySubscriptionCallback;
-
 /// 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)
 
@@ -142,17 +168,19 @@ public:
    */
   Subscription(
     std::shared_ptr<rcl_node_t> node_handle,
+    const rosidl_message_type_support_t & ts,
     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>(),
+      ts,
       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),
@@ -165,11 +193,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
@@ -179,6 +208,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_) {
@@ -188,16 +222,23 @@ public:
         return;
       }
     }
-    auto typed_message = std::static_pointer_cast<MessageT>(message);
+    auto typed_message = std::static_pointer_cast<CallbackMessageT>(message);
     any_callback_.dispatch(typed_message, message_info);
   }
 
+  /// Return the loaned message.
+  /** \param message message to be returned */
   void return_message(std::shared_ptr<void> & message)
   {
-    auto typed_message = std::static_pointer_cast<MessageT>(message);
+    auto typed_message = std::static_pointer_cast<CallbackMessageT>(message);
     message_memory_strategy_->return_message(typed_message);
   }
 
+  void return_serialized_message(std::shared_ptr<rcl_serialized_message_t> & message)
+  {
+    message_memory_strategy_->return_serialized_message(message);
+  }
+
   void handle_intra_process_message(
     rcl_interfaces::msg::IntraProcessMessage & ipm,
     const rmw_message_info_t & message_info)
@@ -225,7 +266,7 @@ public:
   }
 
   using GetMessageCallbackType =
-      std::function<void(uint64_t, uint64_t, uint64_t, MessageUniquePtr &)>;
+    std::function<void(uint64_t, uint64_t, uint64_t, MessageUniquePtr &)>;
   using MatchesAnyPublishersCallbackType = std::function<bool(const rmw_gid_t *)>;
 
   /// Implemenation detail.
@@ -235,16 +276,25 @@ public:
     MatchesAnyPublishersCallbackType matches_any_publisher_callback,
     const rcl_subscription_options_t & intra_process_options)
   {
-    if (rcl_subscription_init(
-        &intra_process_subscription_handle_, node_handle_.get(),
-        rclcpp::type_support::get_intra_process_message_msg_type_support(),
-        (std::string(get_topic_name()) + "__intra").c_str(),
-        &intra_process_options) != RCL_RET_OK)
-    {
-      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
-      throw std::runtime_error(
-        std::string("could not create intra process subscription: ") + rcl_get_error_string_safe());
-      // *INDENT-ON*
+    std::string intra_process_topic_name = std::string(get_topic_name()) + "/_intra";
+    rcl_ret_t ret = rcl_subscription_init(
+      intra_process_subscription_handle_.get(),
+      node_handle_.get(),
+      rclcpp::type_support::get_intra_process_message_msg_type_support(),
+      intra_process_topic_name.c_str(),
+      &intra_process_options);
+    if (ret != RCL_RET_OK) {
+      if (ret == RCL_RET_TOPIC_NAME_INVALID) {
+        auto rcl_node_handle = node_handle_.get();
+        // this will throw on any validation problem
+        rcl_reset_error();
+        expand_topic_or_service_name(
+          intra_process_topic_name,
+          rcl_node_get_name(rcl_node_handle),
+          rcl_node_get_namespace(rcl_node_handle));
+      }
+
+      rclcpp::exceptions::throw_from_rcl_error(ret, "could not create intra process subscription");
     }
 
     intra_process_subscription_id_ = intra_process_subscription_id;
@@ -253,28 +303,27 @@ public:
   }
 
   /// 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
-  message_memory_strategy_;
+  AnySubscriptionCallback<CallbackMessageT, Alloc> any_callback_;
+  typename message_memory_strategy::MessageMemoryStrategy<CallbackMessageT, Alloc>::SharedPtr
+    message_memory_strategy_;
 
   GetMessageCallbackType get_intra_process_message_callback_;
   MatchesAnyPublishersCallbackType matches_any_intra_process_publishers_;
   uint64_t intra_process_subscription_id_;
 };
 
-}  // namespace subscription
 }  // namespace rclcpp
 
 #endif  // RCLCPP__SUBSCRIPTION_HPP_

rclcpp/include/rclcpp/subscription_factory.hpp

@@ -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"
@@ -33,7 +34,8 @@ namespace rclcpp
 {
 
 /// Factory with functions used to create a Subscription<MessageT>.
-/* This factory class is used to encapsulate the template generated functions
+/**
+ * This factory class is used to encapsulate the template generated functions
  * which are used during the creation of a Message type specific subscription
  * within a non-templated class.
  *
@@ -46,39 +48,46 @@ struct SubscriptionFactory
 {
   // Creates a Subscription<MessageT> object and returns it as a SubscriptionBase.
   using SubscriptionFactoryFunction = std::function<
-      rclcpp::subscription::SubscriptionBase::SharedPtr(
-        rclcpp::node_interfaces::NodeBaseInterface * node_base,
-        const std::string & topic_name,
-        rcl_subscription_options_t & subscription_options)>;
+    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::subscription::SubscriptionBase::SharedPtr subscription,
-        const rcl_subscription_options_t & subscription_options)>;
+  using SetupIntraProcessFunction = std::function<
+    void(
+      rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm,
+      rclcpp::SubscriptionBase::SharedPtr subscription,
+      const rcl_subscription_options_t & subscription_options)>;
 
   SetupIntraProcessFunction setup_intra_process;
 };
 
 /// Return a SubscriptionFactory with functions for creating a SubscriptionT<MessageT, Alloc>.
-template<typename MessageT, typename CallbackT, typename Alloc, typename 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::subscription::AnySubscriptionCallback;
-  AnySubscriptionCallback<MessageT, Alloc> any_subscription_callback(allocator);
+  using rclcpp::AnySubscriptionCallback;
+  AnySubscriptionCallback<CallbackMessageT, Alloc> any_subscription_callback(allocator);
   any_subscription_callback.set(std::forward<CallbackT>(callback));
 
   auto message_alloc =
-    std::make_shared<typename subscription::Subscription<MessageT, Alloc>::MessageAlloc>();
+    std::make_shared<typename Subscription<CallbackMessageT, Alloc>::MessageAlloc>();
 
   // factory function that creates a MessageT specific SubscriptionT
   factory.create_typed_subscription =
@@ -86,16 +95,17 @@ create_subscription_factory(
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
     const std::string & topic_name,
     rcl_subscription_options_t & subscription_options
-    ) -> rclcpp::subscription::SubscriptionBase::SharedPtr
+    ) -> 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::Subscription;
-      using rclcpp::subscription::SubscriptionBase;
+      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,
@@ -108,14 +118,14 @@ create_subscription_factory(
   factory.setup_intra_process =
     [message_alloc](
     rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm,
-    rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
+    rclcpp::SubscriptionBase::SharedPtr subscription,
     const rcl_subscription_options_t & subscription_options)
     {
       rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = ipm;
       uint64_t intra_process_subscription_id = ipm->add_subscription(subscription);
 
       auto intra_process_options = rcl_subscription_get_default_options();
-      intra_process_options.allocator = rclcpp::allocator::get_rcl_allocator<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;
@@ -126,7 +136,7 @@ create_subscription_factory(
         uint64_t publisher_id,
         uint64_t message_sequence,
         uint64_t subscription_id,
-        typename rclcpp::subscription::Subscription<MessageT, Alloc>::MessageUniquePtr & message)
+        typename rclcpp::Subscription<CallbackMessageT, Alloc>::MessageUniquePtr & message)
         {
           auto ipm = weak_ipm.lock();
           if (!ipm) {
@@ -134,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);
         };
 

rclcpp/include/rclcpp/subscription_traits.hpp

@@ -0,0 +1,80 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__SUBSCRIPTION_TRAITS_HPP_
+#define RCLCPP__SUBSCRIPTION_TRAITS_HPP_
+
+#include <memory>
+
+#include "rclcpp/function_traits.hpp"
+
+namespace rclcpp
+{
+namespace subscription_traits
+{
+
+/*
+ * The current version of uncrustify has a misinterpretion here
+ * between `:` used for inheritance vs for initializer list
+ * The result is that whenever a templated struct is used,
+ * the colon has to be without any whitespace next to it whereas
+ * when no template is used, the colon has to be separated by a space.
+ * Cheers!
+ */
+template<typename T>
+struct is_serialized_subscription_argument : std::false_type
+{};
+
+template<>
+struct is_serialized_subscription_argument<rcl_serialized_message_t>: std::true_type
+{};
+
+template<>
+struct is_serialized_subscription_argument<std::shared_ptr<rcl_serialized_message_t>>
+  : std::true_type
+{};
+
+template<typename T>
+struct is_serialized_subscription : is_serialized_subscription_argument<T>
+{};
+
+template<typename CallbackT>
+struct is_serialized_callback
+  : is_serialized_subscription_argument<
+    typename rclcpp::function_traits::function_traits<CallbackT>::template argument_type<0>>
+{};
+
+template<typename MessageT>
+struct extract_message_type
+{
+  using type = typename std::remove_cv<MessageT>::type;
+};
+
+template<typename MessageT>
+struct extract_message_type<std::shared_ptr<MessageT>>: extract_message_type<MessageT>
+{};
+
+template<typename MessageT, typename Deleter>
+struct extract_message_type<std::unique_ptr<MessageT, Deleter>>: extract_message_type<MessageT>
+{};
+
+template<typename CallbackT>
+struct has_message_type : extract_message_type<
+    typename rclcpp::function_traits::function_traits<CallbackT>::template argument_type<0>>
+{};
+
+}  // namespace subscription_traits
+}  // namespace rclcpp
+
+#endif  // RCLCPP__SUBSCRIPTION_TRAITS_HPP_

rclcpp/include/rclcpp/time.hpp

@@ -0,0 +1,119 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__TIME_HPP_
+#define RCLCPP__TIME_HPP_
+
+#include "builtin_interfaces/msg/time.hpp"
+
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcl/time.h"
+
+#include "rclcpp/duration.hpp"
+
+namespace rclcpp
+{
+
+class Clock;
+
+class Time
+{
+public:
+  RCLCPP_PUBLIC
+  Time(int32_t seconds, uint32_t nanoseconds, rcl_clock_type_t clock_type = RCL_SYSTEM_TIME);
+
+  RCLCPP_PUBLIC
+  explicit Time(int64_t nanoseconds = 0, rcl_clock_type_t clock = RCL_SYSTEM_TIME);
+
+  RCLCPP_PUBLIC
+  Time(const Time & rhs);
+
+  RCLCPP_PUBLIC
+  Time(
+    const builtin_interfaces::msg::Time & time_msg,
+    rcl_clock_type_t ros_time = RCL_ROS_TIME);
+
+  RCLCPP_PUBLIC
+  explicit Time(const rcl_time_point_t & time_point);
+
+  RCLCPP_PUBLIC
+  virtual ~Time();
+
+  RCLCPP_PUBLIC
+  operator builtin_interfaces::msg::Time() const;
+
+  RCLCPP_PUBLIC
+  Time &
+  operator=(const Time & rhs);
+
+  RCLCPP_PUBLIC
+  Time &
+  operator=(const builtin_interfaces::msg::Time & time_msg);
+
+  RCLCPP_PUBLIC
+  bool
+  operator==(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator!=(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator<(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator<=(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator>=(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator>(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  Time
+  operator+(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  Duration
+  operator-(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  Time
+  operator-(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  rcl_time_point_value_t
+  nanoseconds() const;
+
+  RCLCPP_PUBLIC
+  rcl_clock_type_t
+  get_clock_type() const;
+
+private:
+  rcl_time_point_t rcl_time_;
+  friend Clock;  // Allow clock to manipulate internal data
+};
+
+Time
+operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs);
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__TIME_HPP_

rclcpp/include/rclcpp/time_source.hpp

@@ -0,0 +1,129 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__TIME_SOURCE_HPP_
+#define RCLCPP__TIME_SOURCE_HPP_
+
+#include <memory>
+#include <vector>
+
+#include "rcl/time.h"
+
+#include "builtin_interfaces/msg/time.hpp"
+#include "rosgraph_msgs/msg/clock.hpp"
+#include "rcl_interfaces/msg/parameter_event.hpp"
+
+#include "rclcpp/node.hpp"
+#include "rclcpp/parameter_client.hpp"
+#include "rclcpp/parameter_events_filter.hpp"
+
+
+namespace rclcpp
+{
+class Clock;
+
+class TimeSource
+{
+public:
+  RCLCPP_PUBLIC
+  explicit TimeSource(rclcpp::Node::SharedPtr node);
+
+  RCLCPP_PUBLIC
+  TimeSource();
+
+  RCLCPP_PUBLIC
+  void attachNode(rclcpp::Node::SharedPtr node);
+
+  RCLCPP_PUBLIC
+  void attachNode(
+    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_PUBLIC
+  void detachNode();
+
+  /// Attach a clock to the time source to be updated
+  /**
+   * \throws std::invalid_argument if node is nullptr
+   */
+  RCLCPP_PUBLIC
+  void attachClock(rclcpp::Clock::SharedPtr clock);
+
+  RCLCPP_PUBLIC
+  void detachClock(rclcpp::Clock::SharedPtr clock);
+
+  RCLCPP_PUBLIC
+  ~TimeSource();
+
+private:
+  // Preserve the node reference
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
+  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
+
+  // The subscription for the clock callback
+  using MessageT = rosgraph_msgs::msg::Clock;
+  using Alloc = std::allocator<void>;
+  using SubscriptionT = rclcpp::Subscription<MessageT, Alloc>;
+  std::shared_ptr<SubscriptionT> clock_subscription_;
+
+  // The clock callback itself
+  void clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg);
+
+  // Parameter Client pointer
+  std::shared_ptr<rclcpp::AsyncParametersClient> parameter_client_;
+
+  // Parameter Event subscription
+  using ParamMessageT = rcl_interfaces::msg::ParameterEvent;
+  using ParamSubscriptionT = rclcpp::Subscription<ParamMessageT, Alloc>;
+  std::shared_ptr<ParamSubscriptionT> parameter_subscription_;
+
+  // Callback for parameter updates
+  void on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event);
+
+  // An enum to hold the parameter state
+  enum UseSimTimeParameterState {UNSET, SET_TRUE, SET_FALSE};
+  UseSimTimeParameterState parameter_state_;
+
+  // An internal method to use in the clock callback that iterates and enables all clocks
+  void enable_ros_time();
+  // An internal method to use in the clock callback that iterates and disables all clocks
+  void disable_ros_time();
+
+  // Internal helper functions used inside iterators
+  static void enable_ros_time(rclcpp::Clock::SharedPtr clock);
+  static void disable_ros_time(rclcpp::Clock::SharedPtr clock);
+  static void set_clock(
+    const builtin_interfaces::msg::Time::SharedPtr msg,
+    bool set_ros_time_enabled,
+    rclcpp::Clock::SharedPtr clock);
+
+  // Local storage of validity of ROS time
+  // This is needed when new clocks are added.
+  bool ros_time_active_;
+  // Last set message to be passed to newly registered clocks
+  rosgraph_msgs::msg::Clock::SharedPtr last_msg_set_;
+
+  // A lock to protect iterating the associated_clocks_ field.
+  std::mutex clock_list_lock_;
+  // A vector to store references to associated clocks.
+  std::vector<rclcpp::Clock::SharedPtr> associated_clocks_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__TIME_SOURCE_HPP_

rclcpp/include/rclcpp/timer.hpp

@@ -37,8 +37,6 @@
 
 namespace rclcpp
 {
-namespace timer
-{
 
 class TimerBase
 {
@@ -55,22 +53,26 @@ public:
   void
   cancel();
 
+  RCLCPP_PUBLIC
+  void
+  reset();
+
   RCLCPP_PUBLIC
   virtual void
   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.
-  // \return A std::chrono::duration representing the relative time until the next callback.
+  /** \return A std::chrono::duration representing the relative time until the next callback. */
   RCLCPP_PUBLIC
   std::chrono::nanoseconds
   time_until_trigger();
 
   /// Is the clock steady (i.e. is the time between ticks constant?)
-  // \return True if the clock used by this timer is steady.
+  /** \return True if the clock used by this timer is steady. */
   virtual bool is_steady() = 0;
 
   /// Check if the timer is ready to trigger the callback.
@@ -83,7 +85,7 @@ public:
   bool is_ready();
 
 protected:
-  rcl_timer_t timer_handle_ = rcl_get_zero_initialized_timer();
+  std::shared_ptr<rcl_timer_t> timer_handle_;
 };
 
 
@@ -120,15 +122,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()
   {
-    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;
     }
@@ -178,7 +177,6 @@ protected:
 template<typename CallbackType>
 using WallTimer = GenericTimer<CallbackType, std::chrono::steady_clock>;
 
-}  // namespace timer
 }  // namespace rclcpp
 
 #endif  // RCLCPP__TIMER_HPP_

rclcpp/include/rclcpp/utilities.hpp

@@ -17,6 +17,8 @@
 
 #include <chrono>
 #include <functional>
+#include <limits>
+#include <vector>
 
 #include "rclcpp/visibility_control.hpp"
 
@@ -44,9 +46,6 @@ std::string to_string(T value)
 
 namespace rclcpp
 {
-namespace utilities
-{
-
 /// Initialize communications via the rmw implementation and set up a global signal handler.
 /**
  * \param[in] argc Number of arguments.
@@ -54,10 +53,36 @@ namespace utilities
  */
 RCLCPP_PUBLIC
 void
-init(int argc, char * argv[]);
+init(int argc, char const * const argv[]);
+
+/// Initialize communications via the rmw implementation and set up a global signal handler.
+/**
+ * Additionally removes ROS-specific arguments from the argument vector.
+ * \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>
+init_and_remove_ros_arguments(int argc, char const * const argv[]);
+
+/// Remove ROS-specific arguments from argument vector.
+/**
+ * Some arguments may not have been intended as ROS arguments.
+ * This function populates a the aruments 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.
+/** \return True if SIGINT hasn't fired yet, false otherwise. */
 RCLCPP_PUBLIC
 bool
 ok();
@@ -68,39 +93,39 @@ void
 shutdown();
 
 /// Register a function to be called when shutdown is called.
-/* Calling the callbacks is the last thing shutdown() does. */
+/** 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.
 /**
- * The first time that this function is called for a given waitset a new guard
+ * 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 waitset. This mechanism is designed to ensure
- * that the same guard condition is not reused across waitsets (e.g., when
+ * 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[waitset] waitset Pointer to the rcl_wait_set_t that will be using the
+ * \param 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_sigint_guard_condition(rcl_wait_set_t * waitset);
+get_sigint_guard_condition(rcl_wait_set_t * wait_set);
 
 /// Release the previously allocated guard condition that manages the signal handler.
 /**
- * If you previously called get_sigint_guard_condition() for a given waitset
+ * 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 waitset.
- * \param[waitset] waitset Pointer to the rcl_wait_set_t that was using the
+ * 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.
  */
 RCLCPP_PUBLIC
 void
-release_sigint_guard_condition(rcl_wait_set_t * waitset);
+release_sigint_guard_condition(rcl_wait_set_t * wait_set);
 
 /// Use the global condition variable to block for the specified amount of time.
 /**
@@ -111,7 +136,74 @@ RCLCPP_PUBLIC
 bool
 sleep_for(const std::chrono::nanoseconds & nanoseconds);
 
-}  // namespace utilities
+/// 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));
+}
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__UTILITIES_HPP_

rclcpp/package.xml

@@ -2,32 +2,41 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp</name>
-  <version>0.0.0</version>
+  <version>0.5.1</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>rmw_implementation_cmake</build_depend>
+  <build_depend>rosgraph_msgs</build_depend>
   <build_depend>rosidl_generator_cpp</build_depend>
+  <build_depend>rosidl_typesupport_c</build_depend>
   <build_depend>rosidl_typesupport_cpp</build_depend>
+  <build_export_depend>builtin_interfaces</build_export_depend>
   <build_export_depend>rcl_interfaces</build_export_depend>
+  <build_export_depend>rosgraph_msgs</build_export_depend>
   <build_export_depend>rosidl_generator_cpp</build_export_depend>
+  <build_export_depend>rosidl_typesupport_c</build_export_depend>
   <build_export_depend>rosidl_typesupport_cpp</build_export_depend>
 
   <depend>rcl</depend>
+  <depend>rcl_yaml_param_parser</depend>
   <depend>rmw_implementation</depend>
 
   <exec_depend>ament_cmake</exec_depend>
 
+  <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>rmw</test_depend>
+  <test_depend>rmw_implementation_cmake</test_depend>
+  <test_depend>test_msgs</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

rclcpp/rclcpp-extras.cmake

@@ -25,6 +25,5 @@ macro(_rclcpp_register_package_hook)
   endif()
 endmacro()
 
-include("${rclcpp_DIR}/get_rclcpp_information.cmake")
 include("${rclcpp_DIR}/rclcpp_create_node_main.cmake")
 include("${rclcpp_DIR}/rclcpp_register_node_plugins.cmake")

rclcpp/resource/logging.hpp.em

@@ -0,0 +1,105 @@
+// generated from rclcpp/resource/logging.hpp.em
+
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__LOGGING_HPP_
+#define RCLCPP__LOGGING_HPP_
+
+#include <type_traits>
+
+#include "rclcpp/logger.hpp"
+#include "rcutils/logging_macros.h"
+
+// These are used for compiling out logging macros lower than a minimum severity.
+#define RCLCPP_LOG_MIN_SEVERITY_DEBUG 0
+#define RCLCPP_LOG_MIN_SEVERITY_INFO 1
+#define RCLCPP_LOG_MIN_SEVERITY_WARN 2
+#define RCLCPP_LOG_MIN_SEVERITY_ERROR 3
+#define RCLCPP_LOG_MIN_SEVERITY_FATAL 4
+#define RCLCPP_LOG_MIN_SEVERITY_NONE 5
+
+/**
+ * \def RCLCPP_LOG_MIN_SEVERITY
+ * Define RCLCPP_LOG_MIN_SEVERITY=RCLCPP_LOG_MIN_SEVERITY_[DEBUG|INFO|WARN|ERROR|FATAL]
+ * in your build options to compile out anything below that severity.
+ * Use RCLCPP_LOG_MIN_SEVERITY_NONE to compile out all macros.
+ */
+#ifndef RCLCPP_LOG_MIN_SEVERITY
+#define RCLCPP_LOG_MIN_SEVERITY RCLCPP_LOG_MIN_SEVERITY_DEBUG
+#endif
+
+@{
+from rcutils.logging import feature_combinations
+from rcutils.logging import get_macro_parameters
+from rcutils.logging import get_suffix_from_features
+from rcutils.logging import severities
+
+# TODO(dhood): Implement the throttle macro using time sources available in rclcpp
+excluded_features = ['named', 'throttle']
+def is_supported_feature_combination(feature_combination):
+    is_excluded = any([ef in feature_combination for ef in excluded_features])
+    return not is_excluded
+}@
+@[for severity in severities]@
+/** @@name Logging macros for severity @(severity).
+ */
+///@@{
+#if (RCLCPP_LOG_MIN_SEVERITY > RCLCPP_LOG_MIN_SEVERITY_@(severity))
+// empty logging macros for severity @(severity) when being disabled at compile time
+@[ for feature_combination in [fc for fc in feature_combinations if is_supported_feature_combination(fc)]]@
+@{suffix = get_suffix_from_features(feature_combination)}@
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_@(severity)@(suffix)(...)
+@[ end for]@
+
+#else
+@[ for feature_combination in [fc for fc in feature_combinations if is_supported_feature_combination(fc)]]@
+@{suffix = get_suffix_from_features(feature_combination)}@
+/**
+ * \def RCLCPP_@(severity)@(suffix)
+ * Log a message with severity @(severity)@
+@[ if feature_combinations[feature_combination].doc_lines]@
+ with the following conditions:
+@[ else]@
+.
+@[ end if]@
+@[ for doc_line in feature_combinations[feature_combination].doc_lines]@
+ * @(doc_line)
+@[ end for]@
+ * \param logger The `rclcpp::Logger` to use
+@[ for param_name, doc_line in feature_combinations[feature_combination].params.items()]@
+ * \param @(param_name) @(doc_line)
+@[ end for]@
+ * \param ... The format string, followed by the variable arguments for the format string
+ */
+#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( \
+@{params = get_macro_parameters(feature_combination).keys()}@
+@[ if params]@
+@(''.join(['    ' + p + ', \\\n' for p in params]))@
+@[ end if]@
+    logger.get_name(), \
+    __VA_ARGS__)
+
+@[ end for]@
+#endif
+///@@}
+
+@[end for]@
+
+#endif  // RCLCPP__LOGGING_HPP_

rclcpp/src/rclcpp/callback_group.cpp

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

rclcpp/src/rclcpp/client.cpp

@@ -14,6 +14,7 @@
 
 #include "rclcpp/client.hpp"
 
+#include <algorithm>
 #include <chrono>
 #include <cstdio>
 #include <memory>
@@ -26,40 +27,73 @@
 #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::client::ClientBase;
+using rclcpp::ClientBase;
 using rclcpp::exceptions::InvalidNodeError;
 using rclcpp::exceptions::throw_from_rcl_error;
 
 ClientBase::ClientBase(
   rclcpp::node_interfaces::NodeBaseInterface * node_base,
-  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
-  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)
-{}
-
-ClientBase::~ClientBase() {}
-
-const std::string &
-ClientBase::get_service_name() const
+  node_handle_(node_base->get_shared_rcl_node_handle())
 {
-  return this->service_name_;
+  std::weak_ptr<rcl_node_t> weak_node_handle(node_handle_);
+  client_handle_ = std::shared_ptr<rcl_client_t>(
+    new rcl_client_t, [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_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp"),
+            "Error in destruction of rcl client handle: %s", rcl_get_error_string_safe());
+          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;
+    });
+  *client_handle_.get() = rcl_get_zero_initialized_client();
 }
 
-const rcl_client_t *
+ClientBase::~ClientBase()
+{
+  // Make sure the client handle is destructed as early as possible and before the node handle
+  client_handle_.reset();
+}
+
+const char *
+ClientBase::get_service_name() const
+{
+  return rcl_client_get_service_name(this->get_client_handle().get());
+}
+
+std::shared_ptr<rcl_client_t>
+ClientBase::get_client_handle()
+{
+  return client_handle_;
+}
+
+std::shared_ptr<const rcl_client_t>
 ClientBase::get_client_handle() const
 {
-  return &client_handle_;
+  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 (ret != RCL_RET_OK) {
     throw_from_rcl_error(ret, "rcl_service_server_is_available failed");
   }
@@ -70,6 +104,12 @@ bool
 ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
 {
   auto start = std::chrono::steady_clock::now();
+  // make an event to reuse, rather than create a new one each time
+  auto node_ptr = node_graph_.lock();
+  if (!node_ptr) {
+    throw InvalidNodeError();
+  }
+  auto event = node_ptr->get_graph_event();
   // check to see if the server is ready immediately
   if (this->service_is_ready()) {
     return true;
@@ -78,12 +118,6 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
     // check was non-blocking, return immediately
     return false;
   }
-  // make an event to reuse, rather than create a new one each time
-  auto node_ptr = node_graph_.lock();
-  if (!node_ptr) {
-    throw InvalidNodeError();
-  }
-  auto event = node_ptr->get_graph_event();
   // update the time even on the first loop to account for time spent in the first call
   // to this->server_is_ready()
   std::chrono::nanoseconds time_to_wait = timeout - (std::chrono::steady_clock::now() - start);
@@ -93,28 +127,38 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
     time_to_wait = std::chrono::nanoseconds(0);
   }
   // continue forever if timeout is negative, otherwise continue until out of time_to_wait
-  // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
   do {
-    if (!rclcpp::utilities::ok()) {
+    if (!rclcpp::ok()) {
       return false;
     }
-    node_ptr->wait_for_graph_change(event, time_to_wait);
-    event->check_and_clear();  // reset the event
-
-    // always check if the service is ready, even if the graph event wasn't triggered
-    // this is needed to avoid a race condition that is specific to the Connext RMW implementation
+    // 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 (timeout < std::chrono::nanoseconds(0) || time_to_wait > std::chrono::nanoseconds(0));
-  // *INDENT-ON*
+  } while (time_to_wait > std::chrono::nanoseconds(0) || timeout < std::chrono::nanoseconds(0));
   return false;  // timeout exceeded while waiting for the server to be ready
 }
 
 rcl_node_t *
+ClientBase::get_rcl_node_handle()
+{
+  return node_handle_.get();
+}
+
+const rcl_node_t *
 ClientBase::get_rcl_node_handle() const
 {
   return node_handle_.get();

rclcpp/src/rclcpp/clock.cpp

@@ -0,0 +1,170 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/clock.hpp"
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "builtin_interfaces/msg/time.hpp"
+
+#include "rcl/time.h"
+
+#include "rclcpp/exceptions.hpp"
+
+#include "rcutils/logging_macros.h"
+
+namespace rclcpp
+{
+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)
+: pre_callback(pre_callback),
+  post_callback(post_callback),
+  notice_threshold(threshold)
+{}
+
+Clock::Clock(rcl_clock_type_t clock_type)
+{
+  allocator_ = rcl_get_default_allocator();
+  auto ret = rcl_clock_init(clock_type, &rcl_clock_, &allocator_);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "could not get current time stamp");
+  }
+}
+
+Clock::~Clock()
+{
+  auto ret = rcl_clock_fini(&rcl_clock_);
+  if (ret != RCL_RET_OK) {
+    RCUTILS_LOG_ERROR("Failed to fini rcl clock.");
+  }
+}
+
+Time
+Clock::now()
+{
+  Time now(0, 0, rcl_clock_.type);
+
+  auto ret = rcl_clock_get_now(&rcl_clock_, &now.rcl_time_);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "could not get current time stamp");
+  }
+
+  return now;
+}
+
+bool
+Clock::ros_time_is_active()
+{
+  if (!rcl_clock_valid(&rcl_clock_)) {
+    RCUTILS_LOG_ERROR("ROS time not valid!");
+    return false;
+  }
+
+  bool is_enabled;
+  auto ret = rcl_is_enabled_ros_time_override(&rcl_clock_, &is_enabled);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "Failed to check ros_time_override_status");
+  }
+  return is_enabled;
+}
+
+rcl_clock_type_t
+Clock::get_clock_type()
+{
+  return rcl_clock_.type;
+}
+
+rclcpp::JumpHandler::SharedPtr
+Clock::create_jump_callback(
+  std::function<void()> pre_callback,
+  std::function<void(const TimeJump &)> post_callback,
+  JumpThreshold & 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);
+  }
+  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();
+  }
+}
+
+void
+Clock::invoke_postjump_callbacks(
+  const std::vector<JumpHandler::SharedPtr> & callbacks,
+  const TimeJump & jump)
+{
+  for (auto cb : callbacks) {
+    cb->post_callback(jump);
+  }
+}
+
+}  // namespace rclcpp

rclcpp/src/rclcpp/context.cpp

@@ -14,6 +14,6 @@
 
 #include "rclcpp/context.hpp"
 
-using rclcpp::context::Context;
+using rclcpp::Context;
 
 Context::Context() {}

rclcpp/src/rclcpp/duration.cpp

@@ -0,0 +1,220 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <cmath>
+#include <cstdlib>
+#include <limits>
+#include <utility>
+
+#include "rclcpp/clock.hpp"
+#include "rclcpp/time.hpp"
+
+#include "builtin_interfaces/msg/duration.hpp"
+
+#include "rcl/time.h"
+
+#include "rclcpp/exceptions.hpp"
+
+#include "rcutils/logging_macros.h"
+
+namespace rclcpp
+{
+
+Duration::Duration(int32_t seconds, uint32_t nanoseconds)
+{
+  rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast<int64_t>(seconds));
+  rcl_duration_.nanoseconds += nanoseconds;
+}
+
+Duration::Duration(int64_t nanoseconds)
+{
+  rcl_duration_.nanoseconds = nanoseconds;
+}
+
+Duration::Duration(std::chrono::nanoseconds nanoseconds)
+{
+  rcl_duration_.nanoseconds = nanoseconds.count();
+}
+
+Duration::Duration(const Duration & rhs)
+{
+  rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds;
+}
+
+Duration::Duration(
+  const builtin_interfaces::msg::Duration & duration_msg)
+{
+  rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast<uint64_t>(duration_msg.sec));
+  rcl_duration_.nanoseconds += duration_msg.nanosec;
+}
+
+Duration::Duration(const rcl_duration_t & duration)
+: rcl_duration_(duration)
+{
+  // noop
+}
+
+Duration::~Duration()
+{
+}
+
+Duration::operator builtin_interfaces::msg::Duration() const
+{
+  builtin_interfaces::msg::Duration msg_duration;
+  msg_duration.sec = static_cast<std::int32_t>(RCL_NS_TO_S(rcl_duration_.nanoseconds));
+  msg_duration.nanosec =
+    static_cast<std::uint32_t>(rcl_duration_.nanoseconds % (1000 * 1000 * 1000));
+  return msg_duration;
+}
+
+Duration &
+Duration::operator=(const Duration & rhs)
+{
+  rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds;
+  return *this;
+}
+
+Duration &
+Duration::operator=(const builtin_interfaces::msg::Duration & duration_msg)
+{
+  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;
+}
+
+bool
+Duration::operator==(const rclcpp::Duration & rhs) const
+{
+  return rcl_duration_.nanoseconds == rhs.rcl_duration_.nanoseconds;
+}
+
+bool
+Duration::operator<(const rclcpp::Duration & rhs) const
+{
+  return rcl_duration_.nanoseconds < rhs.rcl_duration_.nanoseconds;
+}
+
+bool
+Duration::operator<=(const rclcpp::Duration & rhs) const
+{
+  return rcl_duration_.nanoseconds <= rhs.rcl_duration_.nanoseconds;
+}
+
+bool
+Duration::operator>=(const rclcpp::Duration & rhs) const
+{
+  return rcl_duration_.nanoseconds >= rhs.rcl_duration_.nanoseconds;
+}
+
+bool
+Duration::operator>(const rclcpp::Duration & rhs) const
+{
+  return rcl_duration_.nanoseconds > rhs.rcl_duration_.nanoseconds;
+}
+
+void
+bounds_check_duration_sum(int64_t lhsns, int64_t rhsns, uint64_t max)
+{
+  auto abs_lhs = (uint64_t)std::abs(lhsns);
+  auto abs_rhs = (uint64_t)std::abs(rhsns);
+
+  if (lhsns > 0 && rhsns > 0) {
+    if (abs_lhs + abs_rhs > (uint64_t) max) {
+      throw std::overflow_error("addition leads to int64_t overflow");
+    }
+  } else if (lhsns < 0 && rhsns < 0) {
+    if (abs_lhs + abs_rhs > (uint64_t) max) {
+      throw std::underflow_error("addition leads to int64_t underflow");
+    }
+  }
+}
+
+Duration
+Duration::operator+(const rclcpp::Duration & rhs) const
+{
+  bounds_check_duration_sum(
+    this->rcl_duration_.nanoseconds,
+    rhs.rcl_duration_.nanoseconds,
+    std::numeric_limits<rcl_duration_value_t>::max());
+  return Duration(
+    rcl_duration_.nanoseconds + rhs.rcl_duration_.nanoseconds);
+}
+
+void
+bounds_check_duration_difference(int64_t lhsns, int64_t rhsns, uint64_t max)
+{
+  auto abs_lhs = (uint64_t)std::abs(lhsns);
+  auto abs_rhs = (uint64_t)std::abs(rhsns);
+
+  if (lhsns > 0 && rhsns < 0) {
+    if (abs_lhs + abs_rhs > (uint64_t) max) {
+      throw std::overflow_error("duration subtraction leads to int64_t overflow");
+    }
+  } else if (lhsns < 0 && rhsns > 0) {
+    if (abs_lhs + abs_rhs > (uint64_t) max) {
+      throw std::underflow_error("duration subtraction leads to int64_t underflow");
+    }
+  }
+}
+
+Duration
+Duration::operator-(const rclcpp::Duration & rhs) const
+{
+  bounds_check_duration_difference(
+    this->rcl_duration_.nanoseconds,
+    rhs.rcl_duration_.nanoseconds,
+    std::numeric_limits<rcl_duration_value_t>::max());
+
+  return Duration(
+    rcl_duration_.nanoseconds - rhs.rcl_duration_.nanoseconds);
+}
+
+void
+bounds_check_duration_scale(int64_t dns, double scale, uint64_t max)
+{
+  auto abs_dns = static_cast<uint64_t>(std::abs(dns));
+  auto abs_scale = std::abs(scale);
+
+  if (abs_scale > 1.0 && abs_dns > static_cast<uint64_t>(max / abs_scale)) {
+    if ((dns > 0 && scale > 0) || (dns < 0 && scale < 0)) {
+      throw std::overflow_error("duration scaling leads to int64_t overflow");
+    } else {
+      throw std::underflow_error("duration scaling leads to int64_t underflow");
+    }
+  }
+}
+
+Duration
+Duration::operator*(double scale) const
+{
+  if (!std::isfinite(scale)) {
+    throw std::runtime_error("abnormal scale in rclcpp::Duration");
+  }
+  bounds_check_duration_scale(
+    this->rcl_duration_.nanoseconds,
+    scale,
+    std::numeric_limits<rcl_duration_value_t>::max());
+  return Duration(static_cast<rcl_duration_value_t>(rcl_duration_.nanoseconds * scale));
+}
+
+rcl_duration_value_t
+Duration::nanoseconds() const
+{
+  return rcl_duration_.nanoseconds;
+}
+
+}  // namespace rclcpp

rclcpp/src/rclcpp/event.cpp

@@ -16,8 +16,6 @@
 
 namespace rclcpp
 {
-namespace event
-{
 
 Event::Event()
 : state_(false) {}
@@ -40,5 +38,4 @@ Event::check_and_clear()
   return state_.exchange(false);
 }
 
-}  // namespace event
 }  // namespace rclcpp

rclcpp/src/rclcpp/exceptions.cpp

@@ -18,18 +18,40 @@
 #include <functional>
 #include <string>
 
+using namespace std::string_literals;
+
 namespace rclcpp
 {
 namespace exceptions
 {
 
+std::string
+NameValidationError::format_error(
+  const char * name_type,
+  const char * name,
+  const char * error_msg,
+  size_t invalid_index)
+{
+  std::string msg = "";
+  msg += "Invalid "s + name_type + ": " + error_msg + ":\n";
+  msg += "  '"s + name + "'\n";
+  msg += "   "s + std::string(invalid_index, ' ') + "^\n";
+  return msg;
+}
+
 void
-throw_from_rcl_error(rcl_ret_t ret, const std::string & prefix, bool reset_error)
+throw_from_rcl_error(
+  rcl_ret_t ret,
+  const std::string & prefix,
+  const rcl_error_state_t * error_state,
+  void (*reset_error)())
 {
   if (RCL_RET_OK == ret) {
     throw std::invalid_argument("ret is RCL_RET_OK");
   }
-  const rcl_error_state_t * error_state = rcl_get_error_state();
+  if (!error_state) {
+    error_state = rcl_get_error_state();
+  }
   if (!error_state) {
     throw std::runtime_error("rcl error state is not set");
   }
@@ -39,7 +61,7 @@ throw_from_rcl_error(rcl_ret_t ret, const std::string & prefix, bool reset_error
   }
   RCLErrorBase base_exc(ret, error_state);
   if (reset_error) {
-    rcl_reset_error();
+    reset_error();
   }
   switch (ret) {
     case RCL_RET_BAD_ALLOC:

rclcpp/src/rclcpp/executor.cpp

@@ -20,6 +20,7 @@
 #include "rcl/allocator.h"
 #include "rcl/error_handling.h"
 
+#include "rclcpp/exceptions.hpp"
 #include "rclcpp/executor.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/scope_exit.hpp"
@@ -27,6 +28,8 @@
 
 #include "rcl_interfaces/msg/intra_process_message.hpp"
 
+#include "rcutils/logging_macros.h"
+
 using rclcpp::executor::AnyExecutable;
 using rclcpp::executor::Executor;
 using rclcpp::executor::ExecutorArgs;
@@ -49,41 +52,59 @@ Executor::Executor(const ExecutorArgs & args)
   // and one for the executor's guard cond (interrupt_guard_condition_)
 
   // Put the global ctrl-c guard condition in
-  memory_strategy_->add_guard_condition(rclcpp::utilities::get_sigint_guard_condition(&waitset_));
+  memory_strategy_->add_guard_condition(rclcpp::get_sigint_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(
-      &waitset_, 0, 2, 0, 0, 0, allocator) != RCL_RET_OK)
+      &wait_set_, 0, 2, 0, 0, 0, allocator) != RCL_RET_OK)
   {
-    fprintf(stderr,
-      "[rclcpp::error] failed to create waitset: %s\n", rcl_get_error_string_safe());
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "failed to create wait set: %s", rcl_get_error_string_safe());
+    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_safe());
+      rcl_reset_error();
     }
-    throw std::runtime_error("Failed to create waitset in Executor constructor");
+    throw std::runtime_error("Failed to create wait set in Executor constructor");
   }
 }
 
 Executor::~Executor()
 {
-  // Finalize the waitset.
-  if (rcl_wait_set_fini(&waitset_) != RCL_RET_OK) {
-    fprintf(stderr,
-      "[rclcpp::error] failed to destroy waitset: %s\n", rcl_get_error_string_safe());
+  // Disassocate all nodes
+  for (auto & weak_node : weak_nodes_) {
+    auto node = weak_node.lock();
+    if (node) {
+      std::atomic_bool & has_executor = node->get_associated_with_executor_atomic();
+      has_executor.store(false);
+    }
+  }
+  weak_nodes_.clear();
+
+  // Finalize the wait set.
+  if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) {
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "failed to destroy wait set: %s", rcl_get_error_string_safe());
+    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_safe());
+    rcl_reset_error();
   }
   // Remove and release the sigint guard condition
   memory_strategy_->remove_guard_condition(
-    rclcpp::utilities::get_sigint_guard_condition(&waitset_));
-  rclcpp::utilities::release_sigint_guard_condition(&waitset_);
+    rclcpp::get_sigint_guard_condition(&wait_set_));
+  rclcpp::release_sigint_guard_condition(&wait_set_);
 }
 
 void
@@ -114,7 +135,7 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt
 }
 
 void
-Executor::add_node(std::shared_ptr<rclcpp::node::Node> node_ptr, bool notify)
+Executor::add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify)
 {
   this->add_node(node_ptr->get_node_base_interface(), notify);
 }
@@ -126,14 +147,12 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node
   weak_nodes_.erase(
     std::remove_if(
       weak_nodes_.begin(), weak_nodes_.end(),
-      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
       [&](rclcpp::node_interfaces::NodeBaseInterface::WeakPtr & i)
       {
         bool matched = (i.lock() == node_ptr);
         node_removed |= matched;
         return matched;
       }
-      // *INDENT-ON*
     )
   );
   std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
@@ -150,7 +169,7 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node
 }
 
 void
-Executor::remove_node(std::shared_ptr<rclcpp::node::Node> node_ptr, bool notify)
+Executor::remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify)
 {
   this->remove_node(node_ptr->get_node_base_interface(), notify);
 }
@@ -175,7 +194,7 @@ Executor::spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr n
 }
 
 void
-Executor::spin_node_some(std::shared_ptr<rclcpp::node::Node> node)
+Executor::spin_node_some(std::shared_ptr<rclcpp::Node> node)
 {
   this->spin_node_some(node->get_node_base_interface());
 }
@@ -187,9 +206,13 @@ Executor::spin_some()
     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()) {
+    AnyExecutable any_exec;
+    if (get_next_executable(any_exec, std::chrono::milliseconds::zero())) {
+      execute_any_executable(any_exec);
+    } else {
+      break;
+    }
   }
 }
 
@@ -200,8 +223,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);
   }
 }
@@ -225,28 +248,28 @@ 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);
   }
   // 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) {
@@ -256,32 +279,53 @@ Executor::execute_any_executable(AnyExecutable::SharedPtr any_exec)
 
 void
 Executor::execute_subscription(
-  rclcpp::subscription::SubscriptionBase::SharedPtr 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_safe());
+      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());
+    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_safe());
+      rcl_reset_error();
+    }
+    subscription->return_message(message);
   }
-  subscription->return_message(message);
 }
 
 void
 Executor::execute_intra_process_subscription(
-  rclcpp::subscription::SubscriptionBase::SharedPtr subscription)
+  rclcpp::SubscriptionBase::SharedPtr subscription)
 {
   rcl_interfaces::msg::IntraProcessMessage ipm;
   rmw_message_info_t message_info;
   rcl_ret_t status = rcl_take(
-    subscription->get_intra_process_subscription_handle(),
+    subscription->get_intra_process_subscription_handle().get(),
     &ipm,
     &message_info);
 
@@ -289,54 +333,60 @@ 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",
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "take failed for intra process subscription on topic '%s': %s",
       subscription->get_topic_name(), rcl_get_error_string_safe());
+    rcl_reset_error();
   }
 }
 
 void
 Executor::execute_timer(
-  rclcpp::timer::TimerBase::SharedPtr timer)
+  rclcpp::TimerBase::SharedPtr timer)
 {
   timer->execute_callback();
 }
 
 void
 Executor::execute_service(
-  rclcpp::service::ServiceBase::SharedPtr service)
+  rclcpp::ServiceBase::SharedPtr service)
 {
   auto request_header = service->create_request_header();
   std::shared_ptr<void> request = service->create_request();
   rcl_ret_t status = rcl_take_request(
-    service->get_service_handle(),
+    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_safe());
+    rcl_reset_error();
   }
 }
 
 void
 Executor::execute_client(
-  rclcpp::client::ClientBase::SharedPtr client)
+  rclcpp::ClientBase::SharedPtr client)
 {
   auto request_header = client->create_request_header();
   std::shared_ptr<void> response = client->create_response();
   rcl_ret_t status = rcl_take_response(
-    client->get_client_handle(),
+    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_SERVICE_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());
+  } else if (status != RCL_RET_CLIENT_TAKE_FAILED) {
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "take response failed for client of service '%s': %s",
+      client->get_service_name(), rcl_get_error_string_safe());
+    rcl_reset_error();
   }
 }
 
@@ -352,85 +402,87 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
     weak_nodes_.erase(
       remove_if(
         weak_nodes_.begin(), weak_nodes_.end(),
-        // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
         [](rclcpp::node_interfaces::NodeBaseInterface::WeakPtr i)
         {
           return i.expired();
         }
-        // *INDENT-ON*
       )
     );
   }
+  // clear 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");
+  }
 
   if (rcl_wait_set_resize_subscriptions(
-      &waitset_, memory_strategy_->number_of_ready_subscriptions()) != RCL_RET_OK)
+      &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 waitset : ") +
+            std::string("Couldn't resize the number of subscriptions in wait set : ") +
             rcl_get_error_string_safe());
   }
 
   if (rcl_wait_set_resize_services(
-      &waitset_, memory_strategy_->number_of_ready_services()) != RCL_RET_OK)
+      &wait_set_, memory_strategy_->number_of_ready_services()) != RCL_RET_OK)
   {
     throw std::runtime_error(
-            std::string("Couldn't resize the number of services in waitset : ") +
+            std::string("Couldn't resize the number of services in wait set : ") +
             rcl_get_error_string_safe());
   }
 
   if (rcl_wait_set_resize_clients(
-      &waitset_, memory_strategy_->number_of_ready_clients()) != RCL_RET_OK)
+      &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 waitset : ") +
+            std::string("Couldn't resize the number of clients in wait set : ") +
             rcl_get_error_string_safe());
   }
 
   if (rcl_wait_set_resize_guard_conditions(
-      &waitset_, memory_strategy_->number_of_guard_conditions()) != RCL_RET_OK)
+      &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 waitset : ") +
+            std::string("Couldn't resize the number of guard_conditions in wait set : ") +
             rcl_get_error_string_safe());
   }
 
   if (rcl_wait_set_resize_timers(
-      &waitset_, memory_strategy_->number_of_ready_timers()) != RCL_RET_OK)
+      &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 waitset : ") +
+            std::string("Couldn't resize the number of timers in wait set : ") +
             rcl_get_error_string_safe());
   }
 
-  if (!memory_strategy_->add_handles_to_waitset(&waitset_)) {
-    throw std::runtime_error("Couldn't fill waitset");
+  if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) {
+    throw std::runtime_error("Couldn't fill wait set");
   }
   rcl_ret_t status =
-    rcl_wait(&waitset_, std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count());
+    rcl_wait(&wait_set_, std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count());
   if (status == RCL_RET_WAIT_SET_EMPTY) {
-    fprintf(stderr, "Warning: empty waitset received in rcl_wait(). This should never happen.\n");
+    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) {
-    throw std::runtime_error(std::string("rcl_wait() failed: ") + rcl_get_error_string_safe());
+    using rclcpp::exceptions::throw_from_rcl_error;
+    throw_from_rcl_error(status, "rcl_wait() failed");
   }
 
-  // check the null handles in the waitset and remove them from the handles in memory strategy
+  // 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(&waitset_);
-  if (rcl_wait_set_clear_subscriptions(&waitset_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear subscriptions from waitset");
-  }
-  if (rcl_wait_set_clear_services(&waitset_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear servicess from waitset");
-  }
-  if (rcl_wait_set_clear_clients(&waitset_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear clients from waitset");
-  }
-  if (rcl_wait_set_clear_guard_conditions(&waitset_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear guard conditions from waitset");
-  }
-  if (rcl_wait_set_clear_timers(&waitset_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear timers from waitset");
-  }
+  memory_strategy_->remove_null_handles(&wait_set_);
 }
 
 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
@@ -455,7 +507,7 @@ Executor::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr gro
 }
 
 rclcpp::callback_group::CallbackGroup::SharedPtr
-Executor::get_group_by_timer(rclcpp::timer::TimerBase::SharedPtr timer)
+Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer)
 {
   for (auto & weak_node : weak_nodes_) {
     auto node = weak_node.lock();
@@ -479,7 +531,7 @@ Executor::get_group_by_timer(rclcpp::timer::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();
@@ -494,8 +546,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;
         }
@@ -504,67 +556,69 @@ 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;
   }
   // 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 &
@@ -574,7 +628,7 @@ rclcpp::executor::operator<<(std::ostream & os, const FutureReturnCode & future_
 }
 
 std::string
-rclcpp::executor::to_string(const FutureReturnCode & future_return_code)
+rclcpp::executor::to_string(const FutureReturnCode &future_return_code)
 {
   using enum_type = std::underlying_type<FutureReturnCode>::type;
   std::string prefix = "Unknown enum value (";

rclcpp/src/rclcpp/executors.cpp

@@ -22,7 +22,7 @@ rclcpp::spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr
 }
 
 void
-rclcpp::spin_some(rclcpp::node::Node::SharedPtr node_ptr)
+rclcpp::spin_some(rclcpp::Node::SharedPtr node_ptr)
 {
   rclcpp::spin_some(node_ptr->get_node_base_interface());
 }
@@ -37,7 +37,7 @@ rclcpp::spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
 }
 
 void
-rclcpp::spin(rclcpp::node::Node::SharedPtr node_ptr)
+rclcpp::spin(rclcpp::Node::SharedPtr node_ptr)
 {
   rclcpp::spin(node_ptr->get_node_base_interface());
 }

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -16,17 +16,21 @@
 
 #include <chrono>
 #include <functional>
+#include <memory>
 #include <vector>
 
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/scope_exit.hpp"
 
-using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor;
+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,37 @@ MultiThreadedExecutor::get_number_of_threads()
 void
 MultiThreadedExecutor::run(size_t)
 {
-  while (rclcpp::utilities::ok() && spinning.load()) {
-    executor::AnyExecutable::SharedPtr any_exec;
+  while (rclcpp::ok() && spinning.load()) {
+    executor::AnyExecutable any_exec;
     {
       std::lock_guard<std::mutex> wait_lock(wait_mutex_);
-      if (!rclcpp::utilities::ok() || !spinning.load()) {
+      if (!rclcpp::ok() || !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.
+        std::lock_guard<std::mutex> lock(scheduled_timers_mutex_);
+        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> lock(scheduled_timers_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,9 +13,10 @@
 // limitations under the License.
 
 #include "rclcpp/executors/single_threaded_executor.hpp"
+#include "rclcpp/any_executable.hpp"
 #include "rclcpp/scope_exit.hpp"
 
-using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor;
+using rclcpp::executors::SingleThreadedExecutor;
 
 SingleThreadedExecutor::SingleThreadedExecutor(const rclcpp::executor::ExecutorArgs & args)
 : executor::Executor(args) {}
@@ -26,11 +27,13 @@ void
 SingleThreadedExecutor::spin()
 {
   if (spinning.exchange(true)) {
-    throw std::runtime_error("spin_some() called while already spinning");
+    throw std::runtime_error("spin() called while already spinning");
   }
   RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
-  while (rclcpp::utilities::ok() && spinning.load()) {
-    auto any_exec = get_next_executable();
-    execute_any_executable(any_exec);
+  while (rclcpp::ok() && 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

@@ -0,0 +1,210 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/expand_topic_or_service_name.hpp"
+
+#include <string>
+
+#include "rcl/expand_topic_name.h"
+#include "rcl/validate_topic_name.h"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/scope_exit.hpp"
+#include "rcutils/logging_macros.h"
+#include "rcutils/types/string_map.h"
+#include "rmw/error_handling.h"
+#include "rmw/validate_namespace.h"
+#include "rmw/validate_node_name.h"
+#include "rmw/validate_full_topic_name.h"
+
+using rclcpp::exceptions::throw_from_rcl_error;
+
+std::string
+rclcpp::expand_topic_or_service_name(
+  const std::string & name,
+  const std::string & node_name,
+  const std::string & namespace_,
+  bool is_service)
+{
+  char * expanded_topic = nullptr;
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  rcutils_allocator_t rcutils_allocator = rcutils_get_default_allocator();
+  rcutils_string_map_t substitutions_map = rcutils_get_zero_initialized_string_map();
+
+  rcutils_ret_t rcutils_ret = rcutils_string_map_init(&substitutions_map, 0, rcutils_allocator);
+  if (rcutils_ret != RCUTILS_RET_OK) {
+    if (rcutils_ret == RCUTILS_RET_BAD_ALLOC) {
+      throw_from_rcl_error(RCL_RET_BAD_ALLOC, "", rcutils_get_error_state(), rcutils_reset_error);
+    } else {
+      throw_from_rcl_error(RCL_RET_ERROR, "", rcutils_get_error_state(), rcutils_reset_error);
+    }
+  }
+  rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&substitutions_map);
+  if (ret != RCL_RET_OK) {
+    rcutils_error_state_t error_state;
+    if (rcutils_error_state_copy(rcl_get_error_state(), &error_state) != RCUTILS_RET_OK) {
+      throw std::bad_alloc();
+    }
+    auto error_state_scope_exit = rclcpp::make_scope_exit(
+      [&error_state]() {
+        rcutils_error_state_fini(&error_state);
+      });
+    // finalize the string map before throwing
+    rcutils_ret = rcutils_string_map_fini(&substitutions_map);
+    if (rcutils_ret != RCUTILS_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "failed to fini string_map (%d) during error handling: %s",
+        rcutils_ret,
+        rcutils_get_error_string_safe());
+      rcutils_reset_error();
+    }
+    throw_from_rcl_error(ret, "", &error_state);
+  }
+
+  ret = rcl_expand_topic_name(
+    name.c_str(),
+    node_name.c_str(),
+    namespace_.c_str(),
+    &substitutions_map,
+    allocator,
+    &expanded_topic);
+
+  std::string result;
+  if (ret == RCL_RET_OK) {
+    result = expanded_topic;
+    allocator.deallocate(expanded_topic, allocator.state);
+  }
+
+  rcutils_ret = rcutils_string_map_fini(&substitutions_map);
+  if (rcutils_ret != RCUTILS_RET_OK) {
+    throw_from_rcl_error(RCL_RET_ERROR, "", rcutils_get_error_state(), rcutils_reset_error);
+  }
+
+  // expansion failed
+  if (ret != RCL_RET_OK) {
+    // if invalid topic or unknown substitution
+    if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) {
+      rcl_reset_error();  // explicitly discard error from rcl_expand_topic_name()
+      int validation_result;
+      size_t invalid_index;
+      rcl_ret_t ret = rcl_validate_topic_name(name.c_str(), &validation_result, &invalid_index);
+      if (ret != RCL_RET_OK) {
+        throw_from_rcl_error(ret);
+      }
+
+      if (validation_result != RCL_TOPIC_NAME_VALID) {
+        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");
+      }
+
+      // if invalid node name
+    } else if (ret == RCL_RET_NODE_INVALID_NAME) {
+      rcl_reset_error();  // explicitly discard error from rcl_expand_topic_name()
+      int validation_result;
+      size_t invalid_index;
+      rmw_ret_t rmw_ret =
+        rmw_validate_node_name(node_name.c_str(), &validation_result, &invalid_index);
+      if (rmw_ret != RMW_RET_OK) {
+        if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
+          throw_from_rcl_error(
+            RCL_RET_INVALID_ARGUMENT, "failed to validate node name",
+            rmw_get_error_state(), rmw_reset_error);
+        }
+        throw_from_rcl_error(
+          RCL_RET_ERROR, "failed to validate node name",
+          rmw_get_error_state(), rmw_reset_error);
+      }
+
+      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()
+      int validation_result;
+      size_t invalid_index;
+      rmw_ret_t rmw_ret =
+        rmw_validate_namespace(namespace_.c_str(), &validation_result, &invalid_index);
+      if (rmw_ret != RMW_RET_OK) {
+        if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
+          throw_from_rcl_error(
+            RCL_RET_INVALID_ARGUMENT, "failed to validate namespace",
+            rmw_get_error_state(), rmw_reset_error);
+        }
+        throw_from_rcl_error(
+          RCL_RET_ERROR, "failed to validate namespace",
+          rmw_get_error_state(), rmw_reset_error);
+      }
+
+      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);
+    }
+  }
+
+  // expand succeeded, but full name validation may fail still
+  int validation_result;
+  size_t invalid_index;
+  rmw_ret_t rmw_ret =
+    rmw_validate_full_topic_name(result.c_str(), &validation_result, &invalid_index);
+  if (rmw_ret != RMW_RET_OK) {
+    if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
+      throw_from_rcl_error(
+        RCL_RET_INVALID_ARGUMENT, "failed to validate full topic name",
+        rmw_get_error_state(), rmw_reset_error);
+    }
+    throw_from_rcl_error(
+      RCL_RET_ERROR, "failed to validate full topic name",
+      rmw_get_error_state(), rmw_reset_error);
+  }
+
+  if (validation_result != RMW_TOPIC_VALID) {
+    if (is_service) {
+      throw rclcpp::exceptions::InvalidServiceNameError(
+              result.c_str(),
+              rmw_full_topic_name_validation_result_string(validation_result),
+              invalid_index);
+    } else {
+      throw rclcpp::exceptions::InvalidTopicNameError(
+              result.c_str(),
+              rmw_full_topic_name_validation_result_string(validation_result),
+              invalid_index);
+    }
+  }
+
+  return result;
+}

rclcpp/src/rclcpp/graph_listener.cpp

@@ -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
@@ -43,7 +45,7 @@ GraphListener::GraphListener()
     throw_from_rcl_error(ret, "failed to create interrupt guard condition");
   }
 
-  shutdown_guard_condition_ = rclcpp::utilities::get_sigint_guard_condition(&wait_set_);
+  shutdown_guard_condition_ = rclcpp::get_sigint_guard_condition(&wait_set_);
 }
 
 GraphListener::~GraphListener()
@@ -75,12 +77,13 @@ GraphListener::start_if_not_started()
     // This is important to ensure that the wait set is finalized before
     // destruction of static objects occurs.
     std::weak_ptr<GraphListener> weak_this = shared_from_this();
-    rclcpp::utilities::on_shutdown([weak_this]() {
-          auto shared_this = weak_this.lock();
-          if (shared_this) {
-            shared_this->shutdown();
-          }
-        });
+    rclcpp::on_shutdown(
+      [weak_this]() {
+        auto shared_this = weak_this.lock();
+        if (shared_this) {
+          shared_this->shutdown();
+        }
+      });
     // Start the listener thread.
     listener_thread_ = std::thread(&GraphListener::run, this);
     is_started_ = true;
@@ -93,13 +96,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());
   }
 }
@@ -334,7 +340,7 @@ GraphListener::shutdown()
       throw_from_rcl_error(ret, "failed to finalize interrupt guard condition");
     }
     if (shutdown_guard_condition_) {
-      rclcpp::utilities::release_sigint_guard_condition(&wait_set_);
+      rclcpp::release_sigint_guard_condition(&wait_set_);
       shutdown_guard_condition_ = nullptr;
     }
     if (is_started_) {

rclcpp/src/rclcpp/intra_process_manager.cpp

@@ -31,7 +31,7 @@ IntraProcessManager::~IntraProcessManager()
 
 uint64_t
 IntraProcessManager::add_subscription(
-  rclcpp::subscription::SubscriptionBase::SharedPtr subscription)
+  rclcpp::SubscriptionBase::SharedPtr subscription)
 {
   auto id = IntraProcessManager::get_next_unique_id();
   impl_->add_subscription(id, subscription);

rclcpp/src/rclcpp/logger.cpp

@@ -0,0 +1,33 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+
+#include "rclcpp/logger.hpp"
+
+namespace rclcpp
+{
+
+rclcpp::Logger
+get_logger(const std::string & name)
+{
+#if RCLCPP_LOGGING_ENABLED
+  return rclcpp::Logger(name);
+#else
+  (void)name;
+  return rclcpp::Logger();
+#endif
+}
+
+}  // 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::subscription::SubscriptionBase::SharedPtr
+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();
@@ -46,8 +48,9 @@ MemoryStrategy::get_subscription_by_handle(
   return nullptr;
 }
 
-rclcpp::service::ServiceBase::SharedPtr
-MemoryStrategy::get_service_by_handle(const rcl_service_t * service_handle,
+rclcpp::ServiceBase::SharedPtr
+MemoryStrategy::get_service_by_handle(
+  std::shared_ptr<const rcl_service_t> service_handle,
   const WeakNodeVector & weak_nodes)
 {
   for (auto & weak_node : weak_nodes) {
@@ -71,8 +74,9 @@ MemoryStrategy::get_service_by_handle(const rcl_service_t * service_handle,
   return nullptr;
 }
 
-rclcpp::client::ClientBase::SharedPtr
-MemoryStrategy::get_client_by_handle(const rcl_client_t * client_handle,
+rclcpp::ClientBase::SharedPtr
+MemoryStrategy::get_client_by_handle(
+  std::shared_ptr<const rcl_client_t> client_handle,
   const WeakNodeVector & weak_nodes)
 {
   for (auto & weak_node : weak_nodes) {
@@ -97,7 +101,8 @@ MemoryStrategy::get_client_by_handle(const rcl_client_t * client_handle,
 }
 
 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
-MemoryStrategy::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group,
+MemoryStrategy::get_node_by_group(
+  rclcpp::callback_group::CallbackGroup::SharedPtr group,
   const WeakNodeVector & weak_nodes)
 {
   if (!group) {
@@ -120,7 +125,7 @@ MemoryStrategy::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedP
 
 rclcpp::callback_group::CallbackGroup::SharedPtr
 MemoryStrategy::get_group_by_subscription(
-  rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
+  rclcpp::SubscriptionBase::SharedPtr subscription,
   const WeakNodeVector & weak_nodes)
 {
   for (auto & weak_node : weak_nodes) {
@@ -146,7 +151,7 @@ MemoryStrategy::get_group_by_subscription(
 
 rclcpp::callback_group::CallbackGroup::SharedPtr
 MemoryStrategy::get_group_by_service(
-  rclcpp::service::ServiceBase::SharedPtr service,
+  rclcpp::ServiceBase::SharedPtr service,
   const WeakNodeVector & weak_nodes)
 {
   for (auto & weak_node : weak_nodes) {
@@ -172,7 +177,7 @@ MemoryStrategy::get_group_by_service(
 
 rclcpp::callback_group::CallbackGroup::SharedPtr
 MemoryStrategy::get_group_by_client(
-  rclcpp::client::ClientBase::SharedPtr client,
+  rclcpp::ClientBase::SharedPtr client,
   const WeakNodeVector & weak_nodes)
 {
   for (auto & weak_node : weak_nodes) {

rclcpp/src/rclcpp/node.cpp

@@ -24,34 +24,61 @@
 #include "rclcpp/graph_listener.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/node_interfaces/node_base.hpp"
+#include "rclcpp/node_interfaces/node_clock.hpp"
 #include "rclcpp/node_interfaces/node_graph.hpp"
+#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_timers.hpp"
 #include "rclcpp/node_interfaces/node_topics.hpp"
 
-using rclcpp::node::Node;
+using rclcpp::Node;
 using rclcpp::exceptions::throw_from_rcl_error;
 
-Node::Node(const std::string & node_name, bool use_intra_process_comms)
+Node::Node(
+  const std::string & node_name,
+  const std::string & namespace_,
+  bool use_intra_process_comms)
 : Node(
     node_name,
+    namespace_,
     rclcpp::contexts::default_context::get_global_default_context(),
-    use_intra_process_comms)
+    {},
+    {},
+    true,
+    use_intra_process_comms,
+    true)
 {}
 
 Node::Node(
   const std::string & node_name,
-  rclcpp::context::Context::SharedPtr context,
-  bool use_intra_process_comms)
-: node_base_(new rclcpp::node_interfaces::NodeBase(node_name, context)),
+  const std::string & namespace_,
+  rclcpp::Context::SharedPtr context,
+  const std::vector<std::string> & arguments,
+  const std::vector<rclcpp::Parameter> & initial_parameters,
+  bool use_global_arguments,
+  bool use_intra_process_comms,
+  bool start_parameter_services)
+: node_base_(new rclcpp::node_interfaces::NodeBase(
+      node_name, namespace_, context, arguments, use_global_arguments)),
   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_base_,
+      node_topics_,
+      node_services_,
+      initial_parameters,
+      use_intra_process_comms,
+      start_parameter_services
+    )),
+  node_clock_(new rclcpp::node_interfaces::NodeClock(
+      node_base_,
+      node_topics_,
+      node_graph_,
+      node_services_
     )),
   use_intra_process_comms_(use_intra_process_comms)
 {
@@ -66,6 +93,18 @@ Node::get_name() const
   return node_base_->get_name();
 }
 
+const char *
+Node::get_namespace() const
+{
+  return node_base_->get_namespace();
+}
+
+rclcpp::Logger
+Node::get_logger() const
+{
+  return node_logging_->get_logger();
+}
+
 rclcpp::callback_group::CallbackGroup::SharedPtr
 Node::create_callback_group(
   rclcpp::callback_group::CallbackGroupType group_type)
@@ -81,33 +120,34 @@ 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);
 }
 
-bool Node::get_parameter(const std::string & name,
-  rclcpp::parameter::ParameterVariant & parameter) const
+bool Node::get_parameter(
+  const std::string & name,
+  rclcpp::Parameter & parameter) const
 {
   return node_parameters_->get_parameter(name, parameter);
 }
@@ -133,12 +173,18 @@ Node::list_parameters(
   return node_parameters_->list_parameters(prefixes, depth);
 }
 
-std::map<std::string, std::string>
+std::map<std::string, std::vector<std::string>>
 Node::get_topic_names_and_types() const
 {
   return node_graph_->get_topic_names_and_types();
 }
 
+std::map<std::string, std::vector<std::string>>
+Node::get_service_names_and_types() const
+{
+  return node_graph_->get_service_names_and_types();
+}
+
 size_t
 Node::count_publishers(const std::string & topic_name) const
 {
@@ -157,7 +203,7 @@ Node::get_callback_groups() const
   return node_base_->get_callback_groups();
 }
 
-rclcpp::event::Event::SharedPtr
+rclcpp::Event::SharedPtr
 Node::get_graph_event()
 {
   return node_graph_->get_graph_event();
@@ -165,18 +211,36 @@ Node::get_graph_event()
 
 void
 Node::wait_for_graph_change(
-  rclcpp::event::Event::SharedPtr event,
+  rclcpp::Event::SharedPtr event,
   std::chrono::nanoseconds timeout)
 {
   node_graph_->wait_for_graph_change(event, timeout);
 }
 
+rclcpp::Clock::SharedPtr
+Node::get_clock()
+{
+  return node_clock_->get_clock();
+}
+
+rclcpp::Time
+Node::now()
+{
+  return node_clock_->get_clock()->now();
+}
+
 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
 Node::get_node_base_interface()
 {
   return node_base_;
 }
 
+rclcpp::node_interfaces::NodeClockInterface::SharedPtr
+Node::get_node_clock_interface()
+{
+  return node_clock_;
+}
+
 rclcpp::node_interfaces::NodeGraphInterface::SharedPtr
 Node::get_node_graph_interface()
 {

rclcpp/src/rclcpp/node_interfaces/node_base.cpp

@@ -19,7 +19,11 @@
 
 #include "rclcpp/node_interfaces/node_base.hpp"
 
+#include "rcl/arguments.h"
 #include "rclcpp/exceptions.hpp"
+#include "rcutils/logging_macros.h"
+#include "rmw/validate_node_name.h"
+#include "rmw/validate_namespace.h"
 
 using rclcpp::exceptions::throw_from_rcl_error;
 
@@ -27,7 +31,10 @@ using rclcpp::node_interfaces::NodeBase;
 
 NodeBase::NodeBase(
   const std::string & node_name,
-  rclcpp::context::Context::SharedPtr context)
+  const std::string & namespace_,
+  rclcpp::Context::SharedPtr context,
+  const std::vector<std::string> & arguments,
+  bool use_global_arguments)
 : context_(context),
   node_handle_(nullptr),
   default_callback_group_(nullptr),
@@ -45,8 +52,9 @@ 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_safe());
       }
     };
 
@@ -79,26 +87,103 @@ NodeBase::NodeBase(
   }
 
   // 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();
+  std::unique_ptr<const char *[]> c_args;
+  if (!arguments.empty()) {
+    c_args.reset(new const char *[arguments.size()]);
+    for (std::size_t i = 0; i < arguments.size(); ++i) {
+      c_args[i] = arguments[i].c_str();
+    }
+  }
+  // TODO(sloretz) Pass an allocator to argument parsing
+  if (arguments.size() > std::numeric_limits<int>::max()) {
+    throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "Too many args");
+  }
+  ret = rcl_parse_arguments(
+    static_cast<int>(arguments.size()), c_args.get(), rcl_get_default_allocator(),
+    &(options.arguments));
+  if (RCL_RET_OK != ret) {
+    finalize_notify_guard_condition();
+    throw_from_rcl_error(ret, "failed to parse arguments");
+  }
+
+  options.use_global_arguments = use_global_arguments;
   // TODO(wjwwood): pass the Allocator to the options
   options.domain_id = domain_id;
-  ret = rcl_node_init(rcl_node, node_name.c_str(), &options);
+
+  ret = rcl_node_init(rcl_node.get(), node_name.c_str(), namespace_.c_str(), &options);
   if (ret != RCL_RET_OK) {
     // Finalize the interrupt guard condition.
     finalize_notify_guard_condition();
+    // Finalize previously allocated node arguments
+    if (RCL_RET_OK != rcl_arguments_fini(&options.arguments)) {
+      // Print message because exception will be thrown later in this code block
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "Failed to fini arguments during error handling: %s", rcl_get_error_string_safe());
+      rcl_reset_error();
+    }
 
+    if (ret == RCL_RET_NODE_INVALID_NAME) {
+      rcl_reset_error();  // discard rcl_node_init error
+      int validation_result;
+      size_t invalid_index;
+      rmw_ret_t rmw_ret =
+        rmw_validate_node_name(node_name.c_str(), &validation_result, &invalid_index);
+      if (rmw_ret != RMW_RET_OK) {
+        if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
+          throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "failed to validate node name");
+        }
+        throw_from_rcl_error(RCL_RET_ERROR, "failed to validate node name");
+      }
+
+      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) {
+      rcl_reset_error();  // discard rcl_node_init error
+      int validation_result;
+      size_t invalid_index;
+      rmw_ret_t rmw_ret =
+        rmw_validate_namespace(namespace_.c_str(), &validation_result, &invalid_index);
+      if (rmw_ret != RMW_RET_OK) {
+        if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
+          throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "failed to validate namespace");
+        }
+        throw_from_rcl_error(RCL_RET_ERROR, "failed to validate namespace");
+      }
+
+      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_t * node) -> void {
-    if (rcl_node_fini(node) != RCL_RET_OK) {
-      fprintf(
-        stderr, "Error in destruction of rcl node handle: %s\n", rcl_get_error_string_safe());
-    }
-    delete node;
-  });
+  node_handle_.reset(
+    rcl_node.release(),
+    [](rcl_node_t * node) -> void {
+      if (rcl_node_fini(node) != RCL_RET_OK) {
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Error in destruction of rcl node handle: %s", rcl_get_error_string_safe());
+      }
+      delete node;
+    });
 
   // Create the default callback group.
   using rclcpp::callback_group::CallbackGroupType;
@@ -106,6 +191,15 @@ NodeBase::NodeBase(
 
   // Indicate the notify_guard_condition is now valid.
   notify_guard_condition_is_valid_ = true;
+
+  // Finalize previously allocated node arguments
+  if (RCL_RET_OK != rcl_arguments_fini(&options.arguments)) {
+    // print message because throwing would prevent the destructor from being called
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "Failed to fini arguments: %s", rcl_get_error_string_safe());
+    rcl_reset_error();
+  }
 }
 
 NodeBase::~NodeBase()
@@ -115,8 +209,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_safe());
     }
   }
 }
@@ -127,7 +222,13 @@ NodeBase::get_name() const
   return rcl_node_get_name(node_handle_.get());
 }
 
-rclcpp::context::Context::SharedPtr
+const char *
+NodeBase::get_namespace() const
+{
+  return rcl_node_get_namespace(node_handle_.get());
+}
+
+rclcpp::Context::SharedPtr
 NodeBase::get_context()
 {
   return context_;

rclcpp/src/rclcpp/node_interfaces/node_clock.cpp

@@ -0,0 +1,48 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/node_interfaces/node_clock.hpp"
+
+#include <memory>
+#include <string>
+
+using rclcpp::node_interfaces::NodeClock;
+
+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)
+: node_base_(node_base),
+  node_topics_(node_topics),
+  node_graph_(node_graph),
+  node_services_(node_services),
+  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()
+{}
+
+std::shared_ptr<rclcpp::Clock>
+NodeClock::get_clock()
+{
+  return ros_clock_;
+}

rclcpp/src/rclcpp/node_interfaces/node_graph.cpp

@@ -16,6 +16,7 @@
 
 #include <map>
 #include <string>
+#include <vector>
 
 #include "rcl/graph.h"
 #include "rclcpp/exceptions.hpp"
@@ -44,44 +45,141 @@ NodeGraph::~NodeGraph()
   }
 }
 
-std::map<std::string, std::string>
-NodeGraph::get_topic_names_and_types() const
+std::map<std::string, std::vector<std::string>>
+NodeGraph::get_topic_names_and_types(bool no_demangle) const
 {
-  rcl_topic_names_and_types_t topic_names_and_types =
-    rcl_get_zero_initialized_topic_names_and_types();
+  rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types();
 
-  auto ret = rcl_get_topic_names_and_types(node_base_->get_rcl_node_handle(),
-      &topic_names_and_types);
-  if (ret != RMW_RET_OK) {
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto ret = rcl_get_topic_names_and_types(
+    node_base_->get_rcl_node_handle(),
+    &allocator,
+    no_demangle,
+    &topic_names_and_types);
+  if (ret != RCL_RET_OK) {
+    auto error_msg = std::string("failed to get topic names and types: ") +
+      rcl_get_error_string_safe();
+    rcl_reset_error();
+    if (rcl_names_and_types_fini(&topic_names_and_types) != RCL_RET_OK) {
+      error_msg += std::string(", failed also to cleanup topic names and types, leaking memory: ") +
+        rcl_get_error_string_safe();
+    }
+    throw std::runtime_error(error_msg + rcl_get_error_string_safe());
+  }
+
+  std::map<std::string, std::vector<std::string>> topics_and_types;
+  for (size_t i = 0; i < topic_names_and_types.names.size; ++i) {
+    std::string topic_name = topic_names_and_types.names.data[i];
+    for (size_t j = 0; j < topic_names_and_types.types[i].size; ++j) {
+      topics_and_types[topic_name].emplace_back(topic_names_and_types.types[i].data[j]);
+    }
+  }
+
+  ret = rcl_names_and_types_fini(&topic_names_and_types);
+  if (ret != RCL_RET_OK) {
     // *INDENT-OFF*
     throw std::runtime_error(
-      std::string("could not get topic names and types: ") + rmw_get_error_string_safe());
+      std::string("could not destroy topic names and types: ") + rcl_get_error_string_safe());
     // *INDENT-ON*
   }
 
-  std::map<std::string, std::string> topics;
-  for (size_t i = 0; i < topic_names_and_types.topic_count; ++i) {
-    topics[topic_names_and_types.topic_names[i]] = topic_names_and_types.type_names[i];
+  return topics_and_types;
+}
+
+std::map<std::string, std::vector<std::string>>
+NodeGraph::get_service_names_and_types() const
+{
+  rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types();
+
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto ret = rcl_get_service_names_and_types(
+    node_base_->get_rcl_node_handle(),
+    &allocator,
+    &service_names_and_types);
+  if (ret != RCL_RET_OK) {
+    auto error_msg = std::string("failed to get service names and types: ") +
+      rcl_get_error_string_safe();
+    rcl_reset_error();
+    if (rcl_names_and_types_fini(&service_names_and_types) != RCL_RET_OK) {
+      error_msg +=
+        std::string(", failed also to cleanup service names and types, leaking memory: ") +
+        rcl_get_error_string_safe();
+    }
+    throw std::runtime_error(error_msg + rcl_get_error_string_safe());
   }
 
-  ret = rmw_destroy_topic_names_and_types(&topic_names_and_types);
-  if (ret != RMW_RET_OK) {
+  std::map<std::string, std::vector<std::string>> services_and_types;
+  for (size_t i = 0; i < service_names_and_types.names.size; ++i) {
+    std::string service_name = service_names_and_types.names.data[i];
+    for (size_t j = 0; j < service_names_and_types.types[i].size; ++j) {
+      services_and_types[service_name].emplace_back(service_names_and_types.types[i].data[j]);
+    }
+  }
+
+  ret = rcl_names_and_types_fini(&service_names_and_types);
+  if (ret != RCL_RET_OK) {
     // *INDENT-OFF*
     throw std::runtime_error(
-      std::string("could not destroy topic names and types: ") + rmw_get_error_string_safe());
+      std::string("could not destroy service names and types: ") + rcl_get_error_string_safe());
     // *INDENT-ON*
   }
 
-  return topics;
+  return services_and_types;
+}
+
+std::vector<std::string>
+NodeGraph::get_node_names() const
+{
+  rcutils_string_array_t node_names_c =
+    rcutils_get_zero_initialized_string_array();
+
+  auto allocator = rcl_get_default_allocator();
+  auto ret = rcl_get_node_names(
+    node_base_->get_rcl_node_handle(),
+    allocator,
+    &node_names_c);
+  if (ret != RCL_RET_OK) {
+    auto error_msg = std::string("failed to get node names: ") + rcl_get_error_string_safe();
+    rcl_reset_error();
+    if (rcutils_string_array_fini(&node_names_c) != RCUTILS_RET_OK) {
+      error_msg += std::string(", failed also to cleanup node names, leaking memory: ") +
+        rcl_get_error_string_safe();
+    }
+    // TODO(karsten1987): Append rcutils_error_message once it's in master
+    throw std::runtime_error(error_msg);
+  }
+
+  std::vector<std::string> node_names(node_names_c.size);
+  for (size_t i = 0; i < node_names_c.size; ++i) {
+    if (node_names_c.data[i]) {
+      node_names[i] = node_names_c.data[i];
+    }
+  }
+  ret = rcutils_string_array_fini(&node_names_c);
+  if (ret != RCUTILS_RET_OK) {
+    // *INDENT-OFF*
+    // TODO(karsten1987): Append rcutils_error_message once it's in master
+    throw std::runtime_error(
+      std::string("could not destroy node names: "));
+    // *INDENT-ON*
+  }
+
+  return node_names;
 }
 
 size_t
 NodeGraph::count_publishers(const std::string & topic_name) const
 {
+  auto rcl_node_handle = node_base_->get_rcl_node_handle();
+
+  auto fqdn = rclcpp::expand_topic_or_service_name(
+    topic_name,
+    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(rcl_node_get_rmw_handle(node_base_->get_rcl_node_handle()),
-      topic_name.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(
@@ -94,10 +192,16 @@ NodeGraph::count_publishers(const std::string & topic_name) const
 size_t
 NodeGraph::count_subscribers(const std::string & topic_name) const
 {
+  auto rcl_node_handle = node_base_->get_rcl_node_handle();
+
+  auto fqdn = rclcpp::expand_topic_or_service_name(
+    topic_name,
+    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(rcl_node_get_rmw_handle(node_base_->get_rcl_node_handle()),
-      topic_name.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(
@@ -133,9 +237,9 @@ NodeGraph::notify_graph_change()
         std::remove_if(
           graph_events_.begin(),
           graph_events_.end(),
-          [](const rclcpp::event::Event::WeakPtr & wptr) {
-        return wptr.expired();
-      }),
+          [](const rclcpp::Event::WeakPtr & wptr) {
+            return wptr.expired();
+          }),
         graph_events_.end());
       // update graph_users_count_
       graph_users_count_.store(graph_events_.size());
@@ -158,24 +262,24 @@ NodeGraph::notify_shutdown()
   graph_cv_.notify_all();
 }
 
-rclcpp::event::Event::SharedPtr
+rclcpp::Event::SharedPtr
 NodeGraph::get_graph_event()
 {
-  auto event = rclcpp::event::Event::make_shared();
+  auto event = rclcpp::Event::make_shared();
   std::lock_guard<std::mutex> graph_changed_lock(graph_mutex_);
+  graph_events_.push_back(event);
+  graph_users_count_++;
   // on first call, add node to graph_listener_
   if (should_add_to_graph_listener_.exchange(false)) {
     graph_listener_->add_node(this);
     graph_listener_->start_if_not_started();
   }
-  graph_events_.push_back(event);
-  graph_users_count_++;
   return event;
 }
 
 void
 NodeGraph::wait_for_graph_change(
-  rclcpp::event::Event::SharedPtr event,
+  rclcpp::Event::SharedPtr event,
   std::chrono::nanoseconds timeout)
 {
   using rclcpp::exceptions::InvalidEventError;
@@ -196,10 +300,13 @@ NodeGraph::wait_for_graph_change(
       throw EventNotRegisteredError();
     }
   }
+  auto pred = [&event]() {
+      return event->check() || !rclcpp::ok();
+    };
   std::unique_lock<std::mutex> graph_lock(graph_mutex_);
-  graph_cv_.wait_for(graph_lock, timeout, [&event]() {
-    return event->check() || !rclcpp::utilities::ok();
-  });
+  if (!pred()) {
+    graph_cv_.wait_for(graph_lock, timeout, pred);
+  }
 }
 
 size_t

rclcpp/src/rclcpp/node_interfaces/node_logging.cpp

@@ -0,0 +1,39 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/node_interfaces/node_logging.hpp"
+
+using rclcpp::node_interfaces::NodeLogging;
+
+NodeLogging::NodeLogging(rclcpp::node_interfaces::NodeBaseInterface * node_base)
+: node_base_(node_base)
+{
+  logger_ = rclcpp::get_logger(this->get_logger_name());
+}
+
+NodeLogging::~NodeLogging()
+{
+}
+
+rclcpp::Logger
+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,6 +14,8 @@
 
 #include "rclcpp/node_interfaces/node_parameters.hpp"
 
+#include <rcl_yaml_param_parser/parser.h>
+
 #include <map>
 #include <memory>
 #include <string>
@@ -22,27 +24,136 @@
 
 #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 std::vector<rclcpp::Parameter> & initial_parameters,
+  bool use_intra_process,
+  bool start_parameter_services)
 {
   using MessageT = rcl_interfaces::msg::ParameterEvent;
-  using PublisherT = rclcpp::publisher::Publisher<MessageT>;
+  using PublisherT = rclcpp::Publisher<MessageT>;
   using AllocatorT = std::allocator<void>;
   // TODO(wjwwood): expose this allocator through the Parameter interface.
   auto allocator = std::make_shared<AllocatorT>();
 
+  if (start_parameter_services) {
+    parameter_service_ = std::make_shared<ParameterService>(node_base, node_services, this);
+  }
+
   events_publisher_ = rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
-    node_topics_,
+    node_topics.get(),
     "parameter_events",
     rmw_qos_profile_parameter_events,
     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 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) {
+    get_yaml_paths(rcl_get_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");
+  }
+  std::string combined_name;
+  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
+  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)) {
+      throw std::runtime_error("Failed to parse parameters " + yaml_path);
+    }
+
+    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()
@@ -50,7 +161,7 @@ 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) {
@@ -62,10 +173,10 @@ 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>();
 
   // TODO(jacquelinekay): handle parameter constraints
@@ -81,20 +192,34 @@ NodeParameters::set_parameters_atomically(
   }
 
   for (auto p : parameters) {
-    if (parameters_.find(p.get_name()) == parameters_.end()) {
-      if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
-        parameter_event->new_parameters.push_back(p.to_parameter());
+    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) {
-      parameter_event->changed_parameters.push_back(p.to_parameter());
     } else {
-      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 (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);
@@ -102,17 +227,17 @@ NodeParameters::set_parameters_atomically(
   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) {
-      return name == kv.first;
-    }))
+      [&name](const std::pair<std::string, rclcpp::Parameter> & kv) {
+        return name == kv.first;
+      }))
     {
       results.push_back(parameters_.at(name));
     }
@@ -120,10 +245,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;
@@ -135,7 +260,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_);
 
@@ -154,8 +279,8 @@ NodeParameters::describe_parameters(const std::vector<std::string> & names) cons
   std::vector<rcl_interfaces::msg::ParameterDescriptor> results;
   for (auto & kv : parameters_) {
     if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
-      return name == kv.first;
-    }))
+        return name == kv.first;
+      }))
     {
       rcl_interfaces::msg::ParameterDescriptor parameter_descriptor;
       parameter_descriptor.name = kv.first;
@@ -173,8 +298,8 @@ NodeParameters::get_parameter_types(const std::vector<std::string> & names) cons
   std::vector<uint8_t> results;
   for (auto & kv : parameters_) {
     if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
-      return name == kv.first;
-    }))
+        return name == kv.first;
+      }))
     {
       results.push_back(kv.second.get_type());
     } else {
@@ -190,26 +315,29 @@ NodeParameters::list_parameters(const std::vector<std::string> & prefixes, uint6
   std::lock_guard<std::mutex> lock(mutex_);
   rcl_interfaces::msg::ListParametersResult result;
 
-  // TODO(esteve): define parameter separator, use "." for now
+  // TODO(mikaelarguedas) define parameter separator different from "/" to avoid ambiguity
+  // using "." for now
+  const char * separator = ".";
   for (auto & kv : parameters_) {
-    if (((prefixes.size() == 0) &&
+    bool get_all = (prefixes.size() == 0) &&
       ((depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) ||
-      (static_cast<uint64_t>(std::count(kv.first.begin(), kv.first.end(), '.')) < depth))) ||
-      (std::any_of(prefixes.cbegin(), prefixes.cend(), [&kv, &depth](const std::string & prefix) {
-      if (kv.first == prefix) {
-        return true;
-      } else if (kv.first.find(prefix + ".") == 0) {
-        size_t length = prefix.length();
-        std::string substr = kv.first.substr(length);
-        // Cast as unsigned integer to avoid warning
-        return (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) ||
-        (static_cast<uint64_t>(std::count(substr.begin(), substr.end(), '.')) < depth);
-      }
-      return false;
-    })))
-    {
+      (static_cast<uint64_t>(std::count(kv.first.begin(), kv.first.end(), *separator)) < depth));
+    bool prefix_matches = std::any_of(prefixes.cbegin(), prefixes.cend(),
+        [&kv, &depth, &separator](const std::string & prefix) {
+          if (kv.first == prefix) {
+            return true;
+          } else if (kv.first.find(prefix + separator) == 0) {
+            size_t length = prefix.length();
+            std::string substr = kv.first.substr(length);
+            // Cast as unsigned integer to avoid warning
+            return (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) ||
+            (static_cast<uint64_t>(std::count(substr.begin(), substr.end(), *separator)) < depth);
+          }
+          return false;
+        });
+    if (get_all || prefix_matches) {
       result.names.push_back(kv.first);
-      size_t last_separator = kv.first.find_last_of('.');
+      size_t last_separator = kv.first.find_last_of(separator);
       if (std::string::npos != last_separator) {
         std::string prefix = kv.first.substr(0, last_separator);
         if (std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) ==
@@ -226,5 +354,9 @@ NodeParameters::list_parameters(const std::vector<std::string> & prefixes, uint6
 void
 NodeParameters::register_param_change_callback(ParametersCallbackFunction callback)
 {
+  if (parameters_callback_) {
+    RCUTILS_LOG_WARN("param_change_callback already registered, "
+      "overwriting previous callback")
+  }
   parameters_callback_ = callback;
 }

rclcpp/src/rclcpp/node_interfaces/node_services.cpp

@@ -27,7 +27,7 @@ NodeServices::~NodeServices()
 
 void
 NodeServices::add_service(
-  rclcpp::service::ServiceBase::SharedPtr service_base_ptr,
+  rclcpp::ServiceBase::SharedPtr service_base_ptr,
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
   if (group) {
@@ -45,7 +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 waitset on service creation: ") + rmw_get_error_string()
+              std::string(
+                "Failed to notify wait set on service creation: ") + rmw_get_error_string()
       );
     }
   }
@@ -53,7 +54,7 @@ NodeServices::add_service(
 
 void
 NodeServices::add_client(
-  rclcpp::client::ClientBase::SharedPtr client_base_ptr,
+  rclcpp::ClientBase::SharedPtr client_base_ptr,
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
   if (group) {
@@ -71,7 +72,7 @@ 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 waitset on client creation: ") + rmw_get_error_string()
+              std::string("Failed to notify wait set on client creation: ") + rmw_get_error_string()
       );
     }
   }

rclcpp/src/rclcpp/node_interfaces/node_timers.cpp

@@ -27,7 +27,7 @@ NodeTimers::~NodeTimers()
 
 void
 NodeTimers::add_timer(
-  rclcpp::timer::TimerBase::SharedPtr timer,
+  rclcpp::TimerBase::SharedPtr timer,
   rclcpp::callback_group::CallbackGroup::SharedPtr callback_group)
 {
   if (callback_group) {
@@ -42,6 +42,6 @@ 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 waitset on timer creation: ") + rmw_get_error_string());
+              "Failed to notify wait set on timer creation: ") + rmw_get_error_string());
   }
 }