first try of node like something

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

.gitignore

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

CONTRIBUTING.md

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

rclcpp/CHANGELOG.rst

@@ -2,6 +2,123 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.7.2 (2019-05-08)
+------------------
+* Added new way to specify QoS settings for publishers and subscriptions. (`#713 <https://github.com/ros2/rclcpp/issues/713>`_)
+  * The new way requires that you specify a history depth when creating a publisher or subscription.
+  * In the past it was possible to create one without specifying any history depth, but these signatures have been deprecated.
+* Deprecated ``shared_ptr`` and raw pointer versions of ``Publisher<T>::publish()``. (`#709 <https://github.com/ros2/rclcpp/issues/709>`_)
+* Implemented API to set callbacks for liveliness and deadline QoS events for publishers and subscriptions. (`#695 <https://github.com/ros2/rclcpp/issues/695>`_)
+* Fixed a segmentation fault when publishing a parameter event when they ought to be disabled. (`#714 <https://github.com/ros2/rclcpp/issues/714>`_)
+* Changes required for upcoming pre-allocation API. (`#711 <https://github.com/ros2/rclcpp/issues/711>`_)
+* Changed ``Node::get_node_names()`` to return the full node names rather than just the base name. (`#698 <https://github.com/ros2/rclcpp/issues/698>`_)
+* Remove logic made redundant by the `ros2/rcl#255 <https://github.com/ros2/rcl/issues/255>`_ pull request. (`#712 <https://github.com/ros2/rclcpp/issues/712>`_)
+* Various improvements for ``rclcpp::Clock``. (`#696 <https://github.com/ros2/rclcpp/issues/696>`_)
+  * Fixed uninitialized bool in ``clock.cpp``.
+  * Fixed up includes of ``clock.hpp/cpp``.
+  * Added documentation for exceptions to ``clock.hpp``.
+  * Adjusted function signature of getters of ``clock.hpp/cpp``.
+  * Removed raw pointers to ``Clock::create_jump_callback``.
+  * Removed unnecessary ``rclcpp`` namespace reference from ``clock.cpp``.
+  * Changed exception to ``bad_alloc`` on ``JumpHandler`` allocation failure.
+  * Fixed missing ``nullptr`` check in ``Clock::on_time_jump``.
+  * Added ``JumpHandler::callback`` types.
+  * Added warning for lifetime of Clock and JumpHandler
+* Fixed bug left over from the `pull request #495 <https://github.com/ros2/rclcpp/pull/495>`_. (`#708 <https://github.com/ros2/rclcpp/issues/708>`_)
+* Changed the ``IntraProcessManager`` to be capable of storing ``shared_ptr<const T>`` in addition to ``unique_ptr<T>``. (`#690 <https://github.com/ros2/rclcpp/issues/690>`_)
+* Contributors: Alberto Soragna, Dima Dorezyuk, M. M, Michael Carroll, Michael Jeronimo, Tully Foote, William Woodall, ivanpauno, jhdcs
+
+0.7.1 (2019-04-26)
+------------------
+* Added read only parameters. (`#495 <https://github.com/ros2/rclcpp/issues/495>`_)
+* Fixed a concurrency problem in the multithreaded executor. (`#703 <https://github.com/ros2/rclcpp/issues/703>`_)
+* Fixup utilities. (`#692 <https://github.com/ros2/rclcpp/issues/692>`_)
+* Added method to read timer cancellation. (`#697 <https://github.com/ros2/rclcpp/issues/697>`_)
+* Added Exception Generator function for implementing "from_rcl_error". (`#678 <https://github.com/ros2/rclcpp/issues/678>`_)
+* Updated initialization of rmw_qos_profile_t struct instances. (`#691 <https://github.com/ros2/rclcpp/issues/691>`_)
+* Removed the const value from the logger before comparison. (`#680 <https://github.com/ros2/rclcpp/issues/680>`_)
+* Contributors: Devin Bonnie, Dima Dorezyuk, Guillaume Autran, M. M, Shane Loretz, Víctor Mayoral Vilches, William Woodall, jhdcs
+
+0.7.0 (2019-04-14)
+------------------
+* Added Options-struct interfaces for creating publishers/subscribers (pre-QoS, standalone). (`#673 <https://github.com/ros2/rclcpp/issues/673>`_)
+* Replaced strncpy with memcpy. (`#684 <https://github.com/ros2/rclcpp/issues/684>`_)
+* Replaced const char * with a std::array<char, TOPIC_NAME_LENGTH> as the key of IPM IDTopicMap. (`#671 <https://github.com/ros2/rclcpp/issues/671>`_)
+* Refactored SignalHandler logger to avoid race during destruction. (`#682 <https://github.com/ros2/rclcpp/issues/682>`_)
+* Introduce rclcpp_components to implement composition. (`#665 <https://github.com/ros2/rclcpp/issues/665>`_)
+* Added QoS policy check when configuring intraprocess, skip interprocess publish when possible. (`#674 <https://github.com/ros2/rclcpp/issues/674>`_)
+* Updated to use do { .. } while(0) around content of logging macros. (`#681 <https://github.com/ros2/rclcpp/issues/681>`_)
+* Added function to get publisher's actual QoS settings. (`#667 <https://github.com/ros2/rclcpp/issues/667>`_)
+* Updated to avoid race that triggers timer too often. (`#621 <https://github.com/ros2/rclcpp/issues/621>`_)
+* Exposed get_fully_qualified_name in NodeBase API. (`#662 <https://github.com/ros2/rclcpp/issues/662>`_)
+* Updated to use ament_target_dependencies where possible. (`#659 <https://github.com/ros2/rclcpp/issues/659>`_)
+* Fixed wait for service memory leak bug. (`#656 <https://github.com/ros2/rclcpp/issues/656>`_)
+* Fixed test_time_source test. (`#639 <https://github.com/ros2/rclcpp/issues/639>`_)
+* Fixed hard-coded duration type representation so int64_t isn't assumed. (`#648 <https://github.com/ros2/rclcpp/issues/648>`_)
+* Fixed cppcheck warning. (`#646 <https://github.com/ros2/rclcpp/issues/646>`_)
+* Added count matching api and intra-process subscriber count. (`#628 <https://github.com/ros2/rclcpp/issues/628>`_)
+* Added Sub Node alternative. (`#581 <https://github.com/ros2/rclcpp/issues/581>`_)
+* Replaced 'auto' with 'const auto &'. (`#630 <https://github.com/ros2/rclcpp/issues/630>`_)
+* Set Parameter Event Publisher settings. `#591 <https://github.com/ros2/rclcpp/issues/591>`_ (`#614 <https://github.com/ros2/rclcpp/issues/614>`_)
+* Replaced node constructor arguments with NodeOptions. (`#622 <https://github.com/ros2/rclcpp/issues/622>`_)
+* Updated to pass context to wait set (`#617 <https://github.com/ros2/rclcpp/issues/617>`_)
+* Added API to get parameters in a map. (`#575 <https://github.com/ros2/rclcpp/issues/575>`_)
+* Updated Bind usage since it is is no longer in std::__1. (`#618 <https://github.com/ros2/rclcpp/issues/618>`_)
+* Fixed errors from uncrustify v0.68. (`#613 <https://github.com/ros2/rclcpp/issues/613>`_)
+* Added new constructors for SyncParameterClient. (`#612 <https://github.com/ros2/rclcpp/issues/612>`_)
+* Contributors: Alberto Soragna, Chris Lalancette, Dirk Thomas, Emerson Knapp, Francisco Martín Rico, Jacob Perron, Marko Durkovic, Michael Carroll, Peter Baughman, Shane Loretz, Wei Liu, William Woodall, Yutaka Kondo, ivanpauno, kuzai, rarvolt
+
+0.6.2 (2018-12-13)
+------------------
+* Updated to use signal safe synchronization with platform specific semaphores (`#607 <https://github.com/ros2/rclcpp/issues/607>`_)
+* Resolved startup race condition for sim time (`#608 <https://github.com/ros2/rclcpp/issues/608>`_)
+  Resolves `#595 <https://github.com/ros2/rclcpp/issues/595>`_
+* Contributors: Tully Foote, William Woodall
+
+0.6.1 (2018-12-07)
+------------------
+* Added wait_for_action_server() for action clients (`#598 <https://github.com/ros2/rclcpp/issues/598>`_)
+* Added node path and time stamp to parameter event message (`#584 <https://github.com/ros2/rclcpp/issues/584>`_)
+* Updated to allow removing a waitable (`#597 <https://github.com/ros2/rclcpp/issues/597>`_)
+* Refactored init to allow for non-global init (`#587 <https://github.com/ros2/rclcpp/issues/587>`_)
+* Fixed wrong use of constructor and hanging test (`#596 <https://github.com/ros2/rclcpp/issues/596>`_)
+* Added class Waitable (`#589 <https://github.com/ros2/rclcpp/issues/589>`_)
+* Updated rcl_wait_set_add\_* calls (`#586 <https://github.com/ros2/rclcpp/issues/586>`_)
+* Contributors: Dirk Thomas, Jacob Perron, Shane Loretz, William Woodall, bpwilcox
+
+0.6.0 (2018-11-19)
+------------------
+* Updated to use new error handling API from rcutils (`#577 <https://github.com/ros2/rclcpp/issues/577>`_)
+* Added a warning when publishing if publisher is not active (`#574 <https://github.com/ros2/rclcpp/issues/574>`_)
+* Added logging macro signature that accepts std::string (`#573 <https://github.com/ros2/rclcpp/issues/573>`_)
+* Added virtual destructors to classes with virtual functions. (`#566 <https://github.com/ros2/rclcpp/issues/566>`_)
+* Added semicolons to all RCLCPP and RCUTILS macros. (`#565 <https://github.com/ros2/rclcpp/issues/565>`_)
+* Removed std::binary_function usage (`#561 <https://github.com/ros2/rclcpp/issues/561>`_)
+* Updated to avoid auto-activating ROS time if clock topic is being published (`#559 <https://github.com/ros2/rclcpp/issues/559>`_)
+* Fixed cpplint on xenial (`#556 <https://github.com/ros2/rclcpp/issues/556>`_)
+* Added get_parameter_or_set_default. (`#551 <https://github.com/ros2/rclcpp/issues/551>`_)
+* Added max_duration to spin_some() (`#558 <https://github.com/ros2/rclcpp/issues/558>`_)
+* Updated to output rcl error message when yaml parsing fails (`#557 <https://github.com/ros2/rclcpp/issues/557>`_)
+* Updated to make sure timer is fini'd before clock (`#553 <https://github.com/ros2/rclcpp/issues/553>`_)
+* Get node names and namespaces (`#545 <https://github.com/ros2/rclcpp/issues/545>`_)
+* Fixed and improved documentation  (`#546 <https://github.com/ros2/rclcpp/issues/546>`_)
+* Updated to use rcl_clock_t jump callbacks (`#543 <https://github.com/ros2/rclcpp/issues/543>`_)
+* Updated to use rcl consolidated wait set functions (`#540 <https://github.com/ros2/rclcpp/issues/540>`_)
+* Addeed TIME_MAX and DURATION_MAX functions (`#538 <https://github.com/ros2/rclcpp/issues/538>`_)
+* Updated to publish shared_ptr of rcl_serialized_message (`#541 <https://github.com/ros2/rclcpp/issues/541>`_)
+* Added Time::is_zero and Duration::seconds (`#536 <https://github.com/ros2/rclcpp/issues/536>`_)
+* Changed to log an error message instead of throwing exception in destructor (`#535 <https://github.com/ros2/rclcpp/issues/535>`_)
+* Updated to relax tolerance of now test because timing affected by OS scheduling (`#533 <https://github.com/ros2/rclcpp/issues/533>`_)
+* Removed incorrect exception on sec < 0 (`#527 <https://github.com/ros2/rclcpp/issues/527>`_)
+* Added rclcpp::Time::seconds() (`#526 <https://github.com/ros2/rclcpp/issues/526>`_)
+* Updated Timer API to construct TimerBase/GenericTimer with Clock (`#523 <https://github.com/ros2/rclcpp/issues/523>`_)
+* Added rclcpp::is_initialized() (`#522 <https://github.com/ros2/rclcpp/issues/522>`_)
+* Added support for jump handlers with only pre- or post-jump callback (`#517 <https://github.com/ros2/rclcpp/issues/517>`_)
+* Removed use of uninitialized CMake var (`#512 <https://github.com/ros2/rclcpp/issues/512>`_)
+* Updated for Uncrustify 0.67 (`#510 <https://github.com/ros2/rclcpp/issues/510>`_)
+* Added get_node_names API from node. (`#508 <https://github.com/ros2/rclcpp/issues/508>`_)
+* Contributors: Anis Ladram, Chris Lalancette, Dirk Thomas, Francisco Martín Rico, Karsten Knese, Michael Carroll, Mikael Arguedas, Sagnik Basu, Shane Loretz, Sriram Raghunathan, William Woodall, chapulina, dhood
+
 0.5.0 (2018-06-25)
 ------------------
 * Fixed a bug in the multi-threaded executor which could cause it to take a timer (potentially other types of wait-able items) more than once to be worked one. (`#383 <https://github.com/ros2/rclcpp/issues/383>`_)

rclcpp/CMakeLists.txt

@@ -19,12 +19,7 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
 endif()
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  # we dont use add_compile_options with pedantic in message packages
-  # because the Python C extensions dont comply with it
-  # TODO(mikaelarguedas) change to add_compile_options
-  # once this is not a message package anymore
-  # https://github.com/ros2/system_tests/issues/191
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+  add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
 include_directories(include)
@@ -45,34 +40,42 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/executors/multi_threaded_executor.cpp
   src/rclcpp/executors/single_threaded_executor.cpp
   src/rclcpp/graph_listener.cpp
+  src/rclcpp/init_options.cpp
   src/rclcpp/intra_process_manager.cpp
   src/rclcpp/intra_process_manager_impl.cpp
   src/rclcpp/logger.cpp
   src/rclcpp/memory_strategies.cpp
   src/rclcpp/memory_strategy.cpp
   src/rclcpp/node.cpp
+  src/rclcpp/node_options.cpp
   src/rclcpp/node_interfaces/node_base.cpp
   src/rclcpp/node_interfaces/node_clock.cpp
   src/rclcpp/node_interfaces/node_graph.cpp
   src/rclcpp/node_interfaces/node_logging.cpp
   src/rclcpp/node_interfaces/node_parameters.cpp
   src/rclcpp/node_interfaces/node_services.cpp
+  src/rclcpp/node_interfaces/node_time_source.cpp
   src/rclcpp/node_interfaces/node_timers.cpp
   src/rclcpp/node_interfaces/node_topics.cpp
+  src/rclcpp/node_interfaces/node_waitables.cpp
   src/rclcpp/parameter.cpp
   src/rclcpp/parameter_value.cpp
   src/rclcpp/parameter_client.cpp
   src/rclcpp/parameter_events_filter.cpp
   src/rclcpp/parameter_map.cpp
   src/rclcpp/parameter_service.cpp
-  src/rclcpp/publisher.cpp
+  src/rclcpp/publisher_base.cpp
+  src/rclcpp/qos.cpp
+  src/rclcpp/qos_event.cpp
   src/rclcpp/service.cpp
-  src/rclcpp/subscription.cpp
+  src/rclcpp/signal_handler.cpp
+  src/rclcpp/subscription_base.cpp
   src/rclcpp/time.cpp
   src/rclcpp/time_source.cpp
   src/rclcpp/timer.cpp
   src/rclcpp/type_support.cpp
   src/rclcpp/utilities.cpp
+  src/rclcpp/waitable.cpp
 )
 
 # "watch" template for changes
@@ -82,19 +85,40 @@ configure_file(
   COPYONLY
 )
 # generate header with logging macros
-set(python_code
+set(python_code_logging
   "import em"
   "em.invoke(['-o', 'include/rclcpp/logging.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/logging.hpp.em'])")
-string(REPLACE ";" "$<SEMICOLON>" python_code "${python_code}")
+string(REPLACE ";" "$<SEMICOLON>" python_code_logging "${python_code_logging}")
 add_custom_command(OUTPUT include/rclcpp/logging.hpp
   COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp"
-  COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_code}"
+  COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_code_logging}"
   DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/logging.hpp.em.watch"
   COMMENT "Expanding logging.hpp.em"
   VERBATIM
 )
 list(APPEND ${PROJECT_NAME}_SRCS
   include/rclcpp/logging.hpp)
+
+# "watch" template for changes
+configure_file(
+  "resource/interface_traits.hpp.em"
+  "interface_traits.hpp.em.watch"
+  COPYONLY
+)
+set(python_code_interface_traits
+  "import em"
+  "em.invoke(['-o', 'include/rclcpp/node_interfaces/interface_traits.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/interface_traits.hpp.em'])")
+string(REPLACE ";" "$<SEMICOLON>" python_code_interface_traits "${python_code_interface_traits}")
+add_custom_command(OUTPUT include/rclcpp/node_interfaces/interface_traits.hpp
+  COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp/node_interfaces"
+  COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_code_interface_traits}"
+  DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/interface_traits.hpp.em.watch"
+  COMMENT "Expanding interfae_traits.hpp.em"
+  VERBATIM
+)
+list(APPEND ${PROJECT_NAME}_SRCS
+  include/rclcpp/node_interfaces/interface_traits.hpp)
+
 include_directories("${CMAKE_CURRENT_BINARY_DIR}/include")
 
 add_library(${PROJECT_NAME}
@@ -140,96 +164,122 @@ if(BUILD_TESTING)
 
   find_package(rmw_implementation_cmake REQUIRED)
 
+  include(cmake/rclcpp_add_build_failure_test.cmake)
+
   ament_add_gtest(test_client test/test_client.cpp)
   if(TARGET test_client)
-    target_include_directories(test_client PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_client
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_client ${PROJECT_NAME})
   endif()
   ament_add_gtest(test_expand_topic_or_service_name test/test_expand_topic_or_service_name.cpp)
   if(TARGET test_expand_topic_or_service_name)
-    target_include_directories(test_expand_topic_or_service_name PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_expand_topic_or_service_name
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME})
   endif()
   ament_add_gtest(test_function_traits test/test_function_traits.cpp)
   if(TARGET test_function_traits)
-    target_include_directories(test_function_traits PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_function_traits
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
   endif()
   ament_add_gtest(test_mapped_ring_buffer test/test_mapped_ring_buffer.cpp)
   if(TARGET test_mapped_ring_buffer)
-    target_include_directories(test_mapped_ring_buffer PUBLIC
-      ${rcl_INCLUDE_DIRS}
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_mapped_ring_buffer
+      "rcl"
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
   endif()
   ament_add_gtest(test_intra_process_manager test/test_intra_process_manager.cpp)
   if(TARGET test_intra_process_manager)
-    target_include_directories(test_intra_process_manager PUBLIC
-      ${rcl_INCLUDE_DIRS}
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_intra_process_manager
+      "rcl"
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
   endif()
   ament_add_gtest(test_node test/test_node.cpp)
   if(TARGET test_node)
-    target_include_directories(test_node PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_node
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_node ${PROJECT_NAME})
   endif()
+
+  ament_add_gtest(test_node_interfaces__get_node_interfaces
+    test/node_interfaces/test_get_node_interfaces.cpp)
+  if(TARGET test_node_interfaces__get_node_interfaces)
+    target_link_libraries(test_node_interfaces__get_node_interfaces ${PROJECT_NAME})
+  endif()
+
+  # TODO(wjwwood): reenable these build failure tests when I can get Jenkins to ignore their output
+  # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_rclcpp_node
+  #   test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp)
+  # target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node
+  #   ${PROJECT_NAME})
+
+  # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
+  #   test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp)
+  # target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
+  #   ${PROJECT_NAME})
+
+  # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_wrapped_node
+  #   test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp)
+  # target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node
+  #   ${PROJECT_NAME})
+
+  # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_wrapped_node
+  #   test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp)
+  # target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
+  #   ${PROJECT_NAME})
+
   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}
+    ament_target_dependencies(test_node_global_args
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_node_global_args ${PROJECT_NAME})
   endif()
-  ament_add_gtest(test_node_initial_parameters test/test_node_initial_parameters.cpp)
-  if(TARGET test_node_initial_parameters)
-    target_link_libraries(test_node_initial_parameters ${PROJECT_NAME})
-  endif()
   ament_add_gtest(test_parameter_events_filter test/test_parameter_events_filter.cpp)
   if(TARGET test_parameter_events_filter)
-    target_include_directories(test_parameter_events_filter PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_parameter_events_filter
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_parameter_events_filter ${PROJECT_NAME})
   endif()
   ament_add_gtest(test_parameter test/test_parameter.cpp)
   if(TARGET test_parameter)
-    target_include_directories(test_parameter PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_parameter
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_parameter ${PROJECT_NAME})
   endif()
@@ -239,72 +289,87 @@ if(BUILD_TESTING)
   endif()
   ament_add_gtest(test_publisher test/test_publisher.cpp)
   if(TARGET test_publisher)
-    target_include_directories(test_publisher PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_publisher
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_publisher ${PROJECT_NAME})
   endif()
-  ament_add_gtest(test_rate test/test_rate.cpp
-    ENV RCL_ASSERT_RMW_ID_MATCHES=${rmw_implementation})
+  ament_add_gtest(test_publisher_subscription_count_api test/test_publisher_subscription_count_api.cpp)
+  if(TARGET test_publisher_subscription_count_api)
+    ament_target_dependencies(test_publisher_subscription_count_api
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
+    )
+    target_link_libraries(test_publisher_subscription_count_api ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_rate test/test_rate.cpp)
   if(TARGET test_rate)
-    target_include_directories(test_rate PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_rate
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_rate
       ${PROJECT_NAME}
     )
   endif()
-  ament_add_gtest(test_serialized_message_allocator test/test_serialized_message_allocator.cpp
-    ENV RCL_ASSERT_RMW_ID_MATCHES=${rmw_implementation})
+  ament_add_gtest(test_serialized_message_allocator test/test_serialized_message_allocator.cpp)
   if(TARGET test_serialized_message_allocator)
-    target_include_directories(test_serialized_message_allocator PUBLIC
-      ${test_msgs_INCLUDE_DIRS}
+    ament_target_dependencies(test_serialized_message_allocator
+      test_msgs
     )
     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}
+    ament_target_dependencies(test_service
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_service ${PROJECT_NAME})
   endif()
   ament_add_gtest(test_subscription test/test_subscription.cpp)
   if(TARGET test_subscription)
-    target_include_directories(test_subscription PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_subscription
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_subscription ${PROJECT_NAME})
   endif()
+  ament_add_gtest(test_subscription_publisher_count_api test/test_subscription_publisher_count_api.cpp)
+  if(TARGET test_subscription_publisher_count_api)
+    ament_target_dependencies(test_subscription_publisher_count_api
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
+    )
+    target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME})
+  endif()
   find_package(test_msgs REQUIRED)
   ament_add_gtest(test_subscription_traits test/test_subscription_traits.cpp)
   if(TARGET test_subscription_traits)
-    target_include_directories(test_subscription_traits PUBLIC
-      ${rcl_INCLUDE_DIRS}
+    ament_target_dependencies(test_subscription_traits
+      "rcl"
+      "test_msgs"
     )
-  ament_target_dependencies(test_subscription_traits
-    "test_msgs"
-  )
   endif()
   ament_add_gtest(test_find_weak_nodes test/test_find_weak_nodes.cpp)
   if(TARGET test_find_weak_nodes)
-    target_include_directories(test_find_weak_nodes PUBLIC
-      ${rcl_INCLUDE_DIRS}
+    ament_target_dependencies(test_find_weak_nodes
+      "rcl"
     )
     target_link_libraries(test_find_weak_nodes ${PROJECT_NAME})
   endif()
@@ -328,8 +393,8 @@ if(BUILD_TESTING)
   ament_add_gtest(test_externally_defined_services test/test_externally_defined_services.cpp
     APPEND_LIBRARY_DIRS "${append_library_dirs}")
   if(TARGET test_externally_defined_services)
-    target_include_directories(test_externally_defined_services PUBLIC
-      ${rcl_INCLUDE_DIRS}
+    ament_target_dependencies(test_externally_defined_services
+      "rcl"
     )
     target_link_libraries(test_externally_defined_services ${PROJECT_NAME})
     foreach(typesupport_impl_cpp ${typesupport_impls_cpp})
@@ -372,6 +437,14 @@ if(BUILD_TESTING)
     target_link_libraries(test_time ${PROJECT_NAME})
   endif()
 
+  ament_add_gtest(test_timer test/test_timer.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_timer)
+    ament_target_dependencies(test_timer
+      "rcl")
+    target_link_libraries(test_timer ${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)
@@ -388,6 +461,22 @@ if(BUILD_TESTING)
     target_link_libraries(test_utilities ${PROJECT_NAME})
   endif()
 
+  ament_add_gtest(test_init test/test_init.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_init)
+    ament_target_dependencies(test_init
+      "rcl")
+    target_link_libraries(test_init ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_interface_traits test/test_interface_traits.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_interface_traits)
+    ament_target_dependencies(test_interface_traits
+      "rcl")
+    target_link_libraries(test_interface_traits ${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)
@@ -395,16 +484,20 @@ if(BUILD_TESTING)
       "rcl")
     target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME})
   endif()
+
+  ament_add_gtest(test_local_parameters test/test_local_parameters.cpp)
+  if(TARGET test_local_parameters)
+    ament_target_dependencies(test_local_parameters
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
+    )
+    target_link_libraries(test_local_parameters ${PROJECT_NAME})
+  endif()
 endif()
 
-ament_package(
-  CONFIG_EXTRAS rclcpp-extras.cmake
-)
-
-install(
-  DIRECTORY cmake
-  DESTINATION share/${PROJECT_NAME}
-)
+ament_package()
 
 install(
   DIRECTORY include/ ${CMAKE_CURRENT_BINARY_DIR}/include/

rclcpp/cmake/rclcpp_add_build_failure_test.cmake

@@ -0,0 +1,56 @@
+# Copyright 2019 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+#
+# Register a test which tries to compile a file and expects it to fail to build.
+#
+# This will create two targets, one by the given target name and a test target
+# which has the same name prefixed with `test_`.
+# For example, if target is `should_not_compile__use_const_argument` then there
+# will be an executable target called `should_not_compile__use_const_argument`
+# and a test target called `test_should_not_compile__use_const_argument`.
+#
+# :param target: the name of the target to be created
+# :type target: string
+# :param ARGN: the list of source files to be used to create the test executable
+# :type ARGN: list of strings
+#
+macro(rclcpp_add_build_failure_test target)
+  if(${ARGC} EQUAL 0)
+    message(
+      FATAL_ERROR
+      "rclcpp_add_build_failure_test() requires a target name and "
+      "at least one source file")
+  endif()
+
+  add_executable(${target} ${ARGN})
+  set_target_properties(${target}
+    PROPERTIES
+      EXCLUDE_FROM_ALL TRUE
+      EXCLUDE_FROM_DEFAULT_BUILD TRUE)
+
+  add_test(
+    NAME test_${target}
+    COMMAND
+      ${CMAKE_COMMAND}
+      --build .
+      --target ${target}
+      --config $<CONFIGURATION>
+    WORKING_DIRECTORY ${CMAKE_BINARY_DIR})
+  set_tests_properties(test_${target}
+    PROPERTIES
+      WILL_FAIL TRUE
+      LABELS "build_failure"
+  )
+endmacro()

rclcpp/cmake/rclcpp_create_node_main.cmake

@@ -1,26 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-
-set(rclcpp_node_main_SRC "${rclcpp_DIR}/../../../src/rclcpp/node_main.cpp")
-
-function(rclcpp_create_node_main node_library_target)
-  if(NOT TARGET ${node_library_target})
-    message(FATAL_ERROR "rclcpp_create_node_main() the first argument must be a valid target name")
-  endif()
-  set(executable_name_ ${node_library_target}_node)
-  add_executable(${executable_name_} ${rclcpp_node_main_SRC})
-  target_link_libraries(${executable_name_} ${node_library_target})
-  install(TARGETS ${executable_name_} DESTINATION bin)
-endfunction()

rclcpp/include/rclcpp/allocator/allocator_common.hpp

@@ -65,7 +65,8 @@ 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 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)
 {
@@ -83,7 +84,8 @@ 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 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)
 {

rclcpp/include/rclcpp/any_executable.hpp

@@ -25,6 +25,7 @@
 #include "rclcpp/subscription.hpp"
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
 
 namespace rclcpp
 {
@@ -45,6 +46,7 @@ struct AnyExecutable
   rclcpp::TimerBase::SharedPtr timer;
   rclcpp::ServiceBase::SharedPtr service;
   rclcpp::ClientBase::SharedPtr client;
+  rclcpp::Waitable::SharedPtr waitable;
   // These are used to keep the scope on the containing items
   rclcpp::callback_group::CallbackGroup::SharedPtr callback_group;
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base;

rclcpp/include/rclcpp/any_service_callback.hpp

@@ -32,12 +32,12 @@ class AnyServiceCallback
 {
 private:
   using SharedPtrCallback = std::function<
-    void(
+    void (
       const std::shared_ptr<typename ServiceT::Request>,
       std::shared_ptr<typename ServiceT::Response>
     )>;
   using SharedPtrWithRequestHeaderCallback = std::function<
-    void(
+    void (
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<typename ServiceT::Request>,
       std::shared_ptr<typename ServiceT::Response>

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -36,17 +36,18 @@ class AnySubscriptionCallback
   using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
   using MessageAlloc = typename MessageAllocTraits::allocator_type;
   using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
+  using ConstMessageSharedPtr = std::shared_ptr<const MessageT>;
   using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
 
-  using SharedPtrCallback = std::function<void(const std::shared_ptr<MessageT>)>;
+  using SharedPtrCallback = std::function<void (const std::shared_ptr<MessageT>)>;
   using SharedPtrWithInfoCallback =
-    std::function<void(const std::shared_ptr<MessageT>, const rmw_message_info_t &)>;
-  using ConstSharedPtrCallback = std::function<void(const std::shared_ptr<const MessageT>)>;
+    std::function<void (const std::shared_ptr<MessageT>, const rmw_message_info_t &)>;
+  using ConstSharedPtrCallback = std::function<void (const std::shared_ptr<const MessageT>)>;
   using ConstSharedPtrWithInfoCallback =
-    std::function<void(const std::shared_ptr<const MessageT>, const rmw_message_info_t &)>;
-  using UniquePtrCallback = std::function<void(MessageUniquePtr)>;
+    std::function<void (const std::shared_ptr<const MessageT>, const rmw_message_info_t &)>;
+  using UniquePtrCallback = std::function<void (MessageUniquePtr)>;
   using UniquePtrWithInfoCallback =
-    std::function<void(MessageUniquePtr, const rmw_message_info_t &)>;
+    std::function<void (MessageUniquePtr, const rmw_message_info_t &)>;
 
   SharedPtrCallback shared_ptr_callback_;
   SharedPtrWithInfoCallback shared_ptr_with_info_callback_;
@@ -154,7 +155,6 @@ public:
   void dispatch(
     std::shared_ptr<MessageT> message, const rmw_message_info_t & message_info)
   {
-    (void)message_info;
     if (shared_ptr_callback_) {
       shared_ptr_callback_(message);
     } else if (shared_ptr_with_info_callback_) {
@@ -177,30 +177,50 @@ public:
   }
 
   void dispatch_intra_process(
-    MessageUniquePtr & message, const rmw_message_info_t & message_info)
+    ConstMessageSharedPtr message, const rmw_message_info_t & message_info)
+  {
+    if (const_shared_ptr_callback_) {
+      const_shared_ptr_callback_(message);
+    } else if (const_shared_ptr_with_info_callback_) {
+      const_shared_ptr_with_info_callback_(message, message_info);
+    } else {
+      if (unique_ptr_callback_ || unique_ptr_with_info_callback_ ||
+        shared_ptr_callback_ || shared_ptr_with_info_callback_)
+      {
+        throw std::runtime_error("unexpected dispatch_intra_process const shared "
+                "message call with no const shared_ptr callback");
+      } else {
+        throw std::runtime_error("unexpected message without any callback set");
+      }
+    }
+  }
+
+  void dispatch_intra_process(
+    MessageUniquePtr message, const rmw_message_info_t & message_info)
   {
-    (void)message_info;
     if (shared_ptr_callback_) {
       typename std::shared_ptr<MessageT> shared_message = std::move(message);
       shared_ptr_callback_(shared_message);
     } else if (shared_ptr_with_info_callback_) {
       typename std::shared_ptr<MessageT> shared_message = std::move(message);
       shared_ptr_with_info_callback_(shared_message, message_info);
-    } else if (const_shared_ptr_callback_) {
-      typename std::shared_ptr<MessageT const> const_shared_message = std::move(message);
-      const_shared_ptr_callback_(const_shared_message);
-    } else if (const_shared_ptr_with_info_callback_) {
-      typename std::shared_ptr<MessageT const> const_shared_message = std::move(message);
-      const_shared_ptr_with_info_callback_(const_shared_message, message_info);
     } else if (unique_ptr_callback_) {
       unique_ptr_callback_(std::move(message));
     } else if (unique_ptr_with_info_callback_) {
       unique_ptr_with_info_callback_(std::move(message), message_info);
+    } else if (const_shared_ptr_callback_ || const_shared_ptr_with_info_callback_) {
+      throw std::runtime_error("unexpected dispatch_intra_process unique message call"
+              " with const shared_ptr callback");
     } else {
       throw std::runtime_error("unexpected message without any callback set");
     }
   }
 
+  bool use_take_shared_method()
+  {
+    return const_shared_ptr_callback_ || const_shared_ptr_with_info_callback_;
+  }
+
 private:
   std::shared_ptr<MessageAlloc> message_allocator_;
   MessageDeleter message_deleter_;

rclcpp/include/rclcpp/callback_group.hpp

@@ -21,10 +21,12 @@
 #include <vector>
 
 #include "rclcpp/client.hpp"
+#include "rclcpp/publisher.hpp"
 #include "rclcpp/service.hpp"
 #include "rclcpp/subscription.hpp"
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
 
 namespace rclcpp
 {
@@ -35,6 +37,7 @@ namespace node_interfaces
 class NodeServices;
 class NodeTimers;
 class NodeTopics;
+class NodeWaitables;
 }  // namespace node_interfaces
 
 namespace callback_group
@@ -51,6 +54,7 @@ class CallbackGroup
   friend class rclcpp::node_interfaces::NodeServices;
   friend class rclcpp::node_interfaces::NodeTimers;
   friend class rclcpp::node_interfaces::NodeTopics;
+  friend class rclcpp::node_interfaces::NodeWaitables;
 
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup)
@@ -74,6 +78,10 @@ public:
   const std::vector<rclcpp::ClientBase::WeakPtr> &
   get_client_ptrs() const;
 
+  RCLCPP_PUBLIC
+  const std::vector<rclcpp::Waitable::WeakPtr> &
+  get_waitable_ptrs() const;
+
   RCLCPP_PUBLIC
   std::atomic_bool &
   can_be_taken_from();
@@ -85,6 +93,10 @@ public:
 protected:
   RCLCPP_DISABLE_COPY(CallbackGroup)
 
+  RCLCPP_PUBLIC
+  void
+  add_publisher(const rclcpp::PublisherBase::SharedPtr publisher_ptr);
+
   RCLCPP_PUBLIC
   void
   add_subscription(const rclcpp::SubscriptionBase::SharedPtr subscription_ptr);
@@ -101,6 +113,14 @@ protected:
   void
   add_client(const rclcpp::ClientBase::SharedPtr client_ptr);
 
+  RCLCPP_PUBLIC
+  void
+  add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr);
+
+  RCLCPP_PUBLIC
+  void
+  remove_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) noexcept;
+
   CallbackGroupType type_;
   // Mutex to protect the subsequent vectors of pointers.
   mutable std::mutex mutex_;
@@ -108,6 +128,7 @@ protected:
   std::vector<rclcpp::TimerBase::WeakPtr> timer_ptrs_;
   std::vector<rclcpp::ServiceBase::WeakPtr> service_ptrs_;
   std::vector<rclcpp::ClientBase::WeakPtr> client_ptrs_;
+  std::vector<rclcpp::Waitable::WeakPtr> waitable_ptrs_;
   std::atomic_bool can_be_taken_from_;
 };
 

rclcpp/include/rclcpp/client.hpp

@@ -78,10 +78,10 @@ public:
   bool
   service_is_ready() const;
 
-  template<typename RatioT = std::milli>
+  template<typename RepT = int64_t, typename RatioT = std::milli>
   bool
   wait_for_service(
-    std::chrono::duration<int64_t, RatioT> timeout = std::chrono::duration<int64_t, RatioT>(-1))
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
   {
     return wait_for_service_nanoseconds(
       std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
@@ -110,6 +110,7 @@ protected:
 
   rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
   std::shared_ptr<rcl_node_t> node_handle_;
+  std::shared_ptr<rclcpp::Context> context_;
 
   std::shared_ptr<rcl_client_t> client_handle_;
 };
@@ -130,8 +131,8 @@ public:
   using SharedFuture = std::shared_future<SharedResponse>;
   using SharedFutureWithRequest = std::shared_future<std::pair<SharedRequest, SharedResponse>>;
 
-  using CallbackType = std::function<void(SharedFuture)>;
-  using CallbackWithRequestType = std::function<void(SharedFutureWithRequest)>;
+  using CallbackType = std::function<void (SharedFuture)>;
+  using CallbackWithRequestType = std::function<void (SharedFutureWithRequest)>;
 
   RCLCPP_SMART_PTR_DEFINITIONS(Client)
 
@@ -171,13 +172,13 @@ public:
   }
 
   std::shared_ptr<void>
-  create_response()
+  create_response() override
   {
     return std::shared_ptr<void>(new typename ServiceT::Response());
   }
 
   std::shared_ptr<rmw_request_id_t>
-  create_request_header()
+  create_request_header() override
   {
     // TODO(wjwwood): This should probably use rmw_request_id's allocator.
     //                (since it is a C type)
@@ -187,7 +188,7 @@ public:
   void
   handle_response(
     std::shared_ptr<rmw_request_id_t> request_header,
-    std::shared_ptr<void> response)
+    std::shared_ptr<void> response) override
   {
     std::unique_lock<std::mutex> lock(pending_requests_mutex_);
     auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(response);

rclcpp/include/rclcpp/clock.hpp

@@ -16,9 +16,6 @@
 #define RCLCPP__CLOCK_HPP_
 
 #include <functional>
-#include <memory>
-#include <mutex>
-#include <vector>
 
 #include "rclcpp/macros.hpp"
 #include "rclcpp/time.hpp"
@@ -31,53 +28,22 @@ 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;
+  using pre_callback_t = std::function<void ()>;
+  using post_callback_t = std::function<void (const rcl_time_jump_t &)>;
+
+  JumpHandler(
+    pre_callback_t pre_callback,
+    post_callback_t post_callback,
+    const rcl_jump_threshold_t & threshold);
+
+  pre_callback_t pre_callback;
+  post_callback_t post_callback;
+  rcl_jump_threshold_t notice_threshold;
 };
 
 class Clock
@@ -85,59 +51,89 @@ class Clock
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(Clock)
 
+  /// Default c'tor
+  /**
+   * Initializes the clock instance with the given clock_type.
+   *
+   * \param clock_type type of the clock.
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
+   */
   RCLCPP_PUBLIC
   explicit Clock(rcl_clock_type_t clock_type = RCL_SYSTEM_TIME);
 
   RCLCPP_PUBLIC
   ~Clock();
 
+  /**
+   * Returns current time from the time source specified by clock_type.
+   *
+   * \return current time.
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
+   */
   RCLCPP_PUBLIC
   Time
   now();
 
+  /**
+   * Returns the clock of the type `RCL_ROS_TIME` is active.
+   *
+   * \return true if the clock is active
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw if
+   * the current clock does not have the clock_type `RCL_ROS_TIME`.
+   */
   RCLCPP_PUBLIC
   bool
   ros_time_is_active();
 
+  RCLCPP_PUBLIC
+  rcl_clock_t *
+  get_clock_handle() noexcept;
+
   RCLCPP_PUBLIC
   rcl_clock_type_t
-  get_clock_type();
+  get_clock_type() const noexcept;
 
   // 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.
+   *
+   * Function will register callbacks to the callback queue. On time jump all
+   * callbacks will be executed whose threshold is greater then the time jump;
+   * The logic will first call selected pre_callbacks and then all selected
+   * post_callbacks.
+   *
+   * Function is only applicable if the clock_type is `RCL_ROS_TIME`
+   *
+   * \param pre_callback. Must be non-throwing
+   * \param post_callback. Must be non-throwing.
+   * \param threshold. Callbacks will be triggered if the time jump is greater
+   * then the threshold.
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
+   * \throws std::bad_alloc if the allocation of the JumpHandler fails.
+   * \warning the instance of the clock must remain valid as long as any created
+   * JumpHandler.
    */
   RCLCPP_PUBLIC
   JumpHandler::SharedPtr
   create_jump_callback(
-    std::function<void()> pre_callback,
-    std::function<void(const TimeJump &)> post_callback,
-    JumpThreshold & threshold);
+    JumpHandler::pre_callback_t pre_callback,
+    JumpHandler::post_callback_t post_callback,
+    const rcl_jump_threshold_t & threshold);
 
 private:
-  // A method for TimeSource to get a list of callbacks to invoke while updating
+  // Invoke time jump callback
   RCLCPP_PUBLIC
-  std::vector<JumpHandler::SharedPtr>
-  get_triggered_callback_handlers(const TimeJump & jump);
-
-  // Invoke callbacks that are valid and outside threshold.
-  RCLCPP_PUBLIC
-  static void invoke_prejump_callbacks(
-    const std::vector<JumpHandler::SharedPtr> & callbacks);
-
-  RCLCPP_PUBLIC
-  static void invoke_postjump_callbacks(
-    const std::vector<JumpHandler::SharedPtr> & callbacks,
-    const TimeJump & jump);
+  static void
+  on_time_jump(
+    const struct rcl_time_jump_t * time_jump,
+    bool before_jump,
+    void * user_data);
 
   /// Internal storage backed by rcl
   rcl_clock_t rcl_clock_;
   friend TimeSource;  /// Allow TimeSource to access the rcl_clock_ datatype.
   rcl_allocator_t allocator_;
-
-  std::mutex callback_list_mutex_;
-  std::vector<std::weak_ptr<JumpHandler>> active_jump_handlers_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/context.hpp

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

rclcpp/include/rclcpp/create_publisher.hpp

@@ -18,19 +18,30 @@
 #include <memory>
 #include <string>
 
+#include "rclcpp/node_interfaces/get_node_topics_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/node_options.hpp"
 #include "rclcpp/publisher_factory.hpp"
+#include "rclcpp/publisher_options.hpp"
+#include "rclcpp/qos.hpp"
 #include "rmw/qos_profiles.h"
 
 namespace rclcpp
 {
 
-template<typename MessageT, typename AllocatorT, typename PublisherT>
+template<
+  typename MessageT,
+  typename AllocatorT = std::allocator<void>,
+  typename PublisherT = ::rclcpp::Publisher<MessageT, AllocatorT>>
+// cppcheck-suppress syntaxError // bug in cppcheck 1.82 for [[deprecated]] on templated function
+[[deprecated("use alternative rclcpp::create_publisher() signatures")]]
 std::shared_ptr<PublisherT>
 create_publisher(
   rclcpp::node_interfaces::NodeTopicsInterface * node_topics,
   const std::string & topic_name,
   const rmw_qos_profile_t & qos_profile,
+  const PublisherEventCallbacks & event_callbacks,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group,
   bool use_intra_process_comms,
   std::shared_ptr<AllocatorT> allocator)
 {
@@ -39,10 +50,69 @@ create_publisher(
 
   auto pub = node_topics->create_publisher(
     topic_name,
-    rclcpp::create_publisher_factory<MessageT, AllocatorT, PublisherT>(allocator),
+    rclcpp::create_publisher_factory<MessageT, AllocatorT, PublisherT>(event_callbacks, allocator),
     publisher_options,
     use_intra_process_comms);
-  node_topics->add_publisher(pub);
+
+  node_topics->add_publisher(pub, group);
+
+  return std::dynamic_pointer_cast<PublisherT>(pub);
+}
+
+/// Create and return a publisher of the given MessageT type.
+/**
+ * The NodeT type only needs to have a method called get_node_topics_interface()
+ * which returns a shared_ptr to a NodeTopicsInterface.
+ */
+template<
+  typename MessageT,
+  typename AllocatorT = std::allocator<void>,
+  typename PublisherT = ::rclcpp::Publisher<MessageT, AllocatorT>,
+  typename NodeT>
+std::shared_ptr<PublisherT>
+create_publisher(
+  NodeT & node,
+  const std::string & topic_name,
+  const rclcpp::QoS & qos,
+  const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options = (
+    rclcpp::PublisherOptionsWithAllocator<AllocatorT>()
+))
+{
+  using rclcpp::node_interfaces::get_node_topics_interface;
+  auto node_topics = get_node_topics_interface(node);
+
+  std::shared_ptr<AllocatorT> allocator = options.allocator;
+  if (!allocator) {
+    allocator = std::make_shared<AllocatorT>();
+  }
+
+  bool use_intra_process;
+  switch (options.use_intra_process_comm) {
+    case IntraProcessSetting::Enable:
+      use_intra_process = true;
+      break;
+    case IntraProcessSetting::Disable:
+      use_intra_process = false;
+      break;
+    case IntraProcessSetting::NodeDefault:
+      use_intra_process = node_topics->get_node_base_interface()->get_use_intra_process_default();
+      break;
+    default:
+      throw std::runtime_error("Unrecognized IntraProcessSetting value");
+      break;
+  }
+
+  // TODO(wjwwood): convert all of the interfaces to use QoS and PublisherOptionsBase
+  auto pub = node_topics->create_publisher(
+    topic_name,
+    rclcpp::create_publisher_factory<MessageT, AllocatorT, PublisherT>(
+      options.event_callbacks,
+      allocator
+    ),
+    options.template to_rcl_publisher_options<MessageT>(qos),
+    use_intra_process
+  );
+  node_topics->add_publisher(pub, options.callback_group);
   return std::dynamic_pointer_cast<PublisherT>(pub);
 }
 

rclcpp/include/rclcpp/create_subscription.hpp

@@ -19,8 +19,11 @@
 #include <string>
 #include <utility>
 
+#include "rclcpp/node_interfaces/get_node_topics_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
 #include "rclcpp/subscription_factory.hpp"
+#include "rclcpp/subscription_options.hpp"
+#include "rclcpp/qos.hpp"
 #include "rmw/qos_profiles.h"
 
 namespace rclcpp
@@ -32,12 +35,15 @@ template<
   typename AllocatorT,
   typename CallbackMessageT,
   typename SubscriptionT = rclcpp::Subscription<CallbackMessageT, AllocatorT>>
+// cppcheck-suppress syntaxError // bug in cppcheck 1.82 for [[deprecated]] on templated function
+[[deprecated("use alternative rclcpp::create_subscription() signatures")]]
 typename std::shared_ptr<SubscriptionT>
 create_subscription(
   rclcpp::node_interfaces::NodeTopicsInterface * node_topics,
   const std::string & topic_name,
   CallbackT && callback,
   const rmw_qos_profile_t & qos_profile,
+  const SubscriptionEventCallbacks & event_callbacks,
   rclcpp::callback_group::CallbackGroup::SharedPtr group,
   bool ignore_local_publications,
   bool use_intra_process_comms,
@@ -52,7 +58,10 @@ create_subscription(
 
   auto factory = rclcpp::create_subscription_factory
     <MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT>(
-    std::forward<CallbackT>(callback), msg_mem_strat, allocator);
+    std::forward<CallbackT>(callback),
+    event_callbacks,
+    msg_mem_strat,
+    allocator);
 
   auto sub = node_topics->create_subscription(
     topic_name,
@@ -63,6 +72,75 @@ create_subscription(
   return std::dynamic_pointer_cast<SubscriptionT>(sub);
 }
 
+/// Create and return a subscription of the given MessageT type.
+/**
+ * The NodeT type only needs to have a method called get_node_topics_interface()
+ * which returns a shared_ptr to a NodeTopicsInterface, or be a
+ * NodeTopicsInterface pointer itself.
+ */
+template<
+  typename MessageT,
+  typename CallbackT,
+  typename AllocatorT = std::allocator<void>,
+  typename CallbackMessageT =
+  typename rclcpp::subscription_traits::has_message_type<CallbackT>::type,
+  typename SubscriptionT = rclcpp::Subscription<CallbackMessageT, AllocatorT>,
+  typename NodeT>
+typename std::shared_ptr<SubscriptionT>
+create_subscription(
+  NodeT && node,
+  const std::string & topic_name,
+  const rclcpp::QoS & qos,
+  CallbackT && callback,
+  const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
+    rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
+  ),
+  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
+    CallbackMessageT, AllocatorT>::SharedPtr
+  msg_mem_strat = nullptr)
+{
+  using rclcpp::node_interfaces::get_node_topics_interface;
+  auto node_topics = get_node_topics_interface(std::forward<NodeT>(node));
+
+  if (!msg_mem_strat) {
+    using rclcpp::message_memory_strategy::MessageMemoryStrategy;
+    msg_mem_strat = MessageMemoryStrategy<CallbackMessageT, AllocatorT>::create_default();
+  }
+
+  std::shared_ptr<AllocatorT> allocator = options.allocator;
+  if (!allocator) {
+    allocator = std::make_shared<AllocatorT>();
+  }
+  auto factory = rclcpp::create_subscription_factory
+    <MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT>(
+    std::forward<CallbackT>(callback), options.event_callbacks, msg_mem_strat, allocator);
+
+  bool use_intra_process;
+  switch (options.use_intra_process_comm) {
+    case IntraProcessSetting::Enable:
+      use_intra_process = true;
+      break;
+    case IntraProcessSetting::Disable:
+      use_intra_process = false;
+      break;
+    case IntraProcessSetting::NodeDefault:
+      use_intra_process = node_topics->get_node_base_interface()->get_use_intra_process_default();
+      break;
+    default:
+      throw std::runtime_error("Unrecognized IntraProcessSetting value");
+      break;
+  }
+
+  // TODO(wjwwood): convert all of the interfaces to use QoS and SubscriptionOptionsBase
+  auto sub = node_topics->create_subscription(
+    topic_name,
+    factory,
+    options.template to_rcl_subscription_options<MessageT>(qos),
+    use_intra_process);
+  node_topics->add_subscription(sub, options.callback_group);
+  return std::dynamic_pointer_cast<SubscriptionT>(sub);
+}
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__CREATE_SUBSCRIPTION_HPP_

rclcpp/include/rclcpp/duration.hpp

@@ -23,80 +23,87 @@
 
 namespace rclcpp
 {
-class Duration
+class RCLCPP_PUBLIC Duration
 {
 public:
-  RCLCPP_PUBLIC
   Duration(int32_t seconds, uint32_t nanoseconds);
 
-  RCLCPP_PUBLIC
-  explicit Duration(
-    rcl_duration_value_t nanoseconds);
+  explicit Duration(rcl_duration_value_t nanoseconds);
 
-  RCLCPP_PUBLIC
-  explicit Duration(
-    std::chrono::nanoseconds nanoseconds);
+  explicit Duration(std::chrono::nanoseconds nanoseconds);
 
-  RCLCPP_PUBLIC
-  Duration(
-    const builtin_interfaces::msg::Duration & duration_msg);
+  // intentionally not using explicit to create a conversion constructor
+  template<class Rep, class Period>
+  // cppcheck-suppress noExplicitConstructor
+  Duration(const std::chrono::duration<Rep, Period> & duration)  // NOLINT(runtime/explicit)
+  : Duration(std::chrono::duration_cast<std::chrono::nanoseconds>(duration))
+  {}
+
+  // cppcheck-suppress noExplicitConstructor
+  Duration(const builtin_interfaces::msg::Duration & duration_msg);  // NOLINT(runtime/explicit)
 
-  RCLCPP_PUBLIC
   explicit Duration(const rcl_duration_t & duration);
 
-  RCLCPP_PUBLIC
   Duration(const Duration & rhs);
 
-  RCLCPP_PUBLIC
-  virtual ~Duration();
+  virtual ~Duration() = default;
 
-  RCLCPP_PUBLIC
   operator builtin_interfaces::msg::Duration() const;
 
-  RCLCPP_PUBLIC
+  // cppcheck-suppress operatorEq // this is a false positive from cppcheck
   Duration &
   operator=(const Duration & rhs);
 
-  RCLCPP_PUBLIC
   Duration &
   operator=(const builtin_interfaces::msg::Duration & Duration_msg);
 
-  RCLCPP_PUBLIC
   bool
   operator==(const rclcpp::Duration & rhs) const;
 
-  RCLCPP_PUBLIC
   bool
   operator<(const rclcpp::Duration & rhs) const;
 
-  RCLCPP_PUBLIC
   bool
   operator<=(const rclcpp::Duration & rhs) const;
 
-  RCLCPP_PUBLIC
   bool
   operator>=(const rclcpp::Duration & rhs) const;
 
-  RCLCPP_PUBLIC
   bool
   operator>(const rclcpp::Duration & rhs) const;
 
-  RCLCPP_PUBLIC
   Duration
   operator+(const rclcpp::Duration & rhs) const;
 
-  RCLCPP_PUBLIC
   Duration
   operator-(const rclcpp::Duration & rhs) const;
 
-  RCLCPP_PUBLIC
+  static
+  Duration
+  max();
+
   Duration
   operator*(double scale) const;
 
-  RCLCPP_PUBLIC
   rcl_duration_value_t
   nanoseconds() const;
 
+  /// \return the duration in seconds as a floating point number.
+  /// \warning Depending on sizeof(double) there could be significant precision loss.
+  /// When an exact time is required use nanoseconds() instead.
+  double
+  seconds() const;
+
+  template<class DurationT>
+  DurationT
+  to_chrono() const
+  {
+    return std::chrono::duration_cast<DurationT>(std::chrono::nanoseconds(this->nanoseconds()));
+  }
+
+  rmw_time_t
+  to_rmw_time() const;
+
 private:
   rcl_duration_t rcl_duration_;
 };

rclcpp/include/rclcpp/exceptions.hpp

@@ -110,13 +110,15 @@ public:
  * \throws std::runtime_error if the rcl_get_error_state returns 0
  * \throws RCLErrorBase some child class exception based on ret
  */
+/* *INDENT-OFF* */  // Uncrustify cannot yet understand [[noreturn]] properly
 RCLCPP_PUBLIC
 void
-throw_from_rcl_error(
+throw_from_rcl_error [[noreturn]] (
   rcl_ret_t ret,
   const std::string & prefix = "",
   const rcl_error_state_t * error_state = nullptr,
-  void (*reset_error)() = rcl_reset_error);
+  void (* reset_error)() = rcl_reset_error);
+/* *INDENT-ON* */
 
 class RCLErrorBase
 {
@@ -185,14 +187,35 @@ public:
 class InvalidParametersException : public std::runtime_error
 {
 public:
-  // Inherit constructors from runtime_error;
+  // Inherit constructors from runtime_error.
   using std::runtime_error::runtime_error;
 };
 
-/// Throwing if passed parameter value is invalid.
+/// Thrown if passed parameter value is invalid.
 class InvalidParameterValueException : public std::runtime_error
 {
-  // Inherit constructors from runtime_error;
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
+/// Thrown if parameter is already declared.
+class ParameterAlreadyDeclaredException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
+/// Thrown if parameter is not declared, e.g. either set or get was called without first declaring.
+class ParameterNotDeclaredException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
+/// Thrown if parameter is immutable and therefore cannot be undeclared.
+class ParameterImmutableException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error.
   using std::runtime_error::runtime_error;
 };
 

rclcpp/include/rclcpp/executor.hpp

@@ -28,9 +28,10 @@
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
 
-#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/memory_strategies.hpp"
 #include "rclcpp/memory_strategy.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -65,16 +66,20 @@ to_string(const FutureReturnCode & future_return_code);
  */
 struct ExecutorArgs
 {
+  ExecutorArgs()
+  : memory_strategy(memory_strategies::create_default_strategy()),
+    context(rclcpp::contexts::default_context::get_global_default_context()),
+    max_conditions(0)
+  {}
+
   memory_strategy::MemoryStrategy::SharedPtr memory_strategy;
-  size_t max_conditions = 0;
+  std::shared_ptr<rclcpp::Context> context;
+  size_t max_conditions;
 };
 
 static inline ExecutorArgs create_default_executor_arguments()
 {
-  ExecutorArgs args;
-  args.memory_strategy = memory_strategies::create_default_strategy();
-  args.max_conditions = 0;
-  return args;
+  return ExecutorArgs();
 }
 
 /// Coordinate the order and timing of available communication tasks.
@@ -95,7 +100,7 @@ public:
   /// Default constructor.
   // \param[in] ms The memory strategy to be used with this executor.
   RCLCPP_PUBLIC
-  explicit Executor(const ExecutorArgs & args = create_default_executor_arguments());
+  explicit Executor(const ExecutorArgs & args = ExecutorArgs());
 
   /// Default destructor.
   RCLCPP_PUBLIC
@@ -146,11 +151,11 @@ public:
    * spin_node_once to block indefinitely (the default behavior). A timeout of 0 causes this
    * function to be non-blocking.
    */
-  template<typename T = std::milli>
+  template<typename RepT = int64_t, typename T = std::milli>
   void
   spin_node_once(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
-    std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
+    std::chrono::duration<RepT, T> timeout = std::chrono::duration<RepT, T>(-1))
   {
     return spin_node_once_nanoseconds(
       node,
@@ -159,11 +164,11 @@ public:
   }
 
   /// Convenience function which takes Node and forwards NodeBaseInterface.
-  template<typename NodeT = rclcpp::Node, typename T = std::milli>
+  template<typename NodeT = rclcpp::Node, typename RepT = int64_t, typename T = std::milli>
   void
   spin_node_once(
     std::shared_ptr<NodeT> node,
-    std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
+    std::chrono::duration<RepT, T> timeout = std::chrono::duration<RepT, T>(-1))
   {
     return spin_node_once_nanoseconds(
       node->get_node_base_interface(),
@@ -190,10 +195,14 @@ public:
    * single-threaded model of execution.
    * Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function
    * to block (which may have unintended consequences).
+   *
+   * \param[in] max_duration The maximum amount of time to spend executing work, or 0 for no limit.
+   * Note that spin_some() may take longer than this time as it only returns once max_duration has
+   * been exceeded.
    */
   RCLCPP_PUBLIC
   virtual void
-  spin_some();
+  spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0));
 
   RCLCPP_PUBLIC
   virtual void
@@ -209,11 +218,11 @@ public:
    *   code.
    * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
    */
-  template<typename ResponseT, typename TimeT = std::milli>
+  template<typename ResponseT, typename TimeRepT = int64_t, typename TimeT = std::milli>
   FutureReturnCode
   spin_until_future_complete(
     std::shared_future<ResponseT> & future,
-    std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
+    std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
   {
     // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
     // inside a callback executed by an executor.
@@ -233,7 +242,7 @@ public:
     }
     std::chrono::nanoseconds timeout_left = timeout_ns;
 
-    while (rclcpp::ok()) {
+    while (rclcpp::ok(this->context_)) {
       // Do one item of work.
       spin_once(timeout_left);
       // Check if the future is set, return SUCCESS if it is.
@@ -349,6 +358,9 @@ protected:
   /// The memory strategy: an interface for handling user-defined memory allocation strategies.
   memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;
 
+  /// The context associated with this executor.
+  std::shared_ptr<rclcpp::Context> context_;
+
 private:
   RCLCPP_DISABLE_COPY(Executor)
 

rclcpp/include/rclcpp/executors.hpp

@@ -65,13 +65,13 @@ using rclcpp::executors::SingleThreadedExecutor;
  *   If the time spent inside the blocking loop exceeds this timeout, return a `TIMEOUT` return code.
  * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
  */
-template<typename ResponseT, typename TimeT = std::milli>
+template<typename ResponseT, typename TimeRepT = int64_t, typename TimeT = std::milli>
 rclcpp::executor::FutureReturnCode
 spin_node_until_future_complete(
   rclcpp::executor::Executor & executor,
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
   std::shared_future<ResponseT> & future,
-  std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
+  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
   // inside a callback executed by an executor.
@@ -81,13 +81,14 @@ spin_node_until_future_complete(
   return retcode;
 }
 
-template<typename NodeT = rclcpp::Node, typename ResponseT, typename TimeT = std::milli>
+template<typename NodeT = rclcpp::Node, typename ResponseT, typename TimeRepT = int64_t,
+  typename TimeT = std::milli>
 rclcpp::executor::FutureReturnCode
 spin_node_until_future_complete(
   rclcpp::executor::Executor & executor,
   std::shared_ptr<NodeT> node_ptr,
   std::shared_future<ResponseT> & future,
-  std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
+  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   return rclcpp::executors::spin_node_until_future_complete(
     executor,
@@ -98,23 +99,24 @@ spin_node_until_future_complete(
 
 }  // namespace executors
 
-template<typename FutureT, typename TimeT = std::milli>
+template<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::milli>
 rclcpp::executor::FutureReturnCode
 spin_until_future_complete(
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
   std::shared_future<FutureT> & future,
-  std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
+  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   rclcpp::executors::SingleThreadedExecutor executor;
   return executors::spin_node_until_future_complete<FutureT>(executor, node_ptr, future, timeout);
 }
 
-template<typename NodeT = rclcpp::Node, typename FutureT, typename TimeT = std::milli>
+template<typename NodeT = rclcpp::Node, typename FutureT, typename TimeRepT = int64_t,
+  typename TimeT = std::milli>
 rclcpp::executor::FutureReturnCode
 spin_until_future_complete(
   std::shared_ptr<NodeT> node_ptr,
   std::shared_future<FutureT> & future,
-  std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
+  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout);
 }

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -45,11 +45,13 @@ public:
    * once.
    *
    * \param args common arguments for all executors
+   * \param number_of_threads number of threads to have in the thread pool,
+   *   the default 0 will use the number of cpu cores found instead
    * \param yield_before_execute if true std::this_thread::yield() is called
    */
   RCLCPP_PUBLIC
   MultiThreadedExecutor(
-    const executor::ExecutorArgs & args = rclcpp::executor::create_default_executor_arguments(),
+    const executor::ExecutorArgs & args = executor::ExecutorArgs(),
     size_t number_of_threads = 0,
     bool yield_before_execute = false);
 
@@ -76,7 +78,6 @@ private:
   size_t number_of_threads_;
   bool yield_before_execute_;
 
-  std::mutex scheduled_timers_mutex_;
   std::set<TimerBase::SharedPtr> scheduled_timers_;
 };
 

rclcpp/include/rclcpp/executors/single_threaded_executor.hpp

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

rclcpp/include/rclcpp/function_traits.hpp

@@ -49,14 +49,14 @@ template<typename FunctionT>
 struct function_traits
 {
   using arguments = typename tuple_tail<
-    typename function_traits<decltype( & FunctionT::operator())>::arguments>::type;
+    typename function_traits<decltype( &FunctionT::operator())>::arguments>::type;
 
   static constexpr std::size_t arity = std::tuple_size<arguments>::value;
 
   template<std::size_t N>
   using argument_type = typename std::tuple_element<N, arguments>::type;
 
-  using return_type = typename function_traits<decltype( & FunctionT::operator())>::return_type;
+  using return_type = typename function_traits<decltype( &FunctionT::operator())>::return_type;
 };
 
 // Free functions
@@ -81,7 +81,7 @@ struct function_traits<ReturnTypeT (*)(Args ...)>: function_traits<ReturnTypeT(A
 // std::bind for object methods
 template<typename ClassT, typename ReturnTypeT, typename ... Args, typename ... FArgs>
 #if defined _LIBCPP_VERSION  // libc++ (Clang)
-struct function_traits<std::__1::__bind<ReturnTypeT (ClassT::*)(Args ...), FArgs ...>>
+struct function_traits<std::__bind<ReturnTypeT (ClassT::*)(Args ...), FArgs ...>>
 #elif defined _GLIBCXX_RELEASE  // glibc++ (GNU C++ >= 7.1)
 struct function_traits<std::_Bind<ReturnTypeT(ClassT::*(FArgs ...))(Args ...)>>
 #elif defined __GLIBCXX__  // glibc++ (GNU C++)
@@ -99,7 +99,7 @@ struct function_traits<
 // std::bind for free functions
 template<typename ReturnTypeT, typename ... Args, typename ... FArgs>
 #if defined _LIBCPP_VERSION  // libc++ (Clang)
-struct function_traits<std::__1::__bind<ReturnTypeT( &)(Args ...), FArgs ...>>
+struct function_traits<std::__bind<ReturnTypeT( &)(Args ...), FArgs ...>>
 #elif defined __GLIBCXX__  // glibc++ (GNU C++)
 struct function_traits<std::_Bind<ReturnTypeT(*(FArgs ...))(Args ...)>>
 #elif defined _MSC_VER  // MS Visual Studio
@@ -130,20 +130,20 @@ struct function_traits<FunctionT &&>: function_traits<FunctionT>
  */
 template<std::size_t Arity, typename FunctorT>
 struct arity_comparator : std::integral_constant<
-    bool, (Arity == function_traits<FunctorT>::arity)>{};
+    bool, (Arity == function_traits<FunctorT>::arity)> {};
 
 template<typename FunctorT, typename ... Args>
 struct check_arguments : std::is_same<
     typename function_traits<FunctorT>::arguments,
     std::tuple<Args ...>
-  >
+>
 {};
 
 template<typename FunctorAT, typename FunctorBT>
 struct same_arguments : std::is_same<
     typename function_traits<FunctorAT>::arguments,
     typename function_traits<FunctorBT>::arguments
-  >
+>
 {};
 
 }  // namespace function_traits

rclcpp/include/rclcpp/graph_listener.hpp

@@ -23,6 +23,7 @@
 
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
+#include "rclcpp/context.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -62,7 +63,7 @@ class GraphListener : public std::enable_shared_from_this<GraphListener>
 {
 public:
   RCLCPP_PUBLIC
-  GraphListener();
+  explicit GraphListener(std::shared_ptr<rclcpp::Context> parent_context);
 
   RCLCPP_PUBLIC
   virtual ~GraphListener();
@@ -133,6 +134,12 @@ public:
   void
   shutdown();
 
+  /// Nothrow version of shutdown(), logs to RCLCPP_ERROR instead.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  shutdown(const std::nothrow_t &) noexcept;
+
   /// Return true if shutdown() has been called, else false.
   RCLCPP_PUBLIC
   virtual
@@ -154,6 +161,12 @@ protected:
 private:
   RCLCPP_DISABLE_COPY(GraphListener)
 
+  /** \internal */
+  void
+  __shutdown(bool);
+
+  rclcpp::Context::SharedPtr parent_context_;
+
   std::thread listener_thread_;
   bool is_started_;
   std::atomic_bool is_shutdown_;
@@ -164,6 +177,7 @@ private:
   std::vector<rclcpp::node_interfaces::NodeGraphInterface *> node_graph_interfaces_;
 
   rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
+  std::shared_ptr<rcl_context_t> interrupt_guard_condition_context_;
   rcl_guard_condition_t * shutdown_guard_condition_;
   rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
 };

rclcpp/include/rclcpp/init_options.hpp

@@ -0,0 +1,69 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__INIT_OPTIONS_HPP_
+#define RCLCPP__INIT_OPTIONS_HPP_
+
+#include <memory>
+
+#include "rcl/init_options.h"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Encapsulation of options for initializing rclcpp.
+class InitOptions
+{
+public:
+  /// If true, the context will be shutdown on SIGINT by the signal handler (if it was installed).
+  bool shutdown_on_sigint = true;
+
+  /// Constructor which allows you to specify the allocator used within the init options.
+  RCLCPP_PUBLIC
+  explicit InitOptions(rcl_allocator_t allocator = rcl_get_default_allocator());
+
+  /// Constructor which is initialized by an existing init_options.
+  RCLCPP_PUBLIC
+  explicit InitOptions(const rcl_init_options_t & init_options);
+
+  /// Copy constructor.
+  RCLCPP_PUBLIC
+  InitOptions(const InitOptions & other);
+
+  /// Assignment operator.
+  RCLCPP_PUBLIC
+  InitOptions &
+  operator=(const InitOptions & other);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~InitOptions();
+
+  /// Return the rcl init options.
+  RCLCPP_PUBLIC
+  const rcl_init_options_t *
+  get_rcl_init_options() const;
+
+protected:
+  void
+  finalize_init_options();
+
+private:
+  std::unique_ptr<rcl_init_options_t> init_options_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__INIT_OPTIONS_HPP_

rclcpp/include/rclcpp/intra_process_manager.hpp

@@ -25,14 +25,15 @@
 #include <memory>
 #include <mutex>
 #include <unordered_map>
+#include <utility>
 #include <set>
 
 #include "rclcpp/allocator/allocator_deleter.hpp"
 #include "rclcpp/intra_process_manager_impl.hpp"
 #include "rclcpp/mapped_ring_buffer.hpp"
 #include "rclcpp/macros.hpp"
-#include "rclcpp/publisher.hpp"
-#include "rclcpp/subscription.hpp"
+#include "rclcpp/publisher_base.hpp"
+#include "rclcpp/subscription_base.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -184,21 +185,11 @@ public:
    * \param buffer_size if 0 (default) a size is calculated based on the QoS.
    * \return an unsigned 64-bit integer which is the publisher's unique id.
    */
-  template<typename MessageT, typename Alloc>
+  RCLCPP_PUBLIC
   uint64_t
   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<MessageT, Alloc>::MessageAlloc
-      >::make_shared(size, publisher->get_allocator());
-    impl_->add_publisher(id, publisher, mrb, size);
-    return id;
-  }
+    rclcpp::PublisherBase::SharedPtr publisher,
+    size_t buffer_size = 0);
 
   /// Unregister a publisher using the publisher's unique id.
   /**
@@ -242,12 +233,11 @@ public:
    * \return the message sequence number.
    */
   template<
-    typename MessageT, typename Alloc = std::allocator<void>,
-    typename Deleter = std::default_delete<MessageT>>
+    typename MessageT, typename Alloc = std::allocator<void>>
   uint64_t
   store_intra_process_message(
     uint64_t intra_process_publisher_id,
-    std::unique_ptr<MessageT, Deleter> & message)
+    std::shared_ptr<const MessageT> message)
   {
     using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
     using TypedMRB = typename mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc>;
@@ -270,6 +260,35 @@ public:
     return message_seq;
   }
 
+  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,
+    std::unique_ptr<MessageT, Deleter> message)
+  {
+    using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
+    using TypedMRB = typename mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc>;
+    uint64_t message_seq = 0;
+    mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = impl_->get_publisher_info_for_id(
+      intra_process_publisher_id, message_seq);
+    typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(buffer);
+    if (!typed_buffer) {
+      throw std::runtime_error("Typecast failed due to incorrect message type");
+    }
+
+    // Insert the message into the ring buffer using the message_seq to identify it.
+    bool did_replace = typed_buffer->push_and_replace(message_seq, std::move(message));
+    // TODO(wjwwood): do something when a message was displaced. log debug?
+    (void)did_replace;  // Avoid unused variable warning.
+
+    impl_->store_intra_process_message(intra_process_publisher_id, message_seq);
+
+    // Return the message sequence which is sent to the subscription.
+    return message_seq;
+  }
+
   /// Take an intra process message.
   /**
    * The intra_process_publisher_id and message_sequence_number parameters
@@ -326,7 +345,7 @@ public:
       message_sequence_number,
       requesting_subscriptions_intra_process_id,
       target_subs_size
-      );
+    );
     typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(buffer);
     if (!typed_buffer) {
       return;
@@ -334,10 +353,45 @@ public:
     // Return a copy or the unique_ptr (ownership) depending on how many subscriptions are left.
     if (target_subs_size) {
       // There are more subscriptions to serve, return a copy.
-      typed_buffer->get_copy_at_key(message_sequence_number, message);
+      typed_buffer->get(message_sequence_number, message);
     } else {
       // This is the last one to be returned, transfer ownership.
-      typed_buffer->pop_at_key(message_sequence_number, message);
+      typed_buffer->pop(message_sequence_number, message);
+    }
+  }
+
+  template<
+    typename MessageT, typename Alloc = std::allocator<void>>
+  void
+  take_intra_process_message(
+    uint64_t intra_process_publisher_id,
+    uint64_t message_sequence_number,
+    uint64_t requesting_subscriptions_intra_process_id,
+    std::shared_ptr<const MessageT> & message)
+  {
+    using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
+    using TypedMRB = mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc>;
+    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,
+      requesting_subscriptions_intra_process_id,
+      target_subs_size
+    );
+    typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(buffer);
+    if (!typed_buffer) {
+      return;
+    }
+    // Return a copy or the unique_ptr (ownership) depending on how many subscriptions are left.
+    if (target_subs_size) {
+      // There are more subscriptions to serve, return a copy.
+      typed_buffer->get(message_sequence_number, message);
+    } else {
+      // This is the last one to be returned, transfer ownership.
+      typed_buffer->pop(message_sequence_number, message);
     }
   }
 
@@ -346,6 +400,11 @@ public:
   bool
   matches_any_publishers(const rmw_gid_t * id) const;
 
+  /// Return the number of intraprocess subscriptions to a topic, given the publisher id.
+  RCLCPP_PUBLIC
+  size_t
+  get_subscription_count(uint64_t intra_process_publisher_id) const;
+
 private:
   RCLCPP_PUBLIC
   static uint64_t

rclcpp/include/rclcpp/intra_process_manager_impl.hpp

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

rclcpp/include/rclcpp/intra_process_setting.hpp

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

rclcpp/include/rclcpp/logger.hpp

@@ -20,6 +20,8 @@
 
 #include "rclcpp/visibility_control.hpp"
 
+#include "rcl/node.h"
+
 /**
  * \def RCLCPP_LOGGING_ENABLED
  * When this define evaluates to true (default), logger factory functions will
@@ -60,6 +62,18 @@ RCLCPP_PUBLIC
 Logger
 get_logger(const std::string & name);
 
+/// Return a named logger using an rcl_node_t.
+/**
+ * This is a convenience function that does error checking and returns the node
+ * logger name, or "rclcpp" if it is unable to get the node name.
+ *
+ * \param[in] node the rcl node from which to get the logger name
+ * \return a logger based on the node name, or "rclcpp" if there's an error
+ */
+RCLCPP_PUBLIC
+Logger
+get_node_logger(const rcl_node_t * node);
+
 class Logger
 {
 private:

rclcpp/include/rclcpp/mapped_ring_buffer.hpp

@@ -20,6 +20,7 @@
 #include <cstdint>
 #include <memory>
 #include <mutex>
+#include <stdexcept>
 #include <utility>
 #include <vector>
 
@@ -38,7 +39,7 @@ public:
   RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBufferBase)
 };
 
-/// Ring buffer container of unique_ptr's of T, which can be accessed by a key.
+/// Ring buffer container of shared_ptr's or unique_ptr's of T, which can be accessed by a key.
 /**
  * T must be a CopyConstructable and CopyAssignable.
  * This class can be used in a container by using the base class MappedRingBufferBase.
@@ -64,6 +65,7 @@ public:
   using ElemAlloc = typename ElemAllocTraits::allocator_type;
   using ElemDeleter = allocator::Deleter<ElemAlloc, T>;
 
+  using ConstElemSharedPtr = std::shared_ptr<const T>;
   using ElemUniquePtr = std::unique_ptr<T, ElemDeleter>;
 
   /// Constructor.
@@ -101,57 +103,33 @@ public:
    * \param value if the key is found, the value is stored in this parameter
    */
   void
-  get_copy_at_key(uint64_t key, ElemUniquePtr & value)
+  get(uint64_t key, ElemUniquePtr & value)
   {
     std::lock_guard<std::mutex> lock(data_mutex_);
     auto it = get_iterator_of_key(key);
     value = nullptr;
     if (it != elements_.end() && it->in_use) {
-      auto ptr = ElemAllocTraits::allocate(*allocator_.get(), 1);
-      ElemAllocTraits::construct(*allocator_.get(), ptr, *it->value);
-      value = ElemUniquePtr(ptr);
+      if (it->unique_value) {
+        ElemDeleter deleter = it->unique_value.get_deleter();
+        auto ptr = ElemAllocTraits::allocate(*allocator_.get(), 1);
+        ElemAllocTraits::construct(*allocator_.get(), ptr, *it->unique_value);
+        value = ElemUniquePtr(ptr, deleter);
+      } else if (it->shared_value) {
+        ElemDeleter * deleter = std::get_deleter<ElemDeleter, const T>(it->shared_value);
+        auto ptr = ElemAllocTraits::allocate(*allocator_.get(), 1);
+        ElemAllocTraits::construct(*allocator_.get(), ptr, *it->shared_value);
+        if (deleter) {
+          value = ElemUniquePtr(ptr, *deleter);
+        } else {
+          value = ElemUniquePtr(ptr);
+        }
+      } else {
+        throw std::runtime_error("Unexpected empty MappedRingBuffer element.");
+      }
     }
   }
 
-  /// 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.
-   * This method will allocate in order to store a copy.
-   *
-   * The key is not guaranteed to be unique, see the class docs for more.
-   *
-   * The ownership of the currently stored object is returned, but a copy is
-   * made and stored in its place.
-   * This means that multiple calls to this function for a particular element
-   * will result in returning the copied and stored object not the original.
-   * This also means that later calls to pop_at_key will not return the
-   * originally stored object, since it was returned by the first call to this
-   * method.
-   *
-   * The contents of value before the method is called are discarded.
-   *
-   * \param key the key associated with the stored value
-   * \param value if the key is found, the value is stored in this parameter
-   */
-  void
-  get_ownership_at_key(uint64_t key, ElemUniquePtr & value)
-  {
-    std::lock_guard<std::mutex> lock(data_mutex_);
-    auto it = get_iterator_of_key(key);
-    value = nullptr;
-    if (it != elements_.end() && it->in_use) {
-      // Make a copy.
-      auto ptr = ElemAllocTraits::allocate(*allocator_.get(), 1);
-      ElemAllocTraits::construct(*allocator_.get(), ptr, *it->value);
-      auto copy = ElemUniquePtr(ptr);
-      // Return the original.
-      value.swap(it->value);
-      // Store the copy.
-      it->value.swap(copy);
-    }
-  }
-
-  /// Return ownership of the value stored in the ring buffer at the given key.
+  /// Share 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.
    *
@@ -163,13 +141,90 @@ public:
    * \param value if the key is found, the value is stored in this parameter
    */
   void
-  pop_at_key(uint64_t key, ElemUniquePtr & value)
+  get(uint64_t key, ConstElemSharedPtr & value)
+  {
+    std::lock_guard<std::mutex> lock(data_mutex_);
+    auto it = get_iterator_of_key(key);
+    value.reset();
+    if (it != elements_.end() && it->in_use) {
+      if (!it->shared_value) {
+        // The stored unique_ptr is upgraded to a shared_ptr here.
+        // All the remaining get and pop calls done with unique_ptr
+        // signature will receive a copy.
+        if (!it->unique_value) {
+          throw std::runtime_error("Unexpected empty MappedRingBuffer element.");
+        }
+        it->shared_value = std::move(it->unique_value);
+      }
+      value = it->shared_value;
+    }
+  }
+
+  /// Give the ownership of the stored value to the caller if possible, or copy and release.
+  /**
+   * The key is matched if an element in the ring buffer has a matching key.
+   * This method may allocate in order to return a copy.
+   *
+   * If the stored value is a shared_ptr, it is not possible to downgrade it to a unique_ptr.
+   * In that case, a copy is returned and the stored value is released.
+   *
+   * The key is not guaranteed to be unique, see the class docs for more.
+   *
+   * The contents of value before the method is called are discarded.
+   *
+   * \param key the key associated with the stored value
+   * \param value if the key is found, the value is stored in this parameter
+   */
+  void
+  pop(uint64_t key, ElemUniquePtr & value)
   {
     std::lock_guard<std::mutex> lock(data_mutex_);
     auto it = get_iterator_of_key(key);
     value = nullptr;
     if (it != elements_.end() && it->in_use) {
-      value.swap(it->value);
+      if (it->unique_value) {
+        value = std::move(it->unique_value);
+      } else if (it->shared_value) {
+        auto ptr = ElemAllocTraits::allocate(*allocator_.get(), 1);
+        ElemAllocTraits::construct(*allocator_.get(), ptr, *it->shared_value);
+        auto deleter = std::get_deleter<ElemDeleter, const T>(it->shared_value);
+        if (deleter) {
+          value = ElemUniquePtr(ptr, *deleter);
+        } else {
+          value = ElemUniquePtr(ptr);
+        }
+        it->shared_value.reset();
+      } else {
+        throw std::runtime_error("Unexpected empty MappedRingBuffer element.");
+      }
+      it->in_use = false;
+    }
+  }
+
+  /// Give the ownership of the stored value to the caller, at the given 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.
+   *
+   * The contents of value before the method is called are discarded.
+   *
+   * \param key the key associated with the stored value
+   * \param value if the key is found, the value is stored in this parameter
+   */
+  void
+  pop(uint64_t key, ConstElemSharedPtr & value)
+  {
+    std::lock_guard<std::mutex> lock(data_mutex_);
+    auto it = get_iterator_of_key(key);
+    if (it != elements_.end() && it->in_use) {
+      if (it->shared_value) {
+        value = std::move(it->shared_value);
+      } else if (it->unique_value) {
+        value = std::move(it->unique_value);
+      } else {
+        throw std::runtime_error("Unexpected empty MappedRingBuffer element.");
+      }
       it->in_use = false;
     }
   }
@@ -180,29 +235,44 @@ public:
    * It is up to the user to ensure the key is unique.
    * This method should not allocate memory.
    *
-   * After insertion, if a pair was replaced, then value will contain ownership
-   * of that displaced value. Otherwise it will be a nullptr.
+   * After insertion the value will be a nullptr.
+   * If a pair were replaced, its smart pointer is reset.
    *
    * \param key the key associated with the value to be stored
    * \param value the value to store, and optionally the value displaced
    */
   bool
-  push_and_replace(uint64_t key, ElemUniquePtr & value)
+  push_and_replace(uint64_t key, ConstElemSharedPtr value)
   {
     std::lock_guard<std::mutex> lock(data_mutex_);
     bool did_replace = elements_[head_].in_use;
-    elements_[head_].key = key;
-    elements_[head_].value.swap(value);
-    elements_[head_].in_use = true;
+    Element & element = elements_[head_];
+    element.key = key;
+    element.unique_value.reset();
+    element.shared_value.reset();
+    element.shared_value = value;
+    element.in_use = true;
     head_ = (head_ + 1) % elements_.size();
     return did_replace;
   }
 
+  /// Insert a key-value pair, displacing an existing pair if necessary.
+  /**
+   * See `bool push_and_replace(uint64_t key, const ConstElemSharedPtr & value)`.
+   */
   bool
-  push_and_replace(uint64_t key, ElemUniquePtr && value)
+  push_and_replace(uint64_t key, ElemUniquePtr value)
   {
-    ElemUniquePtr temp = std::move(value);
-    return push_and_replace(key, temp);
+    std::lock_guard<std::mutex> lock(data_mutex_);
+    bool did_replace = elements_[head_].in_use;
+    Element & element = elements_[head_];
+    element.key = key;
+    element.unique_value.reset();
+    element.shared_value.reset();
+    element.unique_value = std::move(value);
+    element.in_use = true;
+    head_ = (head_ + 1) % elements_.size();
+    return did_replace;
   }
 
   /// Return true if the key is found in the ring buffer, otherwise false.
@@ -216,27 +286,28 @@ public:
 private:
   RCLCPP_DISABLE_COPY(MappedRingBuffer<T, Alloc>)
 
-  struct element
+  struct Element
   {
     uint64_t key;
-    ElemUniquePtr value;
+    ElemUniquePtr unique_value;
+    ConstElemSharedPtr shared_value;
     bool in_use;
   };
 
-  using VectorAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<element>;
+  using VectorAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<Element>;
 
-  typename std::vector<element, VectorAlloc>::iterator
+  typename std::vector<Element, VectorAlloc>::iterator
   get_iterator_of_key(uint64_t key)
   {
     auto it = std::find_if(
       elements_.begin(), elements_.end(),
-      [key](element & e) -> bool {
+      [key](Element & e) -> bool {
         return e.key == key && e.in_use;
       });
     return it;
   }
 
-  std::vector<element, VectorAlloc> elements_;
+  std::vector<Element, VectorAlloc> elements_;
   size_t head_;
   std::shared_ptr<ElemAlloc> allocator_;
   std::mutex data_mutex_;

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -25,6 +25,7 @@
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
 
 namespace rclcpp
 {
@@ -43,13 +44,17 @@ public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy)
   using WeakNodeVector = std::vector<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>;
 
+  virtual ~MemoryStrategy() = default;
+
   virtual bool collect_entities(const WeakNodeVector & weak_nodes) = 0;
 
   virtual size_t number_of_ready_subscriptions() const = 0;
   virtual size_t number_of_ready_services() const = 0;
   virtual size_t number_of_ready_clients() const = 0;
+  virtual size_t number_of_ready_events() const = 0;
   virtual size_t number_of_ready_timers() const = 0;
   virtual size_t number_of_guard_conditions() const = 0;
+  virtual size_t number_of_waitables() const = 0;
 
   virtual bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) = 0;
   virtual void clear_handles() = 0;
@@ -74,6 +79,11 @@ public:
     rclcpp::executor::AnyExecutable & any_exec,
     const WeakNodeVector & weak_nodes) = 0;
 
+  virtual void
+  get_next_waitable(
+    rclcpp::executor::AnyExecutable & any_exec,
+    const WeakNodeVector & weak_nodes) = 0;
+
   virtual rcl_allocator_t
   get_allocator() = 0;
 
@@ -111,6 +121,11 @@ public:
   get_group_by_client(
     rclcpp::ClientBase::SharedPtr client,
     const WeakNodeVector & weak_nodes);
+
+  static rclcpp::callback_group::CallbackGroup::SharedPtr
+  get_group_by_waitable(
+    rclcpp::Waitable::SharedPtr waitable,
+    const WeakNodeVector & weak_nodes);
 };
 
 }  // namespace memory_strategy

rclcpp/include/rclcpp/message_memory_strategy.hpp

@@ -25,6 +25,8 @@
 #include "rclcpp/macros.hpp"
 #include "rclcpp/visibility_control.hpp"
 
+#include "rcutils/logging_macros.h"
+
 #include "rmw/serialized_message.h"
 
 namespace rclcpp
@@ -69,6 +71,8 @@ public:
     rcutils_allocator_ = allocator::get_rcl_allocator<char, BufferAlloc>(*buffer_allocator_.get());
   }
 
+  virtual ~MessageMemoryStrategy() = default;
+
   /// Default factory method
   static SharedPtr create_default()
   {
@@ -96,7 +100,9 @@ public:
           auto ret = rmw_serialized_message_fini(msg);
           delete msg;
           if (ret != RCL_RET_OK) {
-            rclcpp::exceptions::throw_from_rcl_error(ret, "leaking memory");
+            RCUTILS_LOG_ERROR_NAMED(
+              "rclcpp",
+              "failed to destroy serialized message: %s", rcl_get_error_string().str);
           }
         });
 

rclcpp/include/rclcpp/node_impl.hpp

@@ -36,11 +36,12 @@
 #include "rcl_interfaces/msg/intra_process_message.hpp"
 
 #include "rclcpp/contexts/default_context.hpp"
-#include "rclcpp/intra_process_manager.hpp"
-#include "rclcpp/parameter.hpp"
 #include "rclcpp/create_publisher.hpp"
 #include "rclcpp/create_service.hpp"
 #include "rclcpp/create_subscription.hpp"
+#include "rclcpp/intra_process_manager.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -51,35 +52,82 @@
 namespace rclcpp
 {
 
-template<typename MessageT, typename Alloc, typename PublisherT>
-std::shared_ptr<PublisherT>
-Node::create_publisher(
-  const std::string & topic_name, size_t qos_history_depth,
-  std::shared_ptr<Alloc> allocator)
+RCLCPP_LOCAL
+inline
+std::string
+extend_name_with_sub_namespace(const std::string & name, const std::string & sub_namespace)
 {
-  if (!allocator) {
-    allocator = std::make_shared<Alloc>();
+  std::string name_with_sub_namespace(name);
+  if (sub_namespace != "" && name.front() != '/' && name.front() != '~') {
+    name_with_sub_namespace = sub_namespace + "/" + name;
   }
-  rmw_qos_profile_t qos = rmw_qos_profile_default;
-  qos.depth = qos_history_depth;
-  return this->create_publisher<MessageT, Alloc, PublisherT>(topic_name, qos, allocator);
+  return name_with_sub_namespace;
 }
 
-template<typename MessageT, typename Alloc, typename PublisherT>
+template<typename MessageT, typename AllocatorT, typename PublisherT>
 std::shared_ptr<PublisherT>
 Node::create_publisher(
-  const std::string & topic_name, const rmw_qos_profile_t & qos_profile,
-  std::shared_ptr<Alloc> allocator)
+  const std::string & topic_name,
+  const rclcpp::QoS & qos,
+  const PublisherOptionsWithAllocator<AllocatorT> & options)
 {
-  if (!allocator) {
-    allocator = std::make_shared<Alloc>();
-  }
-  return rclcpp::create_publisher<MessageT, Alloc, PublisherT>(
-    this->node_topics_.get(),
-    topic_name,
-    qos_profile,
-    use_intra_process_comms_,
-    allocator);
+  return rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
+    *this,
+    extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()),
+    qos,
+    options);
+}
+
+template<typename MessageT, typename AllocatorT, typename PublisherT>
+std::shared_ptr<PublisherT>
+Node::create_publisher(
+  const std::string & topic_name,
+  size_t qos_history_depth,
+  std::shared_ptr<AllocatorT> allocator)
+{
+  PublisherOptionsWithAllocator<AllocatorT> pub_options;
+  pub_options.allocator = allocator;
+  return this->create_publisher<MessageT, AllocatorT, PublisherT>(
+    topic_name, rclcpp::QoS(rclcpp::KeepLast(qos_history_depth)), pub_options);
+}
+
+template<typename MessageT, typename AllocatorT, typename PublisherT>
+std::shared_ptr<PublisherT>
+Node::create_publisher(
+  const std::string & topic_name,
+  const rmw_qos_profile_t & qos_profile,
+  std::shared_ptr<AllocatorT> allocator)
+{
+  rclcpp::QoS qos(rclcpp::QoSInitialization::from_rmw(qos_profile));
+  qos.get_rmw_qos_profile() = qos_profile;
+
+  PublisherOptionsWithAllocator<AllocatorT> pub_options;
+  pub_options.allocator = allocator;
+  return this->create_publisher<MessageT, AllocatorT, PublisherT>(topic_name, qos, pub_options);
+}
+
+template<
+  typename MessageT,
+  typename CallbackT,
+  typename AllocatorT,
+  typename SubscriptionT>
+std::shared_ptr<SubscriptionT>
+Node::create_subscription(
+  const std::string & topic_name,
+  const rclcpp::QoS & qos,
+  CallbackT && callback,
+  const SubscriptionOptionsWithAllocator<AllocatorT> & options,
+  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
+    typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, AllocatorT>::SharedPtr
+  msg_mem_strat)
+{
+  return rclcpp::create_subscription<MessageT>(
+    *this,
+    extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()),
+    qos,
+    std::forward<CallbackT>(callback),
+    options,
+    msg_mem_strat);
 }
 
 template<
@@ -99,27 +147,16 @@ Node::create_subscription(
   msg_mem_strat,
   std::shared_ptr<Alloc> allocator)
 {
-  using CallbackMessageT = typename rclcpp::subscription_traits::has_message_type<CallbackT>::type;
+  rclcpp::QoS qos(rclcpp::QoSInitialization::from_rmw(qos_profile));
+  qos.get_rmw_qos_profile() = qos_profile;
 
-  if (!allocator) {
-    allocator = std::make_shared<Alloc>();
-  }
+  SubscriptionOptionsWithAllocator<Alloc> sub_options;
+  sub_options.callback_group = group;
+  sub_options.ignore_local_publications = ignore_local_publications;
+  sub_options.allocator = allocator;
 
-  if (!msg_mem_strat) {
-    using rclcpp::message_memory_strategy::MessageMemoryStrategy;
-    msg_mem_strat = MessageMemoryStrategy<CallbackMessageT, Alloc>::create_default();
-  }
-
-  return rclcpp::create_subscription<MessageT, CallbackT, Alloc, CallbackMessageT, SubscriptionT>(
-    this->node_topics_.get(),
-    topic_name,
-    std::forward<CallbackT>(callback),
-    qos_profile,
-    group,
-    ignore_local_publications,
-    use_intra_process_comms_,
-    msg_mem_strat,
-    allocator);
+  return this->create_subscription<MessageT, CallbackT, Alloc, SubscriptionT>(
+    topic_name, qos, std::forward<CallbackT>(callback), sub_options, msg_mem_strat);
 }
 
 template<
@@ -139,28 +176,30 @@ Node::create_subscription(
   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>(
+  SubscriptionOptionsWithAllocator<Alloc> sub_options;
+  sub_options.callback_group = group;
+  sub_options.ignore_local_publications = ignore_local_publications;
+  sub_options.allocator = allocator;
+
+  return this->create_subscription<MessageT, CallbackT, Alloc, SubscriptionT>(
     topic_name,
+    rclcpp::QoS(rclcpp::KeepLast(qos_history_depth)),
     std::forward<CallbackT>(callback),
-    qos,
-    group,
-    ignore_local_publications,
-    msg_mem_strat,
-    allocator);
+    sub_options,
+    msg_mem_strat);
 }
 
-template<typename DurationT, typename CallbackT>
+template<typename DurationRepT, typename DurationT, typename CallbackT>
 typename rclcpp::WallTimer<CallbackT>::SharedPtr
 Node::create_wall_timer(
-  std::chrono::duration<int64_t, DurationT> period,
+  std::chrono::duration<DurationRepT, DurationT> period,
   CallbackT callback,
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
   auto timer = rclcpp::WallTimer<CallbackT>::make_shared(
     std::chrono::duration_cast<std::chrono::nanoseconds>(period),
-    std::move(callback));
+    std::move(callback),
+    this->node_base_->get_context());
   node_timers_->add_timer(timer, group);
   return timer;
 }
@@ -181,7 +220,7 @@ Node::create_client(
   auto cli = Client<ServiceT>::make_shared(
     node_base_.get(),
     node_graph_,
-    service_name,
+    extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
     options);
 
   auto cli_base_ptr = std::dynamic_pointer_cast<ClientBase>(cli);
@@ -198,15 +237,68 @@ Node::create_service(
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
   return rclcpp::create_service<ServiceT, CallbackT>(
-    node_base_, node_services_,
-    service_name, std::forward<CallbackT>(callback), qos_profile, group);
+    node_base_,
+    node_services_,
+    extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
+    std::forward<CallbackT>(callback),
+    qos_profile,
+    group);
 }
 
-template<typename CallbackT>
-void
-Node::register_param_change_callback(CallbackT && callback)
+template<typename ParameterT>
+auto
+Node::declare_parameter(
+  const std::string & name,
+  const ParameterT & default_value,
+  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor)
 {
-  this->node_parameters_->register_param_change_callback(std::forward<CallbackT>(callback));
+  return this->declare_parameter(
+    name,
+    rclcpp::ParameterValue(default_value),
+    parameter_descriptor
+  ).get<ParameterT>();
+}
+
+template<typename ParameterT>
+std::vector<ParameterT>
+Node::declare_parameters(
+  const std::string & namespace_,
+  const std::map<std::string, ParameterT> & parameters)
+{
+  std::vector<ParameterT> result;
+  std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + ".");
+  std::transform(
+    parameters.begin(), parameters.end(), std::back_inserter(result),
+    [this, &normalized_namespace](auto element) {
+      return this->declare_parameter(normalized_namespace + element.first, element.second);
+    }
+  );
+  return result;
+}
+
+template<typename ParameterT>
+std::vector<ParameterT>
+Node::declare_parameters(
+  const std::string & namespace_,
+  const std::map<
+    std::string,
+    std::pair<ParameterT, rcl_interfaces::msg::ParameterDescriptor>
+  > & parameters)
+{
+  std::vector<ParameterT> result;
+  std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + ".");
+  std::transform(
+    parameters.begin(), parameters.end(), std::back_inserter(result),
+    [this, &normalized_namespace](auto element) {
+      return static_cast<ParameterT>(
+        this->declare_parameter(
+          normalized_namespace + element.first,
+          element.second.first,
+          element.second.second)
+      );
+    }
+  );
+  return result;
 }
 
 template<typename ParameterT>
@@ -215,22 +307,49 @@ Node::set_parameter_if_not_set(
   const std::string & name,
   const ParameterT & value)
 {
-  rclcpp::Parameter parameter;
-  if (!this->get_parameter(name, parameter)) {
-    this->set_parameters({
-        rclcpp::Parameter(name, value),
-      });
+  if (
+    !this->has_parameter(name) ||
+    this->describe_parameter(name).type == PARAMETER_NOT_SET)
+  {
+    this->set_parameter(rclcpp::Parameter(name, value));
   }
 }
 
+// this is a partially-specialized version of set_parameter_if_not_set above,
+// where our concrete type for ParameterT is std::map, but the to-be-determined
+// type is the value in the map.
+template<typename ParameterT>
+void
+Node::set_parameters_if_not_set(
+  const std::string & name,
+  const std::map<std::string, ParameterT> & values)
+{
+  std::vector<rclcpp::Parameter> params;
+
+  for (const auto & val : values) {
+    std::string parameter_name = name + "." + val.first;
+    if (
+      !this->has_parameter(parameter_name) ||
+      this->describe_parameter(parameter_name).type == PARAMETER_NOT_SET)
+    {
+      params.push_back(rclcpp::Parameter(parameter_name, val.second));
+    }
+  }
+
+  this->set_parameters(params);
+}
+
 template<typename ParameterT>
 bool
-Node::get_parameter(const std::string & name, ParameterT & value) const
+Node::get_parameter(const std::string & name, ParameterT & parameter) const
 {
-  rclcpp::Parameter parameter;
-  bool result = get_parameter(name, parameter);
+  std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace());
+
+  rclcpp::Parameter parameter_variant;
+
+  bool result = get_parameter(sub_name, parameter_variant);
   if (result) {
-    value = parameter.get_value<ParameterT>();
+    parameter = static_cast<ParameterT>(parameter_variant.get_value<ParameterT>());
   }
 
   return result;
@@ -240,16 +359,63 @@ template<typename ParameterT>
 bool
 Node::get_parameter_or(
   const std::string & name,
-  ParameterT & value,
+  ParameterT & parameter,
   const ParameterT & alternative_value) const
 {
-  bool got_parameter = get_parameter(name, value);
+  std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace());
+
+  bool got_parameter = get_parameter(sub_name, parameter);
   if (!got_parameter) {
-    value = alternative_value;
+    parameter = alternative_value;
   }
   return got_parameter;
 }
 
+// this is a partially-specialized version of get_parameter above,
+// where our concrete type for ParameterT is std::map, but the to-be-determined
+// type is the value in the map.
+template<typename ParameterT>
+bool
+Node::get_parameters(
+  const std::string & prefix,
+  std::map<std::string, ParameterT> & values) const
+{
+  std::map<std::string, rclcpp::Parameter> params;
+  bool result = node_parameters_->get_parameters_by_prefix(prefix, params);
+  if (result) {
+    for (const auto & param : params) {
+      values[param.first] = static_cast<ParameterT>(param.second.get_value<ParameterT>());
+    }
+  }
+
+  return result;
+}
+
+template<typename ParameterT>
+void
+Node::get_parameter_or_set(
+  const std::string & name,
+  ParameterT & value,
+  const ParameterT & alternative_value)
+{
+  std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace());
+
+  bool got_parameter = get_parameter(sub_name, value);
+  if (!got_parameter) {
+    this->set_parameters({
+        rclcpp::Parameter(sub_name, alternative_value),
+      });
+    value = alternative_value;
+  }
+}
+
+template<typename CallbackT>
+void
+Node::register_param_change_callback(CallbackT && callback)
+{
+  this->node_parameters_->register_param_change_callback(std::forward<CallbackT>(callback));
+}
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__NODE_IMPL_HPP_

rclcpp/include/rclcpp/node_interfaces/get_node_topics_interface.hpp

@@ -0,0 +1,147 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__GET_NODE_TOPICS_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__GET_NODE_TOPICS_INTERFACE_HPP_
+
+#include <memory>
+#include <utility>
+#include <type_traits>
+
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+
+/// This header provides the get_node_topics_interface() template function.
+/**
+ * This function is useful for getting the NodeTopicsInterface pointer from
+ * various kinds of Node-like classes.
+ *
+ * It's able to get the NodeTopicsInterface pointer so long as the class
+ * has a method called ``get_node_topics_interface()`` which returns
+ * either a pointer (const or not) to a NodeTopicsInterface or a
+ * std::shared_ptr to a NodeTopicsInterface.
+ */
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+namespace detail
+{
+
+// This is a meta-programming checker for if a given Node-like object has a
+// getter called get_node_topics_interface() which returns various types,
+// e.g. const pointer or a shared pointer.
+template<typename NodeType, typename ReturnType>
+struct has_get_node_topics_interface
+{
+private:
+  template<typename T>
+  static constexpr
+  auto
+  check(T *)->typename std::is_same<
+    decltype(std::declval<T>().get_node_topics_interface()),
+    ReturnType
+  >::type;
+
+  template<typename>
+  static constexpr
+  std::false_type
+  check(...);
+
+public:
+  using type = decltype(check<NodeType>(nullptr));
+  static constexpr bool value = type::value;
+};
+
+// If NodeType is a pointer to NodeTopicsInterface already (just normal function overload).
+inline
+rclcpp::node_interfaces::NodeTopicsInterface *
+get_node_topics_interface_from_pointer(rclcpp::node_interfaces::NodeTopicsInterface * pointer)
+{
+  return pointer;
+}
+
+// If NodeType has a method called get_node_topics_interface() which returns a shared pointer.
+template<
+  typename NodeType,
+  typename std::enable_if<has_get_node_topics_interface<
+    typename std::remove_pointer<NodeType>::type,
+    std::shared_ptr<rclcpp::node_interfaces::NodeTopicsInterface>
+  >::value, int>::type = 0
+>
+rclcpp::node_interfaces::NodeTopicsInterface *
+get_node_topics_interface_from_pointer(NodeType node_pointer)
+{
+  return node_pointer->get_node_topics_interface().get();
+}
+
+// If NodeType has a method called get_node_topics_interface() which returns a pointer.
+template<
+  typename NodeType,
+  typename std::enable_if<has_get_node_topics_interface<
+    typename std::remove_pointer<NodeType>::type,
+    rclcpp::node_interfaces::NodeTopicsInterface *
+  >::value, int>::type = 0
+>
+rclcpp::node_interfaces::NodeTopicsInterface *
+get_node_topics_interface_from_pointer(NodeType node_pointer)
+{
+  return node_pointer->get_node_topics_interface();
+}
+
+// Forward shared_ptr's to const node pointer signatures.
+template<
+  typename NodeType,
+  typename std::enable_if<std::is_same<
+    NodeType,
+    typename std::shared_ptr<typename std::remove_pointer<NodeType>::type::element_type> *
+  >::value, int>::type = 0
+>
+rclcpp::node_interfaces::NodeTopicsInterface *
+get_node_topics_interface_from_pointer(NodeType node_shared_pointer)
+{
+  return get_node_topics_interface_from_pointer(node_shared_pointer->get());
+}
+
+}  // namespace detail
+
+/// Get the NodeTopicsInterface as a pointer from a pointer to a "Node like" object.
+template<
+  typename NodeType,
+  typename std::enable_if<std::is_pointer<NodeType>::value, int>::type = 0
+>
+rclcpp::node_interfaces::NodeTopicsInterface *
+get_node_topics_interface(NodeType && node_pointer)
+{
+  // Forward pointers to detail implmentation directly.
+  return detail::get_node_topics_interface_from_pointer(std::forward<NodeType>(node_pointer));
+}
+
+/// Get the NodeTopicsInterface as a pointer from a "Node like" object.
+template<
+  typename NodeType,
+  typename std::enable_if<!std::is_pointer<NodeType>::value, int>::type = 0
+>
+rclcpp::node_interfaces::NodeTopicsInterface *
+get_node_topics_interface(NodeType && node_reference)
+{
+  // Forward references to detail implmentation as a pointer.
+  return detail::get_node_topics_interface_from_pointer(&node_reference);
+}
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__GET_NODE_TOPICS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -19,9 +19,11 @@
 #include <string>
 #include <vector>
 
+#include "rcl/node.h"
 #include "rclcpp/context.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_options.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -33,15 +35,15 @@ namespace node_interfaces
 class NodeBase : public NodeBaseInterface
 {
 public:
-  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBaseInterface)
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBase)
 
   RCLCPP_PUBLIC
   NodeBase(
     const std::string & node_name,
     const std::string & namespace_,
     rclcpp::Context::SharedPtr context,
-    const std::vector<std::string> & arguments,
-    bool use_global_arguments);
+    const rcl_node_options_t & rcl_node_options,
+    bool use_intra_process_default);
 
   RCLCPP_PUBLIC
   virtual
@@ -57,6 +59,11 @@ public:
   const char *
   get_namespace() const;
 
+  RCLCPP_PUBLIC
+  virtual
+  const char *
+  get_fully_qualified_name() const;
+
   RCLCPP_PUBLIC
   virtual
   rclcpp::Context::SharedPtr
@@ -82,6 +89,11 @@ public:
   std::shared_ptr<const rcl_node_t>
   get_shared_rcl_node_handle() const;
 
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  assert_liveliness() const;
+
   RCLCPP_PUBLIC
   virtual
   rclcpp::callback_group::CallbackGroup::SharedPtr
@@ -117,10 +129,16 @@ public:
   std::unique_lock<std::recursive_mutex>
   acquire_notify_guard_condition_lock() const;
 
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  get_use_intra_process_default() const;
+
 private:
   RCLCPP_DISABLE_COPY(NodeBase)
 
   rclcpp::Context::SharedPtr context_;
+  bool use_intra_process_default_;
 
   std::shared_ptr<rcl_node_t> node_handle_;
 

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -38,6 +38,10 @@ class NodeBaseInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBaseInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeBaseInterface() = default;
+
   /// Return the name of the node.
   /** \return The name of the node. */
   RCLCPP_PUBLIC
@@ -52,6 +56,13 @@ public:
   const char *
   get_namespace() const = 0;
 
+  /// Return the fully qualified name of the node.
+  /** \return The fully qualified name of the node. */
+  RCLCPP_PUBLIC
+  virtual
+  const char *
+  get_fully_qualified_name() const = 0;
+
   /// Return the context of the node.
   /** \return SharedPtr to the node's context. */
   RCLCPP_PUBLIC
@@ -91,6 +102,12 @@ public:
   std::shared_ptr<const rcl_node_t>
   get_shared_rcl_node_handle() const = 0;
 
+  /// Manually assert that this Node is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE).
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  assert_liveliness() const = 0;
+
   /// Create and return a callback group.
   RCLCPP_PUBLIC
   virtual
@@ -138,6 +155,12 @@ public:
   virtual
   std::unique_lock<std::recursive_mutex>
   acquire_notify_guard_condition_lock() const = 0;
+
+  /// Return the default preference for using intra process communication.
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  get_use_intra_process_default() const = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_clock.hpp

@@ -41,7 +41,8 @@ public:
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
     rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
     rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
-    rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services);
+    rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging);
 
   RCLCPP_PUBLIC
   virtual
@@ -60,9 +61,9 @@ private:
   rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
 
   rclcpp::Clock::SharedPtr ros_clock_;
-  rclcpp::TimeSource time_source_;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp

@@ -31,6 +31,10 @@ class NodeClockInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeClockInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeClockInterface() = default;
+
   /// Get a ROS clock which will be kept up to date by the node.
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

@@ -20,6 +20,7 @@
 #include <memory>
 #include <mutex>
 #include <string>
+#include <utility>
 #include <vector>
 
 #include "rcl/guard_condition.h"
@@ -69,6 +70,11 @@ public:
   std::vector<std::string>
   get_node_names() const;
 
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<std::pair<std::string, std::string>>
+  get_node_names_and_namespaces() const;
+
   RCLCPP_PUBLIC
   virtual
   size_t

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

@@ -18,6 +18,7 @@
 #include <chrono>
 #include <map>
 #include <string>
+#include <utility>
 #include <vector>
 
 #include "rcl/guard_condition.h"
@@ -37,6 +38,10 @@ class NodeGraphInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeGraphInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeGraphInterface() = default;
+
   /// Return a map of existing topic names to list of topic types.
   /**
    * A topic is considered to exist when at least one publisher or subscriber
@@ -66,6 +71,12 @@ public:
   std::vector<std::string>
   get_node_names() const = 0;
 
+  /// Return a vector of existing node names and namespaces (pair of string).
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<std::pair<std::string, std::string>>
+  get_node_names_and_namespaces() const = 0;
+
   /// Return the number of publishers that are advertised on a given topic.
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp

@@ -33,6 +33,10 @@ class NodeLoggingInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeLoggingInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeLoggingInterface() = default;
+
   /// Return the logger of the node.
   /** \return The logger of the node. */
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -40,6 +40,16 @@ namespace rclcpp
 namespace node_interfaces
 {
 
+// Internal struct for holding useful info about parameters
+struct ParameterInfo
+{
+  /// Current value of the parameter.
+  rclcpp::ParameterValue value;
+
+  /// A description of the parameter
+  rcl_interfaces::msg::ParameterDescriptor descriptor;
+};
+
 /// Implementation of the NodeParameters part of the Node API.
 class NodeParameters : public NodeParametersInterface
 {
@@ -49,77 +59,113 @@ public:
   RCLCPP_PUBLIC
   NodeParameters(
     const node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    const node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
     const node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
     const node_interfaces::NodeServicesInterface::SharedPtr node_services,
+    const node_interfaces::NodeClockInterface::SharedPtr node_clock,
     const std::vector<Parameter> & initial_parameters,
-    bool use_intra_process,
-    bool start_parameter_services);
+    bool start_parameter_services,
+    bool start_parameter_event_publisher,
+    const rclcpp::QoS & parameter_event_qos,
+    const rclcpp::PublisherOptionsBase & parameter_event_publisher_options,
+    bool allow_undeclared_parameters,
+    bool automatically_declare_initial_parameters);
 
   RCLCPP_PUBLIC
   virtual
   ~NodeParameters();
 
   RCLCPP_PUBLIC
-  virtual
+  const rclcpp::ParameterValue &
+  declare_parameter(
+    const std::string & name,
+    const rclcpp::ParameterValue & default_value,
+    const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor) override;
+
+  RCLCPP_PUBLIC
+  void
+  undeclare_parameter(const std::string & name) override;
+
+  RCLCPP_PUBLIC
+  bool
+  has_parameter(const std::string & name) const override;
+
+  RCLCPP_PUBLIC
   std::vector<rcl_interfaces::msg::SetParametersResult>
   set_parameters(
-    const std::vector<rclcpp::Parameter> & parameters);
+    const std::vector<rclcpp::Parameter> & parameters) override;
 
   RCLCPP_PUBLIC
-  virtual
   rcl_interfaces::msg::SetParametersResult
   set_parameters_atomically(
-    const std::vector<rclcpp::Parameter> & parameters);
+    const std::vector<rclcpp::Parameter> & parameters) override;
 
   RCLCPP_PUBLIC
-  virtual
   std::vector<rclcpp::Parameter>
-  get_parameters(const std::vector<std::string> & names) const;
+  get_parameters(const std::vector<std::string> & names) const override;
 
   RCLCPP_PUBLIC
-  virtual
   rclcpp::Parameter
-  get_parameter(const std::string & name) const;
+  get_parameter(const std::string & name) const override;
 
   RCLCPP_PUBLIC
-  virtual
   bool
   get_parameter(
     const std::string & name,
-    rclcpp::Parameter & parameter) const;
+    rclcpp::Parameter & parameter) const override;
+
+  RCLCPP_PUBLIC
+  bool
+  get_parameters_by_prefix(
+    const std::string & prefix,
+    std::map<std::string, rclcpp::Parameter> & parameters) const override;
 
   RCLCPP_PUBLIC
-  virtual
   std::vector<rcl_interfaces::msg::ParameterDescriptor>
-  describe_parameters(const std::vector<std::string> & names) const;
+  describe_parameters(const std::vector<std::string> & names) const override;
 
   RCLCPP_PUBLIC
-  virtual
   std::vector<uint8_t>
-  get_parameter_types(const std::vector<std::string> & names) const;
+  get_parameter_types(const std::vector<std::string> & names) const override;
 
   RCLCPP_PUBLIC
-  virtual
   rcl_interfaces::msg::ListParametersResult
-  list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const;
+  list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const override;
 
   RCLCPP_PUBLIC
-  virtual
+  OnParametersSetCallbackType
+  set_on_parameters_set_callback(OnParametersSetCallbackType callback) override;
+
+  [[deprecated("use set_on_parameters_set_callback() instead")]]
+  RCLCPP_PUBLIC
   void
-  register_param_change_callback(ParametersCallbackFunction callback);
+  register_param_change_callback(OnParametersSetCallbackType callback) override;
+
+  RCLCPP_PUBLIC
+  const std::map<std::string, rclcpp::ParameterValue> &
+  get_initial_parameter_values() const override;
 
 private:
   RCLCPP_DISABLE_COPY(NodeParameters)
 
   mutable std::mutex mutex_;
 
-  ParametersCallbackFunction parameters_callback_ = nullptr;
+  OnParametersSetCallbackType on_parameters_set_callback_ = nullptr;
 
-  std::map<std::string, rclcpp::Parameter> parameters_;
+  std::map<std::string, ParameterInfo> parameters_;
+
+  std::map<std::string, rclcpp::ParameterValue> initial_parameter_values_;
+
+  bool allow_undeclared_ = false;
 
   Publisher<rcl_interfaces::msg::ParameterEvent>::SharedPtr events_publisher_;
 
   std::shared_ptr<ParameterService> parameter_service_;
+
+  std::string combined_name_;
+
+  node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
+  node_interfaces::NodeClockInterface::SharedPtr node_clock_;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
 
+#include <map>
 #include <string>
 #include <vector>
 
@@ -39,10 +40,51 @@ public:
 
   RCLCPP_PUBLIC
   virtual
-  std::vector<rcl_interfaces::msg::SetParametersResult>
-  set_parameters(
-    const std::vector<rclcpp::Parameter> & parameters) = 0;
+  ~NodeParametersInterface() = default;
 
+  /// Declare and initialize a parameter.
+  /**
+   * \sa rclcpp::Node::declare_parameter
+   */
+  RCLCPP_PUBLIC
+  virtual
+  const rclcpp::ParameterValue &
+  declare_parameter(
+    const std::string & name,
+    const rclcpp::ParameterValue & default_value,
+    const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor) = 0;
+
+  /// Undeclare a parameter.
+  /**
+   * \sa rclcpp::Node::undeclare_parameter
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  undeclare_parameter(const std::string & name) = 0;
+
+  /// Return true if the parameter has been declared, otherwise false.
+  /**
+   * \sa rclcpp::Node::has_parameter
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  has_parameter(const std::string & name) const = 0;
+
+  /// Set one or more parameters, one at a time.
+  /**
+   * \sa rclcpp::Node::set_parameters
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<rcl_interfaces::msg::SetParametersResult>
+  set_parameters(const std::vector<rclcpp::Parameter> & parameters) = 0;
+
+  /// Set and initialize a parameter, all at once.
+  /**
+   * \sa rclcpp::Node::set_parameters_atomically
+   */
   RCLCPP_PUBLIC
   virtual
   rcl_interfaces::msg::SetParametersResult
@@ -85,6 +127,20 @@ public:
     const std::string & name,
     rclcpp::Parameter & parameter) const = 0;
 
+  /// Get all parameters that have the specified prefix into the parameters map.
+  /*
+   * \param[in] prefix the name of the prefix to look for.
+   * \param[out] parameters a map of parameters that matched the prefix.
+   * \return true if any parameters with the prefix exists on the node, or
+   * \return false otherwise.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  get_parameters_by_prefix(
+    const std::string & prefix,
+    std::map<std::string, rclcpp::Parameter> & parameters) const = 0;
+
   RCLCPP_PUBLIC
   virtual
   std::vector<rcl_interfaces::msg::ParameterDescriptor>
@@ -100,14 +156,34 @@ 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> &)>;
+  using OnParametersSetCallbackType =
+    std::function<
+    rcl_interfaces::msg::SetParametersResult(const std::vector<rclcpp::Parameter> &)
+    >;
 
+  using ParametersCallbackFunction [[deprecated("use OnParametersSetCallbackType instead")]] =
+    OnParametersSetCallbackType;
+
+  /// Register a callback for when parameters are being set, return an existing one.
+  /**
+   * \sa rclcpp::Node::set_on_parameters_set_callback
+   */
+  RCLCPP_PUBLIC
+  virtual
+  OnParametersSetCallbackType
+  set_on_parameters_set_callback(OnParametersSetCallbackType callback) = 0;
+
+  [[deprecated("use set_on_parameters_set_callback() instead")]]
   RCLCPP_PUBLIC
   virtual
   void
-  register_param_change_callback(ParametersCallbackFunction callback) = 0;
+  register_param_change_callback(OnParametersSetCallbackType callback) = 0;
+
+  /// Return the initial parameter values used by the NodeParameters to override default values.
+  RCLCPP_PUBLIC
+  virtual
+  const std::map<std::string, rclcpp::ParameterValue> &
+  get_initial_parameter_values() const = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_topics.hpp

@@ -42,44 +42,44 @@ public:
   explicit NodeTopics(rclcpp::node_interfaces::NodeBaseInterface * node_base);
 
   RCLCPP_PUBLIC
-  virtual
-  ~NodeTopics();
+  ~NodeTopics() override;
 
   RCLCPP_PUBLIC
-  virtual
   rclcpp::PublisherBase::SharedPtr
   create_publisher(
     const std::string & topic_name,
     const rclcpp::PublisherFactory & publisher_factory,
-    rcl_publisher_options_t & publisher_options,
-    bool use_intra_process);
+    const rcl_publisher_options_t & publisher_options,
+    bool use_intra_process) override;
 
   RCLCPP_PUBLIC
-  virtual
   void
   add_publisher(
-    rclcpp::PublisherBase::SharedPtr publisher);
+    rclcpp::PublisherBase::SharedPtr publisher,
+    rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) override;
 
   RCLCPP_PUBLIC
-  virtual
   rclcpp::SubscriptionBase::SharedPtr
   create_subscription(
     const std::string & topic_name,
     const rclcpp::SubscriptionFactory & subscription_factory,
-    rcl_subscription_options_t & subscription_options,
-    bool use_intra_process);
+    const rcl_subscription_options_t & subscription_options,
+    bool use_intra_process) override;
 
   RCLCPP_PUBLIC
-  virtual
   void
   add_subscription(
     rclcpp::SubscriptionBase::SharedPtr subscription,
-    rclcpp::callback_group::CallbackGroup::SharedPtr callback_group);
+    rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) override;
+
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeBaseInterface *
+  get_node_base_interface() const override;
 
 private:
   RCLCPP_DISABLE_COPY(NodeTopics)
 
-  NodeBaseInterface * node_base_;
+  rclcpp::node_interfaces::NodeBaseInterface * node_base_;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp

@@ -40,20 +40,25 @@ class NodeTopicsInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTopicsInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTopicsInterface() = default;
+
   RCLCPP_PUBLIC
   virtual
   rclcpp::PublisherBase::SharedPtr
   create_publisher(
     const std::string & topic_name,
     const rclcpp::PublisherFactory & publisher_factory,
-    rcl_publisher_options_t & publisher_options,
+    const rcl_publisher_options_t & publisher_options,
     bool use_intra_process) = 0;
 
   RCLCPP_PUBLIC
   virtual
   void
   add_publisher(
-    rclcpp::PublisherBase::SharedPtr publisher) = 0;
+    rclcpp::PublisherBase::SharedPtr publisher,
+    rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0;
 
   RCLCPP_PUBLIC
   virtual
@@ -61,7 +66,7 @@ public:
   create_subscription(
     const std::string & topic_name,
     const rclcpp::SubscriptionFactory & subscription_factory,
-    rcl_subscription_options_t & subscription_options,
+    const rcl_subscription_options_t & subscription_options,
     bool use_intra_process) = 0;
 
   RCLCPP_PUBLIC
@@ -70,6 +75,11 @@ public:
   add_subscription(
     rclcpp::SubscriptionBase::SharedPtr subscription,
     rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::node_interfaces::NodeBaseInterface *
+  get_node_base_interface() const = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp

@@ -0,0 +1,67 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_WAITABLES_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_WAITABLES_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/client.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_waitables_interface.hpp"
+#include "rclcpp/waitable.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeWaitables part of the Node API.
+class NodeWaitables : public NodeWaitablesInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeWaitables)
+
+  RCLCPP_PUBLIC
+  explicit NodeWaitables(rclcpp::node_interfaces::NodeBaseInterface * node_base);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeWaitables();
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_waitable(
+    rclcpp::Waitable::SharedPtr waitable_base_ptr,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group);
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  remove_waitable(
+    rclcpp::Waitable::SharedPtr waitable_ptr,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group) noexcept;
+
+private:
+  RCLCPP_DISABLE_COPY(NodeWaitables)
+
+  rclcpp::node_interfaces::NodeBaseInterface * node_base_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_HPP_

rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp

@@ -0,0 +1,57 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeWaitables part of the Node API.
+class NodeWaitablesInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeWaitablesInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeWaitablesInterface() = default;
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_waitable(
+    rclcpp::Waitable::SharedPtr waitable_ptr,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0;
+
+  /// \note this function should not throw because it may be called in destructors
+  RCLCPP_PUBLIC
+  virtual
+  void
+  remove_waitable(
+    rclcpp::Waitable::SharedPtr waitable_ptr,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group) noexcept = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_

rclcpp/include/rclcpp/node_options.hpp

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

rclcpp/include/rclcpp/parameter.hpp

@@ -28,21 +28,59 @@
 namespace rclcpp
 {
 
+class Parameter;
+
+namespace node_interfaces
+{
+struct ParameterInfo;
+}  // namespace node_interfaces
+
+namespace detail
+{
+
+// This helper function is required because you cannot do specialization on a
+// class method, so instead we specialize this template function and call it
+// from the unspecialized, but dependent, class method.
+template<typename T>
+auto
+get_value_helper(const rclcpp::Parameter * parameter);
+
+}  // namespace detail
+
 /// Structure to store an arbitrary parameter with templated get/set methods.
 class Parameter
 {
 public:
+  /// Construct with an empty name and a parameter value of type rclcpp::PARAMETER_NOT_SET.
   RCLCPP_PUBLIC
   Parameter();
 
+  /// Construct with given name and a parameter value of type rclcpp::PARAMETER_NOT_SET.
+  RCLCPP_PUBLIC
+  explicit Parameter(const std::string & name);
+
+  /// Construct with given name and given parameter value.
   RCLCPP_PUBLIC
   Parameter(const std::string & name, const ParameterValue & value);
 
+  /// Construct with given name and given parameter value.
   template<typename ValueTypeT>
-  explicit Parameter(const std::string & name, ValueTypeT value)
+  Parameter(const std::string & name, ValueTypeT value)
   : Parameter(name, ParameterValue(value))
-  {
-  }
+  {}
+
+  RCLCPP_PUBLIC
+  explicit Parameter(const rclcpp::node_interfaces::ParameterInfo & parameter_info);
+
+  /// Equal operator.
+  RCLCPP_PUBLIC
+  bool
+  operator==(const Parameter & rhs) const;
+
+  /// Not equal operator.
+  RCLCPP_PUBLIC
+  bool
+  operator!=(const Parameter & rhs) const;
 
   RCLCPP_PUBLIC
   ParameterType
@@ -60,6 +98,11 @@ public:
   rcl_interfaces::msg::ParameterValue
   get_value_message() const;
 
+  /// Get the internal storage for the parameter value.
+  RCLCPP_PUBLIC
+  const rclcpp::ParameterValue &
+  get_parameter_value() const;
+
   /// Get value of parameter using rclcpp::ParameterType as template argument.
   template<ParameterType ParamT>
   decltype(auto)
@@ -71,10 +114,7 @@ public:
   /// Get value of parameter using c++ types as template argument.
   template<typename T>
   decltype(auto)
-  get_value() const
-  {
-    return value_.get<T>();
-  }
+  get_value() const;
 
   RCLCPP_PUBLIC
   bool
@@ -142,6 +182,49 @@ RCLCPP_PUBLIC
 std::ostream &
 operator<<(std::ostream & os, const std::vector<Parameter> & parameters);
 
+namespace detail
+{
+
+template<typename T>
+auto
+get_value_helper(const rclcpp::Parameter * parameter)
+{
+  return parameter->get_parameter_value().get<T>();
+}
+
+// Specialization allowing Parameter::get() to return a const ref to the parameter value object.
+template<>
+inline
+auto
+get_value_helper<rclcpp::ParameterValue>(const rclcpp::Parameter * parameter)
+{
+  return parameter->get_parameter_value();
+}
+
+// Specialization allowing Parameter::get() to return a const ref to the parameter itself.
+template<>
+inline
+auto
+get_value_helper<rclcpp::Parameter>(const rclcpp::Parameter * parameter)
+{
+  // Use this lambda to ensure it's a const reference being returned (and not a copy).
+  auto type_enforcing_lambda =
+    [&parameter]() -> const rclcpp::Parameter & {
+      return *parameter;
+    };
+  return type_enforcing_lambda();
+}
+
+}  // namespace detail
+
+template<typename T>
+decltype(auto)
+Parameter::get_value() const
+{
+  // use the helper to specialize for the ParameterValue and Parameter cases.
+  return detail::get_value_helper<T>(this);
+}
+
 }  // namespace rclcpp
 
 namespace std

rclcpp/include/rclcpp/parameter_client.hpp

@@ -110,38 +110,33 @@ public:
 
   template<
     typename CallbackT,
-    typename Alloc = std::allocator<void>,
-    typename SubscriptionT =
-    rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent, Alloc>>
+    typename AllocatorT = std::allocator<void>>
   typename rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
-  on_parameter_event(CallbackT && callback)
+  on_parameter_event(
+    CallbackT && callback,
+    const rclcpp::QoS & qos = (
+      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default))
+    ),
+    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
+      rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
+  ))
   {
-    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(),
+    return rclcpp::create_subscription<rcl_interfaces::msg::ParameterEvent>(
+      this->node_topics_interface_,
       "parameter_events",
+      qos,
       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>());
+      options);
   }
 
   RCLCPP_PUBLIC
   bool
   service_is_ready() const;
 
-  template<typename RatioT = std::milli>
+  template<typename RepT = int64_t, typename RatioT = std::milli>
   bool
   wait_for_service(
-    std::chrono::duration<int64_t, RatioT> timeout = std::chrono::duration<int64_t, RatioT>(-1))
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
   {
     return wait_for_service_nanoseconds(
       std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
@@ -154,7 +149,7 @@ protected:
   wait_for_service_nanoseconds(std::chrono::nanoseconds timeout);
 
 private:
-  const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface_;
+  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_;
@@ -185,6 +180,29 @@ public:
     const std::string & remote_node_name = "",
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
+  RCLCPP_PUBLIC
+  explicit SyncParametersClient(
+    rclcpp::Node * node,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+
+  RCLCPP_PUBLIC
+  SyncParametersClient(
+    rclcpp::executor::Executor::SharedPtr executor,
+    rclcpp::Node * node,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+
+  RCLCPP_PUBLIC
+  SyncParametersClient(
+    rclcpp::executor::Executor::SharedPtr executor,
+    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+    const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
+    const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
+    const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+
   RCLCPP_PUBLIC
   std::vector<rclcpp::Parameter>
   get_parameters(const std::vector<std::string> & parameter_names);
@@ -258,17 +276,17 @@ public:
     return async_parameters_client_->service_is_ready();
   }
 
-  template<typename RatioT = std::milli>
+  template<typename RepT = int64_t, typename RatioT = std::milli>
   bool
   wait_for_service(
-    std::chrono::duration<int64_t, RatioT> timeout = std::chrono::duration<int64_t, RatioT>(-1))
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
   {
     return async_parameters_client_->wait_for_service(timeout);
   }
 
 private:
   rclcpp::executor::Executor::SharedPtr executor_;
-  rclcpp::Node::SharedPtr node_;
+  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_;
   AsyncParametersClient::SharedPtr async_parameters_client_;
 };
 

rclcpp/include/rclcpp/parameter_service.hpp

@@ -43,7 +43,7 @@ public:
   explicit ParameterService(
     const std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
     const std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
-    node_interfaces::NodeParametersInterface * node_params,
+    rclcpp::node_interfaces::NodeParametersInterface * node_params,
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
 private:

rclcpp/include/rclcpp/parameter_value.hpp

@@ -28,7 +28,8 @@
 
 namespace rclcpp
 {
-enum ParameterType
+
+enum ParameterType : uint8_t
 {
   PARAMETER_NOT_SET = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET,
   PARAMETER_BOOL = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL,
@@ -45,11 +46,11 @@ enum ParameterType
 /// Return the name of a parameter type
 RCLCPP_PUBLIC
 std::string
-to_string(const ParameterType type);
+to_string(ParameterType type);
 
 RCLCPP_PUBLIC
 std::ostream &
-operator<<(std::ostream & os, const ParameterType type);
+operator<<(std::ostream & os, ParameterType type);
 
 /// Indicate the parameter type does not match the expected type.
 class ParameterTypeException : public std::runtime_error
@@ -129,10 +130,21 @@ public:
   rcl_interfaces::msg::ParameterValue
   to_value_msg() const;
 
+  /// Equal operator.
+  RCLCPP_PUBLIC
+  bool
+  operator==(const ParameterValue & rhs) const;
+
+  /// Not equal operator.
+  RCLCPP_PUBLIC
+  bool
+  operator!=(const ParameterValue & rhs) const;
+
   // The following get() variants require the use of ParameterType
 
   template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_BOOL, bool>::type
+  constexpr
+  typename std::enable_if<type == ParameterType::PARAMETER_BOOL, const bool &>::type
   get() const
   {
     if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) {
@@ -142,7 +154,8 @@ public:
   }
 
   template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_INTEGER, int64_t>::type
+  constexpr
+  typename std::enable_if<type == ParameterType::PARAMETER_INTEGER, const int64_t &>::type
   get() const
   {
     if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) {
@@ -152,7 +165,8 @@ public:
   }
 
   template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_DOUBLE, double>::type
+  constexpr
+  typename std::enable_if<type == ParameterType::PARAMETER_DOUBLE, const double &>::type
   get() const
   {
     if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) {
@@ -162,6 +176,7 @@ public:
   }
 
   template<ParameterType type>
+  constexpr
   typename std::enable_if<type == ParameterType::PARAMETER_STRING, const std::string &>::type
   get() const
   {
@@ -172,6 +187,7 @@ public:
   }
 
   template<ParameterType type>
+  constexpr
   typename std::enable_if<
     type == ParameterType::PARAMETER_BYTE_ARRAY, const std::vector<uint8_t> &>::type
   get() const
@@ -183,6 +199,7 @@ public:
   }
 
   template<ParameterType type>
+  constexpr
   typename std::enable_if<
     type == ParameterType::PARAMETER_BOOL_ARRAY, const std::vector<bool> &>::type
   get() const
@@ -194,6 +211,7 @@ public:
   }
 
   template<ParameterType type>
+  constexpr
   typename std::enable_if<
     type == ParameterType::PARAMETER_INTEGER_ARRAY, const std::vector<int64_t> &>::type
   get() const
@@ -205,6 +223,7 @@ public:
   }
 
   template<ParameterType type>
+  constexpr
   typename std::enable_if<
     type == ParameterType::PARAMETER_DOUBLE_ARRAY, const std::vector<double> &>::type
   get() const
@@ -216,6 +235,7 @@ public:
   }
 
   template<ParameterType type>
+  constexpr
   typename std::enable_if<
     type == ParameterType::PARAMETER_STRING_ARRAY, const std::vector<std::string> &>::type
   get() const
@@ -229,28 +249,32 @@ public:
   // 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
+  constexpr
+  typename std::enable_if<std::is_same<type, bool>::value, const bool &>::type
   get() const
   {
     return get<ParameterType::PARAMETER_BOOL>();
   }
 
   template<typename type>
+  constexpr
   typename std::enable_if<
-    std::is_integral<type>::value && !std::is_same<type, bool>::value, int64_t>::type
+    std::is_integral<type>::value && !std::is_same<type, bool>::value, const 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
+  constexpr
+  typename std::enable_if<std::is_floating_point<type>::value, const double &>::type
   get() const
   {
     return get<ParameterType::PARAMETER_DOUBLE>();
   }
 
   template<typename type>
+  constexpr
   typename std::enable_if<std::is_convertible<type, std::string>::value, const std::string &>::type
   get() const
   {
@@ -258,6 +282,7 @@ public:
   }
 
   template<typename type>
+  constexpr
   typename std::enable_if<
     std::is_convertible<
       type, const std::vector<uint8_t> &>::value, const std::vector<uint8_t> &>::type
@@ -267,6 +292,7 @@ public:
   }
 
   template<typename type>
+  constexpr
   typename std::enable_if<
     std::is_convertible<
       type, const std::vector<bool> &>::value, const std::vector<bool> &>::type
@@ -276,6 +302,7 @@ public:
   }
 
   template<typename type>
+  constexpr
   typename std::enable_if<
     std::is_convertible<
       type, const std::vector<int64_t> &>::value, const std::vector<int64_t> &>::type
@@ -285,6 +312,7 @@ public:
   }
 
   template<typename type>
+  constexpr
   typename std::enable_if<
     std::is_convertible<
       type, const std::vector<double> &>::value, const std::vector<double> &>::type
@@ -294,6 +322,7 @@ public:
   }
 
   template<typename type>
+  constexpr
   typename std::enable_if<
     std::is_convertible<
       type, const std::vector<std::string> &>::value, const std::vector<std::string> &>::type

rclcpp/include/rclcpp/publisher.hpp

@@ -23,6 +23,7 @@
 #include <memory>
 #include <sstream>
 #include <string>
+#include <utility>
 
 #include "rcl/error_handling.h"
 #include "rcl/publisher.h"
@@ -31,125 +32,15 @@
 
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/allocator/allocator_deleter.hpp"
+#include "rclcpp/intra_process_manager.hpp"
 #include "rclcpp/macros.hpp"
-#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/publisher_base.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
 
-// Forward declaration is used for friend statement.
-namespace node_interfaces
-{
-class NodeTopicsInterface;
-}
-
-class PublisherBase
-{
-  friend ::rclcpp::node_interfaces::NodeTopicsInterface;
-
-public:
-  RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase)
-
-  /// Default constructor.
-  /**
-   * Typically, a publisher is not created through this method, but instead is created through a
-   * call to `Node::create_publisher`.
-   * \param[in] node_base A pointer to the NodeBaseInterface for the parent node.
-   * \param[in] topic The topic that this publisher publishes on.
-   * \param[in] type_support The type support structure for the type to be published.
-   * \param[in] publisher_options QoS settings for this publisher.
-   */
-  RCLCPP_PUBLIC
-  PublisherBase(
-    rclcpp::node_interfaces::NodeBaseInterface * node_base,
-    const std::string & topic,
-    const rosidl_message_type_support_t & type_support,
-    const rcl_publisher_options_t & publisher_options);
-
-  RCLCPP_PUBLIC
-  virtual ~PublisherBase();
-
-  /// Get the topic that this publisher publishes on.
-  /** \return The topic name. */
-  RCLCPP_PUBLIC
-  const char *
-  get_topic_name() const;
-
-  /// Get the queue size for this publisher.
-  /** \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. */
-  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. */
-  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.
-   * \param[in] gid Reference to a gid.
-   * \return True if the publisher's gid matches the input.
-   */
-  RCLCPP_PUBLIC
-  bool
-  operator==(const rmw_gid_t & gid) const;
-
-  /// Compare this publisher to a pointer gid.
-  /**
-   * A wrapper for comparing this publisher's gid to the input using rmw_compare_gids_equal.
-   * \param[in] gid A pointer to a gid.
-   * \return True if this publisher's gid matches the input.
-   */
-  RCLCPP_PUBLIC
-  bool
-  operator==(const rmw_gid_t * gid) const;
-
-  using StoreMessageCallbackT = std::function<uint64_t(uint64_t, void *, const std::type_info &)>;
-
-  /// Implementation utility function used to setup intra process publishing after creation.
-  RCLCPP_PUBLIC
-  void
-  setup_intra_process(
-    uint64_t intra_process_publisher_id,
-    StoreMessageCallbackT callback,
-    const rcl_publisher_options_t & intra_process_options);
-
-protected:
-  std::shared_ptr<rcl_node_t> rcl_node_handle_;
-
-  rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher();
-  rcl_publisher_t intra_process_publisher_handle_ = rcl_get_zero_initialized_publisher();
-
-  uint64_t intra_process_publisher_id_;
-  StoreMessageCallbackT store_intra_process_message_;
-
-  rmw_gid_t rmw_gid_;
-  rmw_gid_t intra_process_rmw_gid_;
-};
-
 /// A publisher publishes messages of any type to a topic.
 template<typename MessageT, typename Alloc = std::allocator<void>>
 class Publisher : public PublisherBase
@@ -159,6 +50,7 @@ public:
   using MessageAlloc = typename MessageAllocTraits::allocator_type;
   using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
   using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
+  using MessageSharedPtr = std::shared_ptr<const MessageT>;
 
   RCLCPP_SMART_PTR_DEFINITIONS(Publisher<MessageT, Alloc>)
 
@@ -166,6 +58,7 @@ public:
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
     const std::string & topic,
     const rcl_publisher_options_t & publisher_options,
+    const PublisherEventCallbacks & event_callbacks,
     const std::shared_ptr<MessageAlloc> & allocator)
   : PublisherBase(
       node_base,
@@ -175,98 +68,99 @@ public:
     message_allocator_(allocator)
   {
     allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get());
+
+    if (event_callbacks.deadline_callback) {
+      this->add_event_handler(event_callbacks.deadline_callback,
+        RCL_PUBLISHER_OFFERED_DEADLINE_MISSED);
+    }
+    if (event_callbacks.liveliness_callback) {
+      this->add_event_handler(event_callbacks.liveliness_callback,
+        RCL_PUBLISHER_LIVELINESS_LOST);
+    }
   }
 
   virtual ~Publisher()
   {}
 
+  mapped_ring_buffer::MappedRingBufferBase::SharedPtr
+  make_mapped_ring_buffer(size_t size) const override
+  {
+    return mapped_ring_buffer::MappedRingBuffer<
+      MessageT,
+      typename Publisher<MessageT, Alloc>::MessageAlloc
+    >::make_shared(size, this->get_allocator());
+  }
+
   /// Send a message to the topic for this publisher.
   /**
    * This function is templated on the input message type, MessageT.
    * \param[in] msg A shared pointer to the message to send.
    */
   virtual void
-  publish(std::unique_ptr<MessageT, MessageDeleter> & msg)
+  publish(std::unique_ptr<MessageT, MessageDeleter> msg)
   {
-    this->do_inter_process_publish(msg.get());
-    if (store_intra_process_message_) {
-      // Take the pointer from the unique_msg, release it and pass as a void *
-      // to the ipm. The ipm should then capture it again as a unique_ptr of
-      // the correct type.
-      // TODO(wjwwood):
-      //   investigate how to transfer the custom deleter (if there is one)
-      //   from the incoming unique_ptr through to the ipm's unique_ptr.
-      //   See: http://stackoverflow.com/questions/11002641/dynamic-casting-for-unique-ptr
-      MessageT * msg_ptr = msg.get();
-      msg.release();
-      uint64_t message_seq =
-        store_intra_process_message_(intra_process_publisher_id_, msg_ptr, typeid(MessageT));
-      rcl_interfaces::msg::IntraProcessMessage ipm;
-      ipm.publisher_id = intra_process_publisher_id_;
-      ipm.message_sequence = message_seq;
-      auto status = rcl_publish(&intra_process_publisher_handle_, &ipm);
-      if (RCL_RET_OK != status) {
-        rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish intra process message");
-      }
+    if (!intra_process_is_enabled_) {
+      this->do_inter_process_publish(msg.get());
+      return;
+    }
+    // If an interprocess subscription exist, then the unique_ptr is promoted
+    // to a shared_ptr and published.
+    // This allows doing the intraprocess publish first and then doing the
+    // interprocess publish, resulting in lower publish-to-subscribe latency.
+    // It's not possible to do that with an unique_ptr,
+    // as do_intra_process_publish takes the ownership of the message.
+    uint64_t message_seq;
+    bool inter_process_publish_needed =
+      get_subscription_count() > get_intra_process_subscription_count();
+    MessageSharedPtr shared_msg;
+    if (inter_process_publish_needed) {
+      shared_msg = std::move(msg);
+      message_seq =
+        store_intra_process_message(intra_process_publisher_id_, shared_msg);
     } else {
-      // Always destroy the message, even if we don't consume it, for consistency.
-      msg.reset();
+      message_seq =
+        store_intra_process_message(intra_process_publisher_id_, std::move(msg));
+    }
+    this->do_intra_process_publish(message_seq);
+    if (inter_process_publish_needed) {
+      this->do_inter_process_publish(shared_msg.get());
     }
   }
 
+// Skip deprecated attribute in windows, as it raise a warning in template specialization.
+#if !defined(_WIN32)
+  [[deprecated(
+    "publishing an unique_ptr is prefered when using intra process communication."
+    " If using a shared_ptr, use publish(*msg).")]]
+#endif
   virtual void
-  publish(const std::shared_ptr<MessageT> & msg)
+  publish(const std::shared_ptr<const MessageT> & msg)
   {
-    // Avoid allocating when not using intra process.
-    if (!store_intra_process_message_) {
-      // In this case we're not using intra process.
-      return this->do_inter_process_publish(msg.get());
-    }
-    // Otherwise we have to allocate memory in a unique_ptr and pass it along.
-    // TODO(wjwwood):
-    //   The intra process manager should probably also be able to store
-    //   shared_ptr's and do the "smart" thing based on other intra process
-    //   subscriptions. For now call the other publish().
-    auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
-    MessageAllocTraits::construct(*message_allocator_.get(), ptr, *msg.get());
-    MessageUniquePtr unique_msg(ptr, message_deleter_);
-    return this->publish(unique_msg);
-  }
-
-  virtual void
-  publish(std::shared_ptr<const MessageT> msg)
-  {
-    // Avoid allocating when not using intra process.
-    if (!store_intra_process_message_) {
-      // In this case we're not using intra process.
-      return this->do_inter_process_publish(msg.get());
-    }
-    // Otherwise we have to allocate memory in a unique_ptr and pass it along.
-    // TODO(wjwwood):
-    //   The intra process manager should probably also be able to store
-    //   shared_ptr's and do the "smart" thing based on other intra process
-    //   subscriptions. For now call the other publish().
-    auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
-    MessageAllocTraits::construct(*message_allocator_.get(), ptr, *msg.get());
-    MessageUniquePtr unique_msg(ptr, message_deleter_);
-    return this->publish(unique_msg);
+    publish(*msg);
   }
 
   virtual void
   publish(const MessageT & msg)
   {
     // Avoid allocating when not using intra process.
-    if (!store_intra_process_message_) {
+    if (!intra_process_is_enabled_) {
       // In this case we're not using intra process.
       return this->do_inter_process_publish(&msg);
     }
     // Otherwise we have to allocate memory in a unique_ptr and pass it along.
+    // As the message is not const, a copy should be made.
+    // A shared_ptr<const MessageT> could also be constructed here.
     auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
     MessageAllocTraits::construct(*message_allocator_.get(), ptr, msg);
     MessageUniquePtr unique_msg(ptr, message_deleter_);
-    return this->publish(unique_msg);
+    this->publish(std::move(unique_msg));
   }
 
+// Skip deprecated attribute in windows, as it raise a warning in template specialization.
+#if !defined(_WIN32)
+  [[deprecated(
+    "Use publish(*msg). Check against nullptr before calling if necessary.")]]
+#endif
   virtual void
   publish(const MessageT * msg)
   {
@@ -276,17 +170,32 @@ public:
     return this->publish(*msg);
   }
 
+  void
+  publish(const rcl_serialized_message_t & serialized_msg)
+  {
+    return this->do_serialized_publish(&serialized_msg);
+  }
+
+// Skip deprecated attribute in windows, as it raise a warning in template specialization.
+#if !defined(_WIN32)
+  [[deprecated(
+    "Use publish(*serialized_msg). Check against nullptr before calling if necessary.")]]
+#endif
   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");
-    }
+    return this->do_serialized_publish(serialized_msg);
+  }
+
+// Skip deprecated attribute in windows, as it raise a warning in template specialization.
+#if !defined(_WIN32)
+  [[deprecated(
+    "Use publish(*serialized_msg). Check against nullptr before calling if necessary.")]]
+#endif
+  void
+  publish(std::shared_ptr<const rcl_serialized_message_t> serialized_msg)
+  {
+    return this->do_serialized_publish(serialized_msg.get());
   }
 
   std::shared_ptr<MessageAlloc> get_allocator() const
@@ -298,12 +207,93 @@ protected:
   void
   do_inter_process_publish(const MessageT * msg)
   {
-    auto status = rcl_publish(&publisher_handle_, msg);
+    auto status = rcl_publish(&publisher_handle_, msg, nullptr);
+    if (RCL_RET_PUBLISHER_INVALID == status) {
+      rcl_reset_error();  // next call will reset error message if not context
+      if (rcl_publisher_is_valid_except_context(&publisher_handle_)) {
+        rcl_context_t * context = rcl_publisher_get_context(&publisher_handle_);
+        if (nullptr != context && !rcl_context_is_valid(context)) {
+          // publisher is invalid due to context being shutdown
+          return;
+        }
+      }
+    }
     if (RCL_RET_OK != status) {
       rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish message");
     }
   }
 
+  void
+  do_serialized_publish(const rcl_serialized_message_t * serialized_msg)
+  {
+    if (intra_process_is_enabled_) {
+      // 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, nullptr);
+    if (RCL_RET_OK != status) {
+      rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish serialized message");
+    }
+  }
+
+  void
+  do_intra_process_publish(uint64_t message_seq)
+  {
+    rcl_interfaces::msg::IntraProcessMessage ipm;
+    ipm.publisher_id = intra_process_publisher_id_;
+    ipm.message_sequence = message_seq;
+    auto status = rcl_publish(&intra_process_publisher_handle_, &ipm, nullptr);
+    if (RCL_RET_PUBLISHER_INVALID == status) {
+      rcl_reset_error();  // next call will reset error message if not context
+      if (rcl_publisher_is_valid_except_context(&intra_process_publisher_handle_)) {
+        rcl_context_t * context = rcl_publisher_get_context(&intra_process_publisher_handle_);
+        if (nullptr != context && !rcl_context_is_valid(context)) {
+          // publisher is invalid due to context being shutdown
+          return;
+        }
+      }
+    }
+    if (RCL_RET_OK != status) {
+      rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish intra process message");
+    }
+  }
+
+  uint64_t
+  store_intra_process_message(
+    uint64_t publisher_id,
+    std::shared_ptr<const MessageT> msg)
+  {
+    auto ipm = weak_ipm_.lock();
+    if (!ipm) {
+      throw std::runtime_error(
+              "intra process publish called after destruction of intra process manager");
+    }
+    if (!msg) {
+      throw std::runtime_error("cannot publisher msg which is a null pointer");
+    }
+    uint64_t message_seq =
+      ipm->template store_intra_process_message<MessageT, Alloc>(publisher_id, msg);
+    return message_seq;
+  }
+
+  uint64_t
+  store_intra_process_message(
+    uint64_t publisher_id,
+    std::unique_ptr<MessageT, MessageDeleter> msg)
+  {
+    auto ipm = weak_ipm_.lock();
+    if (!ipm) {
+      throw std::runtime_error(
+              "intra process publish called after destruction of intra process manager");
+    }
+    if (!msg) {
+      throw std::runtime_error("cannot publisher msg which is a null pointer");
+    }
+    uint64_t message_seq =
+      ipm->template store_intra_process_message<MessageT, Alloc>(publisher_id, std::move(msg));
+    return message_seq;
+  }
+
   std::shared_ptr<MessageAlloc> message_allocator_;
 
   MessageDeleter message_deleter_;

rclcpp/include/rclcpp/publisher_base.hpp

@@ -0,0 +1,232 @@
+// Copyright 2014 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__PUBLISHER_BASE_HPP_
+#define RCLCPP__PUBLISHER_BASE_HPP_
+
+#include <rmw/error_handling.h>
+#include <rmw/rmw.h>
+
+#include <functional>
+#include <iostream>
+#include <memory>
+#include <sstream>
+#include <string>
+#include <vector>
+
+#include "rcl/publisher.h"
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/mapped_ring_buffer.hpp"
+#include "rclcpp/qos_event.hpp"
+#include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+// Forward declaration is used for friend statement.
+namespace node_interfaces
+{
+class NodeBaseInterface;
+class NodeTopicsInterface;
+}
+
+namespace intra_process_manager
+{
+/**
+ * IntraProcessManager is forward declared here, avoiding a circular inclusion between
+ * `intra_process_manager.hpp` and `publisher_base.hpp`.
+ */
+class IntraProcessManager;
+}
+
+class PublisherBase
+{
+  friend ::rclcpp::node_interfaces::NodeTopicsInterface;
+
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase)
+
+  /// Default constructor.
+  /**
+   * Typically, a publisher is not created through this method, but instead is created through a
+   * call to `Node::create_publisher`.
+   * \param[in] node_base A pointer to the NodeBaseInterface for the parent node.
+   * \param[in] topic The topic that this publisher publishes on.
+   * \param[in] type_support The type support structure for the type to be published.
+   * \param[in] publisher_options QoS settings for this publisher.
+   */
+  RCLCPP_PUBLIC
+  PublisherBase(
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    const std::string & topic,
+    const rosidl_message_type_support_t & type_support,
+    const rcl_publisher_options_t & publisher_options);
+
+  RCLCPP_PUBLIC
+  virtual ~PublisherBase();
+
+  /// Get the topic that this publisher publishes on.
+  /** \return The topic name. */
+  RCLCPP_PUBLIC
+  const char *
+  get_topic_name() const;
+
+  /// Get the queue size for this publisher.
+  /** \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. */
+  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. */
+  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;
+
+  /// Get all the QoS event handlers associated with this publisher.
+  /** \return The vector of QoS event handlers. */
+  RCLCPP_PUBLIC
+  const std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
+  get_event_handlers() const;
+
+  /// Get subscription count
+  /** \return The number of subscriptions. */
+  RCLCPP_PUBLIC
+  size_t
+  get_subscription_count() const;
+
+  /// Get intraprocess subscription count
+  /** \return The number of intraprocess subscriptions. */
+  RCLCPP_PUBLIC
+  size_t
+  get_intra_process_subscription_count() const;
+
+  /// Manually assert that this Publisher is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC).
+  /**
+   * If the rmw Liveliness policy is set to RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, the creator
+   * of this publisher may manually call `assert_liveliness` at some point in time to signal to the
+   * rest of the system that this Node is still alive.
+   *
+   * \return `true` if the liveliness was asserted successfully, otherwise `false`
+   */
+  RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
+  bool
+  assert_liveliness() const;
+
+  /// Get the actual QoS settings, after the defaults have been determined.
+  /**
+   * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT
+   * can only be resolved after the creation of the publisher, and it
+   * depends on the underlying rmw implementation.
+   * If the underlying setting in use can't be represented in ROS terms,
+   * it will be set to RMW_QOS_POLICY_*_UNKNOWN.
+   * May throw runtime_error when an unexpected error occurs.
+   * \return The actual qos settings.
+   */
+  RCLCPP_PUBLIC
+  rmw_qos_profile_t
+  get_actual_qos() const;
+
+  /// Compare this publisher to a gid.
+  /**
+   * Note that this function calls the next function.
+   * \param[in] gid Reference to a gid.
+   * \return True if the publisher's gid matches the input.
+   */
+  RCLCPP_PUBLIC
+  bool
+  operator==(const rmw_gid_t & gid) const;
+
+  /// Compare this publisher to a pointer gid.
+  /**
+   * A wrapper for comparing this publisher's gid to the input using rmw_compare_gids_equal.
+   * \param[in] gid A pointer to a gid.
+   * \return True if this publisher's gid matches the input.
+   */
+  RCLCPP_PUBLIC
+  bool
+  operator==(const rmw_gid_t * gid) const;
+
+  using IntraProcessManagerSharedPtr =
+    std::shared_ptr<rclcpp::intra_process_manager::IntraProcessManager>;
+
+  /// Implementation utility function that creates a typed mapped ring buffer.
+  RCLCPP_PUBLIC
+  mapped_ring_buffer::MappedRingBufferBase::SharedPtr
+  virtual make_mapped_ring_buffer(size_t size) const;
+
+  /// Implementation utility function used to setup intra process publishing after creation.
+  RCLCPP_PUBLIC
+  void
+  setup_intra_process(
+    uint64_t intra_process_publisher_id,
+    IntraProcessManagerSharedPtr ipm,
+    const rcl_publisher_options_t & intra_process_options);
+
+protected:
+  template<typename EventCallbackT>
+  void
+  add_event_handler(
+    const EventCallbackT & callback,
+    const rcl_publisher_event_type_t event_type)
+  {
+    auto handler = std::make_shared<QOSEventHandler<EventCallbackT>>(
+      callback,
+      rcl_publisher_event_init,
+      &publisher_handle_,
+      event_type);
+    event_handlers_.emplace_back(handler);
+  }
+
+  std::shared_ptr<rcl_node_t> rcl_node_handle_;
+
+  rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher();
+  rcl_publisher_t intra_process_publisher_handle_ = rcl_get_zero_initialized_publisher();
+
+  std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
+
+  using IntraProcessManagerWeakPtr =
+    std::weak_ptr<rclcpp::intra_process_manager::IntraProcessManager>;
+  bool intra_process_is_enabled_;
+  IntraProcessManagerWeakPtr weak_ipm_;
+  uint64_t intra_process_publisher_id_;
+
+  rmw_gid_t rmw_gid_;
+  rmw_gid_t intra_process_rmw_gid_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__PUBLISHER_BASE_HPP_

rclcpp/include/rclcpp/publisher_factory.hpp

@@ -49,94 +49,38 @@ struct PublisherFactory
     rclcpp::PublisherBase::SharedPtr(
       rclcpp::node_interfaces::NodeBaseInterface * node_base,
       const std::string & topic_name,
-      rcl_publisher_options_t & publisher_options)>;
+      const rcl_publisher_options_t & publisher_options)>;
 
   PublisherFactoryFunction create_typed_publisher;
-
-  // Adds the PublisherBase to the intraprocess manager with the correctly
-  // templated call to IntraProcessManager::store_intra_process_message.
-  using AddPublisherToIntraProcessManagerFunction = std::function<
-    uint64_t(
-      rclcpp::intra_process_manager::IntraProcessManager * ipm,
-      rclcpp::PublisherBase::SharedPtr publisher)>;
-
-  AddPublisherToIntraProcessManagerFunction add_publisher_to_intra_process_manager;
-
-  // Creates the callback function which is called on each
-  // PublisherT::publish() and which handles the intra process transmission of
-  // the message being published.
-  using SharedPublishCallbackFactoryFunction = std::function<
-    rclcpp::PublisherBase::StoreMessageCallbackT(
-      rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm)>;
-
-  SharedPublishCallbackFactoryFunction create_shared_publish_callback;
 };
 
 /// Return a PublisherFactory with functions setup for creating a PublisherT<MessageT, Alloc>.
 template<typename MessageT, typename Alloc, typename PublisherT>
 PublisherFactory
-create_publisher_factory(std::shared_ptr<Alloc> allocator)
+create_publisher_factory(
+  const PublisherEventCallbacks & event_callbacks,
+  std::shared_ptr<Alloc> allocator)
 {
   PublisherFactory factory;
 
   // factory function that creates a MessageT specific PublisherT
   factory.create_typed_publisher =
-    [allocator](
+    [event_callbacks, allocator](
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
     const std::string & topic_name,
-    rcl_publisher_options_t & publisher_options) -> std::shared_ptr<PublisherT>
+    const rcl_publisher_options_t & publisher_options
+    ) -> std::shared_ptr<PublisherT>
     {
+      auto options_copy = publisher_options;
       auto message_alloc = std::make_shared<typename PublisherT::MessageAlloc>(*allocator.get());
-      publisher_options.allocator = allocator::get_rcl_allocator<MessageT>(*message_alloc.get());
+      options_copy.allocator = allocator::get_rcl_allocator<MessageT>(*message_alloc.get());
 
-      return std::make_shared<PublisherT>(node_base, topic_name, publisher_options, message_alloc);
-    };
-
-  // function to add a publisher to the intra process manager
-  factory.add_publisher_to_intra_process_manager =
-    [](
-    rclcpp::intra_process_manager::IntraProcessManager * ipm,
-    rclcpp::PublisherBase::SharedPtr publisher) -> uint64_t
-    {
-      return ipm->add_publisher<MessageT, Alloc>(std::dynamic_pointer_cast<PublisherT>(publisher));
-    };
-
-  // function to create a shared publish callback std::function
-  using StoreMessageCallbackT = rclcpp::PublisherBase::StoreMessageCallbackT;
-  factory.create_shared_publish_callback =
-    [](rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm) -> StoreMessageCallbackT
-    {
-      rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = ipm;
-
-      // this function is called on each call to publish() and handles storing
-      // of the published message in the intra process manager
-      auto shared_publish_callback =
-        [weak_ipm](uint64_t publisher_id, void * msg, const std::type_info & type_info) -> uint64_t
-        {
-          auto ipm = weak_ipm.lock();
-          if (!ipm) {
-            // TODO(wjwwood): should this just return silently? Or maybe return with a warning?
-            throw std::runtime_error(
-                    "intra process publish called after destruction of intra process manager");
-          }
-          if (!msg) {
-            throw std::runtime_error("cannot publisher msg which is a null pointer");
-          }
-          auto & message_type_info = typeid(MessageT);
-          if (message_type_info != type_info) {
-            throw std::runtime_error(
-                    std::string("published type '") + type_info.name() +
-                    "' is incompatible from the publisher type '" + message_type_info.name() + "'");
-          }
-          MessageT * typed_message_ptr = static_cast<MessageT *>(msg);
-          using MessageDeleter = typename Publisher<MessageT, Alloc>::MessageDeleter;
-          std::unique_ptr<MessageT, MessageDeleter> unique_msg(typed_message_ptr);
-          uint64_t message_seq =
-            ipm->store_intra_process_message<MessageT, Alloc>(publisher_id, unique_msg);
-          return message_seq;
-        };
-
-      return shared_publish_callback;
+      return std::make_shared<PublisherT>(
+        node_base,
+        topic_name,
+        options_copy,
+        event_callbacks,
+        message_alloc);
     };
 
   // return the factory now that it is populated

rclcpp/include/rclcpp/publisher_options.hpp

@@ -0,0 +1,78 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__PUBLISHER_OPTIONS_HPP_
+#define RCLCPP__PUBLISHER_OPTIONS_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/intra_process_setting.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/qos_event.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rcl/publisher.h"
+
+namespace rclcpp
+{
+
+/// Non-templated part of PublisherOptionsWithAllocator<Allocator>.
+struct PublisherOptionsBase
+{
+  /// Setting to explicitly set intraprocess communications.
+  IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault;
+
+  /// Callbacks for various events related to publishers.
+  PublisherEventCallbacks event_callbacks;
+
+  /// Callback group in which the waitable items from the publisher should be placed.
+  rclcpp::callback_group::CallbackGroup::SharedPtr callback_group;
+};
+
+/// Structure containing optional configuration for Publishers.
+template<typename Allocator>
+struct PublisherOptionsWithAllocator : public PublisherOptionsBase
+{
+  /// Optional custom allocator.
+  std::shared_ptr<Allocator> allocator = nullptr;
+
+  PublisherOptionsWithAllocator<Allocator>() {}
+
+  /// Constructor using base class as input.
+  explicit PublisherOptionsWithAllocator(const PublisherOptionsBase & publisher_options_base)
+  : PublisherOptionsBase(publisher_options_base)
+  {}
+
+  /// Convert this class, and a rclcpp::QoS, into an rcl_publisher_options_t.
+  template<typename MessageT>
+  rcl_publisher_options_t
+  to_rcl_publisher_options(const rclcpp::QoS & qos) const
+  {
+    rcl_publisher_options_t result;
+    using AllocatorTraits = std::allocator_traits<Allocator>;
+    using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
+    auto message_alloc = std::make_shared<MessageAllocatorT>(*allocator.get());
+    result.allocator = allocator::get_rcl_allocator<MessageT>(*message_alloc);
+    result.qos = qos.get_rmw_qos_profile();
+    return result;
+  }
+};
+
+using PublisherOptions = PublisherOptionsWithAllocator<std::allocator<void>>;
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__PUBLISHER_OPTIONS_HPP_

rclcpp/include/rclcpp/qos.hpp

@@ -0,0 +1,203 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__QOS_HPP_
+#define RCLCPP__QOS_HPP_
+
+#include <rmw/qos_profiles.h>
+#include <rmw/types.h>
+
+#include "rclcpp/duration.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// QoS initialization values, cannot be created directly, use KeepAll or KeepLast instead.
+struct RCLCPP_PUBLIC QoSInitialization
+{
+  rmw_qos_history_policy_t history_policy;
+  size_t depth;
+
+  /// Constructor which takes both a history policy and a depth (even if it would be unused).
+  QoSInitialization(rmw_qos_history_policy_t history_policy_arg, size_t depth_arg);
+
+  /// Create a QoSInitialization from an existing rmw_qos_profile_t, using its history and depth.
+  static
+  QoSInitialization
+  from_rmw(const rmw_qos_profile_t & rmw_qos);
+};
+
+/// Use to initialize the QoS with the keep_all history setting.
+struct RCLCPP_PUBLIC KeepAll : public rclcpp::QoSInitialization
+{
+  KeepAll();
+};
+
+/// Use to initialize the QoS with the keep_last history setting and the given depth.
+struct RCLCPP_PUBLIC KeepLast : public rclcpp::QoSInitialization
+{
+  explicit KeepLast(size_t depth);
+};
+
+/// Encapsulation of Quality of Service settings.
+class RCLCPP_PUBLIC QoS
+{
+public:
+  /// Constructor which allows you to construct a QoS by giving the only required settings.
+  explicit
+  QoS(
+    const QoSInitialization & qos_initialization,
+    const rmw_qos_profile_t & initial_profile = rmw_qos_profile_default);
+
+  /// Conversion constructor to ease construction in the common case of just specifying depth.
+  /**
+   * Convenience constructor, equivalent to QoS(KeepLast(history_depth)).
+   */
+  // cppcheck-suppress noExplicitConstructor
+  QoS(size_t history_depth);  // NOLINT(runtime/explicit): conversion constructor
+
+  /// Return the rmw qos profile.
+  rmw_qos_profile_t &
+  get_rmw_qos_profile();
+
+  /// Return the rmw qos profile.
+  const rmw_qos_profile_t &
+  get_rmw_qos_profile() const;
+
+  /// Set the history policy.
+  QoS &
+  history(rmw_qos_history_policy_t history);
+
+  /// Set the history to keep last.
+  QoS &
+  keep_last(size_t depth);
+
+  /// Set the history to keep all.
+  QoS &
+  keep_all();
+
+  /// Set the reliability setting.
+  QoS &
+  reliability(rmw_qos_reliability_policy_t reliability);
+
+  /// Set the reliability setting to reliable.
+  QoS &
+  reliable();
+
+  /// Set the reliability setting to best effort.
+  QoS &
+  best_effort();
+
+  /// Set the durability setting.
+  QoS &
+  durability(rmw_qos_durability_policy_t durability);
+
+  /// Set the durability setting to volatile.
+  QoS &
+  volitile();
+
+  /// Set the durability setting to transient local.
+  QoS &
+  transient_local();
+
+  /// Set the deadline setting.
+  QoS &
+  deadline(rmw_time_t deadline);
+
+  /// Set the deadline setting, rclcpp::Duration.
+  QoS &
+  deadline(const rclcpp::Duration & deadline);
+
+  /// Set the lifespan setting.
+  QoS &
+  lifespan(rmw_time_t lifespan);
+
+  /// Set the lifespan setting, rclcpp::Duration.
+  QoS &
+  lifespan(const rclcpp::Duration & lifespan);
+
+  /// Set the liveliness setting.
+  QoS &
+  liveliness(rmw_qos_liveliness_policy_t liveliness);
+
+  /// Set the liveliness_lease_duration setting.
+  QoS &
+  liveliness_lease_duration(rmw_time_t liveliness_lease_duration);
+
+  /// Set the liveliness_lease_duration setting, rclcpp::Duration.
+  QoS &
+  liveliness_lease_duration(const rclcpp::Duration & liveliness_lease_duration);
+
+  /// Set the avoid_ros_namespace_conventions setting.
+  QoS &
+  avoid_ros_namespace_conventions(bool avoid_ros_namespace_conventions);
+
+private:
+  rmw_qos_profile_t rmw_qos_profile_;
+};
+
+class RCLCPP_PUBLIC SensorDataQoS : public QoS
+{
+public:
+  explicit
+  SensorDataQoS(
+    const QoSInitialization & qos_initialization = (
+      QoSInitialization::from_rmw(rmw_qos_profile_sensor_data)
+  ));
+};
+
+class RCLCPP_PUBLIC ParametersQoS : public QoS
+{
+public:
+  explicit
+  ParametersQoS(
+    const QoSInitialization & qos_initialization = (
+      QoSInitialization::from_rmw(rmw_qos_profile_parameters)
+  ));
+};
+
+class RCLCPP_PUBLIC ServicesQoS : public QoS
+{
+public:
+  explicit
+  ServicesQoS(
+    const QoSInitialization & qos_initialization = (
+      QoSInitialization::from_rmw(rmw_qos_profile_services_default)
+  ));
+};
+
+class RCLCPP_PUBLIC ParameterEventsQoS : public QoS
+{
+public:
+  explicit
+  ParameterEventsQoS(
+    const QoSInitialization & qos_initialization = (
+      QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)
+  ));
+};
+
+class RCLCPP_PUBLIC SystemDefaultsQoS : public QoS
+{
+public:
+  explicit
+  SystemDefaultsQoS(
+    const QoSInitialization & qos_initialization = (
+      QoSInitialization::from_rmw(rmw_qos_profile_system_default)
+  ));
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__QOS_HPP_

rclcpp/include/rclcpp/qos_event.hpp

@@ -0,0 +1,126 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__QOS_EVENT_HPP_
+#define RCLCPP__QOS_EVENT_HPP_
+
+#include <functional>
+
+#include "rcl/error_handling.h"
+
+#include "rcutils/logging_macros.h"
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/function_traits.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+
+using QOSDeadlineRequestedInfo = rmw_requested_deadline_missed_status_t;
+using QOSDeadlineOfferedInfo = rmw_offered_deadline_missed_status_t;
+using QOSLivelinessChangedInfo = rmw_liveliness_changed_status_t;
+using QOSLivelinessLostInfo = rmw_liveliness_lost_status_t;
+
+using QOSDeadlineRequestedCallbackType = std::function<void (QOSDeadlineRequestedInfo &)>;
+using QOSDeadlineOfferedCallbackType = std::function<void (QOSDeadlineOfferedInfo &)>;
+using QOSLivelinessChangedCallbackType = std::function<void (QOSLivelinessChangedInfo &)>;
+using QOSLivelinessLostCallbackType = std::function<void (QOSLivelinessLostInfo &)>;
+
+/// Contains callbacks for various types of events a Publisher can receive from the middleware.
+struct PublisherEventCallbacks
+{
+  QOSDeadlineOfferedCallbackType deadline_callback;
+  QOSLivelinessLostCallbackType liveliness_callback;
+};
+
+/// Contains callbacks for non-message events that a Subscription can receive from the middleware.
+struct SubscriptionEventCallbacks
+{
+  QOSDeadlineRequestedCallbackType deadline_callback;
+  QOSLivelinessChangedCallbackType liveliness_callback;
+};
+
+class QOSEventHandlerBase : public Waitable
+{
+public:
+  RCLCPP_PUBLIC
+  virtual ~QOSEventHandlerBase();
+
+  /// Get the number of ready events
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_ready_events() override;
+
+  /// Add the Waitable to a wait set.
+  RCLCPP_PUBLIC
+  bool
+  add_to_wait_set(rcl_wait_set_t * wait_set) override;
+
+  /// Check if the Waitable is ready.
+  RCLCPP_PUBLIC
+  bool
+  is_ready(rcl_wait_set_t * wait_set) override;
+
+protected:
+  rcl_event_t event_handle_;
+  size_t wait_set_event_index_;
+};
+
+template<typename EventCallbackT>
+class QOSEventHandler : public QOSEventHandlerBase
+{
+public:
+  template<typename InitFuncT, typename ParentHandleT, typename EventTypeEnum>
+  QOSEventHandler(
+    const EventCallbackT & callback,
+    InitFuncT init_func,
+    ParentHandleT parent_handle,
+    EventTypeEnum event_type)
+  : event_callback_(callback)
+  {
+    event_handle_ = rcl_get_zero_initialized_event();
+    rcl_ret_t ret = init_func(&event_handle_, parent_handle, event_type);
+    if (ret != RCL_RET_OK) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "could not create event");
+    }
+  }
+
+  /// Execute any entities of the Waitable that are ready.
+  void
+  execute() override
+  {
+    EventCallbackInfoT callback_info;
+
+    rcl_ret_t ret = rcl_take_event(&event_handle_, &callback_info);
+    if (ret != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "Couldn't take event info: %s", rcl_get_error_string().str);
+      return;
+    }
+
+    event_callback_(callback_info);
+  }
+
+private:
+  using EventCallbackInfoT = typename std::remove_reference<typename
+      rclcpp::function_traits::function_traits<EventCallbackT>::template argument_type<0>>::type;
+
+  EventCallbackT event_callback_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__QOS_EVENT_HPP_

rclcpp/include/rclcpp/rclcpp.hpp

@@ -151,5 +151,6 @@
 #include "rclcpp/time.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
 
 #endif  // RCLCPP__RCLCPP_HPP_

rclcpp/include/rclcpp/service.hpp

@@ -89,12 +89,12 @@ class Service : public ServiceBase
 {
 public:
   using CallbackType = std::function<
-    void(
+    void (
       const std::shared_ptr<typename ServiceT::Request>,
       std::shared_ptr<typename ServiceT::Response>)>;
 
   using CallbackWithHeaderType = std::function<
-    void(
+    void (
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<typename ServiceT::Request>,
       std::shared_ptr<typename ServiceT::Response>)>;
@@ -119,9 +119,9 @@ public:
         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"),
+              rclcpp::get_node_logger(handle.get()).get_child("rclcpp"),
               "Error in destruction of rcl service handle: %s",
-              rcl_get_error_string_safe());
+              rcl_get_error_string().str);
             rcl_reset_error();
           }
         } else {
@@ -164,7 +164,7 @@ public:
     any_callback_(any_callback)
   {
     // check if service handle was initialized
-    if (!rcl_service_is_valid(service_handle.get(), nullptr)) {
+    if (!rcl_service_is_valid(service_handle.get())) {
       // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
       throw std::runtime_error(
         std::string("rcl_service_t in constructor argument must be initialized beforehand."));
@@ -182,7 +182,7 @@ public:
     any_callback_(any_callback)
   {
     // check if service handle was initialized
-    if (!rcl_service_is_valid(service_handle, nullptr)) {
+    if (!rcl_service_is_valid(service_handle)) {
       // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
       throw std::runtime_error(
         std::string("rcl_service_t in constructor argument must be initialized beforehand."));

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -87,30 +87,42 @@ public:
     service_handles_.clear();
     client_handles_.clear();
     timer_handles_.clear();
+    waitable_handles_.clear();
   }
 
   virtual void remove_null_handles(rcl_wait_set_t * wait_set)
   {
-    for (size_t i = 0; i < wait_set->size_of_subscriptions; ++i) {
+    // TODO(jacobperron): Check if wait set sizes are what we expect them to be?
+    //                    e.g. wait_set->size_of_clients == client_handles_.size()
+
+    // Important to use subscription_handles_.size() instead of wait set's size since
+    // there may be more subscriptions in the wait set due to Waitables added to the end.
+    // The same logic applies for other entities.
+    for (size_t i = 0; i < subscription_handles_.size(); ++i) {
       if (!wait_set->subscriptions[i]) {
         subscription_handles_[i].reset();
       }
     }
-    for (size_t i = 0; i < wait_set->size_of_services; ++i) {
+    for (size_t i = 0; i < service_handles_.size(); ++i) {
       if (!wait_set->services[i]) {
         service_handles_[i].reset();
       }
     }
-    for (size_t i = 0; i < wait_set->size_of_clients; ++i) {
+    for (size_t i = 0; i < client_handles_.size(); ++i) {
       if (!wait_set->clients[i]) {
         client_handles_[i].reset();
       }
     }
-    for (size_t i = 0; i < wait_set->size_of_timers; ++i) {
+    for (size_t i = 0; i < timer_handles_.size(); ++i) {
       if (!wait_set->timers[i]) {
         timer_handles_[i].reset();
       }
     }
+    for (size_t i = 0; i < waitable_handles_.size(); ++i) {
+      if (!waitable_handles_[i]->is_ready(wait_set)) {
+        waitable_handles_[i].reset();
+      }
+    }
 
     subscription_handles_.erase(
       std::remove(subscription_handles_.begin(), subscription_handles_.end(), nullptr),
@@ -131,6 +143,11 @@ public:
       std::remove(timer_handles_.begin(), timer_handles_.end(), nullptr),
       timer_handles_.end()
     );
+
+    waitable_handles_.erase(
+      std::remove(waitable_handles_.begin(), waitable_handles_.end(), nullptr),
+      waitable_handles_.end()
+    );
   }
 
   bool collect_entities(const WeakNodeVector & weak_nodes)
@@ -175,6 +192,12 @@ public:
             timer_handles_.push_back(timer->get_timer_handle());
           }
         }
+        for (auto & weak_waitable : group->get_waitable_ptrs()) {
+          auto waitable = weak_waitable.lock();
+          if (waitable) {
+            waitable_handles_.push_back(waitable);
+          }
+        }
       }
     }
     return has_invalid_weak_nodes;
@@ -183,47 +206,56 @@ public:
   bool add_handles_to_wait_set(rcl_wait_set_t * wait_set)
   {
     for (auto subscription : subscription_handles_) {
-      if (rcl_wait_set_add_subscription(wait_set, subscription.get()) != RCL_RET_OK) {
+      if (rcl_wait_set_add_subscription(wait_set, subscription.get(), NULL) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
-          "Couldn't add subscription to wait set: %s", rcl_get_error_string_safe());
+          "Couldn't add subscription to wait set: %s", rcl_get_error_string().str);
         return false;
       }
     }
 
     for (auto client : client_handles_) {
-      if (rcl_wait_set_add_client(wait_set, client.get()) != RCL_RET_OK) {
+      if (rcl_wait_set_add_client(wait_set, client.get(), NULL) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
-          "Couldn't add client to wait set: %s", rcl_get_error_string_safe());
+          "Couldn't add client to wait set: %s", rcl_get_error_string().str);
         return false;
       }
     }
 
     for (auto service : service_handles_) {
-      if (rcl_wait_set_add_service(wait_set, service.get()) != RCL_RET_OK) {
+      if (rcl_wait_set_add_service(wait_set, service.get(), NULL) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
-          "Couldn't add service to wait set: %s", rcl_get_error_string_safe());
+          "Couldn't add service to wait set: %s", rcl_get_error_string().str);
         return false;
       }
     }
 
     for (auto timer : timer_handles_) {
-      if (rcl_wait_set_add_timer(wait_set, timer.get()) != RCL_RET_OK) {
+      if (rcl_wait_set_add_timer(wait_set, timer.get(), NULL) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
-          "Couldn't add timer to wait set: %s", rcl_get_error_string_safe());
+          "Couldn't add timer to wait set: %s", rcl_get_error_string().str);
         return false;
       }
     }
 
     for (auto guard_condition : guard_conditions_) {
-      if (rcl_wait_set_add_guard_condition(wait_set, guard_condition) != RCL_RET_OK) {
+      if (rcl_wait_set_add_guard_condition(wait_set, guard_condition, NULL) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
           "Couldn't add guard_condition to wait set: %s",
-          rcl_get_error_string_safe());
+          rcl_get_error_string().str);
+        return false;
+      }
+    }
+
+    for (auto waitable : waitable_handles_) {
+      if (!waitable->add_to_wait_set(wait_set)) {
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Couldn't add waitable to wait set: %s", rcl_get_error_string().str);
         return false;
       }
     }
@@ -342,6 +374,39 @@ public:
     }
   }
 
+  virtual void
+  get_next_waitable(executor::AnyExecutable & any_exec, const WeakNodeVector & weak_nodes)
+  {
+    auto it = waitable_handles_.begin();
+    while (it != waitable_handles_.end()) {
+      auto waitable = *it;
+      if (waitable) {
+        // Find the group for this handle and see if it can be serviced
+        auto group = get_group_by_waitable(waitable, weak_nodes);
+        if (!group) {
+          // Group was not found, meaning the waitable is not valid...
+          // Remove it from the ready list and continue looking
+          it = waitable_handles_.erase(it);
+          continue;
+        }
+        if (!group->can_be_taken_from().load()) {
+          // Group is mutually exclusive and is being used, so skip it for now
+          // Leave it to be checked next time, but continue searching
+          ++it;
+          continue;
+        }
+        // Otherwise it is safe to set and return the any_exec
+        any_exec.waitable = waitable;
+        any_exec.callback_group = group;
+        any_exec.node_base = get_node_by_group(group, weak_nodes);
+        waitable_handles_.erase(it);
+        return;
+      }
+      // Else, the waitable is no longer valid, remove it and continue
+      it = waitable_handles_.erase(it);
+    }
+  }
+
   virtual rcl_allocator_t get_allocator()
   {
     return rclcpp::allocator::get_rcl_allocator<void *, VoidAlloc>(*allocator_.get());
@@ -349,27 +414,61 @@ public:
 
   size_t number_of_ready_subscriptions() const
   {
-    return subscription_handles_.size();
+    size_t number_of_subscriptions = subscription_handles_.size();
+    for (auto waitable : waitable_handles_) {
+      number_of_subscriptions += waitable->get_number_of_ready_subscriptions();
+    }
+    return number_of_subscriptions;
   }
 
   size_t number_of_ready_services() const
   {
-    return service_handles_.size();
+    size_t number_of_services = service_handles_.size();
+    for (auto waitable : waitable_handles_) {
+      number_of_services += waitable->get_number_of_ready_services();
+    }
+    return number_of_services;
+  }
+
+  size_t number_of_ready_events() const
+  {
+    size_t number_of_events = 0;
+    for (auto waitable : waitable_handles_) {
+      number_of_events += waitable->get_number_of_ready_events();
+    }
+    return number_of_events;
   }
 
   size_t number_of_ready_clients() const
   {
-    return client_handles_.size();
+    size_t number_of_clients = client_handles_.size();
+    for (auto waitable : waitable_handles_) {
+      number_of_clients += waitable->get_number_of_ready_clients();
+    }
+    return number_of_clients;
   }
 
   size_t number_of_guard_conditions() const
   {
-    return guard_conditions_.size();
+    size_t number_of_guard_conditions = guard_conditions_.size();
+    for (auto waitable : waitable_handles_) {
+      number_of_guard_conditions += waitable->get_number_of_ready_guard_conditions();
+    }
+    return number_of_guard_conditions;
   }
 
   size_t number_of_ready_timers() const
   {
-    return timer_handles_.size();
+    size_t number_of_timers = timer_handles_.size();
+    for (auto waitable : waitable_handles_) {
+      number_of_timers += waitable->get_number_of_ready_timers();
+    }
+    return number_of_timers;
+  }
+
+  size_t number_of_waitables() const
+  {
+    return waitable_handles_.size();
   }
 
 private:
@@ -383,6 +482,7 @@ private:
   VectorRebind<std::shared_ptr<const rcl_service_t>> service_handles_;
   VectorRebind<std::shared_ptr<const rcl_client_t>> client_handles_;
   VectorRebind<std::shared_ptr<const rcl_timer_t>> timer_handles_;
+  VectorRebind<std::shared_ptr<Waitable>> waitable_handles_;
 
   std::shared_ptr<VoidAlloc> allocator_;
 };

rclcpp/include/rclcpp/subscription.hpp

@@ -23,6 +23,8 @@
 #include <memory>
 #include <sstream>
 #include <string>
+#include <utility>
+
 
 #include "rcl/error_handling.h"
 #include "rcl/subscription.h"
@@ -32,11 +34,15 @@
 #include "rclcpp/any_subscription_callback.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/expand_topic_or_service_name.hpp"
+#include "rclcpp/intra_process_manager.hpp"
+#include "rclcpp/logging.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/message_memory_strategy.hpp"
+#include "rclcpp/subscription_base.hpp"
 #include "rclcpp/subscription_traits.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
 
 namespace rclcpp
 {
@@ -46,100 +52,6 @@ namespace node_interfaces
 class NodeTopicsInterface;
 }  // namespace node_interfaces
 
-/// Virtual base class for subscriptions. This pattern allows us to iterate over different template
-/// specializations of Subscription, among other things.
-class SubscriptionBase
-{
-public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(SubscriptionBase)
-
-  /// Default constructor.
-  /**
-   * \param[in] node_handle The rcl representation of the node that owns this subscription.
-   * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic.
-   * \param[in] topic_name Name of the topic to subscribe to.
-   * \param[in] subscription_options options for the subscription.
-   */
-  RCLCPP_PUBLIC
-  SubscriptionBase(
-    std::shared_ptr<rcl_node_t> node_handle,
-    const rosidl_message_type_support_t & type_support_handle,
-    const std::string & topic_name,
-    const rcl_subscription_options_t & subscription_options,
-    bool is_serialized = false);
-
-  /// Default destructor.
-  RCLCPP_PUBLIC
-  virtual ~SubscriptionBase();
-
-  /// Get the topic that this subscription is subscribed on.
-  RCLCPP_PUBLIC
-  const char *
-  get_topic_name() const;
-
-  RCLCPP_PUBLIC
-  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 std::shared_ptr<rcl_subscription_t>
-  get_intra_process_subscription_handle() const;
-
-  /// Borrow a new message.
-  /** \return Shared pointer to the fresh message. */
-  virtual std::shared_ptr<void>
-  create_message() = 0;
-
-  /// Borrow a new serialized message
-  /** \return Shared pointer to a rcl_message_serialized_t. */
-  virtual std::shared_ptr<rcl_serialized_message_t>
-  create_serialized_message() = 0;
-
-  /// Check if we need to handle the message, and execute the callback if we do.
-  /**
-   * \param[in] message Shared pointer to the message to handle.
-   * \param[in] message_info Metadata associated with this message.
-   */
-  virtual void
-  handle_message(std::shared_ptr<void> & message, const rmw_message_info_t & message_info) = 0;
-
-  /// Return the message borrowed in create_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:
-  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_;
-};
-
 /// Subscription implementation, templated on the type of message this subscription receives.
 template<
   typename CallbackMessageT,
@@ -152,6 +64,7 @@ public:
   using MessageAllocTraits = allocator::AllocRebind<CallbackMessageT, Alloc>;
   using MessageAlloc = typename MessageAllocTraits::allocator_type;
   using MessageDeleter = allocator::Deleter<MessageAlloc, CallbackMessageT>;
+  using ConstMessageSharedPtr = std::shared_ptr<const CallbackMessageT>;
   using MessageUniquePtr = std::unique_ptr<CallbackMessageT, MessageDeleter>;
 
   RCLCPP_SMART_PTR_DEFINITIONS(Subscription)
@@ -161,6 +74,7 @@ public:
    * The constructor for a subscription is almost never called directly. Instead, subscriptions
    * should be instantiated through Node::create_subscription.
    * \param[in] node_handle rcl representation of the node that owns this subscription.
+   * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic.
    * \param[in] topic_name Name of the topic to subscribe to.
    * \param[in] subscription_options options for the subscription.
    * \param[in] callback User defined callback to call when a message is received.
@@ -168,24 +82,32 @@ public:
    */
   Subscription(
     std::shared_ptr<rcl_node_t> node_handle,
-    const rosidl_message_type_support_t & ts,
+    const rosidl_message_type_support_t & type_support_handle,
     const std::string & topic_name,
     const rcl_subscription_options_t & subscription_options,
     AnySubscriptionCallback<CallbackMessageT, Alloc> callback,
+    const SubscriptionEventCallbacks & event_callbacks,
     typename message_memory_strategy::MessageMemoryStrategy<CallbackMessageT, Alloc>::SharedPtr
     memory_strategy = message_memory_strategy::MessageMemoryStrategy<CallbackMessageT,
     Alloc>::create_default())
   : SubscriptionBase(
       node_handle,
-      ts,
+      type_support_handle,
       topic_name,
       subscription_options,
       rclcpp::subscription_traits::is_serialized_subscription_argument<CallbackMessageT>::value),
     any_callback_(callback),
-    message_memory_strategy_(memory_strategy),
-    get_intra_process_message_callback_(nullptr),
-    matches_any_intra_process_publishers_(nullptr)
-  {}
+    message_memory_strategy_(memory_strategy)
+  {
+    if (event_callbacks.deadline_callback) {
+      this->add_event_handler(event_callbacks.deadline_callback,
+        RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);
+    }
+    if (event_callbacks.liveliness_callback) {
+      this->add_event_handler(event_callbacks.liveliness_callback,
+        RCL_SUBSCRIPTION_LIVELINESS_CHANGED);
+    }
+  }
 
   /// Support dynamically setting the message memory strategy.
   /**
@@ -215,12 +137,10 @@ public:
 
   void handle_message(std::shared_ptr<void> & message, const rmw_message_info_t & message_info)
   {
-    if (matches_any_intra_process_publishers_) {
-      if (matches_any_intra_process_publishers_(&message_info.publisher_gid)) {
-        // In this case, the message will be delivered via intra process and
-        // we should ignore this copy of the message.
-        return;
-      }
+    if (matches_any_intra_process_publishers(&message_info.publisher_gid)) {
+      // In this case, the message will be delivered via intra process and
+      // we should ignore this copy of the message.
+      return;
     }
     auto typed_message = std::static_pointer_cast<CallbackMessageT>(message);
     any_callback_.dispatch(typed_message, message_info);
@@ -243,85 +163,109 @@ public:
     rcl_interfaces::msg::IntraProcessMessage & ipm,
     const rmw_message_info_t & message_info)
   {
-    if (!get_intra_process_message_callback_) {
+    if (!use_intra_process_) {
       // throw std::runtime_error(
       //   "handle_intra_process_message called before setup_intra_process");
       // TODO(wjwwood): for now, this could mean that intra process was just not enabled.
       // However, this can only really happen if this node has it disabled, but the other doesn't.
       return;
     }
-    MessageUniquePtr msg;
-    get_intra_process_message_callback_(
-      ipm.publisher_id,
-      ipm.message_sequence,
-      intra_process_subscription_id_,
-      msg);
-    if (!msg) {
-      // This either occurred because the publisher no longer exists or the
-      // message requested is no longer being stored.
-      // TODO(wjwwood): should we notify someone of this? log error, log warning?
-      return;
-    }
-    any_callback_.dispatch_intra_process(msg, message_info);
-  }
-
-  using GetMessageCallbackType =
-    std::function<void(uint64_t, uint64_t, uint64_t, MessageUniquePtr &)>;
-  using MatchesAnyPublishersCallbackType = std::function<bool(const rmw_gid_t *)>;
-
-  /// Implemenation detail.
-  void setup_intra_process(
-    uint64_t intra_process_subscription_id,
-    GetMessageCallbackType get_message_callback,
-    MatchesAnyPublishersCallbackType matches_any_publisher_callback,
-    const rcl_subscription_options_t & intra_process_options)
-  {
-    std::string intra_process_topic_name = std::string(get_topic_name()) + "/_intra";
-    rcl_ret_t ret = rcl_subscription_init(
-      intra_process_subscription_handle_.get(),
-      node_handle_.get(),
-      rclcpp::type_support::get_intra_process_message_msg_type_support(),
-      intra_process_topic_name.c_str(),
-      &intra_process_options);
-    if (ret != RCL_RET_OK) {
-      if (ret == RCL_RET_TOPIC_NAME_INVALID) {
-        auto rcl_node_handle = node_handle_.get();
-        // this will throw on any validation problem
-        rcl_reset_error();
-        expand_topic_or_service_name(
-          intra_process_topic_name,
-          rcl_node_get_name(rcl_node_handle),
-          rcl_node_get_namespace(rcl_node_handle));
+    if (any_callback_.use_take_shared_method()) {
+      ConstMessageSharedPtr msg;
+      take_intra_process_message(
+        ipm.publisher_id,
+        ipm.message_sequence,
+        intra_process_subscription_id_,
+        msg);
+      if (!msg) {
+        // This either occurred because the publisher no longer exists or the
+        // message requested is no longer being stored.
+        RCLCPP_WARN(get_logger("rclcpp"),
+          "Intra process message not longer being stored when trying to handle it");
+        return;
       }
-
-      rclcpp::exceptions::throw_from_rcl_error(ret, "could not create intra process subscription");
+      any_callback_.dispatch_intra_process(msg, message_info);
+    } else {
+      MessageUniquePtr msg;
+      take_intra_process_message(
+        ipm.publisher_id,
+        ipm.message_sequence,
+        intra_process_subscription_id_,
+        msg);
+      if (!msg) {
+        // This either occurred because the publisher no longer exists or the
+        // message requested is no longer being stored.
+        RCLCPP_WARN(get_logger("rclcpp"),
+          "Intra process message not longer being stored when trying to handle it");
+        return;
+      }
+      any_callback_.dispatch_intra_process(std::move(msg), message_info);
     }
-
-    intra_process_subscription_id_ = intra_process_subscription_id;
-    get_intra_process_message_callback_ = get_message_callback;
-    matches_any_intra_process_publishers_ = matches_any_publisher_callback;
   }
 
   /// Implemenation detail.
   const std::shared_ptr<rcl_subscription_t>
   get_intra_process_subscription_handle() const
   {
-    if (!get_intra_process_message_callback_) {
+    if (!use_intra_process_) {
       return nullptr;
     }
     return intra_process_subscription_handle_;
   }
 
 private:
+  void
+  take_intra_process_message(
+    uint64_t publisher_id,
+    uint64_t message_sequence,
+    uint64_t subscription_id,
+    MessageUniquePtr & message)
+  {
+    auto ipm = weak_ipm_.lock();
+    if (!ipm) {
+      throw std::runtime_error(
+              "intra process take called after destruction of intra process manager");
+    }
+    ipm->template take_intra_process_message<CallbackMessageT, Alloc>(
+      publisher_id, message_sequence, subscription_id, message);
+  }
+
+  void
+  take_intra_process_message(
+    uint64_t publisher_id,
+    uint64_t message_sequence,
+    uint64_t subscription_id,
+    ConstMessageSharedPtr & message)
+  {
+    auto ipm = weak_ipm_.lock();
+    if (!ipm) {
+      throw std::runtime_error(
+              "intra process take called after destruction of intra process manager");
+    }
+    ipm->template take_intra_process_message<CallbackMessageT, Alloc>(
+      publisher_id, message_sequence, subscription_id, message);
+  }
+
+  bool
+  matches_any_intra_process_publishers(const rmw_gid_t * sender_gid)
+  {
+    if (!use_intra_process_) {
+      return false;
+    }
+    auto ipm = weak_ipm_.lock();
+    if (!ipm) {
+      throw std::runtime_error(
+              "intra process publisher check called "
+              "after destruction of intra process manager");
+    }
+    return ipm->matches_any_publishers(sender_gid);
+  }
+
   RCLCPP_DISABLE_COPY(Subscription)
 
   AnySubscriptionCallback<CallbackMessageT, Alloc> any_callback_;
   typename message_memory_strategy::MessageMemoryStrategy<CallbackMessageT, Alloc>::SharedPtr
     message_memory_strategy_;
-
-  GetMessageCallbackType get_intra_process_message_callback_;
-  MatchesAnyPublishersCallbackType matches_any_intra_process_publishers_;
-  uint64_t intra_process_subscription_id_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/subscription_base.hpp

@@ -0,0 +1,189 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__SUBSCRIPTION_BASE_HPP_
+#define RCLCPP__SUBSCRIPTION_BASE_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rcl/subscription.h"
+
+#include "rcl_interfaces/msg/intra_process_message.hpp"
+
+#include "rmw/rmw.h"
+
+#include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/qos_event.hpp"
+#include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+namespace node_interfaces
+{
+class NodeTopicsInterface;
+}  // namespace node_interfaces
+
+namespace intra_process_manager
+{
+/**
+ * IntraProcessManager is forward declared here, avoiding a circular inclusion between
+ * `intra_process_manager.hpp` and `subscription_base.hpp`.
+ */
+class IntraProcessManager;
+}
+
+/// Virtual base class for subscriptions. This pattern allows us to iterate over different template
+/// specializations of Subscription, among other things.
+class SubscriptionBase
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(SubscriptionBase)
+
+  /// Default constructor.
+  /**
+   * \param[in] node_handle The rcl representation of the node that owns this subscription.
+   * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic.
+   * \param[in] topic_name Name of the topic to subscribe to.
+   * \param[in] subscription_options options for the subscription.
+   * \param[in] is_serialized is true if the message will be delivered still serialized
+   */
+  RCLCPP_PUBLIC
+  SubscriptionBase(
+    std::shared_ptr<rcl_node_t> node_handle,
+    const rosidl_message_type_support_t & type_support_handle,
+    const std::string & topic_name,
+    const rcl_subscription_options_t & subscription_options,
+    bool is_serialized = false);
+
+  /// Default destructor.
+  RCLCPP_PUBLIC
+  virtual ~SubscriptionBase();
+
+  /// Get the topic that this subscription is subscribed on.
+  RCLCPP_PUBLIC
+  const char *
+  get_topic_name() const;
+
+  RCLCPP_PUBLIC
+  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 std::shared_ptr<rcl_subscription_t>
+  get_intra_process_subscription_handle() const;
+
+  /// Get all the QoS event handlers associated with this subscription.
+  /** \return The vector of QoS event handlers. */
+  RCLCPP_PUBLIC
+  const std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
+  get_event_handlers() const;
+
+  /// Borrow a new message.
+  /** \return Shared pointer to the fresh message. */
+  virtual std::shared_ptr<void>
+  create_message() = 0;
+
+  /// Borrow a new serialized message
+  /** \return Shared pointer to a rcl_message_serialized_t. */
+  virtual std::shared_ptr<rcl_serialized_message_t>
+  create_serialized_message() = 0;
+
+  /// Check if we need to handle the message, and execute the callback if we do.
+  /**
+   * \param[in] message Shared pointer to the message to handle.
+   * \param[in] message_info Metadata associated with this message.
+   */
+  virtual void
+  handle_message(std::shared_ptr<void> & message, const rmw_message_info_t & message_info) = 0;
+
+  /// Return the message borrowed in create_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;
+
+  /// Get matching publisher count.
+  /** \return The number of publishers on this topic. */
+  RCLCPP_PUBLIC
+  size_t
+  get_publisher_count() const;
+
+  using IntraProcessManagerWeakPtr =
+    std::weak_ptr<rclcpp::intra_process_manager::IntraProcessManager>;
+
+  /// Implemenation detail.
+  void setup_intra_process(
+    uint64_t intra_process_subscription_id,
+    IntraProcessManagerWeakPtr weak_ipm,
+    const rcl_subscription_options_t & intra_process_options);
+
+protected:
+  template<typename EventCallbackT>
+  void
+  add_event_handler(
+    const EventCallbackT & callback,
+    const rcl_subscription_event_type_t event_type)
+  {
+    auto handler = std::make_shared<QOSEventHandler<EventCallbackT>>(
+      callback,
+      rcl_subscription_event_init,
+      get_subscription_handle().get(),
+      event_type);
+    event_handlers_.emplace_back(handler);
+  }
+
+  std::shared_ptr<rcl_node_t> node_handle_;
+  std::shared_ptr<rcl_subscription_t> subscription_handle_;
+  std::shared_ptr<rcl_subscription_t> intra_process_subscription_handle_;
+
+  std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
+
+  bool use_intra_process_;
+  IntraProcessManagerWeakPtr weak_ipm_;
+  uint64_t intra_process_subscription_id_;
+
+private:
+  RCLCPP_DISABLE_COPY(SubscriptionBase)
+
+  rosidl_message_type_support_t type_support_;
+  bool is_serialized_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__SUBSCRIPTION_BASE_HPP_

rclcpp/include/rclcpp/subscription_factory.hpp

@@ -51,13 +51,13 @@ struct SubscriptionFactory
     rclcpp::SubscriptionBase::SharedPtr(
       rclcpp::node_interfaces::NodeBaseInterface * node_base,
       const std::string & topic_name,
-      rcl_subscription_options_t & subscription_options)>;
+      const 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(
+    void (
       rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm,
       rclcpp::SubscriptionBase::SharedPtr subscription,
       const rcl_subscription_options_t & subscription_options)>;
@@ -75,6 +75,7 @@ template<
 SubscriptionFactory
 create_subscription_factory(
   CallbackT && callback,
+  const SubscriptionEventCallbacks & event_callbacks,
   typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
     CallbackMessageT, Alloc>::SharedPtr
   msg_mem_strat,
@@ -91,13 +92,14 @@ create_subscription_factory(
 
   // factory function that creates a MessageT specific SubscriptionT
   factory.create_typed_subscription =
-    [allocator, msg_mem_strat, any_subscription_callback, message_alloc](
+    [allocator, msg_mem_strat, any_subscription_callback, event_callbacks, message_alloc](
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
     const std::string & topic_name,
-    rcl_subscription_options_t & subscription_options
+    const rcl_subscription_options_t & subscription_options
     ) -> rclcpp::SubscriptionBase::SharedPtr
     {
-      subscription_options.allocator =
+      auto options_copy = subscription_options;
+      options_copy.allocator =
         rclcpp::allocator::get_rcl_allocator<CallbackMessageT>(*message_alloc.get());
 
       using rclcpp::Subscription;
@@ -107,70 +109,14 @@ create_subscription_factory(
         node_base->get_shared_rcl_node_handle(),
         *rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>(),
         topic_name,
-        subscription_options,
+        options_copy,
         any_subscription_callback,
+        event_callbacks,
         msg_mem_strat);
       auto sub_base_ptr = std::dynamic_pointer_cast<SubscriptionBase>(sub);
       return sub_base_ptr;
     };
 
-  // function that will setup intra process communications for the subscription
-  factory.setup_intra_process =
-    [message_alloc](
-    rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm,
-    rclcpp::SubscriptionBase::SharedPtr subscription,
-    const rcl_subscription_options_t & subscription_options)
-    {
-      rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = ipm;
-      uint64_t intra_process_subscription_id = ipm->add_subscription(subscription);
-
-      auto intra_process_options = rcl_subscription_get_default_options();
-      intra_process_options.allocator = rclcpp::allocator::get_rcl_allocator<CallbackMessageT>(
-        *message_alloc.get());
-      intra_process_options.qos = subscription_options.qos;
-      intra_process_options.ignore_local_publications = false;
-
-      // function that will be called to take a MessageT from the intra process manager
-      auto take_intra_process_message_func =
-        [weak_ipm](
-        uint64_t publisher_id,
-        uint64_t message_sequence,
-        uint64_t subscription_id,
-        typename rclcpp::Subscription<CallbackMessageT, Alloc>::MessageUniquePtr & message)
-        {
-          auto ipm = weak_ipm.lock();
-          if (!ipm) {
-            // TODO(wjwwood): should this just return silently? Or return with a logged warning?
-            throw std::runtime_error(
-                    "intra process take called after destruction of intra process manager");
-          }
-          ipm->take_intra_process_message<CallbackMessageT, Alloc>(
-            publisher_id, message_sequence, subscription_id, message);
-        };
-
-      // function that is called to see if the publisher id matches any local publishers
-      auto matches_any_publisher_func =
-        [weak_ipm](const rmw_gid_t * sender_gid) -> bool
-        {
-          auto ipm = weak_ipm.lock();
-          if (!ipm) {
-            throw std::runtime_error(
-                    "intra process publisher check called "
-                    "after destruction of intra process manager");
-          }
-          return ipm->matches_any_publishers(sender_gid);
-        };
-
-      auto typed_sub_ptr = std::dynamic_pointer_cast<SubscriptionT>(subscription);
-      typed_sub_ptr->setup_intra_process(
-        intra_process_subscription_id,
-        take_intra_process_message_func,
-        matches_any_publisher_func,
-        intra_process_options
-      );
-    };
-  // end definition of factory function to setup intra process
-
   // return the factory now that it is populated
   return factory;
 }

rclcpp/include/rclcpp/subscription_options.hpp

@@ -0,0 +1,79 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__SUBSCRIPTION_OPTIONS_HPP_
+#define RCLCPP__SUBSCRIPTION_OPTIONS_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/intra_process_setting.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/qos_event.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Non-template base class for subscription options.
+struct SubscriptionOptionsBase
+{
+  /// Callbacks for events related to this subscription.
+  SubscriptionEventCallbacks event_callbacks;
+  /// True to ignore local publications.
+  bool ignore_local_publications = false;
+  /// The callback group for this subscription. NULL to use the default callback group.
+  rclcpp::callback_group::CallbackGroup::SharedPtr callback_group = nullptr;
+  /// Setting to explicitly set intraprocess communications.
+  IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault;
+};
+
+/// Structure containing optional configuration for Subscriptions.
+template<typename Allocator>
+struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
+{
+  /// Optional custom allocator.
+  std::shared_ptr<Allocator> allocator = nullptr;
+
+  SubscriptionOptionsWithAllocator<Allocator>() {}
+
+  /// Constructor using base class as input.
+  explicit SubscriptionOptionsWithAllocator(
+    const SubscriptionOptionsBase & subscription_options_base)
+  : SubscriptionOptionsBase(subscription_options_base)
+  {}
+
+  /// Convert this class, with a rclcpp::QoS, into an rcl_subscription_options_t.
+  template<typename MessageT>
+  rcl_subscription_options_t
+  to_rcl_subscription_options(const rclcpp::QoS & qos) const
+  {
+    rcl_subscription_options_t result;
+    using AllocatorTraits = std::allocator_traits<Allocator>;
+    using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
+    auto message_alloc = std::make_shared<MessageAllocatorT>(*allocator.get());
+    result.allocator = allocator::get_rcl_allocator<MessageT>(*message_alloc);
+    result.ignore_local_publications = this->ignore_local_publications;
+    result.qos = qos.get_rmw_qos_profile();
+    return result;
+  }
+};
+
+using SubscriptionOptions = SubscriptionOptionsWithAllocator<std::allocator<void>>;
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__SUBSCRIPTION_OPTIONS_HPP_

rclcpp/include/rclcpp/subscription_traits.hpp

@@ -18,9 +18,13 @@
 #include <memory>
 
 #include "rclcpp/function_traits.hpp"
+#include "rcl/types.h"
 
 namespace rclcpp
 {
+
+class QoS;
+
 namespace subscription_traits
 {
 
@@ -69,7 +73,20 @@ template<typename MessageT, typename Deleter>
 struct extract_message_type<std::unique_ptr<MessageT, Deleter>>: extract_message_type<MessageT>
 {};
 
-template<typename CallbackT>
+template<
+  typename CallbackT,
+  // Do not attempt if CallbackT is an integer (mistaken for depth)
+  typename = std::enable_if_t<!std::is_integral<
+    std::remove_cv_t<std::remove_reference_t<CallbackT>>>::value>,
+  // Do not attempt if CallbackT is a QoS (mistaken for qos)
+  typename = std::enable_if_t<!std::is_base_of<
+    rclcpp::QoS,
+    std::remove_cv_t<std::remove_reference_t<CallbackT>>>::value>,
+  // Do not attempt if CallbackT is a rmw_qos_profile_t (mistaken for qos profile)
+  typename = std::enable_if_t<!std::is_same<
+    rmw_qos_profile_t,
+    std::remove_cv_t<std::remove_reference_t<CallbackT>>>::value>
+>
 struct has_message_type : extract_message_type<
     typename rclcpp::function_traits::function_traits<CallbackT>::template argument_type<0>>
 {};

rclcpp/include/rclcpp/time.hpp

@@ -102,6 +102,17 @@ public:
   rcl_time_point_value_t
   nanoseconds() const;
 
+  RCLCPP_PUBLIC
+  static Time
+  max();
+
+  /// \return the seconds since epoch as a floating point number.
+  /// \warning Depending on sizeof(double) there could be significant precision loss.
+  /// When an exact time is required use nanoseconds() instead.
+  RCLCPP_PUBLIC
+  double
+  seconds() const;
+
   RCLCPP_PUBLIC
   rcl_clock_type_t
   get_clock_type() const;

rclcpp/include/rclcpp/time_source.hpp

@@ -47,10 +47,13 @@ public:
 
   RCLCPP_PUBLIC
   void attachNode(
-    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
-    const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
-    const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
-    const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface);
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+    rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
+    rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface,
+    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface);
 
   RCLCPP_PUBLIC
   void detachNode();
@@ -74,16 +77,29 @@ private:
   rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
+
+  // Store (and update on node attach) logger for logging.
+  Logger logger_;
 
   // The subscription for the clock callback
   using MessageT = rosgraph_msgs::msg::Clock;
   using Alloc = std::allocator<void>;
   using SubscriptionT = rclcpp::Subscription<MessageT, Alloc>;
   std::shared_ptr<SubscriptionT> clock_subscription_;
+  std::mutex clock_sub_lock_;
 
   // The clock callback itself
   void clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg);
 
+  // Create the subscription for the clock topic
+  void create_clock_sub();
+
+  // Destroy the subscription for the clock topic
+  void destroy_clock_sub();
+
   // Parameter Client pointer
   std::shared_ptr<rclcpp::AsyncParametersClient> parameter_client_;
 

rclcpp/include/rclcpp/timer.hpp

@@ -23,6 +23,8 @@
 #include <type_traits>
 #include <utility>
 
+#include "rclcpp/clock.hpp"
+#include "rclcpp/context.hpp"
 #include "rclcpp/function_traits.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/rate.hpp"
@@ -44,7 +46,10 @@ public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase)
 
   RCLCPP_PUBLIC
-  explicit TimerBase(std::chrono::nanoseconds period);
+  explicit TimerBase(
+    Clock::SharedPtr clock,
+    std::chrono::nanoseconds period,
+    rclcpp::Context::SharedPtr context);
 
   RCLCPP_PUBLIC
   ~TimerBase();
@@ -53,6 +58,16 @@ public:
   void
   cancel();
 
+  /// Return the timer cancellation state.
+  /**
+   * \return true if the timer has been cancelled, false otherwise
+   * \throws std::runtime_error if the rcl_get_error_state returns 0
+   * \throws RCLErrorBase some child class exception based on ret
+   */
+  RCLCPP_PUBLIC
+  bool
+  is_canceled();
+
   RCLCPP_PUBLIC
   void
   reset();
@@ -85,21 +100,20 @@ public:
   bool is_ready();
 
 protected:
+  Clock::SharedPtr clock_;
   std::shared_ptr<rcl_timer_t> timer_handle_;
 };
 
 
-using VoidCallbackType = std::function<void()>;
-using TimerCallbackType = std::function<void(TimerBase &)>;
+using VoidCallbackType = std::function<void ()>;
+using TimerCallbackType = std::function<void (TimerBase &)>;
 
-/// Generic timer templated on the clock type. Periodically executes a user-specified callback.
+/// Generic timer. Periodically executes a user-specified callback.
 template<
   typename FunctorT,
-  class Clock,
   typename std::enable_if<
-    (rclcpp::function_traits::same_arguments<FunctorT, VoidCallbackType>::value ||
-    rclcpp::function_traits::same_arguments<FunctorT, TimerCallbackType>::value) &&
-    Clock::is_steady
+    rclcpp::function_traits::same_arguments<FunctorT, VoidCallbackType>::value ||
+    rclcpp::function_traits::same_arguments<FunctorT, TimerCallbackType>::value
   >::type * = nullptr
 >
 class GenericTimer : public TimerBase
@@ -109,11 +123,15 @@ public:
 
   /// Default constructor.
   /**
+   * \param[in] clock The clock providing the current time.
    * \param[in] period The interval at which the timer fires.
    * \param[in] callback User-specified callback function.
    */
-  GenericTimer(std::chrono::nanoseconds period, FunctorT && callback)
-  : TimerBase(period), callback_(std::forward<FunctorT>(callback))
+  explicit GenericTimer(
+    Clock::SharedPtr clock, std::chrono::nanoseconds period, FunctorT && callback,
+    rclcpp::Context::SharedPtr context
+  )
+  : TimerBase(clock, period, context), callback_(std::forward<FunctorT>(callback))
   {
   }
 
@@ -125,7 +143,7 @@ public:
   }
 
   void
-  execute_callback()
+  execute_callback() override
   {
     rcl_ret_t ret = rcl_timer_call(timer_handle_.get());
     if (ret == RCL_RET_TIMER_CANCELED) {
@@ -162,10 +180,10 @@ public:
     callback_(*this);
   }
 
-  virtual bool
-  is_steady()
+  bool
+  is_steady() override
   {
-    return Clock::is_steady;
+    return clock_->get_clock_type() == RCL_STEADY_TIME;
   }
 
 protected:
@@ -174,8 +192,29 @@ protected:
   FunctorT callback_;
 };
 
-template<typename CallbackType>
-using WallTimer = GenericTimer<CallbackType, std::chrono::steady_clock>;
+template<
+  typename FunctorT,
+  typename std::enable_if<
+    rclcpp::function_traits::same_arguments<FunctorT, VoidCallbackType>::value ||
+    rclcpp::function_traits::same_arguments<FunctorT, TimerCallbackType>::value
+  >::type * = nullptr
+>
+class WallTimer : public GenericTimer<FunctorT>
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(WallTimer)
+
+  WallTimer(
+    std::chrono::nanoseconds period,
+    FunctorT && callback,
+    rclcpp::Context::SharedPtr context)
+  : GenericTimer<FunctorT>(
+      std::make_shared<Clock>(RCL_STEADY_TIME), period, std::move(callback), context)
+  {}
+
+protected:
+  RCLCPP_DISABLE_COPY(WallTimer)
+};
 
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/utilities.hpp

@@ -18,18 +18,14 @@
 #include <chrono>
 #include <functional>
 #include <limits>
+#include <string>
 #include <vector>
 
+#include "rclcpp/context.hpp"
+#include "rclcpp/init_options.hpp"
 #include "rclcpp/visibility_control.hpp"
 
-#include "rcl/guard_condition.h"
-#include "rcl/wait.h"
-
-#include "rmw/macros.h"
-#include "rmw/rmw.h"
-
 #ifdef ANDROID
-#include <string>
 #include <sstream>
 
 namespace std
@@ -48,93 +44,175 @@ namespace rclcpp
 {
 /// Initialize communications via the rmw implementation and set up a global signal handler.
 /**
- * \param[in] argc Number of arguments.
- * \param[in] argv Argument vector. Will eventually be used for passing options to rclcpp.
+ * Initializes the global context which is accessible via the function
+ * rclcpp::contexts::default_context::get_global_default_context().
+ * Also, installs the global signal handlers with the function
+ * rclcpp::install_signal_handlers().
+ *
+ * \sa rclcpp::Context::init() for more details on arguments and possible exceptions
  */
 RCLCPP_PUBLIC
 void
-init(int argc, char const * const argv[]);
+init(int argc, char const * const argv[], const InitOptions & init_options = InitOptions());
+
+/// Install the global signal handler for rclcpp.
+/**
+ * This function should only need to be run one time per process.
+ * It is implicitly run by rclcpp::init(), and therefore this function does not
+ * need to be run manually if rclcpp::init() has already been run.
+ *
+ * The signal handler will shutdown all initialized context.
+ * It will also interrupt any blocking functions in ROS allowing them react to
+ * any changes in the state of the system (like shutdown).
+ *
+ * This function is thread-safe.
+ *
+ * \return true if signal handler was installed by this function, false if already installed.
+ */
+RCLCPP_PUBLIC
+bool
+install_signal_handlers();
+
+/// Return true if the signal handlers are installed, otherwise false.
+RCLCPP_PUBLIC
+bool
+signal_handlers_installed();
+
+/// Uninstall the global signal handler for rclcpp.
+/**
+ * This function does not necessarily need to be called, but can be used to
+ * undo what rclcpp::install_signal_handlers() or rclcpp::init() do with
+ * respect to signal handling.
+ * If you choose to use it, this function only needs to be run one time.
+ * It is implicitly run by rclcpp::shutdown(), and therefore this function does
+ * not need to be run manually if rclcpp::shutdown() has already been run.
+ *
+ * This function is thread-safe.
+ *
+ * \return true if signal handler was uninstalled by this function, false if was not installed.
+ */
+RCLCPP_PUBLIC
+bool
+uninstall_signal_handlers();
 
 /// Initialize communications via the rmw implementation and set up a global signal handler.
 /**
  * Additionally removes ROS-specific arguments from the argument vector.
- * \param[in] argc Number of arguments.
- * \param[in] argv Argument vector.
+ *
+ * \sa rclcpp::Context::init() for more details on arguments and possible exceptions
  * \returns Members of the argument vector that are not ROS arguments.
+ * \throws anything remove_ros_arguments can throw
  */
 RCLCPP_PUBLIC
 std::vector<std::string>
-init_and_remove_ros_arguments(int argc, char const * const argv[]);
+init_and_remove_ros_arguments(
+  int argc,
+  char const * const argv[],
+  const InitOptions & init_options = InitOptions());
 
 /// Remove ROS-specific arguments from argument vector.
 /**
  * Some arguments may not have been intended as ROS arguments.
- * This function populates a the aruments in a vector.
+ * This function populates the arguments in a vector.
  * Since the first argument is always assumed to be a process name, the vector
  * will always contain the process name.
  *
  * \param[in] argc Number of arguments.
  * \param[in] argv Argument vector.
  * \returns Members of the argument vector that are not ROS arguments.
+ * \throws anything throw_from_rcl_error can throw
+ * \throws rclcpp::exceptions::RCLErrorBase if the parsing fails
  */
 RCLCPP_PUBLIC
 std::vector<std::string>
 remove_ros_arguments(int argc, char const * const argv[]);
 
 /// Check rclcpp's status.
-/** \return True if SIGINT hasn't fired yet, false otherwise. */
+/**
+ * This may return false for a context which has been shutdown, or for a
+ * context that was shutdown due to SIGINT being received by the rclcpp signal
+ * handler.
+ *
+ * If nullptr is given for the context, then the global context is used, i.e.
+ * the context initialized by rclcpp::init().
+ *
+ * \param[in] context Check for shutdown of this Context.
+ * \return true if shutdown has been called, false otherwise
+ */
 RCLCPP_PUBLIC
 bool
-ok();
+ok(rclcpp::Context::SharedPtr context = nullptr);
 
-/// Notify the signal handler and rmw that rclcpp is shutting down.
-RCLCPP_PUBLIC
-void
-shutdown();
-
-/// Register a function to be called when shutdown is called.
-/** Calling the callbacks is the last thing shutdown() does. */
-RCLCPP_PUBLIC
-void
-on_shutdown(std::function<void(void)> callback);
-
-/// Get a handle to the rmw guard condition that manages the signal handler.
+/// Return true if init() has already been called for the given context.
 /**
- * The first time that this function is called for a given wait set a new guard
- * condition will be created and returned; thereafter the same guard condition
- * will be returned for the same wait set. This mechanism is designed to ensure
- * that the same guard condition is not reused across wait sets (e.g., when
- * using multiple executors in the same process). Will throw an exception if
- * initialization of the guard condition fails.
- * \param wait_set Pointer to the rcl_wait_set_t that will be using the
- * resulting guard condition.
- * \return Pointer to the guard condition.
+ * If nullptr is given for the context, then the global context is used, i.e.
+ * the context initialized by rclcpp::init().
+ *
+ * Deprecated, as it is no longer different from rcl_ok().
+ *
+ * \param[in] context Check for initialization of this Context.
+ * \return true if the context is initialized, and false otherwise
  */
 RCLCPP_PUBLIC
-rcl_guard_condition_t *
-get_sigint_guard_condition(rcl_wait_set_t * wait_set);
+bool
+is_initialized(rclcpp::Context::SharedPtr context = nullptr);
 
-/// Release the previously allocated guard condition that manages the signal handler.
+/// Shutdown rclcpp context, invalidating it for derived entities.
 /**
- * If you previously called get_sigint_guard_condition() for a given wait set
- * to get a sigint guard condition, then you should call release_sigint_guard_condition()
- * when you're done, to free that condition.  Will throw an exception if
- * get_sigint_guard_condition() wasn't previously called for the given wait set.
- * \param wait_set Pointer to the rcl_wait_set_t that was using the
- * resulting guard condition.
+ * If nullptr is given for the context, then the global context is used, i.e.
+ * the context initialized by rclcpp::init().
+ *
+ * If the global context is used, then the signal handlers are also uninstalled.
+ *
+ * This will also cause the "on_shutdown" callbacks to be called.
+ *
+ * \sa rclcpp::Context::shutdown()
+ * \param[in] context to be shutdown
+ * \return true if shutdown was successful, false if context was already shutdown
+ */
+RCLCPP_PUBLIC
+bool
+shutdown(
+  rclcpp::Context::SharedPtr context = nullptr,
+  const std::string & reason = "user called rclcpp::shutdown()");
+
+/// Register a function to be called when shutdown is called on the context.
+/**
+ * If nullptr is given for the context, then the global context is used, i.e.
+ * the context initialized by rclcpp::init().
+ *
+ * These callbacks are called when the associated Context is shutdown with the
+ * Context::shutdown() method.
+ * When shutdown by the SIGINT handler, shutdown, and therefore these callbacks,
+ * is called asynchronously from the dedicated signal handling thread, at some
+ * point after the SIGINT signal is received.
+ *
+ * \sa rclcpp::Context::on_shutdown()
+ * \param[in] callback to be called when the given context is shutdown
+ * \param[in] context with which to associate the context
  */
 RCLCPP_PUBLIC
 void
-release_sigint_guard_condition(rcl_wait_set_t * wait_set);
+on_shutdown(std::function<void()> callback, rclcpp::Context::SharedPtr context = nullptr);
 
 /// Use the global condition variable to block for the specified amount of time.
 /**
+ * This function can be interrupted early if the associated context becomes
+ * invalid due to shutdown() or the signal handler.
+ * \sa rclcpp::Context::sleep_for
+ *
+ * If nullptr is given for the context, then the global context is used, i.e.
+ * the context initialized by rclcpp::init().
+ *
  * \param[in] nanoseconds A std::chrono::duration representing how long to sleep for.
- * \return True if the condition variable did not timeout.
+ * \param[in] context which may interrupt this sleep
+ * \return true if the condition variable did not timeout.
  */
 RCLCPP_PUBLIC
 bool
-sleep_for(const std::chrono::nanoseconds & nanoseconds);
+sleep_for(
+  const std::chrono::nanoseconds & nanoseconds,
+  rclcpp::Context::SharedPtr context = nullptr);
 
 /// Safely check if addition will overflow.
 /**
@@ -204,6 +282,26 @@ sub_will_underflow(const T x, const T y)
   return (y > 0) && (x < (std::numeric_limits<T>::min() + y));
 }
 
+/// Return the given string.
+/**
+ * This function is overloaded to transform any string to C-style string.
+ *
+ * \param[in] string_in is the string to be returned
+ * \return the given string
+ */
+RCLCPP_PUBLIC
+const char *
+get_c_string(const char * string_in);
+
+/// Return the C string from the given std::string.
+/**
+ * \param[in] string_in is a std::string
+ * \return the C string from the std::string
+ */
+RCLCPP_PUBLIC
+const char *
+get_c_string(const std::string & string_in);
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__UTILITIES_HPP_

rclcpp/include/rclcpp/waitable.hpp

@@ -0,0 +1,153 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAITABLE_HPP_
+#define RCLCPP__WAITABLE_HPP_
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcl/wait.h"
+
+namespace rclcpp
+{
+
+class Waitable
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Waitable)
+
+  /// Get the number of ready subscriptions
+  /**
+   * Returns a value of 0 by default.
+   * This should be overridden if the Waitable contains one or more subscriptions.
+   * \return The number of subscriptions associated with the Waitable.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  get_number_of_ready_subscriptions();
+
+  /// Get the number of ready timers
+  /**
+   * Returns a value of 0 by default.
+   * This should be overridden if the Waitable contains one or more timers.
+   * \return The number of timers associated with the Waitable.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  get_number_of_ready_timers();
+
+  /// Get the number of ready clients
+  /**
+   * Returns a value of 0 by default.
+   * This should be overridden if the Waitable contains one or more clients.
+   * \return The number of clients associated with the Waitable.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  get_number_of_ready_clients();
+
+  /// Get the number of ready events
+  /**
+   * Returns a value of 0 by default.
+   * This should be overridden if the Waitable contains one or more events.
+   * \return The number of events associated with the Waitable.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  get_number_of_ready_events();
+
+  /// Get the number of ready services
+  /**
+   * Returns a value of 0 by default.
+   * This should be overridden if the Waitable contains one or more services.
+   * \return The number of services associated with the Waitable.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  get_number_of_ready_services();
+
+  /// Get the number of ready guard_conditions
+  /**
+   * Returns a value of 0 by default.
+   * This should be overridden if the Waitable contains one or more guard_conditions.
+   * \return The number of guard_conditions associated with the Waitable.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  get_number_of_ready_guard_conditions();
+
+  // TODO(jacobperron): smart pointer?
+  /// Add the Waitable to a wait set.
+  /**
+   * \param[in] wait_set A handle to the wait set to add the Waitable to.
+   * \return `true` if the Waitable is added successfully, `false` otherwise.
+   * \throws rclcpp::execptions::RCLError from rcl_wait_set_add_*()
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  add_to_wait_set(rcl_wait_set_t * wait_set) = 0;
+
+  /// Check if the Waitable is ready.
+  /**
+   * The input wait set should be the same that was used in a previously call to
+   * `add_to_wait_set()`.
+   * The wait set should also have been previously waited on with `rcl_wait()`.
+   *
+   * \param[in] wait_set A handle to the wait set the Waitable was previously added to
+   *   and that has been waited on.
+   * \return `true` if the Waitable is ready, `false` otherwise.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  is_ready(rcl_wait_set_t *) = 0;
+
+  /// Execute any entities of the Waitable that are ready.
+  /**
+   * Before calling this method, the Waitable should be added to a wait set,
+   * waited on, and then updated.
+   *
+   * Example usage:
+   *
+   * ```
+   * // ... create a wait set and a Waitable
+   * // Add the Waitable to the wait set
+   * bool add_ret = waitable.add_to_wait_set(wait_set);
+   * // ... error handling
+   * // Wait
+   * rcl_ret_t wait_ret = rcl_wait(wait_set);
+   * // ... error handling
+   * // Update the Waitable
+   * waitable.update(wait_set);
+   * // Execute any entities of the Waitable that may be ready
+   * waitable.execute();
+   * ```
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  execute() = 0;
+};  // class Waitable
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAITABLE_HPP_

rclcpp/package.xml

@@ -2,7 +2,7 @@
 <?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.5.0</version>
+  <version>0.7.2</version>
   <description>The ROS client library in C++.</description>
   <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
   <license>Apache License 2.0</license>

rclcpp/resource/interface_traits.hpp.em

@@ -0,0 +1,77 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__INTERFACE_TRAITS_HPP_
+#define RCLCPP__NODE_INTERFACES__INTERFACE_TRAITS_HPP_
+
+#include <functional>
+#include <type_traits>
+
+@{
+node_interfaces = [
+  'node_base_interface',
+  'node_clock_interface',
+  'node_graph_interface',
+  'node_logging_interface',
+  'node_parameters_interface',
+  'node_services_interface',
+  'node_time_source_interface',
+  'node_timers_interface',
+  'node_topics_interface',
+  'node_waitables_interface',
+]
+
+node_interface_types = [
+  'NodeBaseInterface',
+  'NodeClockInterface',
+  'NodeGraphInterface',
+  'NodeLoggingInterface',
+  'NodeParametersInterface',
+  'NodeServicesInterface',
+  'NodeTimeSourceInterface',
+  'NodeTimersInterface',
+  'NodeTopicsInterface',
+  'NodeWaitablesInterface',
+]
+
+assert (len(node_interfaces) == len(node_interface_types))
+}@
+
+@[for interface_ in node_interfaces]@
+#include "rclcpp/node_interfaces/@(interface_).hpp"
+@[end for]@
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+@[for (interface_, type_) in zip(node_interfaces, node_interface_types)]@
+using @(interface_)_getter_t = std::shared_ptr<rclcpp::node_interfaces::@(type_)>;
+
+template<class T, typename = void>
+struct has_@(interface_) : std::false_type
+{};
+
+template<class T>
+struct has_@(interface_)<
+  T, typename std::enable_if<
+    std::is_same<
+      @(interface_)_getter_t, decltype(std::declval<T>().get_@(interface_)())>::value>::type> : std::true_type
+{};
+
+@[end for]@
+}  // namespace node_interfaces
+}  // namespace rclcpp
+#endif  // RCLCPP__NODE_INTERFACES__INTERFACE_TRAITS_HPP_

rclcpp/resource/logging.hpp.em

@@ -21,6 +21,7 @@
 
 #include "rclcpp/logger.hpp"
 #include "rcutils/logging_macros.h"
+#include "rclcpp/utilities.hpp"
 
 // These are used for compiling out logging macros lower than a minimum severity.
 #define RCLCPP_LOG_MIN_SEVERITY_DEBUG 0
@@ -30,6 +31,9 @@
 #define RCLCPP_LOG_MIN_SEVERITY_FATAL 4
 #define RCLCPP_LOG_MIN_SEVERITY_NONE 5
 
+#define RCLCPP_FIRST_ARG(N, ...) N
+#define RCLCPP_ALL_BUT_FIRST_ARGS(N, ...) __VA_ARGS__
+
 /**
  * \def RCLCPP_LOG_MIN_SEVERITY
  * Define RCLCPP_LOG_MIN_SEVERITY=RCLCPP_LOG_MIN_SEVERITY_[DEBUG|INFO|WARN|ERROR|FATAL]
@@ -67,6 +71,9 @@ def is_supported_feature_combination(feature_combination):
 #else
 @[ for feature_combination in [fc for fc in feature_combinations if is_supported_feature_combination(fc)]]@
 @{suffix = get_suffix_from_features(feature_combination)}@
+// The RCLCPP_@(severity)@(suffix) macro is surrounded by do { .. } while (0)
+// to implement the standard C macro idiom to make the macro safe in all
+// contexts; see http://c-faq.com/cpp/multistmt.html for more information.
 /**
  * \def RCLCPP_@(severity)@(suffix)
  * Log a message with severity @(severity)@
@@ -82,19 +89,24 @@ def is_supported_feature_combination(feature_combination):
 @[ for param_name, doc_line in feature_combinations[feature_combination].params.items()]@
  * \param @(param_name) @(doc_line)
 @[ end for]@
- * \param ... The format string, followed by the variable arguments for the format string
+ * \param ... The format string, followed by the variable arguments for the format string.
+ * It also accepts a single argument of type std::string.
  */
 #define RCLCPP_@(severity)@(suffix)(logger, @(''.join([p + ', ' for p in get_macro_parameters(feature_combination).keys()]))...) \
-  static_assert( \
-    ::std::is_same<std::remove_reference<decltype(logger)>::type, ::rclcpp::Logger>::value, \
-    "First argument to logging macros must be an rclcpp::Logger"); \
-  RCUTILS_LOG_@(severity)@(suffix)_NAMED( \
+  do { \
+    static_assert( \
+      ::std::is_same<typename std::remove_reference<typename std::remove_cv<decltype(logger)>::type>::type, \
+      typename ::rclcpp::Logger>::value, \
+      "First argument to logging macros must be an rclcpp::Logger"); \
+    RCUTILS_LOG_@(severity)@(suffix)_NAMED( \
 @{params = get_macro_parameters(feature_combination).keys()}@
 @[ if params]@
-@(''.join(['    ' + p + ', \\\n' for p in params]))@
+@(''.join(['      ' + p + ', \\\n' for p in params]))@
 @[ end if]@
-    logger.get_name(), \
-    __VA_ARGS__)
+      logger.get_name(), \
+      rclcpp::get_c_string(RCLCPP_FIRST_ARG(__VA_ARGS__, "")), \
+        RCLCPP_ALL_BUT_FIRST_ARGS(__VA_ARGS__,"")); \
+  } while (0)
 
 @[ end for]@
 #endif

rclcpp/src/rclcpp/callback_group.cpp

@@ -51,6 +51,13 @@ CallbackGroup::get_client_ptrs() const
   return client_ptrs_;
 }
 
+const std::vector<rclcpp::Waitable::WeakPtr> &
+CallbackGroup::get_waitable_ptrs() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  return waitable_ptrs_;
+}
+
 std::atomic_bool &
 CallbackGroup::can_be_taken_from()
 {
@@ -91,3 +98,23 @@ CallbackGroup::add_client(const rclcpp::ClientBase::SharedPtr client_ptr)
   std::lock_guard<std::mutex> lock(mutex_);
   client_ptrs_.push_back(client_ptr);
 }
+
+void
+CallbackGroup::add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  waitable_ptrs_.push_back(waitable_ptr);
+}
+
+void
+CallbackGroup::remove_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) noexcept
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  for (auto iter = waitable_ptrs_.begin(); iter != waitable_ptrs_.end(); ++iter) {
+    const auto shared_ptr = iter->lock();
+    if (shared_ptr.get() == waitable_ptr.get()) {
+      waitable_ptrs_.erase(iter);
+      break;
+    }
+  }
+}

rclcpp/src/rclcpp/client.cpp

@@ -37,18 +37,21 @@ ClientBase::ClientBase(
   rclcpp::node_interfaces::NodeBaseInterface * node_base,
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph)
 : node_graph_(node_graph),
-  node_handle_(node_base->get_shared_rcl_node_handle())
+  node_handle_(node_base->get_shared_rcl_node_handle()),
+  context_(node_base->get_context())
 {
   std::weak_ptr<rcl_node_t> weak_node_handle(node_handle_);
-  client_handle_ = std::shared_ptr<rcl_client_t>(
-    new rcl_client_t, [weak_node_handle](rcl_client_t * client)
+  rcl_client_t * new_rcl_client = new rcl_client_t;
+  *new_rcl_client = rcl_get_zero_initialized_client();
+  client_handle_.reset(
+    new_rcl_client, [weak_node_handle](rcl_client_t * client)
     {
       auto handle = weak_node_handle.lock();
       if (handle) {
         if (rcl_client_fini(client, handle.get()) != RCL_RET_OK) {
           RCLCPP_ERROR(
-            rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp"),
-            "Error in destruction of rcl client handle: %s", rcl_get_error_string_safe());
+            rclcpp::get_node_logger(handle.get()).get_child("rclcpp"),
+            "Error in destruction of rcl client handle: %s", rcl_get_error_string().str);
           rcl_reset_error();
         }
       } else {
@@ -59,7 +62,6 @@ ClientBase::ClientBase(
       }
       delete client;
     });
-  *client_handle_.get() = rcl_get_zero_initialized_client();
 }
 
 ClientBase::~ClientBase()
@@ -94,6 +96,13 @@ ClientBase::service_is_ready() const
     this->get_rcl_node_handle(),
     this->get_client_handle().get(),
     &is_ready);
+  if (RCL_RET_NODE_INVALID == ret) {
+    const rcl_node_t * node_handle = this->get_rcl_node_handle();
+    if (node_handle && !rcl_context_is_valid(node_handle->context)) {
+      // context is shutdown, do a soft failure
+      return false;
+    }
+  }
   if (ret != RCL_RET_OK) {
     throw_from_rcl_error(ret, "rcl_service_server_is_available failed");
   }
@@ -109,7 +118,6 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
   if (!node_ptr) {
     throw InvalidNodeError();
   }
-  auto event = node_ptr->get_graph_event();
   // check to see if the server is ready immediately
   if (this->service_is_ready()) {
     return true;
@@ -118,17 +126,20 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
     // check was non-blocking, return immediately
     return false;
   }
+  auto event = node_ptr->get_graph_event();
   // update the time even on the first loop to account for time spent in the first call
   // to this->server_is_ready()
-  std::chrono::nanoseconds time_to_wait = timeout - (std::chrono::steady_clock::now() - start);
-  if (timeout > std::chrono::nanoseconds(0) && time_to_wait < std::chrono::nanoseconds(0)) {
+  std::chrono::nanoseconds time_to_wait =
+    timeout > std::chrono::nanoseconds(0) ?
+    timeout - (std::chrono::steady_clock::now() - start) :
+    std::chrono::nanoseconds::max();
+  if (time_to_wait < std::chrono::nanoseconds(0)) {
     // Do not allow the time_to_wait to become negative when timeout was originally positive.
     // Setting time_to_wait to 0 will allow one non-blocking wait because of the do-while.
     time_to_wait = std::chrono::nanoseconds(0);
   }
-  // continue forever if timeout is negative, otherwise continue until out of time_to_wait
   do {
-    if (!rclcpp::ok()) {
+    if (!rclcpp::ok(this->context_)) {
       return false;
     }
     // Limit each wait to 100ms to workaround an issue specific to the Connext RMW implementation.
@@ -147,8 +158,11 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
       return true;
     }
     // server is not ready, loop if there is time left
-    time_to_wait = timeout - (std::chrono::steady_clock::now() - start);
-  } while (time_to_wait > std::chrono::nanoseconds(0) || timeout < std::chrono::nanoseconds(0));
+    if (timeout > std::chrono::nanoseconds(0)) {
+      time_to_wait = timeout - (std::chrono::steady_clock::now() - start);
+    }
+    // if timeout is negative, time_to_wait will never reach zero
+  } while (time_to_wait > std::chrono::nanoseconds(0));
   return false;  // timeout exceeded while waiting for the server to be ready
 }
 

rclcpp/src/rclcpp/clock.cpp

@@ -14,41 +14,17 @@
 
 #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_t pre_callback,
+  post_callback_t post_callback,
+  const rcl_jump_threshold_t & threshold)
 : pre_callback(pre_callback),
   post_callback(post_callback),
   notice_threshold(threshold)
@@ -59,8 +35,7 @@ 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");
+    exceptions::throw_from_rcl_error(ret, "could not get current time stamp");
   }
 }
 
@@ -77,10 +52,9 @@ Clock::now()
 {
   Time now(0, 0, rcl_clock_.type);
 
-  auto ret = rcl_clock_get_now(&rcl_clock_, &now.rcl_time_);
+  auto ret = rcl_clock_get_now(&rcl_clock_, &now.rcl_time_.nanoseconds);
   if (ret != RCL_RET_OK) {
-    rclcpp::exceptions::throw_from_rcl_error(
-      ret, "could not get current time stamp");
+    exceptions::throw_from_rcl_error(ret, "could not get current time stamp");
   }
 
   return now;
@@ -94,77 +68,76 @@ Clock::ros_time_is_active()
     return false;
   }
 
-  bool is_enabled;
+  bool is_enabled = false;
   auto ret = rcl_is_enabled_ros_time_override(&rcl_clock_, &is_enabled);
   if (ret != RCL_RET_OK) {
-    rclcpp::exceptions::throw_from_rcl_error(
+    exceptions::throw_from_rcl_error(
       ret, "Failed to check ros_time_override_status");
   }
   return is_enabled;
 }
 
+rcl_clock_t *
+Clock::get_clock_handle() noexcept
+{
+  return &rcl_clock_;
+}
+
 rcl_clock_type_t
-Clock::get_clock_type()
+Clock::get_clock_type() const noexcept
 {
   return rcl_clock_.type;
 }
 
-rclcpp::JumpHandler::SharedPtr
+void
+Clock::on_time_jump(
+  const struct rcl_time_jump_t * time_jump,
+  bool before_jump,
+  void * user_data)
+{
+  const auto * handler = static_cast<JumpHandler *>(user_data);
+  if (nullptr == handler) {
+    return;
+  }
+  if (before_jump && handler->pre_callback) {
+    handler->pre_callback();
+  } else if (!before_jump && handler->post_callback) {
+    handler->post_callback(*time_jump);
+  }
+}
+
+JumpHandler::SharedPtr
 Clock::create_jump_callback(
-  std::function<void()> pre_callback,
-  std::function<void(const TimeJump &)> post_callback,
-  JumpThreshold & threshold)
+  JumpHandler::pre_callback_t pre_callback,
+  JumpHandler::post_callback_t post_callback,
+  const rcl_jump_threshold_t & threshold)
 {
-  // JumpHandler jump_callback;
-  auto jump_callback =
-    std::make_shared<rclcpp::JumpHandler>(pre_callback, post_callback, threshold);
-  {
-    std::lock_guard<std::mutex> guard(callback_list_mutex_);
-    active_jump_handlers_.push_back(jump_callback);
+  // Allocate a new jump handler
+  JumpHandler::UniquePtr handler(new JumpHandler(pre_callback, post_callback, threshold));
+  if (nullptr == handler) {
+    throw std::bad_alloc{};
   }
-  return jump_callback;
-}
 
-std::vector<JumpHandler::SharedPtr>
-Clock::get_triggered_callback_handlers(const TimeJump & jump)
-{
-  std::vector<JumpHandler::SharedPtr> callbacks;
-  std::lock_guard<std::mutex> guard(callback_list_mutex_);
-  active_jump_handlers_.erase(
-    std::remove_if(
-      active_jump_handlers_.begin(),
-      active_jump_handlers_.end(),
-      [&callbacks, &jump](const std::weak_ptr<JumpHandler> & wjcb) {
-        if (auto jcb = wjcb.lock()) {
-          if (jcb->notice_threshold.is_exceeded(jump)) {
-            callbacks.push_back(jcb);
-          }
-          return false;
-        }
-        // Lock failed so clear the weak pointer.
-        return true;
-      }),
-    active_jump_handlers_.end());
-  return callbacks;
-}
-
-void
-Clock::invoke_prejump_callbacks(
-  const std::vector<JumpHandler::SharedPtr> & callbacks)
-{
-  for (const auto cb : callbacks) {
-    cb->pre_callback();
+  // Try to add the jump callback to the clock
+  rcl_ret_t ret = rcl_clock_add_jump_callback(&rcl_clock_, threshold,
+      Clock::on_time_jump, handler.get());
+  if (RCL_RET_OK != ret) {
+    exceptions::throw_from_rcl_error(ret, "Failed to add time jump callback");
   }
-}
 
-void
-Clock::invoke_postjump_callbacks(
-  const std::vector<JumpHandler::SharedPtr> & callbacks,
-  const TimeJump & jump)
-{
-  for (auto cb : callbacks) {
-    cb->post_callback(jump);
-  }
+  // *INDENT-OFF*
+  // create shared_ptr that removes the callback automatically when all copies are destructed
+  // TODO(dorezyuk) UB, if the clock leaves scope before the JumpHandler
+  return JumpHandler::SharedPtr(handler.release(), [this](JumpHandler * handler) noexcept {
+    rcl_ret_t ret = rcl_clock_remove_jump_callback(&rcl_clock_, Clock::on_time_jump,
+        handler);
+    delete handler;
+    handler = NULL;
+    if (RCL_RET_OK != ret) {
+      RCUTILS_LOG_ERROR("Failed to remove time jump callback");
+    }
+  });
+  // *INDENT-ON*
 }
 
 }  // namespace rclcpp

rclcpp/src/rclcpp/context.cpp

@@ -14,6 +14,296 @@
 
 #include "rclcpp/context.hpp"
 
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+#include "rcl/init.h"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/logging.hpp"
+#include "rmw/impl/cpp/demangle.hpp"
+
+/// Mutex to protect initialized contexts.
+static std::mutex g_contexts_mutex;
+/// Weak list of context to be shutdown by the signal handler.
+static std::vector<std::weak_ptr<rclcpp::Context>> g_contexts;
+
 using rclcpp::Context;
 
-Context::Context() {}
+Context::Context()
+: rcl_context_(nullptr), shutdown_reason_("") {}
+
+Context::~Context()
+{
+  // acquire the init lock to prevent race conditions with init and shutdown
+  // this will not prevent errors, but will maybe make them easier to reproduce
+  std::lock_guard<std::recursive_mutex> lock(init_mutex_);
+  try {
+    this->shutdown("context destructor was called while still not shutdown");
+    // at this point it is shutdown and cannot reinit
+    // clean_up will finalize the rcl context
+    this->clean_up();
+  } catch (const std::exception & exc) {
+    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "unhandled exception in ~Context(): %s", exc.what());
+  } catch (...) {
+    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "unhandled exception in ~Context()");
+  }
+}
+
+RCLCPP_LOCAL
+void
+__delete_context(rcl_context_t * context)
+{
+  if (context) {
+    if (rcl_context_is_valid(context)) {
+      RCLCPP_ERROR(
+        rclcpp::get_logger("rclcpp"), "rcl context unexpectedly not shutdown during cleanup");
+    } else {
+      // if context pointer is not null and is shutdown, then it's ready for fini
+      rcl_ret_t ret = rcl_context_fini(context);
+      if (RCL_RET_OK != ret) {
+        RCLCPP_ERROR(
+          rclcpp::get_logger("rclcpp"),
+          "failed to finalize context: %s", rcl_get_error_string().str);
+        rcl_reset_error();
+      }
+    }
+    delete context;
+  }
+}
+
+void
+Context::init(
+  int argc,
+  char const * const argv[],
+  const rclcpp::InitOptions & init_options)
+{
+  std::lock_guard<std::recursive_mutex> init_lock(init_mutex_);
+  if (this->is_valid()) {
+    throw rclcpp::ContextAlreadyInitialized();
+  }
+  this->clean_up();
+  rcl_context_.reset(new rcl_context_t, __delete_context);
+  *rcl_context_.get() = rcl_get_zero_initialized_context();
+  rcl_ret_t ret = rcl_init(argc, argv, init_options.get_rcl_init_options(), rcl_context_.get());
+  if (RCL_RET_OK != ret) {
+    rcl_context_.reset();
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl");
+  }
+  init_options_ = init_options;
+
+  std::lock_guard<std::mutex> lock(g_contexts_mutex);
+  g_contexts.push_back(this->shared_from_this());
+}
+
+bool
+Context::is_valid() const
+{
+  // Take a local copy of the shared pointer to avoid it getting nulled under our feet.
+  auto local_rcl_context = rcl_context_;
+  if (!local_rcl_context) {
+    return false;
+  }
+  return rcl_context_is_valid(local_rcl_context.get());
+}
+
+const rclcpp::InitOptions &
+Context::get_init_options() const
+{
+  return init_options_;
+}
+
+rclcpp::InitOptions
+Context::get_init_options()
+{
+  return init_options_;
+}
+
+std::string
+Context::shutdown_reason()
+{
+  std::lock_guard<std::recursive_mutex> lock(init_mutex_);
+  return shutdown_reason_;
+}
+
+bool
+Context::shutdown(const std::string & reason)
+{
+  // prevent races
+  std::lock_guard<std::recursive_mutex> init_lock(init_mutex_);
+  // ensure validity
+  if (!this->is_valid()) {
+    // if it is not valid, then it cannot be shutdown
+    return false;
+  }
+  // rcl shutdown
+  rcl_ret_t ret = rcl_shutdown(rcl_context_.get());
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+  // set shutdown reason
+  shutdown_reason_ = reason;
+  // call each shutdown callback
+  for (const auto & callback : on_shutdown_callbacks_) {
+    callback();
+  }
+  // interrupt all blocking sleep_for() and all blocking executors or wait sets
+  this->interrupt_all_sleep_for();
+  this->interrupt_all_wait_sets();
+  // remove self from the global contexts
+  std::lock_guard<std::mutex> context_lock(g_contexts_mutex);
+  for (auto it = g_contexts.begin(); it != g_contexts.end(); ) {
+    auto shared_context = it->lock();
+    if (shared_context.get() == this) {
+      it = g_contexts.erase(it);
+      break;
+    } else {
+      ++it;
+    }
+  }
+  return true;
+}
+
+rclcpp::Context::OnShutdownCallback
+Context::on_shutdown(OnShutdownCallback callback)
+{
+  on_shutdown_callbacks_.push_back(callback);
+  return callback;
+}
+
+const std::vector<rclcpp::Context::OnShutdownCallback> &
+Context::get_on_shutdown_callbacks() const
+{
+  return on_shutdown_callbacks_;
+}
+
+std::vector<rclcpp::Context::OnShutdownCallback> &
+Context::get_on_shutdown_callbacks()
+{
+  return on_shutdown_callbacks_;
+}
+
+std::shared_ptr<rcl_context_t>
+Context::get_rcl_context()
+{
+  return rcl_context_;
+}
+
+bool
+Context::sleep_for(const std::chrono::nanoseconds & nanoseconds)
+{
+  std::chrono::nanoseconds time_left = nanoseconds;
+  {
+    std::unique_lock<std::mutex> lock(interrupt_mutex_);
+    auto start = std::chrono::steady_clock::now();
+    // this will release the lock while waiting
+    interrupt_condition_variable_.wait_for(lock, nanoseconds);
+    time_left -= std::chrono::steady_clock::now() - start;
+  }
+  if (time_left > std::chrono::nanoseconds::zero() && this->is_valid()) {
+    return sleep_for(time_left);
+  }
+  // Return true if the timeout elapsed successfully, otherwise false.
+  return this->is_valid();
+}
+
+void
+Context::interrupt_all_sleep_for()
+{
+  interrupt_condition_variable_.notify_all();
+}
+
+rcl_guard_condition_t *
+Context::get_interrupt_guard_condition(rcl_wait_set_t * wait_set)
+{
+  std::lock_guard<std::mutex> lock(interrupt_guard_cond_handles_mutex_);
+  auto kv = interrupt_guard_cond_handles_.find(wait_set);
+  if (kv != interrupt_guard_cond_handles_.end()) {
+    return &kv->second;
+  } else {
+    rcl_guard_condition_t handle = rcl_get_zero_initialized_guard_condition();
+    rcl_guard_condition_options_t options = rcl_guard_condition_get_default_options();
+    auto ret = rcl_guard_condition_init(&handle, this->get_rcl_context().get(), options);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't initialize guard condition");
+    }
+    interrupt_guard_cond_handles_.emplace(wait_set, handle);
+    return &interrupt_guard_cond_handles_[wait_set];
+  }
+}
+
+void
+Context::release_interrupt_guard_condition(rcl_wait_set_t * wait_set)
+{
+  std::lock_guard<std::mutex> lock(interrupt_guard_cond_handles_mutex_);
+  auto kv = interrupt_guard_cond_handles_.find(wait_set);
+  if (kv != interrupt_guard_cond_handles_.end()) {
+    rcl_ret_t ret = rcl_guard_condition_fini(&kv->second);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to destroy sigint guard condition");
+    }
+    interrupt_guard_cond_handles_.erase(kv);
+  } else {
+    throw std::runtime_error("Tried to release sigint guard condition for nonexistent wait set");
+  }
+}
+
+void
+Context::release_interrupt_guard_condition(
+  rcl_wait_set_t * wait_set,
+  const std::nothrow_t &) noexcept
+{
+  try {
+    this->release_interrupt_guard_condition(wait_set);
+  } catch (const std::exception & exc) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("rclcpp"),
+      "caught %s exception when releasing interrupt guard condition: %s",
+      rmw::impl::cpp::demangle(exc).c_str(), exc.what());
+  } catch (...) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("rclcpp"),
+      "caught unknown exception when releasing interrupt guard condition");
+  }
+}
+
+void
+Context::interrupt_all_wait_sets()
+{
+  std::lock_guard<std::mutex> lock(interrupt_guard_cond_handles_mutex_);
+  for (auto & kv : interrupt_guard_cond_handles_) {
+    rcl_ret_t status = rcl_trigger_guard_condition(&(kv.second));
+    if (status != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "failed to trigger guard condition in Context::interrupt_all_wait_sets(): %s",
+        rcl_get_error_string().str);
+    }
+  }
+}
+
+void
+Context::clean_up()
+{
+  shutdown_reason_ = "";
+  rcl_context_.reset();
+}
+
+std::vector<Context::SharedPtr>
+rclcpp::get_contexts()
+{
+  std::lock_guard<std::mutex> lock(g_contexts_mutex);
+  std::vector<Context::SharedPtr> shared_contexts;
+  for (auto it = g_contexts.begin(); it != g_contexts.end(); /* noop */) {
+    auto context_ptr = it->lock();
+    if (!context_ptr) {
+      // remove invalid weak context pointers
+      it = g_contexts.erase(it);
+    } else {
+      ++it;
+      shared_contexts.push_back(context_ptr);
+    }
+  }
+  return shared_contexts;
+}

rclcpp/src/rclcpp/duration.cpp

@@ -65,10 +65,6 @@ Duration::Duration(const rcl_duration_t & duration)
   // noop
 }
 
-Duration::~Duration()
-{
-}
-
 Duration::operator builtin_interfaces::msg::Duration() const
 {
   builtin_interfaces::msg::Duration msg_duration;
@@ -88,9 +84,6 @@ Duration::operator=(const Duration & rhs)
 Duration &
 Duration::operator=(const builtin_interfaces::msg::Duration & duration_msg)
 {
-  if (duration_msg.sec < 0) {
-    throw std::runtime_error("cannot store a negative duration point in rclcpp::Duration");
-  }
   rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast<int64_t>(duration_msg.sec));
   rcl_duration_.nanoseconds += duration_msg.nanosec;
   return *this;
@@ -217,4 +210,27 @@ Duration::nanoseconds() const
   return rcl_duration_.nanoseconds;
 }
 
+Duration
+Duration::max()
+{
+  return Duration(std::numeric_limits<int32_t>::max(), 999999999);
+}
+
+double
+Duration::seconds() const
+{
+  return std::chrono::duration<double>(std::chrono::nanoseconds(rcl_duration_.nanoseconds)).count();
+}
+
+rmw_time_t
+Duration::to_rmw_time() const
+{
+  // reuse conversion logic from msg creation
+  builtin_interfaces::msg::Duration msg = *this;
+  rmw_time_t result;
+  result.sec = msg.sec;
+  result.nsec = msg.nanosec;
+  return result;
+}
+
 }  // namespace rclcpp

rclcpp/src/rclcpp/exceptions.cpp

@@ -39,12 +39,12 @@ NameValidationError::format_error(
   return msg;
 }
 
-void
-throw_from_rcl_error(
+std::exception_ptr
+from_rcl_error(
   rcl_ret_t ret,
   const std::string & prefix,
   const rcl_error_state_t * error_state,
-  void (*reset_error)())
+  void (* reset_error)())
 {
   if (RCL_RET_OK == ret) {
     throw std::invalid_argument("ret is RCL_RET_OK");
@@ -55,9 +55,9 @@ throw_from_rcl_error(
   if (!error_state) {
     throw std::runtime_error("rcl error state is not set");
   }
-  std::string formated_prefix = prefix;
+  std::string formatted_prefix = prefix;
   if (!prefix.empty()) {
-    formated_prefix += ": ";
+    formatted_prefix += ": ";
   }
   RCLErrorBase base_exc(ret, error_state);
   if (reset_error) {
@@ -65,17 +65,31 @@ throw_from_rcl_error(
   }
   switch (ret) {
     case RCL_RET_BAD_ALLOC:
-      throw RCLBadAlloc(base_exc);
+      return std::make_exception_ptr(RCLBadAlloc(base_exc));
     case RCL_RET_INVALID_ARGUMENT:
-      throw RCLInvalidArgument(base_exc, formated_prefix);
+      return std::make_exception_ptr(RCLInvalidArgument(base_exc, formatted_prefix));
     default:
-      throw RCLError(base_exc, formated_prefix);
+      return std::make_exception_ptr(RCLError(base_exc, formatted_prefix));
   }
 }
 
+void
+throw_from_rcl_error(
+  rcl_ret_t ret,
+  const std::string & prefix,
+  const rcl_error_state_t * error_state,
+  void (* reset_error)())
+{
+  // We expect this to either throw a standard error,
+  // or to generate an error pointer (which is caught
+  // in err, and immediately thrown)
+  auto err = from_rcl_error(ret, prefix, error_state, reset_error);
+  std::rethrow_exception(err);
+}
+
 RCLErrorBase::RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state)
 : ret(ret), message(error_state->message), file(error_state->file), line(error_state->line_number),
-  formatted_message(rcl_get_error_string_safe())
+  formatted_message(rcl_get_error_string().str)
 {}
 
 RCLError::RCLError(

rclcpp/src/rclcpp/executor.cpp

@@ -30,6 +30,7 @@
 
 #include "rcutils/logging_macros.h"
 
+using rclcpp::exceptions::throw_from_rcl_error;
 using rclcpp::executor::AnyExecutable;
 using rclcpp::executor::Executor;
 using rclcpp::executor::ExecutorArgs;
@@ -40,35 +41,39 @@ Executor::Executor(const ExecutorArgs & args)
   memory_strategy_(args.memory_strategy)
 {
   rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options();
-  if (rcl_guard_condition_init(
-      &interrupt_guard_condition_, guard_condition_options) != RCL_RET_OK)
-  {
-    throw std::runtime_error(
-            std::string("Failed to create interrupt guard condition in Executor constructor: ") +
-            rcl_get_error_string_safe());
+  rcl_ret_t ret = rcl_guard_condition_init(
+    &interrupt_guard_condition_, args.context->get_rcl_context().get(), guard_condition_options);
+  if (RCL_RET_OK != ret) {
+    throw_from_rcl_error(ret, "Failed to create interrupt guard condition in Executor constructor");
   }
 
   // The number of guard conditions is always at least 2: 1 for the ctrl-c guard cond,
   // and one for the executor's guard cond (interrupt_guard_condition_)
 
   // Put the global ctrl-c guard condition in
-  memory_strategy_->add_guard_condition(rclcpp::get_sigint_guard_condition(&wait_set_));
+  memory_strategy_->add_guard_condition(args.context->get_interrupt_guard_condition(&wait_set_));
 
   // Put the executor's guard condition in
   memory_strategy_->add_guard_condition(&interrupt_guard_condition_);
   rcl_allocator_t allocator = memory_strategy_->get_allocator();
 
-  if (rcl_wait_set_init(
-      &wait_set_, 0, 2, 0, 0, 0, allocator) != RCL_RET_OK)
-  {
+  // Store the context for later use.
+  context_ = args.context;
+
+  ret = rcl_wait_set_init(
+    &wait_set_,
+    0, 2, 0, 0, 0, 0,
+    context_->get_rcl_context().get(),
+    allocator);
+  if (RCL_RET_OK != ret) {
     RCUTILS_LOG_ERROR_NAMED(
       "rclcpp",
-      "failed to create wait set: %s", rcl_get_error_string_safe());
+      "failed to create wait set: %s", rcl_get_error_string().str);
     rcl_reset_error();
     if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
       RCUTILS_LOG_ERROR_NAMED(
         "rclcpp",
-        "failed to destroy guard condition: %s", rcl_get_error_string_safe());
+        "failed to destroy guard condition: %s", rcl_get_error_string().str);
       rcl_reset_error();
     }
     throw std::runtime_error("Failed to create wait set in Executor constructor");
@@ -91,20 +96,19 @@ Executor::~Executor()
   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());
+      "failed to destroy wait set: %s", rcl_get_error_string().str);
     rcl_reset_error();
   }
   // Finalize the interrupt guard condition.
   if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
     RCUTILS_LOG_ERROR_NAMED(
       "rclcpp",
-      "failed to destroy guard condition: %s", rcl_get_error_string_safe());
+      "failed to destroy guard condition: %s", rcl_get_error_string().str);
     rcl_reset_error();
   }
   // Remove and release the sigint guard condition
-  memory_strategy_->remove_guard_condition(
-    rclcpp::get_sigint_guard_condition(&wait_set_));
-  rclcpp::release_sigint_guard_condition(&wait_set_);
+  memory_strategy_->remove_guard_condition(context_->get_interrupt_guard_condition(&wait_set_));
+  context_->release_interrupt_guard_condition(&wait_set_, std::nothrow);
 }
 
 void
@@ -127,7 +131,7 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt
   if (notify) {
     // Interrupt waiting to handle new node
     if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-      throw std::runtime_error(rcl_get_error_string_safe());
+      throw std::runtime_error(rcl_get_error_string().str);
     }
   }
   // Add the node's notify condition to the guard condition handles
@@ -161,7 +165,7 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node
     // If the node was matched and removed, interrupt waiting
     if (node_removed) {
       if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-        throw std::runtime_error(rcl_get_error_string_safe());
+        throw std::runtime_error(rcl_get_error_string().str);
       }
     }
   }
@@ -200,13 +204,26 @@ Executor::spin_node_some(std::shared_ptr<rclcpp::Node> node)
 }
 
 void
-Executor::spin_some()
+Executor::spin_some(std::chrono::nanoseconds max_duration)
 {
+  auto start = std::chrono::steady_clock::now();
+  auto max_duration_not_elapsed = [max_duration, start]() {
+      if (std::chrono::nanoseconds(0) == max_duration) {
+        // told to spin forever if need be
+        return true;
+      } else if (std::chrono::steady_clock::now() - start < max_duration) {
+        // told to spin only for some maximum amount of time
+        return true;
+      }
+      // spun too long
+      return false;
+    };
+
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin_some() called while already spinning");
   }
   RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
-  while (spinning.load()) {
+  while (spinning.load() && max_duration_not_elapsed()) {
     AnyExecutable any_exec;
     if (get_next_executable(any_exec, std::chrono::milliseconds::zero())) {
       execute_any_executable(any_exec);
@@ -234,7 +251,7 @@ Executor::cancel()
 {
   spinning.store(false);
   if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-    throw std::runtime_error(rcl_get_error_string_safe());
+    throw std::runtime_error(rcl_get_error_string().str);
   }
 }
 
@@ -268,12 +285,15 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
   if (any_exec.client) {
     execute_client(any_exec.client);
   }
+  if (any_exec.waitable) {
+    any_exec.waitable->execute();
+  }
   // Reset the callback_group, regardless of type
   any_exec.callback_group->can_be_taken_from().store(true);
   // Wake the wait, because it may need to be recalculated or work that
   // was previously blocked is now available.
   if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-    throw std::runtime_error(rcl_get_error_string_safe());
+    throw std::runtime_error(rcl_get_error_string().str);
   }
 }
 
@@ -288,7 +308,7 @@ Executor::execute_subscription(
     auto serialized_msg = subscription->create_serialized_message();
     auto ret = rcl_take_serialized_message(
       subscription->get_subscription_handle().get(),
-      serialized_msg.get(), &message_info);
+      serialized_msg.get(), &message_info, nullptr);
     if (RCL_RET_OK == ret) {
       auto void_serialized_msg = std::static_pointer_cast<void>(serialized_msg);
       subscription->handle_message(void_serialized_msg, message_info);
@@ -296,7 +316,7 @@ Executor::execute_subscription(
       RCUTILS_LOG_ERROR_NAMED(
         "rclcpp",
         "take_serialized failed for subscription on topic '%s': %s",
-        subscription->get_topic_name(), rcl_get_error_string_safe());
+        subscription->get_topic_name(), rcl_get_error_string().str);
       rcl_reset_error();
     }
     subscription->return_serialized_message(serialized_msg);
@@ -304,14 +324,14 @@ Executor::execute_subscription(
     std::shared_ptr<void> message = subscription->create_message();
     auto ret = rcl_take(
       subscription->get_subscription_handle().get(),
-      message.get(), &message_info);
+      message.get(), &message_info, nullptr);
     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());
+        subscription->get_topic_name(), rcl_get_error_string().str);
       rcl_reset_error();
     }
     subscription->return_message(message);
@@ -327,7 +347,8 @@ Executor::execute_intra_process_subscription(
   rcl_ret_t status = rcl_take(
     subscription->get_intra_process_subscription_handle().get(),
     &ipm,
-    &message_info);
+    &message_info,
+    nullptr);
 
   if (status == RCL_RET_OK) {
     message_info.from_intra_process = true;
@@ -336,7 +357,7 @@ Executor::execute_intra_process_subscription(
     RCUTILS_LOG_ERROR_NAMED(
       "rclcpp",
       "take failed for intra process subscription on topic '%s': %s",
-      subscription->get_topic_name(), rcl_get_error_string_safe());
+      subscription->get_topic_name(), rcl_get_error_string().str);
     rcl_reset_error();
   }
 }
@@ -364,7 +385,7 @@ Executor::execute_service(
     RCUTILS_LOG_ERROR_NAMED(
       "rclcpp",
       "take request failed for server of service '%s': %s",
-      service->get_service_name(), rcl_get_error_string_safe());
+      service->get_service_name(), rcl_get_error_string().str);
     rcl_reset_error();
   }
 }
@@ -385,7 +406,7 @@ Executor::execute_client(
     RCUTILS_LOG_ERROR_NAMED(
       "rclcpp",
       "take response failed for client of service '%s': %s",
-      client->get_service_name(), rcl_get_error_string_safe());
+      client->get_service_name(), rcl_get_error_string().str);
     rcl_reset_error();
   }
 }
@@ -410,60 +431,19 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
     );
   }
   // 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_clear(&wait_set_) != RCL_RET_OK) {
+    throw std::runtime_error("Couldn't clear wait set");
   }
 
-  if (rcl_wait_set_resize_subscriptions(
-      &wait_set_, memory_strategy_->number_of_ready_subscriptions()) != RCL_RET_OK)
-  {
+  // The size of waitables are accounted for in size of the other entities
+  rcl_ret_t ret = rcl_wait_set_resize(
+    &wait_set_, memory_strategy_->number_of_ready_subscriptions(),
+    memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(),
+    memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(),
+    memory_strategy_->number_of_ready_events());
+  if (RCL_RET_OK != ret) {
     throw std::runtime_error(
-            std::string("Couldn't resize the number of subscriptions in wait set : ") +
-            rcl_get_error_string_safe());
-  }
-
-  if (rcl_wait_set_resize_services(
-      &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 wait set : ") +
-            rcl_get_error_string_safe());
-  }
-
-  if (rcl_wait_set_resize_clients(
-      &wait_set_, memory_strategy_->number_of_ready_clients()) != RCL_RET_OK)
-  {
-    throw std::runtime_error(
-            std::string("Couldn't resize the number of clients in wait set : ") +
-            rcl_get_error_string_safe());
-  }
-
-  if (rcl_wait_set_resize_guard_conditions(
-      &wait_set_, memory_strategy_->number_of_guard_conditions()) != RCL_RET_OK)
-  {
-    throw std::runtime_error(
-            std::string("Couldn't resize the number of guard_conditions in wait set : ") +
-            rcl_get_error_string_safe());
-  }
-
-  if (rcl_wait_set_resize_timers(
-      &wait_set_, memory_strategy_->number_of_ready_timers()) != RCL_RET_OK)
-  {
-    throw std::runtime_error(
-            std::string("Couldn't resize the number of timers in wait set : ") +
-            rcl_get_error_string_safe());
+            std::string("Couldn't resize the wait set : ") + rcl_get_error_string().str);
   }
 
   if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) {
@@ -579,6 +559,11 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable)
   if (any_executable.client) {
     return true;
   }
+  // Check the waitables to see if there are any that are ready
+  memory_strategy_->get_next_waitable(any_executable, weak_nodes_);
+  if (any_executable.waitable) {
+    return true;
+  }
   // If there is no ready executable, return a null ptr
   return false;
 }
@@ -628,7 +613,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/multi_threaded_executor.cpp

@@ -70,11 +70,11 @@ MultiThreadedExecutor::get_number_of_threads()
 void
 MultiThreadedExecutor::run(size_t)
 {
-  while (rclcpp::ok() && spinning.load()) {
+  while (rclcpp::ok(this->context_) && spinning.load()) {
     executor::AnyExecutable any_exec;
     {
       std::lock_guard<std::mutex> wait_lock(wait_mutex_);
-      if (!rclcpp::ok() || !spinning.load()) {
+      if (!rclcpp::ok(this->context_) || !spinning.load()) {
         return;
       }
       if (!get_next_executable(any_exec)) {
@@ -82,7 +82,6 @@ MultiThreadedExecutor::run(size_t)
       }
       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;
         }
@@ -96,11 +95,14 @@ MultiThreadedExecutor::run(size_t)
     execute_any_executable(any_exec);
 
     if (any_exec.timer) {
-      std::lock_guard<std::mutex> lock(scheduled_timers_mutex_);
+      std::lock_guard<std::mutex> wait_lock(wait_mutex_);
       auto it = scheduled_timers_.find(any_exec.timer);
       if (it != scheduled_timers_.end()) {
         scheduled_timers_.erase(it);
       }
     }
+    // Clear the callback_group to prevent the AnyExecutable destructor from
+    // resetting the callback group `can_be_taken_from`
+    any_exec.callback_group.reset();
   }
 }

rclcpp/src/rclcpp/executors/single_threaded_executor.cpp

@@ -30,7 +30,7 @@ SingleThreadedExecutor::spin()
     throw std::runtime_error("spin() called while already spinning");
   }
   RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
-  while (rclcpp::ok() && spinning.load()) {
+  while (rclcpp::ok(this->context_) && spinning.load()) {
     rclcpp::executor::AnyExecutable any_executable;
     if (get_next_executable(any_executable)) {
       execute_any_executable(any_executable);

rclcpp/src/rclcpp/expand_topic_or_service_name.cpp

@@ -51,14 +51,7 @@ rclcpp::expand_topic_or_service_name(
   }
   rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&substitutions_map);
   if (ret != RCL_RET_OK) {
-    rcutils_error_state_t error_state;
-    if (rcutils_error_state_copy(rcl_get_error_state(), &error_state) != RCUTILS_RET_OK) {
-      throw std::bad_alloc();
-    }
-    auto error_state_scope_exit = rclcpp::make_scope_exit(
-      [&error_state]() {
-        rcutils_error_state_fini(&error_state);
-      });
+    const rcutils_error_state_t * error_state = rcl_get_error_state();
     // finalize the string map before throwing
     rcutils_ret = rcutils_string_map_fini(&substitutions_map);
     if (rcutils_ret != RCUTILS_RET_OK) {
@@ -66,10 +59,10 @@ rclcpp::expand_topic_or_service_name(
         "rclcpp",
         "failed to fini string_map (%d) during error handling: %s",
         rcutils_ret,
-        rcutils_get_error_string_safe());
+        rcutils_get_error_string().str);
       rcutils_reset_error();
     }
-    throw_from_rcl_error(ret, "", &error_state);
+    throw_from_rcl_error(ret, "", error_state);
   }
 
   ret = rcl_expand_topic_name(

rclcpp/src/rclcpp/graph_listener.cpp

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

rclcpp/src/rclcpp/init_options.cpp

@@ -0,0 +1,86 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/init_options.hpp"
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/logging.hpp"
+
+namespace rclcpp
+{
+
+InitOptions::InitOptions(rcl_allocator_t allocator)
+: init_options_(new rcl_init_options_t)
+{
+  *init_options_ = rcl_get_zero_initialized_init_options();
+  rcl_ret_t ret = rcl_init_options_init(init_options_.get(), allocator);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialized rcl init options");
+  }
+}
+
+InitOptions::InitOptions(const rcl_init_options_t & init_options)
+: init_options_(new rcl_init_options_t)
+{
+  *init_options_ = rcl_get_zero_initialized_init_options();
+  rcl_ret_t ret = rcl_init_options_copy(&init_options, init_options_.get());
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to copy rcl init options");
+  }
+}
+
+InitOptions::InitOptions(const InitOptions & other)
+: InitOptions(*other.get_rcl_init_options())
+{}
+
+InitOptions &
+InitOptions::operator=(const InitOptions & other)
+{
+  if (this != &other) {
+    this->finalize_init_options();
+    rcl_ret_t ret = rcl_init_options_copy(other.get_rcl_init_options(), init_options_.get());
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to copy rcl init options");
+    }
+  }
+  return *this;
+}
+
+InitOptions::~InitOptions()
+{
+  this->finalize_init_options();
+}
+
+void
+InitOptions::finalize_init_options()
+{
+  if (init_options_) {
+    rcl_ret_t ret = rcl_init_options_fini(init_options_.get());
+    if (RCL_RET_OK != ret) {
+      RCLCPP_ERROR(
+        rclcpp::get_logger("rclcpp"),
+        "failed to finalize rcl init options: %s", rcl_get_error_string().str);
+      rcl_reset_error();
+    }
+    *init_options_ = rcl_get_zero_initialized_init_options();
+  }
+}
+
+const rcl_init_options_t *
+InitOptions::get_rcl_init_options() const
+{
+  return init_options_.get();
+}
+
+}  // namespace rclcpp

rclcpp/src/rclcpp/intra_process_manager.cpp

@@ -29,6 +29,21 @@ IntraProcessManager::IntraProcessManager(
 IntraProcessManager::~IntraProcessManager()
 {}
 
+uint64_t
+IntraProcessManager::add_publisher(
+  rclcpp::PublisherBase::SharedPtr publisher,
+  size_t buffer_size)
+{
+  auto id = IntraProcessManager::get_next_unique_id();
+  size_t size = buffer_size > 0 ? buffer_size : publisher->get_queue_size();
+  auto mrb = publisher->make_mapped_ring_buffer(size);
+  impl_->add_publisher(id, publisher, mrb, size);
+  if (!mrb) {
+    throw std::runtime_error("failed to create a mapped ring buffer");
+  }
+  return id;
+}
+
 uint64_t
 IntraProcessManager::add_subscription(
   rclcpp::SubscriptionBase::SharedPtr subscription)
@@ -56,6 +71,12 @@ IntraProcessManager::matches_any_publishers(const rmw_gid_t * id) const
   return impl_->matches_any_publishers(id);
 }
 
+size_t
+IntraProcessManager::get_subscription_count(uint64_t intra_process_publisher_id) const
+{
+  return impl_->get_subscription_count(intra_process_publisher_id);
+}
+
 uint64_t
 IntraProcessManager::get_next_unique_id()
 {

rclcpp/src/rclcpp/logger.cpp

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

rclcpp/src/rclcpp/memory_strategy.cpp

@@ -200,3 +200,29 @@ MemoryStrategy::get_group_by_client(
   }
   return nullptr;
 }
+
+rclcpp::callback_group::CallbackGroup::SharedPtr
+MemoryStrategy::get_group_by_waitable(
+  rclcpp::Waitable::SharedPtr waitable,
+  const WeakNodeVector & weak_nodes)
+{
+  for (auto & weak_node : weak_nodes) {
+    auto node = weak_node.lock();
+    if (!node) {
+      continue;
+    }
+    for (auto & weak_group : node->get_callback_groups()) {
+      auto group = weak_group.lock();
+      if (!group) {
+        continue;
+      }
+      for (auto & weak_waitable : group->get_waitable_ptrs()) {
+        auto group_waitable = weak_waitable.lock();
+        if (group_waitable && group_waitable == waitable) {
+          return group;
+        }
+      }
+    }
+  }
+  return nullptr;
+}

rclcpp/src/rclcpp/node.cpp

@@ -27,63 +27,171 @@
 #include "rclcpp/node_interfaces/node_clock.hpp"
 #include "rclcpp/node_interfaces/node_graph.hpp"
 #include "rclcpp/node_interfaces/node_logging.hpp"
+// When compiling this file, Windows produces a deprecation warning for the
+// deprecated function prototype of NodeParameters::register_param_change_callback().
+// Other compilers do not.
+#if defined(_WIN32)
+# pragma warning(push)
+# pragma warning(disable: 4996)
+#endif
 #include "rclcpp/node_interfaces/node_parameters.hpp"
+#if defined(_WIN32)
+# pragma warning(pop)
+#endif
 #include "rclcpp/node_interfaces/node_services.hpp"
+#include "rclcpp/node_interfaces/node_time_source.hpp"
 #include "rclcpp/node_interfaces/node_timers.hpp"
 #include "rclcpp/node_interfaces/node_topics.hpp"
+#include "rclcpp/node_interfaces/node_waitables.hpp"
+
+#include "rmw/validate_namespace.h"
 
 using rclcpp::Node;
+using rclcpp::NodeOptions;
 using rclcpp::exceptions::throw_from_rcl_error;
 
+RCLCPP_LOCAL
+std::string
+extend_sub_namespace(const std::string & existing_sub_namespace, const std::string & extension)
+{
+  // Assumption is that the existing_sub_namespace does not need checking
+  // because it would be checked already when it was set with this function.
+
+  // check if the new sub-namespace extension is absolute
+  if (extension.front() == '/') {
+    throw rclcpp::exceptions::NameValidationError(
+            "sub_namespace",
+            extension.c_str(),
+            "a sub-namespace should not have a leading /",
+            0);
+  }
+
+  std::string new_sub_namespace;
+  if (existing_sub_namespace.empty()) {
+    new_sub_namespace = extension;
+  } else {
+    new_sub_namespace = existing_sub_namespace + "/" + extension;
+  }
+
+  // remove any trailing `/` so that new extensions do no result in `//`
+  if (new_sub_namespace.back() == '/') {
+    new_sub_namespace = new_sub_namespace.substr(0, new_sub_namespace.size() - 1);
+  }
+
+  return new_sub_namespace;
+}
+
+RCLCPP_LOCAL
+std::string
+create_effective_namespace(const std::string & node_namespace, const std::string & sub_namespace)
+{
+  // Assumption is that both the node_namespace and sub_namespace are conforming
+  // and do not need trimming of `/` and other things, as they were validated
+  // in other functions already.
+
+  if (node_namespace.back() == '/') {
+    // this is the special case where node_namespace is just `/`
+    return node_namespace + sub_namespace;
+  } else {
+    return node_namespace + "/" + sub_namespace;
+  }
+}
+
 Node::Node(
   const std::string & node_name,
-  const std::string & namespace_,
-  bool use_intra_process_comms)
-: Node(
-    node_name,
-    namespace_,
-    rclcpp::contexts::default_context::get_global_default_context(),
-    {},
-    {},
-    true,
-    use_intra_process_comms,
-    true)
-{}
+  const NodeOptions & options)
+: Node(node_name, "", options)
+{
+}
 
 Node::Node(
   const std::string & node_name,
   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)
+  const NodeOptions & options)
 : node_base_(new rclcpp::node_interfaces::NodeBase(
-      node_name, namespace_, context, arguments, use_global_arguments)),
+      node_name,
+      namespace_,
+      options.context(),
+      *(options.get_rcl_node_options()),
+      options.use_intra_process_comms())),
   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_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_
+      node_services_,
+      node_logging_
     )),
-  use_intra_process_comms_(use_intra_process_comms)
+  node_parameters_(new rclcpp::node_interfaces::NodeParameters(
+      node_base_,
+      node_logging_,
+      node_topics_,
+      node_services_,
+      node_clock_,
+      options.initial_parameters(),
+      options.start_parameter_services(),
+      options.start_parameter_event_publisher(),
+      options.parameter_event_qos(),
+      options.parameter_event_publisher_options(),
+      options.allow_undeclared_parameters(),
+      options.automatically_declare_initial_parameters()
+    )),
+  node_time_source_(new rclcpp::node_interfaces::NodeTimeSource(
+      node_base_,
+      node_topics_,
+      node_graph_,
+      node_services_,
+      node_logging_,
+      node_clock_,
+      node_parameters_
+    )),
+  node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())),
+  node_options_(options),
+  sub_namespace_(""),
+  effective_namespace_(create_effective_namespace(this->get_namespace(), sub_namespace_))
 {
 }
 
+Node::Node(
+  const Node & other,
+  const std::string & sub_namespace)
+: node_base_(other.node_base_),
+  node_graph_(other.node_graph_),
+  node_logging_(other.node_logging_),
+  node_timers_(other.node_timers_),
+  node_topics_(other.node_topics_),
+  node_services_(other.node_services_),
+  node_clock_(other.node_clock_),
+  node_parameters_(other.node_parameters_),
+  node_options_(other.node_options_),
+  sub_namespace_(extend_sub_namespace(other.get_sub_namespace(), sub_namespace)),
+  effective_namespace_(create_effective_namespace(other.get_namespace(), sub_namespace_))
+{
+  // Validate new effective namespace.
+  int validation_result;
+  size_t invalid_index;
+  rmw_ret_t rmw_ret =
+    rmw_validate_namespace(effective_namespace_.c_str(), &validation_result, &invalid_index);
+
+  if (rmw_ret != RMW_RET_OK) {
+    if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
+      throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "failed to validate subnode namespace");
+    }
+    throw_from_rcl_error(RCL_RET_ERROR, "failed to validate subnode namespace");
+  }
+
+  if (validation_result != RMW_NAMESPACE_VALID) {
+    throw rclcpp::exceptions::InvalidNamespaceError(
+            effective_namespace_.c_str(),
+            rmw_namespace_validation_result_string(validation_result),
+            invalid_index);
+  }
+}
+
 Node::~Node()
 {}
 
@@ -99,6 +207,12 @@ Node::get_namespace() const
   return node_base_->get_namespace();
 }
 
+const char *
+Node::get_fully_qualified_name() const
+{
+  return node_base_->get_fully_qualified_name();
+}
+
 rclcpp::Logger
 Node::get_logger() const
 {
@@ -118,20 +232,57 @@ Node::group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group)
   return node_base_->callback_group_in_node(group);
 }
 
+const rclcpp::ParameterValue &
+Node::declare_parameter(
+  const std::string & name,
+  const rclcpp::ParameterValue & default_value,
+  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor)
+{
+  return this->node_parameters_->declare_parameter(name, default_value, parameter_descriptor);
+}
+
+void
+Node::undeclare_parameter(const std::string & name)
+{
+  this->node_parameters_->undeclare_parameter(name);
+}
+
+bool
+Node::has_parameter(const std::string & name) const
+{
+  return this->node_parameters_->has_parameter(name);
+}
+
+rcl_interfaces::msg::SetParametersResult
+Node::set_parameter(const rclcpp::Parameter & parameter)
+{
+  return this->set_parameters_atomically({parameter});
+}
+
 std::vector<rcl_interfaces::msg::SetParametersResult>
-Node::set_parameters(
-  const std::vector<rclcpp::Parameter> & parameters)
+Node::set_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> & parameters)
+Node::set_parameters_atomically(const std::vector<rclcpp::Parameter> & parameters)
 {
   return node_parameters_->set_parameters_atomically(parameters);
 }
 
+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 & parameter) const
+{
+  return node_parameters_->get_parameter(name, parameter);
+}
+
 std::vector<rclcpp::Parameter>
 Node::get_parameters(
   const std::vector<std::string> & names) const
@@ -139,40 +290,49 @@ Node::get_parameters(
   return node_parameters_->get_parameters(names);
 }
 
-rclcpp::Parameter
-Node::get_parameter(const std::string & name) const
+rcl_interfaces::msg::ParameterDescriptor
+Node::describe_parameter(const std::string & name) const
 {
-  return node_parameters_->get_parameter(name);
-}
-
-bool Node::get_parameter(
-  const std::string & name,
-  rclcpp::Parameter & parameter) const
-{
-  return node_parameters_->get_parameter(name, parameter);
+  auto result = node_parameters_->describe_parameters({name});
+  if (0 == result.size()) {
+    throw rclcpp::exceptions::ParameterNotDeclaredException(name);
+  }
+  if (result.size() > 1) {
+    throw std::runtime_error("number of described parameters unexpectedly more than one");
+  }
+  return result.front();
 }
 
 std::vector<rcl_interfaces::msg::ParameterDescriptor>
-Node::describe_parameters(
-  const std::vector<std::string> & names) const
+Node::describe_parameters(const std::vector<std::string> & names) const
 {
   return node_parameters_->describe_parameters(names);
 }
 
 std::vector<uint8_t>
-Node::get_parameter_types(
-  const std::vector<std::string> & names) const
+Node::get_parameter_types(const std::vector<std::string> & names) const
 {
   return node_parameters_->get_parameter_types(names);
 }
 
 rcl_interfaces::msg::ListParametersResult
-Node::list_parameters(
-  const std::vector<std::string> & prefixes, uint64_t depth) const
+Node::list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const
 {
   return node_parameters_->list_parameters(prefixes, depth);
 }
 
+rclcpp::Node::OnParametersSetCallbackType
+Node::set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback)
+{
+  return node_parameters_->set_on_parameters_set_callback(callback);
+}
+
+std::vector<std::string>
+Node::get_node_names() const
+{
+  return node_graph_->get_node_names();
+}
+
 std::map<std::string, std::vector<std::string>>
 Node::get_topic_names_and_types() const
 {
@@ -247,6 +407,18 @@ Node::get_node_graph_interface()
   return node_graph_;
 }
 
+rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr
+Node::get_node_logging_interface()
+{
+  return node_logging_;
+}
+
+rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr
+Node::get_node_time_source_interface()
+{
+  return node_time_source_;
+}
+
 rclcpp::node_interfaces::NodeTimersInterface::SharedPtr
 Node::get_node_timers_interface()
 {
@@ -270,3 +442,41 @@ Node::get_node_parameters_interface()
 {
   return node_parameters_;
 }
+
+rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr
+Node::get_node_waitables_interface()
+{
+  return node_waitables_;
+}
+
+const std::string &
+Node::get_sub_namespace() const
+{
+  return this->sub_namespace_;
+}
+
+const std::string &
+Node::get_effective_namespace() const
+{
+  return this->effective_namespace_;
+}
+
+Node::SharedPtr
+Node::create_sub_node(const std::string & sub_namespace)
+{
+  // Cannot use make_shared<Node>() here as it requires the constructor to be
+  // public, and this constructor is intentionally protected instead.
+  return std::shared_ptr<Node>(new Node(*this, sub_namespace));
+}
+
+const NodeOptions &
+Node::get_node_options() const
+{
+  return this->node_options_;
+}
+
+bool
+Node::assert_liveliness() const
+{
+  return node_base_->assert_liveliness();
+}

rclcpp/src/rclcpp/node_interfaces/node_base.cpp

@@ -22,8 +22,8 @@
 #include "rcl/arguments.h"
 #include "rclcpp/exceptions.hpp"
 #include "rcutils/logging_macros.h"
-#include "rmw/validate_node_name.h"
 #include "rmw/validate_namespace.h"
+#include "rmw/validate_node_name.h"
 
 using rclcpp::exceptions::throw_from_rcl_error;
 
@@ -33,9 +33,10 @@ NodeBase::NodeBase(
   const std::string & node_name,
   const std::string & namespace_,
   rclcpp::Context::SharedPtr context,
-  const std::vector<std::string> & arguments,
-  bool use_global_arguments)
+  const rcl_node_options_t & rcl_node_options,
+  bool use_intra_process_default)
 : context_(context),
+  use_intra_process_default_(use_intra_process_default),
   node_handle_(nullptr),
   default_callback_group_(nullptr),
   associated_with_executor_(false),
@@ -43,7 +44,8 @@ NodeBase::NodeBase(
 {
   // Setup the guard condition that is notified when changes occur in the graph.
   rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options();
-  rcl_ret_t ret = rcl_guard_condition_init(&notify_guard_condition_, guard_condition_options);
+  rcl_ret_t ret = rcl_guard_condition_init(
+    &notify_guard_condition_, context_->get_rcl_context().get(), guard_condition_options);
   if (ret != RCL_RET_OK) {
     throw_from_rcl_error(ret, "failed to create interrupt guard condition");
   }
@@ -54,77 +56,20 @@ NodeBase::NodeBase(
       if (rcl_guard_condition_fini(&notify_guard_condition_) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
-          "failed to destroy guard condition: %s", rcl_get_error_string_safe());
+          "failed to destroy guard condition: %s", rcl_get_error_string().str);
       }
     };
 
-  // Determine the domain id based on the options and the ROS_DOMAIN_ID env variable.
-  size_t domain_id = 0;
-  char * ros_domain_id = nullptr;
-  const char * env_var = "ROS_DOMAIN_ID";
-#ifndef _WIN32
-  ros_domain_id = getenv(env_var);
-#else
-  size_t ros_domain_id_size;
-  _dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var);
-#endif
-  if (ros_domain_id) {
-    uint32_t number = strtoul(ros_domain_id, NULL, 0);
-    if (number == (std::numeric_limits<uint32_t>::max)()) {
-      // Finalize the interrupt guard condition.
-      finalize_notify_guard_condition();
-#ifdef _WIN32
-      // free the ros_domain_id before throwing, if getenv was used on Windows
-      free(ros_domain_id);
-#endif
-
-      throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number");
-    }
-    domain_id = static_cast<size_t>(number);
-#ifdef _WIN32
-    free(ros_domain_id);
-#endif
-  }
-
   // Create the rcl node and store it in a shared_ptr with a custom destructor.
   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.get(), node_name.c_str(), namespace_.c_str(), &options);
+  ret = rcl_node_init(
+    rcl_node.get(),
+    node_name.c_str(), namespace_.c_str(),
+    context_->get_rcl_context().get(), &rcl_node_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
@@ -180,7 +125,7 @@ NodeBase::NodeBase(
       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());
+          "Error in destruction of rcl node handle: %s", rcl_get_error_string().str);
       }
       delete node;
     });
@@ -191,15 +136,6 @@ 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()
@@ -211,7 +147,7 @@ NodeBase::~NodeBase()
     if (rcl_guard_condition_fini(&notify_guard_condition_) != RCL_RET_OK) {
       RCUTILS_LOG_ERROR_NAMED(
         "rclcpp",
-        "failed to destroy guard condition: %s", rcl_get_error_string_safe());
+        "failed to destroy guard condition: %s", rcl_get_error_string().str);
     }
   }
 }
@@ -228,6 +164,12 @@ NodeBase::get_namespace() const
   return rcl_node_get_namespace(node_handle_.get());
 }
 
+const char *
+NodeBase::get_fully_qualified_name() const
+{
+  return rcl_node_get_fully_qualified_name(node_handle_.get());
+}
+
 rclcpp::Context::SharedPtr
 NodeBase::get_context()
 {
@@ -258,6 +200,12 @@ NodeBase::get_shared_rcl_node_handle() const
   return node_handle_;
 }
 
+bool
+NodeBase::assert_liveliness() const
+{
+  return RCL_RET_OK == rcl_node_assert_liveliness(get_rcl_node_handle());
+}
+
 rclcpp::callback_group::CallbackGroup::SharedPtr
 NodeBase::create_callback_group(rclcpp::callback_group::CallbackGroupType group_type)
 {
@@ -314,3 +262,9 @@ NodeBase::acquire_notify_guard_condition_lock() const
 {
   return std::unique_lock<std::recursive_mutex>(notify_guard_condition_mutex_);
 }
+
+bool
+NodeBase::get_use_intra_process_default() const
+{
+  return use_intra_process_default_;
+}

rclcpp/src/rclcpp/node_interfaces/node_clock.cpp

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

rclcpp/src/rclcpp/node_interfaces/node_graph.cpp

@@ -14,8 +14,10 @@
 
 #include "rclcpp/node_interfaces/node_graph.hpp"
 
+#include <algorithm>
 #include <map>
 #include <string>
+#include <utility>
 #include <vector>
 
 #include "rcl/graph.h"
@@ -29,7 +31,9 @@ using rclcpp::graph_listener::GraphListener;
 
 NodeGraph::NodeGraph(rclcpp::node_interfaces::NodeBaseInterface * node_base)
 : node_base_(node_base),
-  graph_listener_(node_base->get_context()->get_sub_context<GraphListener>()),
+  graph_listener_(
+    node_base->get_context()->get_sub_context<GraphListener>(node_base->get_context())
+  ),
   should_add_to_graph_listener_(true),
   graph_users_count_(0)
 {}
@@ -58,13 +62,13 @@ NodeGraph::get_topic_names_and_types(bool no_demangle) const
     &topic_names_and_types);
   if (ret != RCL_RET_OK) {
     auto error_msg = std::string("failed to get topic names and types: ") +
-      rcl_get_error_string_safe();
+      rcl_get_error_string().str;
     rcl_reset_error();
     if (rcl_names_and_types_fini(&topic_names_and_types) != RCL_RET_OK) {
       error_msg += std::string(", failed also to cleanup topic names and types, leaking memory: ") +
-        rcl_get_error_string_safe();
+        rcl_get_error_string().str;
     }
-    throw std::runtime_error(error_msg + rcl_get_error_string_safe());
+    throw std::runtime_error(error_msg + rcl_get_error_string().str);
   }
 
   std::map<std::string, std::vector<std::string>> topics_and_types;
@@ -79,7 +83,7 @@ NodeGraph::get_topic_names_and_types(bool no_demangle) const
   if (ret != RCL_RET_OK) {
     // *INDENT-OFF*
     throw std::runtime_error(
-      std::string("could not destroy topic names and types: ") + rcl_get_error_string_safe());
+      std::string("could not destroy topic names and types: ") + rcl_get_error_string().str);
     // *INDENT-ON*
   }
 
@@ -98,14 +102,14 @@ NodeGraph::get_service_names_and_types() const
     &service_names_and_types);
   if (ret != RCL_RET_OK) {
     auto error_msg = std::string("failed to get service names and types: ") +
-      rcl_get_error_string_safe();
+      rcl_get_error_string().str;
     rcl_reset_error();
     if (rcl_names_and_types_fini(&service_names_and_types) != RCL_RET_OK) {
       error_msg +=
         std::string(", failed also to cleanup service names and types, leaking memory: ") +
-        rcl_get_error_string_safe();
+        rcl_get_error_string().str;
     }
-    throw std::runtime_error(error_msg + rcl_get_error_string_safe());
+    throw std::runtime_error(error_msg + rcl_get_error_string().str);
   }
 
   std::map<std::string, std::vector<std::string>> services_and_types;
@@ -120,7 +124,7 @@ NodeGraph::get_service_names_and_types() const
   if (ret != RCL_RET_OK) {
     // *INDENT-OFF*
     throw std::runtime_error(
-      std::string("could not destroy service names and types: ") + rcl_get_error_string_safe());
+      std::string("could not destroy service names and types: ") + rcl_get_error_string().str);
     // *INDENT-ON*
   }
 
@@ -129,40 +133,90 @@ NodeGraph::get_service_names_and_types() const
 
 std::vector<std::string>
 NodeGraph::get_node_names() const
+{
+  std::vector<std::string> nodes;
+  auto names_and_namespaces = get_node_names_and_namespaces();
+
+  std::transform(names_and_namespaces.begin(),
+    names_and_namespaces.end(),
+    std::back_inserter(nodes),
+    [](std::pair<std::string, std::string> nns) {
+      std::string return_string;
+      if (nns.second.back() == '/') {
+        return_string = nns.second + nns.first;
+      } else {
+        return_string = nns.second + '/' + nns.first;
+      }
+      // Quick check to make sure that we start with a slash
+      // Since fully-qualified strings need to
+      if (return_string.front() != '/') {
+        return_string = "/" + return_string;
+      }
+      return return_string;
+    }
+  );
+  return nodes;
+}
+
+std::vector<std::pair<std::string, std::string>>
+NodeGraph::get_node_names_and_namespaces() const
 {
   rcutils_string_array_t node_names_c =
     rcutils_get_zero_initialized_string_array();
+  rcutils_string_array_t node_namespaces_c =
+    rcutils_get_zero_initialized_string_array();
 
   auto allocator = rcl_get_default_allocator();
   auto ret = rcl_get_node_names(
     node_base_->get_rcl_node_handle(),
     allocator,
-    &node_names_c);
+    &node_names_c,
+    &node_namespaces_c);
   if (ret != RCL_RET_OK) {
-    auto error_msg = std::string("failed to get node names: ") + rcl_get_error_string_safe();
+    auto error_msg = std::string("failed to get node names: ") + rcl_get_error_string().str;
     rcl_reset_error();
     if (rcutils_string_array_fini(&node_names_c) != RCUTILS_RET_OK) {
       error_msg += std::string(", failed also to cleanup node names, leaking memory: ") +
-        rcl_get_error_string_safe();
+        rcl_get_error_string().str;
+      rcl_reset_error();
+    }
+    if (rcutils_string_array_fini(&node_namespaces_c) != RCUTILS_RET_OK) {
+      error_msg += std::string(", failed also to cleanup node namespaces, leaking memory: ") +
+        rcl_get_error_string().str;
+      rcl_reset_error();
     }
     // TODO(karsten1987): Append rcutils_error_message once it's in master
     throw std::runtime_error(error_msg);
   }
 
-  std::vector<std::string> node_names(node_names_c.size);
+
+  std::vector<std::pair<std::string, std::string>> node_names;
+  node_names.reserve(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];
+    if (node_names_c.data[i] && node_namespaces_c.data[i]) {
+      node_names.emplace_back(node_names_c.data[i], node_namespaces_c.data[i]);
     }
   }
-  ret = rcutils_string_array_fini(&node_names_c);
-  if (ret != RCUTILS_RET_OK) {
+
+  std::string error;
+  rcl_ret_t ret_names = rcutils_string_array_fini(&node_names_c);
+  if (ret_names != RCUTILS_RET_OK) {
     // *INDENT-OFF*
     // TODO(karsten1987): Append rcutils_error_message once it's in master
-    throw std::runtime_error(
-      std::string("could not destroy node names: "));
+    error = "could not destroy node names";
     // *INDENT-ON*
   }
+  rcl_ret_t ret_ns = rcutils_string_array_fini(&node_namespaces_c);
+  if (ret_ns != RCUTILS_RET_OK) {
+    // *INDENT-OFF*
+    // TODO(karsten1987): Append rcutils_error_message once it's in master
+    error += ", could not destroy node namespaces";
+    // *INDENT-ON*
+  }
+
+  if (ret_names != RCUTILS_RET_OK || ret_ns != RCUTILS_RET_OK) {
+    throw std::runtime_error(error);
+  }
 
   return node_names;
 }
@@ -183,7 +237,7 @@ NodeGraph::count_publishers(const std::string & topic_name) const
   if (ret != RMW_RET_OK) {
     // *INDENT-OFF*
     throw std::runtime_error(
-      std::string("could not count publishers: ") + rmw_get_error_string_safe());
+      std::string("could not count publishers: ") + rmw_get_error_string().str);
     // *INDENT-ON*
   }
   return count;
@@ -205,7 +259,7 @@ NodeGraph::count_subscribers(const std::string & topic_name) const
   if (ret != RMW_RET_OK) {
     // *INDENT-OFF*
     throw std::runtime_error(
-      std::string("could not count subscribers: ") + rmw_get_error_string_safe());
+      std::string("could not count subscribers: ") + rmw_get_error_string().str);
     // *INDENT-ON*
   }
   return count;
@@ -300,8 +354,8 @@ NodeGraph::wait_for_graph_change(
       throw EventNotRegisteredError();
     }
   }
-  auto pred = [&event]() {
-      return event->check() || !rclcpp::ok();
+  auto pred = [&event, context = node_base_->get_context()]() {
+      return event->check() || !rclcpp::ok(context);
     };
   std::unique_lock<std::mutex> graph_lock(graph_mutex_);
   if (!pred()) {

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -12,12 +12,23 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+// When compiling this file, Windows produces a deprecation warning for the
+// deprecated function prototype of NodeParameters::register_param_change_callback().
+// Other compilers do not.
+#if defined(_WIN32)
+# pragma warning(push)
+# pragma warning(disable: 4996)
+#endif
 #include "rclcpp/node_interfaces/node_parameters.hpp"
+#if defined(_WIN32)
+# pragma warning(pop)
+#endif
 
 #include <rcl_yaml_param_parser/parser.h>
 
 #include <map>
 #include <memory>
+#include <sstream>
 #include <string>
 #include <utility>
 #include <vector>
@@ -33,28 +44,41 @@ using rclcpp::node_interfaces::NodeParameters;
 
 NodeParameters::NodeParameters(
   const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
-  const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+  const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
   const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
+  const rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
   const std::vector<rclcpp::Parameter> & initial_parameters,
-  bool use_intra_process,
-  bool start_parameter_services)
+  bool start_parameter_services,
+  bool start_parameter_event_publisher,
+  const rclcpp::QoS & parameter_event_qos,
+  const rclcpp::PublisherOptionsBase & parameter_event_publisher_options,
+  bool allow_undeclared_parameters,
+  bool automatically_declare_initial_parameters)
+: allow_undeclared_(allow_undeclared_parameters),
+  events_publisher_(nullptr),
+  node_logging_(node_logging),
+  node_clock_(node_clock)
 {
   using MessageT = rcl_interfaces::msg::ParameterEvent;
   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>();
+  rclcpp::PublisherOptionsWithAllocator<AllocatorT> publisher_options(
+    parameter_event_publisher_options);
+  publisher_options.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.get(),
-    "parameter_events",
-    rmw_qos_profile_parameter_events,
-    use_intra_process,
-    allocator);
+  if (start_parameter_event_publisher) {
+    events_publisher_ = rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
+      node_topics,
+      "parameter_events",
+      parameter_event_qos,
+      publisher_options);
+  }
 
   // Get the node options
   const rcl_node_t * node = node_base->get_rcl_node_handle();
@@ -63,7 +87,7 @@ NodeParameters::NodeParameters(
   }
   const rcl_node_options_t * options = rcl_node_get_options(node);
   if (nullptr == options) {
-    throw std::runtime_error("Need valid node options NodeParameters");
+    throw std::runtime_error("Need valid node options in NodeParameters");
   }
 
   // Get paths to yaml files containing initial parameter values
@@ -92,66 +116,59 @@ NodeParameters::NodeParameters(
 
   // global before local so that local overwrites global
   if (options->use_global_arguments) {
-    get_yaml_paths(rcl_get_global_arguments());
+    auto context_ptr = node_base->get_context()->get_rcl_context();
+    get_yaml_paths(&(context_ptr->global_arguments));
   }
   get_yaml_paths(&(options->arguments));
 
   // Get fully qualified node name post-remapping to use to find node's params in yaml files
-  const std::string node_name = node_base->get_name();
-  const std::string node_namespace = node_base->get_namespace();
-  if (0u == node_namespace.size() || 0u == node_name.size()) {
-    // Should never happen
-    throw std::runtime_error("Node name and namespace were not set");
-  }
-  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;
+  combined_name_ = node_base->get_fully_qualified_name();
 
   // TODO(sloretz) use rcl to parse yaml when circular dependency is solved
+  // See https://github.com/ros2/rcl/issues/252
   for (const std::string & yaml_path : yaml_paths) {
     rcl_params_t * yaml_params = rcl_yaml_node_struct_init(options->allocator);
     if (nullptr == yaml_params) {
       throw std::bad_alloc();
     }
     if (!rcl_parse_yaml_file(yaml_path.c_str(), yaml_params)) {
-      throw std::runtime_error("Failed to parse parameters " + yaml_path);
+      std::ostringstream ss;
+      ss << "Failed to parse parameters from file '" << yaml_path << "': " <<
+        rcl_get_error_string().str;
+      rcl_reset_error();
+      throw std::runtime_error(ss.str());
     }
 
     rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(yaml_params);
     rcl_yaml_node_struct_fini(yaml_params);
-    auto iter = initial_map.find(combined_name);
+    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_parameter_values_[param.get_name()] =
+        rclcpp::ParameterValue(param.get_value_message());
     }
   }
 
   // initial values passed to constructor overwrite yaml file sources
   for (auto & param : initial_parameters) {
-    parameters[param.get_name()] = param;
+    initial_parameter_values_[param.get_name()] =
+      rclcpp::ParameterValue(param.get_value_message());
   }
 
-  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");
+  // If asked, initialize any parameters that ended up in the initial parameter values,
+  // but did not get declared explcitily by this point.
+  if (automatically_declare_initial_parameters) {
+    for (const auto & pair : this->get_initial_parameter_values()) {
+      if (!this->has_parameter(pair.first)) {
+        this->declare_parameter(
+          pair.first,
+          pair.second,
+          rcl_interfaces::msg::ParameterDescriptor());
+      }
     }
   }
 }
@@ -159,70 +176,354 @@ NodeParameters::NodeParameters(
 NodeParameters::~NodeParameters()
 {}
 
+RCLCPP_LOCAL
+bool
+__lockless_has_parameter(
+  const std::map<std::string, rclcpp::node_interfaces::ParameterInfo> & parameters,
+  const std::string & name)
+{
+  return parameters.find(name) != parameters.end();
+}
+
+using OnParametersSetCallbackType =
+  rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType;
+
+RCLCPP_LOCAL
+rcl_interfaces::msg::SetParametersResult
+__set_parameters_atomically_common(
+  const std::vector<rclcpp::Parameter> & parameters,
+  std::map<std::string, rclcpp::node_interfaces::ParameterInfo> & parameter_infos,
+  OnParametersSetCallbackType on_set_parameters_callback)
+{
+  // Call the user callback to see if the new value(s) are allowed.
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+  if (on_set_parameters_callback) {
+    result = on_set_parameters_callback(parameters);
+  }
+
+  // If accepted, actually set the values.
+  if (result.successful) {
+    for (size_t i = 0; i < parameters.size(); ++i) {
+      const std::string & name = parameters[i].get_name();
+      parameter_infos[name].descriptor.name = parameters[i].get_name();
+      parameter_infos[name].descriptor.type = parameters[i].get_type();
+      parameter_infos[name].value = parameters[i].get_parameter_value();
+    }
+  }
+
+  // Either way, return the result.
+  return result;
+}
+
+RCLCPP_LOCAL
+rcl_interfaces::msg::SetParametersResult
+__declare_parameter_common(
+  const std::string & name,
+  const rclcpp::ParameterValue & default_value,
+  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor,
+  std::map<std::string, rclcpp::node_interfaces::ParameterInfo> & parameters_out,
+  const std::map<std::string, rclcpp::ParameterValue> & initial_values,
+  OnParametersSetCallbackType on_set_parameters_callback,
+  rcl_interfaces::msg::ParameterEvent * parameter_event_out)
+{
+  using rclcpp::node_interfaces::ParameterInfo;
+  std::map<std::string, ParameterInfo> parameter_infos {{name, ParameterInfo()}};
+  parameter_infos.at(name).descriptor = parameter_descriptor;
+
+  // Use the value from the initial_values if available, otherwise use the default.
+  const rclcpp::ParameterValue * initial_value = &default_value;
+  auto initial_value_it = initial_values.find(name);
+  if (initial_value_it != initial_values.end()) {
+    initial_value = &initial_value_it->second;
+  }
+
+  // Check with the user's callback to see if the initial value can be set.
+  std::vector<rclcpp::Parameter> parameter_wrappers {rclcpp::Parameter(name, *initial_value)};
+  // This function also takes care of default vs initial value.
+  auto result = __set_parameters_atomically_common(
+    parameter_wrappers,
+    parameter_infos,
+    on_set_parameters_callback);
+
+  // Add declared parameters to storage.
+  parameters_out[name] = parameter_infos.at(name);
+
+  // Extend the given parameter event, if valid.
+  if (parameter_event_out) {
+    parameter_event_out->new_parameters.push_back(parameter_wrappers[0].to_parameter_msg());
+  }
+
+  return result;
+}
+
+const rclcpp::ParameterValue &
+NodeParameters::declare_parameter(
+  const std::string & name,
+  const rclcpp::ParameterValue & default_value,
+  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  // TODO(sloretz) parameter name validation
+  if (name.empty()) {
+    throw rclcpp::exceptions::InvalidParametersException("parameter name must not be empty");
+  }
+
+  // Error if this parameter has already been declared and is different
+  if (__lockless_has_parameter(parameters_, name)) {
+    throw rclcpp::exceptions::ParameterAlreadyDeclaredException(
+            "parameter '" + name + "' has already been declared");
+  }
+
+  rcl_interfaces::msg::ParameterEvent parameter_event;
+  auto result = __declare_parameter_common(
+    name,
+    default_value,
+    parameter_descriptor,
+    parameters_,
+    initial_parameter_values_,
+    on_parameters_set_callback_,
+    &parameter_event);
+
+  // If it failed to be set, then throw an exception.
+  if (!result.successful) {
+    throw rclcpp::exceptions::InvalidParameterValueException(
+            "parameter '" + name + "' could not be set: " + result.reason);
+  }
+
+  // Publish if events_publisher_ is not nullptr, which may be if disabled in the constructor.
+  if (nullptr != events_publisher_) {
+    events_publisher_->publish(parameter_event);
+  }
+
+  return parameters_.at(name).value;
+}
+
+void
+NodeParameters::undeclare_parameter(const std::string & name)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  auto parameter_info = parameters_.find(name);
+  if (parameter_info == parameters_.end()) {
+    throw rclcpp::exceptions::ParameterNotDeclaredException(
+            "cannot undeclare parameter '" + name + "' which has not yet been declared");
+  }
+
+  if (parameter_info->second.descriptor.read_only) {
+    throw rclcpp::exceptions::ParameterImmutableException(
+            "cannot undeclare parameter '" + name + "' because it is read-only");
+  }
+
+  parameters_.erase(parameter_info);
+}
+
+bool
+NodeParameters::has_parameter(const std::string & name) const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  return __lockless_has_parameter(parameters_, name);
+}
+
 std::vector<rcl_interfaces::msg::SetParametersResult>
-NodeParameters::set_parameters(
-  const std::vector<rclcpp::Parameter> & parameters)
+NodeParameters::set_parameters(const std::vector<rclcpp::Parameter> & parameters)
 {
   std::vector<rcl_interfaces::msg::SetParametersResult> results;
-  for (auto p : parameters) {
+  results.reserve(parameters.size());
+
+  for (const auto & p : parameters) {
     auto result = set_parameters_atomically({{p}});
     results.push_back(result);
   }
+
   return results;
 }
 
+template<typename ParameterVectorType>
+auto
+__find_parameter_by_name(
+  ParameterVectorType & parameters,
+  const std::string & name)
+{
+  return std::find_if(
+    parameters.begin(),
+    parameters.end(),
+    [&](auto parameter) {return parameter.get_name() == name;});
+}
+
 rcl_interfaces::msg::SetParametersResult
-NodeParameters::set_parameters_atomically(
-  const std::vector<rclcpp::Parameter> & parameters)
+NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> & parameters)
 {
   std::lock_guard<std::mutex> lock(mutex_);
-  std::map<std::string, rclcpp::Parameter> tmp_map;
-  auto parameter_event = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
 
-  // TODO(jacquelinekay): handle parameter constraints
   rcl_interfaces::msg::SetParametersResult result;
-  if (parameters_callback_) {
-    result = parameters_callback_(parameters);
-  } else {
-    result.successful = true;
+
+  // Check if any of the parameters are read-only, or if any parameters are not
+  // declared.
+  // If not declared, keep track of them in order to declare them later, when
+  // undeclared parameters are allowed, and if they're not allowed, fail.
+  std::vector<const rclcpp::Parameter *> parameters_to_be_declared;
+  for (const auto & parameter : parameters) {
+    const std::string & name = parameter.get_name();
+
+    // Check to make sure the parameter name is valid.
+    if (name.empty()) {
+      throw rclcpp::exceptions::InvalidParametersException("parameter name must not be empty");
+    }
+
+    // Check to see if it is declared.
+    auto parameter_info = parameters_.find(name);
+    if (parameter_info == parameters_.end()) {
+      // If not check to see if undeclared paramaters are allowed, ...
+      if (allow_undeclared_) {
+        // If so, mark the parameter to be declared for the user implicitly.
+        parameters_to_be_declared.push_back(&parameter);
+        // continue as it cannot be read-only, and because the declare will
+        // implicitly set the parameter and parameter_infos is for setting only.
+        continue;
+      } else {
+        // If not, then throw the exception as documented.
+        throw rclcpp::exceptions::ParameterNotDeclaredException(
+                "parameter '" + name + "' cannot be set because it was not declared");
+      }
+    }
+
+    // Check to see if it is read-only.
+    if (parameter_info->second.descriptor.read_only) {
+      result.successful = false;
+      result.reason = "parameter '" + name + "' cannot be set because it is read-only";
+      return result;
+    }
   }
 
+  // Declare parameters into a temporary "staging area", incase one of the declares fail.
+  // We will use the staged changes as input to the "set atomically" action.
+  // We explicitly avoid calling the user callback here, so that it may be called once, with
+  // all the other parameters to be set (already declared parameters).
+  std::map<std::string, rclcpp::node_interfaces::ParameterInfo> staged_parameter_changes;
+  rcl_interfaces::msg::ParameterEvent parameter_event_msg;
+  parameter_event_msg.node = combined_name_;
+  for (auto parameter_to_be_declared : parameters_to_be_declared) {
+    // This should not throw, because we validated the name and checked that
+    // the parameter was not already declared.
+    result = __declare_parameter_common(
+      parameter_to_be_declared->get_name(),
+      parameter_to_be_declared->get_parameter_value(),
+      rcl_interfaces::msg::ParameterDescriptor(),  // Implicit declare uses default descriptor.
+      staged_parameter_changes,
+      initial_parameter_values_,
+      nullptr,  // callback is explicitly null, so that it is called only once, when setting below.
+      &parameter_event_msg);
+    if (!result.successful) {
+      // Declare failed, return knowing that nothing was changed because the
+      // staged changes were not applied.
+      return result;
+    }
+  }
+
+  // If there were implicitly declared parameters, then we may need to copy the input parameters
+  // and then assign the value that was selected after the declare (could be affected by the
+  // initial parameter values).
+  const std::vector<rclcpp::Parameter> * parameters_to_be_set = &parameters;
+  std::vector<rclcpp::Parameter> parameters_copy;
+  if (0 != staged_parameter_changes.size()) {  // If there were any implicitly declared parameters.
+    bool any_initial_values_used = false;
+    for (const auto & staged_parameter_change : staged_parameter_changes) {
+      auto it = __find_parameter_by_name(parameters, staged_parameter_change.first);
+      if (it->get_parameter_value() != staged_parameter_change.second.value) {
+        // In this case, the value of the staged parameter differs from the
+        // input from the user, and therefore we need to update things before setting.
+        any_initial_values_used = true;
+        // No need to search further since at least one initial value needs to be used.
+        break;
+      }
+    }
+    if (any_initial_values_used) {
+      parameters_copy = parameters;
+      for (const auto & staged_parameter_change : staged_parameter_changes) {
+        auto it = __find_parameter_by_name(parameters_copy, staged_parameter_change.first);
+        *it = Parameter(staged_parameter_change.first, staged_parameter_change.second.value);
+      }
+      parameters_to_be_set = &parameters_copy;
+    }
+  }
+
+  // Collect parameters who will have had their type changed to
+  // rclcpp::PARAMETER_NOT_SET so they can later be implicitly undeclared.
+  std::vector<const rclcpp::Parameter *> parameters_to_be_undeclared;
+  for (const auto & parameter : *parameters_to_be_set) {
+    if (rclcpp::PARAMETER_NOT_SET == parameter.get_type()) {
+      auto it = parameters_.find(parameter.get_name());
+      if (it != parameters_.end() && rclcpp::PARAMETER_NOT_SET != it->second.value.get_type()) {
+        parameters_to_be_undeclared.push_back(&parameter);
+      }
+    }
+  }
+
+  // Set the all of the parameters including the ones declared implicitly above.
+  result = __set_parameters_atomically_common(
+    // either the original parameters given by the user, or ones updated with initial values
+    *parameters_to_be_set,
+    // they are actually set on the official parameter storage
+    parameters_,
+    // this will get called once, with all the parameters to be set
+    on_parameters_set_callback_);
+
+  // If not successful, then stop here.
   if (!result.successful) {
     return result;
   }
 
-  for (auto p : parameters) {
-    if (p.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) {
-      if (parameters_.find(p.get_name()) != parameters_.end()) {
-        // case: parameter was set before, and input is "NOT_SET"
-        // therefore we will erase the parameter from parameters_ later
-        parameter_event->deleted_parameters.push_back(p.to_parameter_msg());
-      }
-    } else {
-      if (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;
-    }
+  // If successful, then update the parameter infos from the implicitly declared parameter's.
+  for (const auto & kv_pair : staged_parameter_changes) {
+    // assumption: the parameter is already present in parameters_ due to the above "set"
+    assert(__lockless_has_parameter(parameters_, kv_pair.first));
+    // assumption: the value in parameters_ is the same as the value resulting from the declare
+    assert(parameters_[kv_pair.first].value == kv_pair.second.value);
+    // This assignment should not change the name, type, or value, but may
+    // change other things from the ParameterInfo.
+    parameters_[kv_pair.first] = kv_pair.second;
   }
-  // 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());
+  // Undeclare parameters that need to be.
+  for (auto parameter_to_undeclare : parameters_to_be_undeclared) {
+    auto it = parameters_.find(parameter_to_undeclare->get_name());
+    // assumption: the parameter to be undeclared should be in the parameter infos map
+    assert(it != parameters_.end());
+    if (it != parameters_.end()) {
+      // Remove it and update the parameter event message.
+      parameters_.erase(it);
+      parameter_event_msg.deleted_parameters.push_back(
+        rclcpp::Parameter(it->first, it->second.value).to_parameter_msg());
     }
   }
 
-  std::swap(tmp_map, parameters_);
+  // Update the parameter event message for any parameters which were only set,
+  // and not either declared or undeclared.
+  for (const auto & parameter : *parameters_to_be_set) {
+    if (staged_parameter_changes.find(parameter.get_name()) != staged_parameter_changes.end()) {
+      // This parameter was declared.
+      continue;
+    }
+    auto it = std::find_if(
+      parameters_to_be_undeclared.begin(),
+      parameters_to_be_undeclared.end(),
+      [&parameter](const auto & p) {return p->get_name() == parameter.get_name();});
+    if (it != parameters_to_be_undeclared.end()) {
+      // This parameter was undeclared (deleted).
+      continue;
+    }
+    // This parameter was neither declared nor undeclared.
+    parameter_event_msg.changed_parameters.push_back(parameter.to_parameter_msg());
+  }
 
-  events_publisher_->publish(parameter_event);
+  // Publish if events_publisher_ is not nullptr, which may be if disabled in the constructor.
+  if (nullptr != events_publisher_) {
+    parameter_event_msg.stamp = node_clock_->get_clock()->now();
+    events_publisher_->publish(parameter_event_msg);
+  }
 
   return result;
 }
@@ -232,14 +533,19 @@ NodeParameters::get_parameters(const std::vector<std::string> & names) const
 {
   std::lock_guard<std::mutex> lock(mutex_);
   std::vector<rclcpp::Parameter> results;
+  results.reserve(names.size());
 
   for (auto & name : names) {
-    if (std::any_of(parameters_.cbegin(), parameters_.cend(),
-      [&name](const std::pair<std::string, rclcpp::Parameter> & kv) {
-        return name == kv.first;
-      }))
-    {
-      results.push_back(parameters_.at(name));
+    auto found_parameter = parameters_.find(name);
+    if (found_parameter != parameters_.cend()) {
+      // found
+      results.emplace_back(name, found_parameter->second.value);
+    } else if (this->allow_undeclared_) {
+      // not found, but undeclared allowed
+      results.emplace_back(name, rclcpp::ParameterValue());
+    } else {
+      // not found, and undeclared are not allowed
+      throw rclcpp::exceptions::ParameterNotDeclaredException(name);
     }
   }
   return results;
@@ -252,8 +558,10 @@ NodeParameters::get_parameter(const std::string & name) const
 
   if (get_parameter(name, parameter)) {
     return parameter;
+  } else if (this->allow_undeclared_) {
+    return parameter;
   } else {
-    throw std::out_of_range("Parameter '" + name + "' not set");
+    throw rclcpp::exceptions::ParameterNotDeclaredException(name);
   }
 }
 
@@ -264,30 +572,64 @@ NodeParameters::get_parameter(
 {
   std::lock_guard<std::mutex> lock(mutex_);
 
-  if (parameters_.count(name)) {
-    parameter = parameters_.at(name);
+  auto param_iter = parameters_.find(name);
+  if (
+    parameters_.end() != param_iter &&
+    param_iter->second.value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET)
+  {
+    parameter = {name, param_iter->second.value};
     return true;
   } else {
     return false;
   }
 }
 
+bool
+NodeParameters::get_parameters_by_prefix(
+  const std::string & prefix,
+  std::map<std::string, rclcpp::Parameter> & parameters) const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  std::string prefix_with_dot = prefix.empty() ? prefix : prefix + ".";
+  bool ret = false;
+
+  for (const auto & param : parameters_) {
+    if (param.first.find(prefix_with_dot) == 0 && param.first.length() > prefix_with_dot.length()) {
+      // Found one!
+      parameters[param.first.substr(prefix_with_dot.length())] = rclcpp::Parameter(param.second);
+      ret = true;
+    }
+  }
+
+  return ret;
+}
+
 std::vector<rcl_interfaces::msg::ParameterDescriptor>
 NodeParameters::describe_parameters(const std::vector<std::string> & names) const
 {
   std::lock_guard<std::mutex> lock(mutex_);
   std::vector<rcl_interfaces::msg::ParameterDescriptor> results;
-  for (auto & kv : parameters_) {
-    if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
-        return name == kv.first;
-      }))
-    {
-      rcl_interfaces::msg::ParameterDescriptor parameter_descriptor;
-      parameter_descriptor.name = kv.first;
-      parameter_descriptor.type = kv.second.get_type();
-      results.push_back(parameter_descriptor);
+  results.reserve(names.size());
+
+  for (const auto & name : names) {
+    auto it = parameters_.find(name);
+    if (it != parameters_.cend()) {
+      results.push_back(it->second.descriptor);
+    } else if (allow_undeclared_) {
+      // parameter not found, but undeclared allowed, so return empty
+      rcl_interfaces::msg::ParameterDescriptor default_description;
+      default_description.name = name;
+      results.push_back(default_description);
+    } else {
+      throw rclcpp::exceptions::ParameterNotDeclaredException(name);
     }
   }
+
+  if (results.size() != names.size()) {
+    throw std::runtime_error("results and names unexpectedly different sizes");
+  }
+
   return results;
 }
 
@@ -296,16 +638,24 @@ NodeParameters::get_parameter_types(const std::vector<std::string> & names) cons
 {
   std::lock_guard<std::mutex> lock(mutex_);
   std::vector<uint8_t> results;
-  for (auto & kv : parameters_) {
-    if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
-        return name == kv.first;
-      }))
-    {
-      results.push_back(kv.second.get_type());
-    } else {
+  results.reserve(names.size());
+
+  for (const auto & name : names) {
+    auto it = parameters_.find(name);
+    if (it != parameters_.cend()) {
+      results.push_back(it->second.value.get_type());
+    } else if (allow_undeclared_) {
+      // parameter not found, but undeclared allowed, so return not set
       results.push_back(rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET);
+    } else {
+      throw rclcpp::exceptions::ParameterNotDeclaredException(name);
     }
   }
+
+  if (results.size() != names.size()) {
+    throw std::runtime_error("results and names unexpectedly different sizes");
+  }
+
   return results;
 }
 
@@ -351,12 +701,39 @@ NodeParameters::list_parameters(const std::vector<std::string> & prefixes, uint6
   return result;
 }
 
+NodeParameters::OnParametersSetCallbackType
+NodeParameters::set_on_parameters_set_callback(OnParametersSetCallbackType callback)
+{
+  auto existing_callback = on_parameters_set_callback_;
+  on_parameters_set_callback_ = callback;
+  return existing_callback;
+}
+
+#if !defined(_WIN32)
+# pragma GCC diagnostic push
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#else  // !defined(_WIN32)
+# pragma warning(push)
+# pragma warning(disable: 4996)
+#endif
 void
 NodeParameters::register_param_change_callback(ParametersCallbackFunction callback)
 {
-  if (parameters_callback_) {
-    RCUTILS_LOG_WARN("param_change_callback already registered, "
-      "overwriting previous callback")
+  if (on_parameters_set_callback_) {
+    RCLCPP_WARN(
+      node_logging_->get_logger(),
+      "on_parameters_set_callback already registered, overwriting previous callback");
   }
-  parameters_callback_ = callback;
+  on_parameters_set_callback_ = callback;
+}
+#if !defined(_WIN32)
+# pragma GCC diagnostic pop
+#else  // !defined(_WIN32)
+# pragma warning(pop)
+#endif
+
+const std::map<std::string, rclcpp::ParameterValue> &
+NodeParameters::get_initial_parameter_values() const
+{
+  return initial_parameter_values_;
 }

rclcpp/src/rclcpp/node_interfaces/node_services.cpp

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