30.1.4

Signed-off-by: Alejandro Hernandez Cordero <ahcorde@gmail.com>

.github/ISSUE_TEMPLATE.md

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

README.md

@@ -0,0 +1,18 @@
+# rclcpp
+
+This repository contains the source code for the ROS Client Library for C++ package, included with a standard install of any ROS 2 distro.
+
+rclcpp provides the standard C++ API for interacting with ROS 2.
+
+## Usage
+
+`#include "rclcpp/rclcpp.hpp"` allows use of the most common elements of the ROS 2 system.
+
+The link to the latest API documentation can be found on the [rclcpp package info page](https://docs.ros.org/en/rolling/p/rclcpp).
+
+
+### Examples
+
+The ROS 2 tutorials [Writing a simple publisher and subscriber](https://docs.ros.org/en/rolling/Tutorials/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html).
+and [Writing a simple service and client](https://docs.ros.org/en/rolling/Tutorials/Writing-A-Simple-Cpp-Service-And-Client.html)
+contain some examples of rclcpp APIs in use.

rclcpp/CMakeLists.txt

@@ -1,29 +1,43 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.20)
 
 project(rclcpp)
 
+find_package(Threads REQUIRED)
+
 find_package(ament_cmake_ros REQUIRED)
+find_package(ament_index_cpp REQUIRED)
 find_package(builtin_interfaces REQUIRED)
+find_package(libstatistics_collector REQUIRED)
 find_package(rcl REQUIRED)
 find_package(rcl_interfaces REQUIRED)
+find_package(rcl_logging_interface REQUIRED)
 find_package(rcl_yaml_param_parser REQUIRED)
+find_package(rcpputils REQUIRED)
+find_package(rcutils REQUIRED)
 find_package(rmw REQUIRED)
-find_package(rmw_implementation REQUIRED)
 find_package(rosgraph_msgs REQUIRED)
-find_package(rosidl_generator_cpp REQUIRED)
+find_package(rosidl_dynamic_typesupport REQUIRED)
+find_package(rosidl_runtime_c REQUIRED)
+find_package(rosidl_runtime_cpp REQUIRED)
 find_package(rosidl_typesupport_c REQUIRED)
 find_package(rosidl_typesupport_cpp REQUIRED)
+find_package(statistics_msgs REQUIRED)
+find_package(tracetools REQUIRED)
 
-# Default to C++14
+# TODO(wjwwood): remove this when gtest can build on its own, when using target_compile_features()
+# Default to C++17
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
 endif()
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
+  # About -Wno-sign-conversion: With Clang, -Wconversion implies -Wsign-conversion. There are a number of
+  # implicit sign conversions in rclcpp and gtest.cc, see https://ci.ros2.org/job/ci_osx/9265/.
+  # Hence disabling -Wsign-conversion for now until all those have eventually been fixed.
+  # (from https://github.com/ros2/rclcpp/pull/1188#issuecomment-650229140)
+  add_compile_options(-Wall -Wextra -Wconversion -Wno-sign-conversion -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual)
 endif()
 
-include_directories(include)
-
 set(${PROJECT_NAME}_SRCS
   src/rclcpp/any_executable.cpp
   src/rclcpp/callback_group.cpp
@@ -31,23 +45,50 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/clock.cpp
   src/rclcpp/context.cpp
   src/rclcpp/contexts/default_context.cpp
+  src/rclcpp/create_generic_client.cpp
+  src/rclcpp/detail/add_guard_condition_to_rcl_wait_set.cpp
+  src/rclcpp/detail/resolve_intra_process_buffer_type.cpp
+  src/rclcpp/detail/resolve_parameter_overrides.cpp
+  src/rclcpp/detail/rmw_implementation_specific_payload.cpp
+  src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp
+  src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp
+  src/rclcpp/detail/utilities.cpp
   src/rclcpp/duration.cpp
+  src/rclcpp/dynamic_typesupport/dynamic_message.cpp
+  src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp
+  src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp
+  src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp
+  src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp
   src/rclcpp/event.cpp
-  src/rclcpp/exceptions.cpp
+  src/rclcpp/exceptions/exceptions.cpp
+  src/rclcpp/executable_list.cpp
   src/rclcpp/executor.cpp
+  src/rclcpp/executor_options.cpp
   src/rclcpp/executors.cpp
-  src/rclcpp/expand_topic_or_service_name.cpp
+  src/rclcpp/executors/executor_entities_collection.cpp
+  src/rclcpp/executors/executor_entities_collector.cpp
+  src/rclcpp/executors/executor_notify_waitable.cpp
   src/rclcpp/executors/multi_threaded_executor.cpp
   src/rclcpp/executors/single_threaded_executor.cpp
+  src/rclcpp/expand_topic_or_service_name.cpp
+  src/rclcpp/experimental/executors/events_executor/events_executor.cpp
+  src/rclcpp/experimental/timers_manager.cpp
+  src/rclcpp/future_return_code.cpp
+  src/rclcpp/generic_client.cpp
+  src/rclcpp/generic_publisher.cpp
+  src/rclcpp/generic_service.cpp
+  src/rclcpp/generic_subscription.cpp
   src/rclcpp/graph_listener.cpp
+  src/rclcpp/guard_condition.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/logging_mutex.cpp
   src/rclcpp/memory_strategies.cpp
   src/rclcpp/memory_strategy.cpp
+  src/rclcpp/message_info.cpp
+  src/rclcpp/network_flow_endpoint.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
@@ -57,59 +98,133 @@ set(${PROJECT_NAME}_SRCS
   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_type_descriptions.cpp
   src/rclcpp/node_interfaces/node_waitables.cpp
+  src/rclcpp/node_options.cpp
   src/rclcpp/parameter.cpp
-  src/rclcpp/parameter_value.cpp
   src/rclcpp/parameter_client.cpp
+  src/rclcpp/parameter_descriptor_wrapper.cpp
+  src/rclcpp/parameter_event_handler.cpp
   src/rclcpp/parameter_events_filter.cpp
   src/rclcpp/parameter_map.cpp
   src/rclcpp/parameter_service.cpp
+  src/rclcpp/parameter_value.cpp
   src/rclcpp/publisher_base.cpp
   src/rclcpp/qos.cpp
-  src/rclcpp/qos_event.cpp
+  src/rclcpp/event_handler.cpp
+  src/rclcpp/qos_overriding_options.cpp
+  src/rclcpp/rate.cpp
+  src/rclcpp/serialization.cpp
+  src/rclcpp/serialized_message.cpp
   src/rclcpp/service.cpp
   src/rclcpp/signal_handler.cpp
   src/rclcpp/subscription_base.cpp
+  src/rclcpp/subscription_intra_process_base.cpp
   src/rclcpp/time.cpp
   src/rclcpp/time_source.cpp
   src/rclcpp/timer.cpp
   src/rclcpp/type_support.cpp
+  src/rclcpp/typesupport_helpers.cpp
   src/rclcpp/utilities.cpp
+  src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp
   src/rclcpp/waitable.cpp
 )
 
-# "watch" template for changes
-configure_file(
-  "resource/logging.hpp.em"
-  "logging.hpp.em.watch"
-  COPYONLY
-)
-# generate header with logging macros
-set(python_code
-  "import em"
-  "em.invoke(['-o', 'include/rclcpp/logging.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/logging.hpp.em'])")
-string(REPLACE ";" "$<SEMICOLON>" python_code "${python_code}")
-add_custom_command(OUTPUT include/rclcpp/logging.hpp
-  COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp"
-  COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_code}"
-  DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/logging.hpp.em.watch"
-  COMMENT "Expanding logging.hpp.em"
-  VERBATIM
-)
-list(APPEND ${PROJECT_NAME}_SRCS
-  include/rclcpp/logging.hpp)
-include_directories("${CMAKE_CURRENT_BINARY_DIR}/include")
+# By default, without the settings below, find_package(Python3) will attempt
+# to find the newest python version it can, and additionally will find the
+# most specific version.  For instance, on a system that has
+# /usr/bin/python3.10, /usr/bin/python3.11, and /usr/bin/python3, it will find
+# /usr/bin/python3.11, even if /usr/bin/python3 points to /usr/bin/python3.10.
+# The behavior we want is to prefer the "system" installed version unless the
+# user specifically tells us othewise through the Python3_EXECUTABLE hint.
+# Setting CMP0094 to NEW means that the search will stop after the first
+# python version is found.  Setting Python3_FIND_UNVERSIONED_NAMES means that
+# the search will prefer /usr/bin/python3 over /usr/bin/python3.11.  And that
+# latter functionality is only available in CMake 3.20 or later, so we need
+# at least that version.
+cmake_policy(SET CMP0094 NEW)
+set(Python3_FIND_UNVERSIONED_NAMES FIRST)
 
-add_library(${PROJECT_NAME}
-  ${${PROJECT_NAME}_SRCS})
-# specific order: dependents before dependencies
-ament_target_dependencies(${PROJECT_NAME}
-  "rcl"
-  "rcl_yaml_param_parser"
-  "builtin_interfaces"
-  "rosgraph_msgs"
-  "rosidl_typesupport_cpp"
-  "rosidl_generator_cpp")
+find_package(Python3 REQUIRED COMPONENTS Interpreter)
+
+file(GLOB interface_files "include/rclcpp/node_interfaces/node_*_interface.hpp")
+foreach(interface_file ${interface_files})
+  get_filename_component(interface_name ${interface_file} NAME_WE)
+
+  # "watch" template for changes
+  configure_file(
+    "resource/interface_traits.hpp.em"
+    "${CMAKE_CURRENT_BINARY_DIR}/${interface_name}_traits.hpp.em.watch"
+    COPYONLY
+  )
+  set(python_${interface_name}_traits
+    "import em"
+    "em.invoke(['-D', 'interface_name = \\'${interface_name}\\'', '-o', 'include/rclcpp/node_interfaces/${interface_name}_traits.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/interface_traits.hpp.em'])")
+  string(REPLACE ";" "$<SEMICOLON>" python_${interface_name}_traits "${python_${interface_name}_traits}")
+  add_custom_command(OUTPUT include/rclcpp/node_interfaces/${interface_name}_traits.hpp
+    COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp/node_interfaces"
+    COMMAND Python3::Interpreter ARGS -c "${python_${interface_name}_traits}"
+    DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/${interface_name}_traits.hpp.em.watch"
+    COMMENT "Expanding interface_traits.hpp.em into ${interface_name}_traits.hpp"
+    VERBATIM
+  )
+  list(APPEND ${PROJECT_NAME}_SRCS
+    include/rclcpp/node_interfaces/${interface_name}_traits.hpp)
+
+  # "watch" template for changes
+  configure_file(
+    "resource/get_interface.hpp.em"
+    "get_${interface_name}.hpp.em.watch"
+    COPYONLY
+  )
+  set(python_get_${interface_name}
+    "import em"
+    "em.invoke(['-D', 'interface_name = \\'${interface_name}\\'', '-o', 'include/rclcpp/node_interfaces/get_${interface_name}.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/get_interface.hpp.em'])")
+  string(REPLACE ";" "$<SEMICOLON>" python_get_${interface_name} "${python_get_${interface_name}}")
+  add_custom_command(OUTPUT include/rclcpp/node_interfaces/get_${interface_name}.hpp
+    COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp/node_interfaces"
+    COMMAND Python3::Interpreter ARGS -c "${python_get_${interface_name}}"
+    DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/get_${interface_name}.hpp.em.watch"
+    COMMENT "Expanding get_interface.hpp.em into get_${interface_file}.hpp"
+    VERBATIM
+  )
+  list(APPEND ${PROJECT_NAME}_SRCS
+    include/rclcpp/node_interfaces/get_${interface_name}.hpp)
+endforeach()
+
+add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SRCS})
+target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)
+# TODO(wjwwood): address all deprecation warnings and then remove this
+if(WIN32)
+  target_compile_definitions(${PROJECT_NAME} PUBLIC "_SILENCE_ALL_CXX17_DEPRECATION_WARNINGS")
+endif()
+target_include_directories(${PROJECT_NAME} PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(${PROJECT_NAME} PUBLIC
+  ${builtin_interfaces_TARGETS}
+  libstatistics_collector::libstatistics_collector
+  rcl::rcl
+  ${rcl_interfaces_TARGETS}
+  rcl_yaml_param_parser::rcl_yaml_param_parser
+  rcpputils::rcpputils
+  rcutils::rcutils
+  rmw::rmw
+  ${rosgraph_msgs_TARGETS}
+  rosidl_dynamic_typesupport::rosidl_dynamic_typesupport
+  rosidl_runtime_c::rosidl_runtime_c
+  rosidl_runtime_cpp::rosidl_runtime_cpp
+  rosidl_typesupport_cpp::rosidl_typesupport_cpp
+  ${statistics_msgs_TARGETS}
+  tracetools::tracetools
+  ${CMAKE_THREAD_LIBS_INIT}
+)
+
+target_link_libraries(${PROJECT_NAME} PRIVATE
+  ament_index_cpp::ament_index_cpp
+  rcl_logging_interface::rcl_logging_interface
+)
 
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
@@ -117,360 +232,58 @@ target_compile_definitions(${PROJECT_NAME}
   PRIVATE "RCLCPP_BUILDING_LIBRARY")
 
 install(
-  TARGETS ${PROJECT_NAME}
+  TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
 )
 
-# specific order: dependents before dependencies
-ament_export_include_directories(include)
+# Export old-style CMake variables
+ament_export_include_directories("include/${PROJECT_NAME}")
 ament_export_libraries(${PROJECT_NAME})
 
-ament_export_dependencies(ament_cmake)
-ament_export_dependencies(rcl)
-ament_export_dependencies(builtin_interfaces)
-ament_export_dependencies(rosgraph_msgs)
-ament_export_dependencies(rosidl_typesupport_cpp)
-ament_export_dependencies(rosidl_typesupport_c)
-ament_export_dependencies(rosidl_generator_cpp)
-ament_export_dependencies(rcl_yaml_param_parser)
+# Export modern CMake targets
+ament_export_targets(${PROJECT_NAME})
+
+ament_export_dependencies(
+  builtin_interfaces
+  libstatistics_collector
+  rcl
+  rcl_interfaces
+  rcl_yaml_param_parser
+  rcpputils
+  rcutils
+  rmw
+  rosgraph_msgs
+  rosidl_dynamic_typesupport
+  rosidl_runtime_c
+  rosidl_runtime_cpp
+  rosidl_typesupport_cpp
+  statistics_msgs
+  tracetools
+)
 
 if(BUILD_TESTING)
-  find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 
-  find_package(rmw_implementation_cmake REQUIRED)
-
-  include(cmake/rclcpp_add_build_failure_test.cmake)
-
-  ament_add_gtest(test_client test/test_client.cpp)
-  if(TARGET test_client)
-    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)
-    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)
-    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)
-    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)
-    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)
-    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)
-    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_parameter_events_filter test/test_parameter_events_filter.cpp)
-  if(TARGET test_parameter_events_filter)
-    ament_target_dependencies(test_parameter_events_filter
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_generator_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_parameter_events_filter ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_parameter test/test_parameter.cpp)
-  if(TARGET test_parameter)
-    ament_target_dependencies(test_parameter
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_generator_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_parameter ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_parameter_map test/test_parameter_map.cpp)
-  if(TARGET test_parameter_map)
-    target_link_libraries(test_parameter_map ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_publisher test/test_publisher.cpp)
-  if(TARGET test_publisher)
-    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_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)
-    ament_target_dependencies(test_rate
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_generator_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_rate
-      ${PROJECT_NAME}
-    )
-  endif()
-  ament_add_gtest(test_serialized_message_allocator test/test_serialized_message_allocator.cpp)
-  if(TARGET test_serialized_message_allocator)
-    ament_target_dependencies(test_serialized_message_allocator
-      test_msgs
-    )
-    target_link_libraries(test_serialized_message_allocator
-      ${PROJECT_NAME}
-    )
-  endif()
-  ament_add_gtest(test_service test/test_service.cpp)
-  if(TARGET test_service)
-    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)
-    ament_target_dependencies(test_subscription
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_generator_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_subscription ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_subscription_publisher_count_api test/test_subscription_publisher_count_api.cpp)
-  if(TARGET test_subscription_publisher_count_api)
-    ament_target_dependencies(test_subscription_publisher_count_api
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_generator_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME})
-  endif()
-  find_package(test_msgs REQUIRED)
-  ament_add_gtest(test_subscription_traits test/test_subscription_traits.cpp)
-  if(TARGET test_subscription_traits)
-    ament_target_dependencies(test_subscription_traits
-      "rcl"
-      "test_msgs"
-    )
-  endif()
-  ament_add_gtest(test_find_weak_nodes test/test_find_weak_nodes.cpp)
-  if(TARGET test_find_weak_nodes)
-    ament_target_dependencies(test_find_weak_nodes
-      "rcl"
-    )
-    target_link_libraries(test_find_weak_nodes ${PROJECT_NAME})
-  endif()
-
-  get_default_rmw_implementation(default_rmw)
-  find_package(${default_rmw} REQUIRED)
-  get_rmw_typesupport(typesupport_impls_cpp "${default_rmw}" LANGUAGE "cpp")
-  get_rmw_typesupport(typesupport_impls_c "${default_rmw}" LANGUAGE "c")
-  set(mock_msg_files
-    "test/mock_msgs/srv/Mock.srv")
-  rosidl_generate_interfaces(mock_msgs
-    ${mock_msg_files}
-    LIBRARY_NAME "rclcpp"
-    SKIP_INSTALL)
-
-  set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}")
-  if(WIN32)
-    set(append_library_dirs "${append_library_dirs}/$<CONFIG>")
-  endif()
-
-  ament_add_gtest(test_externally_defined_services test/test_externally_defined_services.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_externally_defined_services)
-    ament_target_dependencies(test_externally_defined_services
-      "rcl"
-    )
-    target_link_libraries(test_externally_defined_services ${PROJECT_NAME})
-    foreach(typesupport_impl_cpp ${typesupport_impls_cpp})
-      rosidl_target_interfaces(test_externally_defined_services
-        mock_msgs ${typesupport_impl_cpp})
-    endforeach()
-    foreach(typesupport_impl_c ${typesupport_impls_c})
-      rosidl_target_interfaces(test_externally_defined_services
-        mock_msgs ${typesupport_impl_c})
-    endforeach()
-  endif()
-
-  ament_add_gtest(test_duration test/test_duration.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_duration)
-    ament_target_dependencies(test_duration
-      "rcl")
-    target_link_libraries(test_duration ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_executor test/test_executor.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_executor)
-    ament_target_dependencies(test_executor
-      "rcl")
-    target_link_libraries(test_executor ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_logger test/test_logger.cpp)
-  target_link_libraries(test_logger ${PROJECT_NAME})
-
-  ament_add_gmock(test_logging test/test_logging.cpp)
-  target_link_libraries(test_logging ${PROJECT_NAME})
-
-  ament_add_gtest(test_time test/test_time.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_time)
-    ament_target_dependencies(test_time
-      "rcl")
-    target_link_libraries(test_time ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_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)
-    ament_target_dependencies(test_time_source
-      "rcl")
-    target_link_libraries(test_time_source ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_utilities test/test_utilities.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_utilities)
-    ament_target_dependencies(test_utilities
-      "rcl")
-    target_link_libraries(test_utilities ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_init test/test_init.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_init)
-    ament_target_dependencies(test_init
-      "rcl")
-    target_link_libraries(test_init ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_multi_threaded_executor test/executors/test_multi_threaded_executor.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_multi_threaded_executor)
-    ament_target_dependencies(test_multi_threaded_executor
-      "rcl")
-    target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_local_parameters test/test_local_parameters.cpp)
-  if(TARGET test_local_parameters)
-    ament_target_dependencies(test_local_parameters
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_generator_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_local_parameters ${PROJECT_NAME})
-  endif()
+  add_subdirectory(test)
 endif()
 
 ament_package()
 
 install(
   DIRECTORY include/ ${CMAKE_CURRENT_BINARY_DIR}/include/
-  DESTINATION include
+  DESTINATION include/${PROJECT_NAME}
 )
+
+if(TEST cppcheck)
+  # must set the property after ament_package()
+  set_tests_properties(cppcheck PROPERTIES TIMEOUT 1200)
+endif()
+
+if(TEST cpplint)
+  set_tests_properties(cpplint PROPERTIES TIMEOUT 180)
+endif()
+
+ament_generate_version_header(${PROJECT_NAME})

rclcpp/Doxyfile

@@ -21,13 +21,38 @@ GENERATE_LATEX         = NO
 ENABLE_PREPROCESSING   = YES
 MACRO_EXPANSION        = YES
 EXPAND_ONLY_PREDEF     = YES
-PREDEFINED             = RCLCPP_PUBLIC=
+EXCLUDE_SYMBOLS        += RCLCPP_STATIC_ASSERT_LOGGER
+EXCLUDE_SYMBOLS        += RCLCPP_LOG
+EXCLUDE_SYMBOLS        += RCLCPP_LOG_ONCE
+EXCLUDE_SYMBOLS        += RCLCPP_LOG_EXPRESSION
+EXCLUDE_SYMBOLS        += RCLCPP_LOG_FUNCTION
+EXCLUDE_SYMBOLS        += RCLCPP_LOG_SKIPFIRST
+EXCLUDE_SYMBOLS        += RCLCPP_LOG_TIME_POINT_FUNC
+EXCLUDE_SYMBOLS        += RCLCPP_LOG_THROTTLE
+EXCLUDE_SYMBOLS        += RCLCPP_LOG_SKIPFIRST_THROTTLE
+EXCLUDE_SYMBOLS        += RCLCPP_LOG_STREAM
+EXCLUDE_SYMBOLS        += RCLCPP_LOG_STREAM_ONCE
+EXCLUDE_SYMBOLS        += RCLCPP_LOG_STREAM_EXPRESSION
+EXCLUDE_SYMBOLS        += RCLCPP_LOG_STREAM_FUNCTION
+EXCLUDE_SYMBOLS        += RCLCPP_LOG_STREAM_SKIPFIRST
+EXCLUDE_SYMBOLS        += RCLCPP_LOG_STREAM_THROTTLE
+EXCLUDE_SYMBOLS        += RCLCPP_LOG_STREAM_SKIPFIRST_THROTTLE
+PREDEFINED             += DOXYGEN_ONLY
+PREDEFINED             += RCLCPP_LOCAL=
+PREDEFINED             += RCLCPP_PUBLIC=
+PREDEFINED             += RCLCPP_PUBLIC_TYPE=
+PREDEFINED             += RCUTILS_WARN_UNUSED=
+PREDEFINED             += RCPPUTILS_TSA_GUARDED_BY(x)=
+PREDEFINED             += RCPPUTILS_TSA_PT_GUARDED_BY(x)=
+PREDEFINED             += RCPPUTILS_TSA_REQUIRES(x)=
+
+DOT_GRAPH_MAX_NODES    = 101
 
 # Tag files that do not exist will produce a warning and cross-project linking will not work.
-TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/"
+#TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/"
 # Consider changing "latest" to the version you want to reference (e.g. beta1 or 1.0.0)
-TAGFILES += "../../../../doxygen_tag_files/rcl.tag=http://docs.ros2.org/latest/api/rcl/"
-TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/"
-TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/"
+#TAGFILES += "../../../../doxygen_tag_files/rcl.tag=http://docs.ros2.org/latest/api/rcl/"
+#TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/"
+#TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/"
 # Uncomment to generate tag files for cross-project linking.
 #GENERATE_TAGFILE = "../../../../doxygen_tag_files/rclcpp.tag"

rclcpp/QUALITY_DECLARATION.md

@@ -0,0 +1,228 @@
+This document is a declaration of software quality for the `rclcpp` package, based on the guidelines in [REP-2004](https://reps.openrobotics.org/rep-2004/).
+
+# rclcpp Quality Declaration
+
+The package `rclcpp` claims to be in the **Quality Level 1** category when it is used with a **Quality Level 1** middleware.
+
+Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Requirements for Quality Level 1 in REP-2004](https://reps.openrobotics.org/rep-2004/).
+
+## Version Policy [1]
+
+### Version Scheme [1.i]
+
+`rclcpp` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#versioning).
+
+### Version Stability [1.ii]
+
+`rclcpp` is at a stable version, i.e. `>= 1.0.0`.
+The current version can be found in its [package.xml](package.xml), and its change history can be found in its [CHANGELOG](CHANGELOG.rst).
+
+### Public API Declaration [1.iii]
+
+All symbols in the installed headers are considered part of the public API.
+
+Except for the exclusions listed below, all installed headers are in the `include` directory of the package, headers in any other folders are not installed and considered private.
+Headers under the folder `experimental` are not considered part of the public API as they have not yet been stabilized. These symbols are namespaced `rclcpp::experimental`.
+Headers under the folder `detail` are not considered part of the public API and are subject to change without notice. These symbols are namespaced `rclcpp::detail`.
+
+### API Stability Policy [1.iv]
+
+`rclcpp` will not break public API within a released ROS distribution, i.e. no major releases once the ROS distribution is released.
+
+### ABI Stability Policy [1.v]
+
+`rclcpp` contains C++ code and therefore must be concerned with ABI stability, and will maintain ABI stability within a ROS distribution.
+
+### ABI and ABI Stability Within a Released ROS Distribution [1.vi]
+
+`rclcpp` will not break API nor ABI within a released ROS distribution, i.e. no major releases once the ROS distribution is released.
+
+## Change Control Process [2]
+
+`rclcpp` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#change-control-process).
+
+### Change Requests [2.i]
+
+All changes will occur through a pull request, check [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#change-control-process) for additional information.
+
+### Contributor Origin [2.ii]
+
+This package uses DCO as its confirmation of contributor origin policy. More information can be found in [CONTRIBUTING](../CONTRIBUTING.md).
+
+### Peer Review Policy [2.iii]
+
+All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#change-control-process) for additional information.
+
+### Continuous Integration [2.iv]
+
+All pull requests must pass CI on all [tier 1 platforms](https://reps.openrobotics.org/rep-2000/#support-tiers)
+
+Currently nightly results can be seen here:
+
+* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp/)
+* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp/)
+* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp/)
+* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp/)
+
+###  Documentation Policy [2.v]
+
+All pull requests must resolve related documentation changes before merging.
+
+## Documentation [3]
+
+### Feature Documentation [3.i]
+
+`rclcpp` has a [feature list](http://docs.ros2.org/latest/api/rclcpp/) and each item in the list links to the corresponding feature documentation. There is documentation for all of the features, and new features require documentation before being added.
+
+### Public API Documentation [3.ii]
+
+The API is publicly available in its [ROS 2 API documentation](http://docs.ros2.org/latest/api/rclcpp/).
+
+### License [3.iii]
+
+The license for `rclcpp` is Apache 2.0, and a summary is in each source file, the type is declared in the [`package.xml`](./package.xml) manifest file, and a full copy of the license is in the [`LICENSE`](../LICENSE) file.
+
+There is an automated test which runs a linter that ensures each file has a license statement. [Here](http://build.ros2.org/view/Rpr/job/Rpr__rclcpp__ubuntu_focal_amd64/lastCompletedBuild/testReport/rclcpp/) can be found a list with the latest results of the various linters being run on the package.
+
+### Copyright Statements [3.iv]
+
+The copyright holders each provide a statement of copyright in each source code file in `rclcpp`.
+
+There is an automated test which runs a linter that ensures each file has at least one copyright statement. Latest linter result report can be seen [here](http://build.ros2.org/view/Rpr/job/Rpr__rclcpp__ubuntu_focal_amd64/lastCompletedBuild/testReport/rclcpp/copyright/).
+
+## Testing [4]
+
+### Feature Testing [4.i]
+
+Each feature in `rclcpp` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp/tree/rolling/test) directory.
+New features are required to have tests before being added.
+
+Currently nightly test results can be seen here:
+
+* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp/)
+* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp/)
+* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp/)
+* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp/)
+
+### Public API Testing [4.ii]
+
+Each part of the public API has tests, and new additions or changes to the public API require tests before being added.
+The tests aim to cover both typical usage and corner cases, but are quantified by contributing to code coverage.
+
+### Coverage [4.iii]
+
+`rclcpp` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#code-coverage), and opts to use line coverage instead of branch coverage.
+
+This includes:
+
+- tracking and reporting line coverage statistics
+- achieving and maintaining a reasonable branch line coverage (90-100%)
+- no lines are manually skipped in coverage calculations
+
+Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers.
+
+Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_src_rclcpp/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#note-on-coverage-runs).
+
+`rclcpp` has a line coverage `>= 95%`, which is calculated over all directories within `rclcpp` with the exception of the `experimental` directory.
+
+### Performance [4.iv]
+
+`rclcpp` follows the recommendations for performance testing of C/C++ code in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#performance), and opts to do performance analysis on each release rather than each change.
+
+The performance tests of `rclcpp` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/rolling/rclcpp/test/benchmark).
+
+System level performance benchmarks that cover features of `rclcpp` can be found at:
+* [Benchmarks](http://build.ros2.org/view/Rci/job/Rci__benchmark_ubuntu_focal_amd64/BenchmarkTable/)
+* [Performance](http://build.ros2.org/view/Rci/job/Rci__nightly-performance_ubuntu_focal_amd64/lastCompletedBuild/)
+
+Changes that introduce regressions in performance must be adequately justified in order to be accepted and merged.
+
+### Linters and Static Analysis [4.v]
+
+`rclcpp` uses and passes all the ROS 2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms.
+
+Currently nightly test results can be seen here:
+* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp/)
+* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp/)
+* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp/)
+* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp/)
+
+## Dependencies [5]
+
+Below are evaluations of each of `rclcpp`'s run-time and build-time dependencies that have been determined to influence the quality.
+
+It has several "buildtool" dependencies, which do not affect the resulting quality of the package, because they do not contribute to the public library API.
+
+It also has several test dependencies, which do not affect the resulting quality of the package, because they are only used to build and run the test code.
+
+### Direct and Optional Runtime ROS Dependencies [5.i]/[5.ii]
+
+`rclcpp` has the following runtime ROS dependencies:
+
+#### `libstatistics_collector`
+
+The `libstatistics_collector` package provides lightweight aggregation utilities to collect statistics and measure message metrics.
+
+It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros-tooling/libstatistics_collector/tree/rolling/QUALITY_DECLARATION.md).
+
+#### `rcl`
+
+`rcl` a library to support implementation of language specific ROS 2 Client Libraries.
+
+It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/rolling/rcl/QUALITY_DECLARATION.md).
+
+#### `rcl_yaml_param_parser`
+
+The `rcl_yaml_param_parser` package provides an API that is used to parse YAML configuration files which may be used to configure ROS and specific nodes.
+
+It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/tree/rolling/rcl_yaml_param_parser/QUALITY_DECLARATION.md).
+
+#### `rcpputils`
+
+The `rcpputils` package provides an API which contains common utilities and data structures useful when programming in C++.
+
+It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcpputils/blob/rolling/QUALITY_DECLARATION.md).
+
+#### `rcutils`
+
+The `rcutils` package provides an API which contains common utilities and data structures useful when programming in C.
+
+It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcutils/blob/rolling/QUALITY_DECLARATION.md).
+
+#### `rmw`
+
+`rmw` is the ROS 2 middleware library.
+
+It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rmw/blob/rolling/rmw/QUALITY_DECLARATION.md).
+
+#### `statistics_msgs`
+
+The `statistics_msgs` package contains ROS 2 message definitions for reporting statistics for topics and system resources.
+
+It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/rolling/statistics_msgs/QUALITY_DECLARATION.md).
+
+#### `tracetools`
+
+The `tracetools` package provides utilities for instrumenting the code in `rclcpp` so that it may be traced for debugging and performance analysis.
+
+It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/ros2_tracing/blob/rolling/tracetools/QUALITY_DECLARATION.md).
+
+### Direct Runtime non-ROS Dependency [5.iii]
+
+`rclcpp` has no run-time or build-time dependencies that need to be considered for this declaration.
+
+## Platform Support [6]
+
+`rclcpp` supports all of the tier 1 platforms as described in [REP-2000](https://reps.openrobotics.org/rep-2000/#support-tiers), and tests each change against all of them.
+
+Currently nightly build status can be seen here:
+* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/rclcpp/)
+* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/rclcpp/)
+* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/rclcpp/)
+* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/rclcpp/)
+
+## Security
+
+### Vulnerability Disclosure Policy [7.i]
+
+This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://reps.openrobotics.org/rep-2006/).

rclcpp/README.md

@@ -0,0 +1,9 @@
+# `rclcpp`
+
+The ROS client library in C++.
+
+The link to the latest rclcpp API documentation, which includes a complete list of its main components and features, can be found on the [rclcpp package info page](https://docs.ros.org/en/rolling/p/rclcpp).
+
+## Quality Declaration
+
+This package claims to be in the **Quality Level 1** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details.

rclcpp/doc/api_review_march_2020.md

@@ -0,0 +1,436 @@
+# API Review for `rclcpp` from March 2020
+
+## Notes
+
+### Off-Topic Questions
+
+> [rclcpp_action] There exists a thread-safe and non-thread-safe way to get the goal result from an action client. We probably want to remove the public interface to the non-thread safe call (or consolidate somehow): https://github.com/ros2/rclcpp/issues/955
+
+`rclcpp_action` is out of scope atm.
+
+**Notes from 2020-03-19**: To be handled in separate API review.
+
+## Architecture
+
+### Calling Syntax and Keeping Node-like Class APIs in Sync
+
+> Currently, much of the API is exposed via the `rclcpp::Node` class, and due to the nature of the current architecture there is a lot of repeated code to expose these methods and then call the implementations which are in other classes like `rclcpp::node_interfaces::NodeTopics`, for example.
+>
+> Also, we have other versions of the class `rclcpp::Node` with different semantics and interfaces, like `rclcpp_lifecycle::LifecycleNode`, and we have been having trouble keeping the interface provided there up to date with how things are done in `rclcpp::Node`. Since `LifecycleNode` has a different API from `Node` in some important cases, it does not just inherit from `Node`.
+>
+> There are two main proposals (as I see it) to try and address this issue, either (a) break up the functionality in `Node` so that it is in separate classes and make `Node` multiple inherit from those classes, and then `LifecycleNode` could selectively inherit from those as well, or (b) change our calling convention from `node->do_thing(...)` to be `do_thing(node, ...)`.
+>
+> For (a) which commonly referred to as the [Policy Based Design Pattern](https://en.wikipedia.org/wiki/Modern_C%2B%2B_Design#Policy-based_design), we'd be reversing previous design decisions which we discussed at length where we decided to use composition over inheritance for various reasons.
+> One of the reasons was testing, with the theory that having simpler separate interfaces we could more easily mock them as needed for testing.
+> The testing goal would still be met, either by keeping the "node_interface" classes as-is or by mocking the classes that node would multiple inherit from, however it's harder to indicate that a function needs a class that multiple inherits from several classes but not others.
+> Also having interdependency between the classes which are inherited from is a bit complicated in this design pattern.
+>
+> For (b), we would be changing how we recommend all code be written (not a trivial thing to do at all), because example code like `auto pub = node->create_publsiher(...);` would be come `auto pub = create_publisher(node, ...);`.
+> This has some distinct advantages, however, in that it allows us to write these functions, like `create_publisher(node, ...)`, so that the node argument can be any class that meets the criteria of the function.
+> That not only means that when we add a feature it automatically works with `Node` and `LifecycleNode` without adding anything to them, it also means that user defined `Node`-like classes will also work, even if they do not inherit from or provide the complete interface for `rclcpp::Node`.
+> Another major downside of this approach is discoverability of the API when using auto-completion in text editors, as `node-><tab>` will often give you a list of methods to explore, but with the new calling convention, there's not way to get an auto complete for code who's first argument is a `Node`-like class.
+>
+> Both of the above approaches address some of the main concerns, which are: keeping `Node` and `LifecycleNode` in sync, reducing the size of the `Node` class so it is more easily maintained, documented, and so that related functions are grouped more clearly.
+
+- https://github.com/ros2/rclcpp/issues/898
+- https://github.com/ros2/rclcpp/issues/509
+- https://github.com/ros2/rclcpp/issues/855
+- https://github.com/ros2/rclcpp/issues/985
+  - subnode feature is in rclcpp::Node only, complicating "node using" API designs
+- http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2014/n4174.pdf
+- https://en.wikipedia.org/wiki/Uniform_Function_Call_Syntax#C++_proposal
+  - "Many programmers are tempted to write member functions to get the benefits of the member function syntax (e.g. "dot-autocomplete" to list member functions);[6] however, this leads to excessive coupling between classes.[7]"
+
+**Suggested Action**: Document the discussion and defer until Foxy.
+
+**Notes from 2020-03-19**:
+
+- Another version of (b) could be to have classes that are constructed with node, e.g. `Publisher(node, ...)` rather than `node->create_publisher(...)`
+- (tfoote) interface class? `NodeInterface<NodeLike>::something(node_like)`
+  - DRY?
+  - `NodeInterface<LifecycleNode>::<tab>` -> only life cycle node methods
+- (karsten) use interface classes directly, e.g. node->get_X_interface()->do_thing()
+- (dirk) use macros to add methods to class
+  - Question: Do we want tab-completable API (specifically list of member functions)?
+- Question: Is consistency in calling between core and non-core features more important than tab-completion?
+- Add better example of adding new feature and not needing to touch `rclcpp::Node`.
+- (dirk) methods and free functions not mutually exclusive.
+
+### Scoped Versus Non-Scoped Entities (e.g. Publishers/Subscriptions)
+
+> Currently, Publisher and Subscription (and similar entities) are scoped, meaning that when they are created they are added to the ROS graph as a side effect, and when they are let out of scope they remove themselves from the graph too.
+> Additionally, they necessarily have shared state with the system, for instance when you are spinning on a node, the executor shares ownership of the Subscriptions with the user.
+> Therefore, the Subscription only gets removed when both the user and executor are done with it.
+>
+> This shared ownership is accomplished with the use of shared pointers and weak pointers.
+>
+> There are a few concerns here, (a) use of shared pointers confuses users, (b) overhead of shared pointers and lack of an ability to use these classes on the stack rather than the heap, and (c) complexity of shutdown of an entity from the users perspective.
+>
+> For (a), some users are overwhelmed by the need to use a shared pointer.
+> In ROS 1 this was avoided by having a class which itself just thinly wraps a shared pointer (see: https://github.com/ros/ros_comm/blob/ac9f88c59a676ca6895e13445fc7d71f398ebe1f/clients/roscpp/include/ros/subscriber.h#L108-L111).
+> This could be achieved in ROS 2 either by doing the same with a wrapper class (at the expense of lots of repeated code), or by eliminating the need for using shared ownership.
+>
+> For (b), for some use cases, especially resource constrained / real-time / safety-critical environments, requiring these classes to be on the heap rather than the stack is at least inconvenient.
+> Additionally, there is a cost associated with using shared pointers, in the storage of shared state and in some implementation the use of locks or at least atomics for thread-safety.
+>
+> For (c), this is the most concerning drawback, because right now when a user lets their shared pointer to a, for example, Subscription go out of scope, a post condition is not that the Subscription is destroyed, nor that it has been removed from the graph.
+> In stead, the behavior is more like "at some point in the future the Subscription will be destroyed and removed from the graph, when the system is done with it".
+> This isn't a very satisfactory contract, as some users may wish to know when the Subscription has been deleted, but cannot easily know that.
+>
+> The benefit to the shared state is a safety net for users.
+> The alternative would be to document that a Subscription, again for example, cannot be deleted until the system is done with it.
+> We'd basically be pushing the responsibility onto the user to ensure the shared ownership is handled properly by the execution of their application, i.e. they create the Subscription, share a reference with the system (adding it by reference to an executor, for example), and they have to make sure the system is done with it before deleting the Subscription.
+>
+>Separately, from the above points, there is the related concern of forcing the user to keep a copy of their entities in scope, whether it be with a shared pointer or a class wrapping one.
+> There is the desire to create it and forget it in some cases.
+> The downside to this is that if/when the user wants to destroy the entity, they have no way of doing that as they have no handle or unique way to address the entity.
+>
+> One proposed solution would be to have a set of "named X" APIs, e.g. `create_named_subscription` rather than just `create_subscription`.
+> This would allow the user to address the Subscription in the future in order to obtain a new reference to it or delete it.
+
+- https://github.com/ros2/rclcpp/issues/506
+- https://github.com/ros2/rclcpp/issues/726
+
+**Suggested Action**: Consolidate to a single issue, and defer.
+
+**Notes from 2020-03-23**:
+
+- (chris) Putting ownership mechanics on user is hard.
+- (dirk) add documentation clearly outlining ownership
+- (shane) warn on unused to catch issues with immediately deleted items
+- (tfoote) debugging output for destruction so it easy to see when reviewing logs
+- (chris) possible to create API that checks for destruction
+  - (william) might lead to complex synchronization issues
+- (tfoote) could add helper classes to make scoped things non-scoped
+  - (shane) concerned that there is no longer "one good way" to do it
+
+### Allow QoS to be configured externally, like we allow remapping of topic names
+
+> Suggestion from @stonier: allow the qos setting on a topic to be changed externally at startup, similar to how we do topic remapping (e.g., do it on the command-line using appropriate syntax).
+>
+> To keep the syntax manageable, we might just allow profiles to be picked.
+
+- https://github.com/ros2/rclcpp/issues/239
+
+**Suggested Action**: Update issue, defer for now.
+
+**Notes from 2020-03-19**:
+
+- (wjwwood) it depends on the QoS setting, but many don't make sense, mostly because they can change some of the behaviors of underlying API
+- (dirk) Should developers expose a parameter instead?
+- (multiple) should be a feature that makes configuring them (after opt-in) consistent
+- (jacob) customers feedback was that this was expected, surprised it was not allowed
+- (karsten) could limit to profiles
+
+## Init/shutdown and Context
+
+### Consider renaming `rclcpp::ok()`
+
+> Old discussion to rename `rclcpp::ok()` to something more specific, like `rclcpp::is_not_shutdown()` or the corollary `rclcpp::is_shutdown()`.
+
+- https://github.com/ros2/rclcpp/issues/3
+
+**Suggested Action**: Defer.
+
+**Notes from 2020-03-19**:
+
+- (shane) preference to not have a negative in the function name
+
+## Executor
+
+### Exposing Scheduling of Tasks in Executor and a Better Default
+
+> Currently there is a hard coded procedure for handling ready tasks in the executor, first timers, then subscriptions, and so on.
+> This scheduling is not fair and results in non-deterministic behavior and starvation issues.
+>
+> We should provide a better default scheduling which is fairer and ideally deterministic, something like round-robin or FIFO.
+>
+> Additionally, we should make it easier to let the user override the scheduling logic in the executor.
+
+- https://github.com/ros2/rclcpp/pull/614
+- https://github.com/ros2/rclcpp/issues/633
+- https://github.com/ros2/rclcpp/issues/392
+
+**Suggested Action**: Follow up on proposals to implement FIFO scheduling and refactor the Executor design to more easily expose the scheduling logic.
+
+**Notes from 2020-03-19**:
+
+- No comments.
+
+### Make it possible to wait on entities (e.g. Subscriptions) without an Executor
+
+> Currently, it is only possible to use things like Timers and Subscriptions and Services with an executor.
+> It should be possible, however, to either poll these entities or wait on them and then decide which to process as a user.
+>
+> This is most easily accomplished with a WaitSet-like class.
+
+- https://github.com/ros2/rclcpp/issues/520
+
+**Suggested Action**: implement WaitSet class in rclcpp so that this is possible, and make "waitable" entities such that they can be polled, e.g. `Subscription`s should have a user facing `take()` method, which can fail if no data is available.
+
+**Notes from 2020-03-19**:
+
+- No comments.
+
+### Make it possible to use multiple executors per node
+
+> Currently, you cannot use more than one executor per node, this limits your options when it comes to distributing work within a node across threads.
+> You can use a multi-threaded executor, or make your own executor which does this, but it is often convenient to be able to spin part of the node separately from the the rest of the node.
+
+- https://github.com/ros2/rclcpp/issues/519
+
+**Suggested Action**: Make this possible, moving the exclusivity to be between an executor and callback groups rather than nodes.
+
+**Notes from 2020-03-19**:
+
+- No comments.
+
+### Implement a Lock-free Executor
+
+> This would presumably be useful for real-time and safety critical systems where locks and any kind of blocking code is considered undesirable.
+
+- https://github.com/ros2/rclcpp/issues/77
+
+**Suggested Action**: Keep in backlog until someone needs it specifically.
+
+**Notes from 2020-03-19**:
+
+- No comments.
+
+### Add implementation of `spin_some()` to the `MultiThreadedExecutor`
+
+> Currently `spin_some()` is only available in the `SingleThreadedExecutor`.
+
+- https://github.com/ros2/rclcpp/issues/85
+
+**Suggested Action**: Defer.
+
+**Notes from 2020-03-19**:
+
+- No comments.
+
+## Node
+
+### Do argument parsing outside of node constructor
+
+> Things that come from command line arguments should be separately passed into the node's constructor rather than passing in arguments and asking the node to do the parsing.
+
+- https://github.com/ros2/rclcpp/issues/492
+
+**Suggested Action**: Defer until after foxy.
+
+**Notes from 2020-03-23**:
+
+- (dirk) may be related to ROS 1 heritage of argc/argv being passed to node directly
+- (shane) impacts rcl API as well, two parts "global options" as well node specific options
+- (dirk) what is the recommendation to users that want to add arguments programmatically
+  - user should be able to get non-ros argc/argv somehow (seems like you can now)
+- (jacob) the argument in NodeOptions are used for application specific argument via component loading as well
+
+## Timer
+
+### Timer based on ROS Time
+
+> `node->create_wall_timer` does exactly what it says; creates a timer that will call the callback when the wall time expires.  But this is almost never what the user wants, since this won’t work properly in simulation.  Suggestion: deprecate `create_wall_timer`, add a new method called `create_timer` that takes the timer to use as one of the arguments, which defaults to ROS_TIME.
+
+- https://github.com/ros2/rclcpp/blob/96ebf59a6045a535730d98fff25e522807c7aa75/rclcpp/include/rclcpp/node.hpp#L219-L230
+- https://github.com/ros2/rclcpp/issues/465
+
+**Suggested Action**: Promote `rclcpp::create_timer()` which is templated on a clock type, as suggested, but leave `create_wall_timer` as a convenience.
+
+**Notes from 2020-03-19**:
+
+- (shane) may be a `rclcpp::create_timer()` that can be used to create a non-wall timer already
+
+## Publisher
+
+## Subscription
+
+### Callback Signature
+
+> Is there a reason the subscription callback must have a smart pointer argument instead of accepting a const-reference argument?
+
+- https://github.com/ros2/rclcpp/blob/96ebf59a6045a535730d98fff25e522807c7aa75/rclcpp/include/rclcpp/any_subscription_callback.hpp#L44-L52
+- https://github.com/ros2/rclcpp/issues/281
+
+**Suggested Action**: Provide const reference as an option, add documentation as to the implications of one callback signature versus others.
+
+**Notes from 2020-03-19**:
+
+- (dirk) have const reference and document it
+
+## Service Server
+
+### Allow for asynchronous Service Server callbacks
+
+> Currently, the only callback signature for Service Servers takes a request and must return a response.
+> This means that all of the activity of the service server has to happen within that function.
+> This can cause issues, specifically if you want to call another service within the current service server's callback, as it causes deadlock issues trying to synchronously call the second service within a spin callback.
+> More generally, it seems likely that long running service server callbacks may be necessary in the future and requiring them to be synchronous would tie up at least on thread in the spinning executor unnecessarily.
+
+- https://github.com/ros2/rclcpp/issues/491
+
+**Suggested Action**: Defer.
+
+**Notes from 2020-03-23**:
+
+- (dirk) likely new API, so possible to backport
+
+## Service Client
+
+### Callback has SharedFuture rather than const reference to response
+
+> Why does the Client::send_async_request take in a callback that has a SharedFuture argument instead of an argument that is simply a const-reference (or smart pointer) to the service response type? The current API seems to imply that the callback ought to check whether the promise is broken or fulfilled before trying to access it. Is that the case? If so, it should be documented in the header.
+
+- https://github.com/ros2/rclcpp/blob/7c1721a0b390be8242a6b824489d0bc861f6a0ad/rclcpp/include/rclcpp/client.hpp#L134
+
+**Suggested Action**: Update ticket and defer.
+
+**Notes from 2020-03-19**:
+
+- (wjwwood) we wanted the user to handle error cases with the future?
+- (dirk) future allows for single callback (rather than one for response and one for error)
+- (jacob) actions uses a "wrapped result" object
+
+### rclcpp missing synchronous `send_request` and issues with deadlocks
+
+> This has been reported by several users, but there is only an `async_send_request` currently. `rclpy` has a synchronous `send_request` but  it has issues with deadlock, specifically if you call it without spinning in another thread then it will deadlock. Or if you call it from within a spin callback when using a single threaded executor, it will deadlock.
+
+- https://discourse.ros.org/t/synchronous-request-to-service-in-callback-results-in-deadlock/12767
+- https://github.com/ros2/rclcpp/issues/975
+- https://github.com/ros2/demos/blob/948b4f4869298f39cfe99d3ae517ad60a72a8909/demo_nodes_cpp/src/services/add_two_ints_client.cpp#L24-L39
+
+**Suggested Action**: Update issue and defer. Also defer decision on reconciling rclpy's send_request.
+
+**Notes from 2020-03-23**:
+
+- (karsten/shane) async spinner helps in rclpy version, rclcpp could emulate
+- (chris) sees three options:
+  - only async (current case in rclcpp)
+  - have sync version, add lots of docs that spinning needs to happen elsewhere (current case for rclpy)
+  - reentrant spinning
+- (william) you either need async/await from language or ".then" syntax (we have this in async_send_request())
+- (chris) more error checking for recursive spinning
+- (chris) weird that rclcpp and rclpy have different API
+- (shane) thinks it is ok to have different API, but rclpy is not ideal
+
+## Parameters
+
+### Expected vs Unexpected parameters
+
+> Allow node author to define expected parameters and what happens when an unexpected parameter is set.
+
+- https://github.com/ros2/rclcpp/issues/475
+- https://github.com/ros2/rclcpp/tree/check_parameters
+
+**Suggested Action**: Defer as nice to have.
+
+**Notes from 2020-03-23**:
+
+- None.
+
+### Implicitly cast integer values for double parameters
+
+> If we try to pass an integer value to a double parameter from the command line or from a parameters YAML file we get a `rclcpp::ParameterTypeException`.
+> For example, passing a parameter from the command line:
+>
+>     ros2 run foo_package foo_node --ros-args -p foo_arg:=1
+>
+> results in the following error:
+>
+>     terminate called after throwing an instance of 'rclcpp::ParameterTypeException'
+>       what():  expected [double] got [integer]
+>
+> and we can fix it by explicitly making our value a floating point number:
+>
+>     ros2 run foo_package foo_node --ros-args -p foo_arg:=1.0
+>
+> But, it seems reasonable to me that if a user forgets to explicitly provide a floating point value that we should implicitly cast an integer to a float (as is done in many programming languages).
+
+- https://github.com/ros2/rclcpp/issues/979
+
+**Suggested Action**: Continue with issue.
+
+**Notes from 2020-03-23**:
+
+- (shane) says "yes please" :)
+
+### Use `std::variant` instead of custom `ParameterValue` class
+
+> This is only possible if C++17 is available, but it would simplify our code, make our interface more standard, and allow us to use constexpr-if to simply our templated code.
+
+**Suggested Action**: Create an issue for future work.
+
+**Notes from 2020-03-23**:
+
+- (chris) not sure churn is worth
+- (ivan) other places for std::variant, like AnySubscriptionCallback
+
+### Cannot set name or value on `Parameter`/`ParameterValue`
+
+> Both `Parameter` and `ParameterValue` are read-only after construction.
+
+- https://github.com/ros2/rclcpp/issues/238
+
+**Suggested Action**: Update issue, possibly close.
+
+**Notes from 2020-03-23**:
+
+- (chris/william) setting values on temporary (local) objects is not reflected in the node, so misleading
+
+## Parameter Clients
+
+### No timeout option with synchronous parameter client calls
+
+> As an example, SyncParametersClient::set_parameters doesn't take a timeout option. So, if anything goes wrong in the service call (e.g. the server goes down), we will get stuck waiting indefinitely.
+
+- https://github.com/ros2/rclcpp/issues/360
+- https://github.com/ros2/rclcpp/blob/96ebf59a6045a535730d98fff25e522807c7aa75/rclcpp/src/rclcpp/parameter_client.cpp#L453-L468
+
+**Suggested Action**: Update issue, decide if it can be taken for Foxy or not.
+
+**Notes from 2020-03-23**:
+
+- (tfoote) Seems like adding a timeout is a good idea.
+
+### Name of AsyncParametersClient inconsistent
+
+> AsyncParameter**s**Client uses plural, when filename is singular (and ParameterService is singular):
+
+- https://github.com/ros2/rclcpp/blob/7c1721a0b390be8242a6b824489d0bc861f6a0ad/rclcpp/include/rclcpp/parameter_client.hpp#L44
+
+**Suggested Action**: Reconcile class and file name, switch to singular name?
+
+**Notes from on-line, post 2020-03-23 meeting**:
+
+- (tfoote) +1 for homogenizing to singular
+
+### `SyncParametersClient::get_parameters` doesn't allow you to detect error cases
+
+> E.g. https://github.com/ros2/rclcpp/blob/249b7d80d8f677edcda05052f598de84f4c2181c/rclcpp/src/rclcpp/parameter_client.cpp#L246-L257 returns an empty vector if something goes wrong which is also a valid response.
+
+- https://github.com/ros2/rclcpp/issues/200
+- https://github.com/ros2/rclcpp/blob/96ebf59a6045a535730d98fff25e522807c7aa75/rclcpp/src/rclcpp/parameter_client.cpp#L412-L426
+
+**Suggested Action**: Throw an exception to indicate if something went wrong and document other expected conditions of the API.
+
+**Notes from on-line, post 2020-03-23 meeting**:
+
+- (tfoote) An empty list is not a valid response unless you passed in an empty list. The return should have the same length as the request in the same order. Any parameters that are not set should return a ParameterVariant with type PARAMETER_NOT_SET. to indicate that it was polled and determined to not be set. Suggested action improve documentation of the API to clarify a short or incomplete.
+- (jacobperron) I think throwing an exception is also a valid action, making it clear that an error occurred.
+- (wjwwood) Using exceptions to indicate an exceptional case (something went wrong) seems reasonable to me too.
+
+## Clock
+
+### Clock Jump callbacks on System or Steady time?
+
+> Currently time jump callbacks are registered via Clock::create_jump_handler(). Jump handlers are only invoked by TimeSource::set_clock(). This is only called if the clock type is RCL_ROS_TIME and ROS time is active.
+
+- https://github.com/ros2/rclcpp/issues/528
+
+**Suggested Action**: Document that time jumping is only detected with ROS time, consider a warning.
+
+**Notes from on-line, post 2020-03-23 meeting**:
+
+- (tfoote) There should be no jumps in steady time. If there's a big change in system time, it doesn't necessarily mean that time jumped, just that you might have been sleeping for a long time. Most ntp systems adjust the slew rate these days instead of jumping but still that's an external process and I don't know of any APIs to introspect the state of the clock. I'm not sure that we have a way to detect jumps in time for system or steady time. To that end I think that we should be clear that we only provide callbacks when simulation time starts or stops, or simulation time jumps. We should also strongly recommend that operators not actively adjust their system clocks while running ROS nodes.
+- (jacobperron) I agree with Tully, if we don't have a way to detect system time jumps then I think we should just document that this only works with ROS time. In addition to documentation, we could log an info or warning message if the user registers jump callback with steady or system time, but it may be unnecessarily noisy.

rclcpp/doc/notes_on_statically_typed_parameters.md

@@ -0,0 +1,141 @@
+# Notes on statically typed parameters
+
+## Introduction
+
+Until ROS 2 Foxy, all parameters could change type anytime, except if the user installed a parameter callback to reject a change.
+This could generate confusing errors, for example:
+
+```
+$ ros2 run demo_nodes_py listener &
+$ ros2 param set /listener use_sim_time not_a_boolean
+[ERROR] [1614712713.233733147] [listener]: use_sim_time parameter set to something besides a bool
+Set parameter successful
+$ ros2 param get /listener use_sim_time
+String value is: not_a_boolean
+```
+
+For most use cases, having static parameter types is enough.
+This article documents some of the decisions that were made when implementing static parameter types enforcement in:
+
+* https://github.com/ros2/rclcpp/pull/1522
+* https://github.com/ros2/rclpy/pull/683
+
+## Allowing dynamically typed parameters
+
+There might be some scenarios where dynamic typing is desired, so a `dynamic_typing` field was added to the [parameter descriptor](https://github.com/ros2/rcl_interfaces/blob/09b5ed93a733adb9deddc47f9a4a8c6e9f584667/rcl_interfaces/msg/ParameterDescriptor.msg#L25).
+It defaults to `false`.
+
+For example:
+
+```cpp
+rcl_interfaces::msg::ParameterDescriptor descriptor;
+descriptor.dynamic_typing = true;
+
+node->declare_parameter("dynamically_typed_parameter", rclcpp::ParameterValue{}, descriptor);
+```
+
+```py
+rcl_interfaces.msg.ParameterDescriptor descriptor;
+descriptor.dynamic_typing = True;
+
+node.declare_parameter("dynamically_typed_parameter", None, descriptor);
+```
+
+## How is the parameter type specified?
+
+The parameter type will be inferred from the default value provided when declaring it.
+
+## Statically typed parameters when allowing undeclared parameters
+
+When undeclared parameters are allowed and a parameter is set without a previous declaration, the parameter will be dynamically typed.
+This is consistent with other allowed behaviors when undeclared parameters are allowed, e.g. trying to get an undeclared parameter returns "NOT_SET".
+Parameter declarations will remain special and dynamic or static typing will be used based on the parameter descriptor (default to static).
+
+## Declaring a parameter without a default value
+
+There might be cases were a default value does not make sense and the user must always provide an override.
+In those cases, the parameter type can be specified explicitly:
+
+```cpp
+// method signature
+template<typename T>
+Node::declare_parameter<T>(std::string name, rcl_interfaces::msg::ParameterDescriptor = rcl_interfaces::msg::ParameterDescriptor{});
+// or alternatively
+Node::declare_parameter(std::string name, rclcpp::ParameterType type, rcl_interfaces::msg::ParameterDescriptor = rcl_interfaces::msg::ParameterDescriptor{});
+
+// examples
+node->declare_parameter<int64_t>("my_integer_parameter");  // declare an integer parameter
+node->declare_parameter("another_integer_parameter", rclcpp::ParameterType::PARAMETER_INTEGER);  // another way to do the same
+```
+
+```py
+# method signature
+Node.declare_parameter(name: str, param_type: rclpy.Parameter.Type, descriptor: rcl_interfaces.msg.ParameterDescriptor = rcl_interfaces.msg.ParameterDescriptor())
+
+# example
+node.declare_parameter('my_integer_parameter', rclpy.Parameter.Type.INTEGER);  # declare an integer parameter
+```
+
+If the parameter may be unused, but when used requires a parameter override, then you could conditionally declare it:
+
+```cpp
+auto mode = node->declare_parameter("mode", "modeA");  // "mode" parameter is an string
+if (mode == "modeB") {
+    node->declare_parameter<int64_t>("param_needed_when_mode_b");  // when "modeB", the user must provide "param_needed_when_mode_b"
+}
+```
+
+## Other migration notes
+
+Declaring a parameter with only a name is deprecated:
+
+```cpp
+node->declare_parameter("my_param");  // this generates a build warning
+```
+
+```py
+node.declare_parameter("my_param");  # this generates a python user warning
+```
+
+Before, you could initialize a parameter with the "NOT SET" value (in cpp `rclcpp::ParameterValue{}`, in python `None`).
+Now this will throw an exception in both cases:
+
+```cpp
+node->declare_parameter("my_param", rclcpp::ParameterValue{});  // not valid, will throw exception
+```
+
+```py
+node.declare_parameter("my_param", None);  # not valid, will raise error
+```
+
+## Possible improvements
+
+### Easier way to declare dynamically typed parameters
+
+Declaring a dynamically typed parameter in `rclcpp` could be considered to be a bit verbose:
+
+```cpp
+rcl_interfaces::msg::ParameterDescriptor descriptor;
+descriptor.dynamic_typing = true;
+
+node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);
+```
+
+the following ways could be supported to make it simpler:
+
+```cpp
+node->declare_parameter(name, rclcpp::PARAMETER_DYNAMIC);
+node->declare_parameter(name, default_value, rclcpp::PARAMETER_DYNAMIC);
+```
+
+or alternatively:
+
+```cpp
+node->declare_parameter(name, default_value, rclcpp::ParameterDescriptor{}.dynamic_typing());
+```
+
+For `rclpy`, there's already a short way to do it:
+
+```py
+node.declare_parameter(name, default_value, rclpy.ParameterDescriptor(dynamic_typing=true));
+```

rclcpp/doc/proposed_node_parameter_callbacks.md

@@ -0,0 +1,29 @@
+# Proposed node parameters callback Design
+
+## Introduction:
+
+The original requirement came in **gazebo_ros_pkgs** for setting individual wheel slip parameters based on global wheel slip value [link to original issue](https://github.com/ros-simulation/gazebo_ros_pkgs/pull/1365).
+
+The main requirement is to set one or more parameters after another parameter is set successfully.
+
+Additionally, it would be nice if users could be notified locally (via a callback) when parameters have been set successfully (i.e. post validation).
+
+Related discussion can be found in [#609](https://github.com/ros2/rclcpp/issues/609) [#1789](https://github.com/ros2/rclcpp/pull/1789)
+
+With the current parameters API, the `add_on_set_parameters_callback` is intended for validation of parameter values before they are set, it should **not** cause any side-effects.
+
+There is also the `ParameterEventHandler` that publishes changes to node parameters on `/parameter_events` topic for external nodes to see. Though the node could subscribe to the `/parameter_events` topic to be notified of changes to its own parameters, it is less than ideal since there is a delay caused by waiting for an executor to process the callback.
+
+We propose adding a `PostSetParametersCallbackHandle` for successful parameter set similar to `OnSetParametersCallbackHandle` for parameter validation. Also, we propose adding a `PreSetParametersCallbackHandle` useful for modifying list of parameters being set.
+
+The validation callback is often abused to trigger side effects in the code, for instance updating class attributes even before a parameter has been set successfully. Instead of relying on the `/parameter_events` topic to be notified of parameter changes, users can register a callback with a new API, `add_post_set_parameters_callback`.
+
+It is possible to use the proposed `add_post_set_parameters_callback` for setting additional parameters, but this might result in infinite recursion and does not allow those additional parameters to be set atomically with the original parameter(s) changed.
+To workaround these issues, we propose adding a "pre set" callback type that can be registered with `add_pre_set_parameters_callback`, which will be triggered before the validation callbacks and can be used to modify the parameter list.
+
+![Desgin API](https://github.com/ros2/rclcpp/blob/deepanshu/local-param-changed-callback-support/rclcpp/doc/param_callback_design.png?raw=true)
+
+## Alternatives
+
+* Users could call `set_parameter` while processing a message from the `/parameter_events` topic, however, there is extra overhead in having to create subscription (as noted earlier).
+* Users could call `set_parameter` inside the "on set" parameters callback, however it is not well-defined how side-effects should handle cases where parameter validation fails.

rclcpp/include/rclcpp/allocator/allocator_common.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_
 #define RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_
 
+#include <cstring>
 #include <memory>
 
 #include "rcl/allocator.h"
@@ -39,6 +40,22 @@ void * retyped_allocate(size_t size, void * untyped_allocator)
   return std::allocator_traits<Alloc>::allocate(*typed_allocator, size);
 }
 
+template<typename Alloc>
+void * retyped_zero_allocate(size_t number_of_elem, size_t size_of_elem, void * untyped_allocator)
+{
+  auto typed_allocator = static_cast<Alloc *>(untyped_allocator);
+  if (!typed_allocator) {
+    throw std::runtime_error("Received incorrect allocator type");
+  }
+  size_t size = number_of_elem * size_of_elem;
+  void * allocated_memory =
+    std::allocator_traits<Alloc>::allocate(*typed_allocator, size);
+  if (allocated_memory) {
+    std::memset(allocated_memory, 0, size);
+  }
+  return allocated_memory;
+}
+
 template<typename T, typename Alloc>
 void retyped_deallocate(void * untyped_pointer, void * untyped_allocator)
 {
@@ -73,6 +90,7 @@ rcl_allocator_t get_rcl_allocator(Alloc & allocator)
   rcl_allocator_t rcl_allocator = rcl_get_default_allocator();
 #ifndef _WIN32
   rcl_allocator.allocate = &retyped_allocate<Alloc>;
+  rcl_allocator.zero_allocate = &retyped_zero_allocate<Alloc>;
   rcl_allocator.deallocate = &retyped_deallocate<T, Alloc>;
   rcl_allocator.reallocate = &retyped_reallocate<T, Alloc>;
   rcl_allocator.state = &allocator;

rclcpp/include/rclcpp/allocator/allocator_deleter.hpp

@@ -41,7 +41,7 @@ public:
   }
 
   template<typename T>
-  AllocatorDeleter(const AllocatorDeleter<T> & a)
+  explicit AllocatorDeleter(const AllocatorDeleter<T> & a)
   {
     allocator_ = a.get_allocator();
   }
@@ -69,18 +69,17 @@ private:
 };
 
 template<typename Alloc, typename T, typename D>
-void set_allocator_for_deleter(D * deleter, Alloc * alloc)
+void set_allocator_for_deleter([[maybe_unused]] D * deleter, [[maybe_unused]] Alloc * alloc)
 {
-  (void) alloc;
-  (void) deleter;
   throw std::runtime_error("Reached unexpected template specialization");
 }
 
 template<typename T, typename U>
-void set_allocator_for_deleter(std::default_delete<T> * deleter, std::allocator<U> * alloc)
+void set_allocator_for_deleter(
+  [[maybe_unused]] std::default_delete<T> * deleter,
+  [[maybe_unused]] std::allocator<U> * alloc)
 {
-  (void) deleter;
-  (void) alloc;
+  // This function is intentionally left empty.
 }
 
 template<typename Alloc, typename T>
@@ -95,7 +94,7 @@ void set_allocator_for_deleter(AllocatorDeleter<T> * deleter, Alloc * alloc)
 template<typename Alloc, typename T>
 using Deleter = typename std::conditional<
   std::is_same<typename std::allocator_traits<Alloc>::template rebind_alloc<T>,
-  typename std::allocator<void>::template rebind<T>::other>::value,
+  std::allocator<T>>::value,
   std::default_delete<T>,
   AllocatorDeleter<Alloc>
   >::type;

rclcpp/include/rclcpp/any_executable.hpp

@@ -29,8 +29,6 @@
 
 namespace rclcpp
 {
-namespace executor
-{
 
 struct AnyExecutable
 {
@@ -42,17 +40,16 @@ struct AnyExecutable
 
   // Only one of the following pointers will be set.
   rclcpp::SubscriptionBase::SharedPtr subscription;
-  rclcpp::SubscriptionBase::SharedPtr subscription_intra_process;
   rclcpp::TimerBase::SharedPtr timer;
   rclcpp::ServiceBase::SharedPtr service;
   rclcpp::ClientBase::SharedPtr client;
   rclcpp::Waitable::SharedPtr waitable;
   // These are used to keep the scope on the containing items
-  rclcpp::callback_group::CallbackGroup::SharedPtr callback_group;
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base;
+  rclcpp::CallbackGroup::SharedPtr callback_group {nullptr};
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base {nullptr};
+  std::shared_ptr<void> data {nullptr};
 };
 
-}  // namespace executor
 }  // namespace rclcpp
 
 #endif  // RCLCPP__ANY_EXECUTABLE_HPP_

rclcpp/include/rclcpp/any_service_callback.hpp

@@ -15,86 +15,226 @@
 #ifndef RCLCPP__ANY_SERVICE_CALLBACK_HPP_
 #define RCLCPP__ANY_SERVICE_CALLBACK_HPP_
 
+#include <variant>
 #include <functional>
 #include <memory>
 #include <stdexcept>
 #include <type_traits>
+#include <utility>
 
 #include "rclcpp/function_traits.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rmw/types.h"
+#include "tracetools/tracetools.h"
+#include "tracetools/utils.hpp"
 
 namespace rclcpp
 {
 
+namespace detail
+{
+template<typename T, typename = void>
+struct can_be_nullptr : std::false_type {};
+
+// Some lambdas define a comparison with nullptr,
+// but we see a warning that they can never be null when using it.
+// We also test if `T &` can be assigned to `nullptr` to avoid the issue.
+template<typename T>
+#ifdef __QNXNTO__
+struct can_be_nullptr<T, std::void_t<
+    decltype(std::declval<T>() == nullptr)>>: std::true_type {};
+#else
+struct can_be_nullptr<T, std::void_t<
+    decltype(std::declval<T>() == nullptr), decltype(std::declval<T &>() = nullptr)>>
+  : std::true_type {};
+#endif
+}  // namespace detail
+
+// Forward declare
+template<typename ServiceT>
+class Service;
+
 template<typename ServiceT>
 class AnyServiceCallback
 {
+public:
+  AnyServiceCallback()
+  : callback_(std::monostate{})
+  {}
+
+  template<
+    typename CallbackT,
+    typename std::enable_if_t<!detail::can_be_nullptr<CallbackT>::value, int> = 0>
+  void
+  set(CallbackT && callback)
+  {
+    // Workaround Windows issue with std::bind
+    if constexpr (
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrCallback
+      >::value)
+    {
+      callback_.template emplace<SharedPtrCallback>(callback);
+    } else if constexpr (  // NOLINT, can't satisfy both cpplint and uncrustify
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrWithRequestHeaderCallback
+      >::value)
+    {
+      callback_.template emplace<SharedPtrWithRequestHeaderCallback>(callback);
+    } else if constexpr (  // NOLINT
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrDeferResponseCallback
+      >::value)
+    {
+      callback_.template emplace<SharedPtrDeferResponseCallback>(callback);
+    } else if constexpr (  // NOLINT
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrDeferResponseCallbackWithServiceHandle
+      >::value)
+    {
+      callback_.template emplace<SharedPtrDeferResponseCallbackWithServiceHandle>(callback);
+    } else {
+      // the else clause is not needed, but anyways we should only be doing this instead
+      // of all the above workaround ...
+      callback_ = std::forward<CallbackT>(callback);
+    }
+  }
+
+  template<
+    typename CallbackT,
+    typename std::enable_if_t<detail::can_be_nullptr<CallbackT>::value, int> = 0>
+  void
+  set(CallbackT && callback)
+  {
+    if (!callback) {
+      throw std::invalid_argument("AnyServiceCallback::set(): callback cannot be nullptr");
+    }
+    // Workaround Windows issue with std::bind
+    if constexpr (
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrCallback
+      >::value)
+    {
+      callback_.template emplace<SharedPtrCallback>(callback);
+    } else if constexpr (  // NOLINT
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrWithRequestHeaderCallback
+      >::value)
+    {
+      callback_.template emplace<SharedPtrWithRequestHeaderCallback>(callback);
+    } else if constexpr (  // NOLINT
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrDeferResponseCallback
+      >::value)
+    {
+      callback_.template emplace<SharedPtrDeferResponseCallback>(callback);
+    } else if constexpr (  // NOLINT
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrDeferResponseCallbackWithServiceHandle
+      >::value)
+    {
+      callback_.template emplace<SharedPtrDeferResponseCallbackWithServiceHandle>(callback);
+    } else {
+      // the else clause is not needed, but anyways we should only be doing this instead
+      // of all the above workaround ...
+      callback_ = std::forward<CallbackT>(callback);
+    }
+  }
+
+  // template<typename Allocator = std::allocator<typename ServiceT::Response>>
+  std::shared_ptr<typename ServiceT::Response>
+  dispatch(
+    const std::shared_ptr<rclcpp::Service<ServiceT>> & service_handle,
+    const std::shared_ptr<rmw_request_id_t> & request_header,
+    std::shared_ptr<typename ServiceT::Request> request)
+  {
+    TRACETOOLS_TRACEPOINT(callback_start, static_cast<const void *>(this), false);
+    if (std::holds_alternative<std::monostate>(callback_)) {
+      // TODO(ivanpauno): Remove the set method, and force the users of this class
+      // to pass a callback at construnciton.
+      throw std::runtime_error{"unexpected request without any callback set"};
+    }
+    if (std::holds_alternative<SharedPtrDeferResponseCallback>(callback_)) {
+      const auto & cb = std::get<SharedPtrDeferResponseCallback>(callback_);
+      cb(request_header, std::move(request));
+      TRACETOOLS_TRACEPOINT(callback_end, static_cast<const void *>(this));
+      return nullptr;
+    }
+    if (std::holds_alternative<SharedPtrDeferResponseCallbackWithServiceHandle>(callback_)) {
+      const auto & cb = std::get<SharedPtrDeferResponseCallbackWithServiceHandle>(callback_);
+      cb(service_handle, request_header, std::move(request));
+      TRACETOOLS_TRACEPOINT(callback_end, static_cast<const void *>(this));
+      return nullptr;
+    }
+    // auto response = allocate_shared<typename ServiceT::Response, Allocator>();
+    auto response = std::make_shared<typename ServiceT::Response>();
+    if (std::holds_alternative<SharedPtrCallback>(callback_)) {
+      (void)request_header;
+      const auto & cb = std::get<SharedPtrCallback>(callback_);
+      cb(std::move(request), response);
+    } else if (std::holds_alternative<SharedPtrWithRequestHeaderCallback>(callback_)) {
+      const auto & cb = std::get<SharedPtrWithRequestHeaderCallback>(callback_);
+      cb(request_header, std::move(request), response);
+    }
+    TRACETOOLS_TRACEPOINT(callback_end, static_cast<const void *>(this));
+    return response;
+  }
+
+  void register_callback_for_tracing()
+  {
+#ifndef TRACETOOLS_DISABLED
+    std::visit(
+      [this](auto && arg) {
+        if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) {
+          char * symbol = tracetools::get_symbol(arg);
+          TRACETOOLS_DO_TRACEPOINT(
+            rclcpp_callback_register,
+            static_cast<const void *>(this),
+            symbol);
+          std::free(symbol);
+        }
+      }, callback_);
+#endif  // TRACETOOLS_DISABLED
+  }
+
 private:
   using SharedPtrCallback = std::function<
     void (
-      const std::shared_ptr<typename ServiceT::Request>,
+      std::shared_ptr<typename ServiceT::Request>,
       std::shared_ptr<typename ServiceT::Response>
     )>;
   using SharedPtrWithRequestHeaderCallback = std::function<
     void (
-      const std::shared_ptr<rmw_request_id_t>,
-      const std::shared_ptr<typename ServiceT::Request>,
+      std::shared_ptr<rmw_request_id_t>,
+      std::shared_ptr<typename ServiceT::Request>,
       std::shared_ptr<typename ServiceT::Response>
     )>;
+  using SharedPtrDeferResponseCallback = std::function<
+    void (
+      std::shared_ptr<rmw_request_id_t>,
+      std::shared_ptr<typename ServiceT::Request>
+    )>;
+  using SharedPtrDeferResponseCallbackWithServiceHandle = std::function<
+    void (
+      std::shared_ptr<rclcpp::Service<ServiceT>>,
+      std::shared_ptr<rmw_request_id_t>,
+      std::shared_ptr<typename ServiceT::Request>
+    )>;
 
-  SharedPtrCallback shared_ptr_callback_;
-  SharedPtrWithRequestHeaderCallback shared_ptr_with_request_header_callback_;
-
-public:
-  AnyServiceCallback()
-  : shared_ptr_callback_(nullptr), shared_ptr_with_request_header_callback_(nullptr)
-  {}
-
-  AnyServiceCallback(const AnyServiceCallback &) = default;
-
-  template<
-    typename CallbackT,
-    typename std::enable_if<
-      rclcpp::function_traits::same_arguments<
-        CallbackT,
-        SharedPtrCallback
-      >::value
-    >::type * = nullptr
-  >
-  void set(CallbackT callback)
-  {
-    shared_ptr_callback_ = callback;
-  }
-
-  template<
-    typename CallbackT,
-    typename std::enable_if<
-      rclcpp::function_traits::same_arguments<
-        CallbackT,
-        SharedPtrWithRequestHeaderCallback
-      >::value
-    >::type * = nullptr
-  >
-  void set(CallbackT callback)
-  {
-    shared_ptr_with_request_header_callback_ = callback;
-  }
-
-  void dispatch(
-    std::shared_ptr<rmw_request_id_t> request_header,
-    std::shared_ptr<typename ServiceT::Request> request,
-    std::shared_ptr<typename ServiceT::Response> response)
-  {
-    if (shared_ptr_callback_ != nullptr) {
-      (void)request_header;
-      shared_ptr_callback_(request, response);
-    } else if (shared_ptr_with_request_header_callback_ != nullptr) {
-      shared_ptr_with_request_header_callback_(request_header, request, response);
-    } else {
-      throw std::runtime_error("unexpected request without any callback set");
-    }
-  }
+  std::variant<
+    std::monostate,
+    SharedPtrCallback,
+    SharedPtrWithRequestHeaderCallback,
+    SharedPtrDeferResponseCallback,
+    SharedPtrDeferResponseCallbackWithServiceHandle> callback_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/callback_group.hpp

@@ -16,14 +16,17 @@
 #define RCLCPP__CALLBACK_GROUP_HPP_
 
 #include <atomic>
+#include <functional>
+#include <memory>
 #include <mutex>
-#include <string>
 #include <vector>
 
 #include "rclcpp/client.hpp"
-#include "rclcpp/publisher.hpp"
+#include "rclcpp/context.hpp"
+#include "rclcpp/guard_condition.hpp"
+#include "rclcpp/publisher_base.hpp"
 #include "rclcpp/service.hpp"
-#include "rclcpp/subscription.hpp"
+#include "rclcpp/subscription_base.hpp"
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rclcpp/waitable.hpp"
@@ -40,9 +43,6 @@ class NodeTopics;
 class NodeWaitables;
 }  // namespace node_interfaces
 
-namespace callback_group
-{
-
 enum class CallbackGroupType
 {
   MutuallyExclusive,
@@ -59,37 +59,170 @@ class CallbackGroup
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup)
 
+  /// Constructor for CallbackGroup.
+  /**
+   * Callback Groups have a type, either 'Mutually Exclusive' or 'Reentrant'
+   * and when creating one the type must be specified.
+   *
+   * Callbacks in Reentrant Callback Groups must be able to:
+   *   - run at the same time as themselves (reentrant)
+   *   - run at the same time as other callbacks in their group
+   *   - run at the same time as other callbacks in other groups
+   *
+   * Callbacks in Mutually Exclusive Callback Groups:
+   *   - will not be run multiple times simultaneously (non-reentrant)
+   *   - will not be run at the same time as other callbacks in their group
+   *   - but must run at the same time as callbacks in other groups
+   *
+   * Additionally, callback groups have a property which determines whether or
+   * not they are added to an executor with their associated node automatically.
+   * When creating a callback group the automatically_add_to_executor_with_node
+   * argument determines this behavior, and if true it will cause the newly
+   * created callback group to be added to an executor with the node when the
+   * Executor::add_node method is used.
+   * If false, this callback group will not be added automatically and would
+   * have to be added to an executor manually using the
+   * Executor::add_callback_group method.
+   *
+   * Whether the node was added to the executor before creating the callback
+   * group, or after, is irrelevant; the callback group will be automatically
+   * added to the executor in either case.
+   *
+   * \param[in] group_type The type of the callback group.
+   * \param[in] context A weak pointer to the context associated with this callback group.
+   * \param[in] automatically_add_to_executor_with_node A boolean that
+   *   determines whether a callback group is automatically added to an executor
+   *   with the node with which it is associated.
+   */
   RCLCPP_PUBLIC
-  explicit CallbackGroup(CallbackGroupType group_type);
+  explicit CallbackGroup(
+    CallbackGroupType group_type,
+    rclcpp::Context::WeakPtr context,
+    bool automatically_add_to_executor_with_node = true);
 
+  /// Default destructor.
   RCLCPP_PUBLIC
-  const std::vector<rclcpp::SubscriptionBase::WeakPtr> &
-  get_subscription_ptrs() const;
+  ~CallbackGroup();
 
+  template<typename Function>
+  rclcpp::SubscriptionBase::SharedPtr
+  find_subscription_ptrs_if(Function func) const
+  {
+    return _find_ptrs_if_impl<rclcpp::SubscriptionBase, Function>(func, subscription_ptrs_);
+  }
+
+  template<typename Function>
+  rclcpp::TimerBase::SharedPtr
+  find_timer_ptrs_if(Function func) const
+  {
+    return _find_ptrs_if_impl<rclcpp::TimerBase, Function>(func, timer_ptrs_);
+  }
+
+  template<typename Function>
+  rclcpp::ServiceBase::SharedPtr
+  find_service_ptrs_if(Function func) const
+  {
+    return _find_ptrs_if_impl<rclcpp::ServiceBase, Function>(func, service_ptrs_);
+  }
+
+  template<typename Function>
+  rclcpp::ClientBase::SharedPtr
+  find_client_ptrs_if(Function func) const
+  {
+    return _find_ptrs_if_impl<rclcpp::ClientBase, Function>(func, client_ptrs_);
+  }
+
+  template<typename Function>
+  rclcpp::Waitable::SharedPtr
+  find_waitable_ptrs_if(Function func) const
+  {
+    return _find_ptrs_if_impl<rclcpp::Waitable, Function>(func, waitable_ptrs_);
+  }
+
+  /// Get the total number of entities in this callback group.
+  /**
+   * \return the number of entities in the callback group.
+   */
   RCLCPP_PUBLIC
-  const std::vector<rclcpp::TimerBase::WeakPtr> &
-  get_timer_ptrs() const;
-
-  RCLCPP_PUBLIC
-  const std::vector<rclcpp::ServiceBase::WeakPtr> &
-  get_service_ptrs() const;
-
-  RCLCPP_PUBLIC
-  const std::vector<rclcpp::ClientBase::WeakPtr> &
-  get_client_ptrs() const;
-
-  RCLCPP_PUBLIC
-  const std::vector<rclcpp::Waitable::WeakPtr> &
-  get_waitable_ptrs() const;
+  size_t
+  size() const;
 
+  /// Return a reference to the 'can be taken' atomic boolean.
+  /**
+   * The resulting bool will be true in the case that no executor is currently
+   * using an executable entity from this group.
+   * The resulting bool will be false in the case that an executor is currently
+   * using an executable entity from this group, and the group policy doesn't
+   * allow a second take (eg mutual exclusion)
+   * \return a reference to the flag
+   */
   RCLCPP_PUBLIC
   std::atomic_bool &
   can_be_taken_from();
 
+  /// Get the group type.
+  /**
+   * \return the group type
+   */
   RCLCPP_PUBLIC
   const CallbackGroupType &
   type() const;
 
+  /// Collect all of the entity pointers contained in this callback group.
+  /**
+   * \param[in] sub_func Function to execute for each subscription
+   * \param[in] service_func Function to execute for each service
+   * \param[in] client_func Function to execute for each client
+   * \param[in] timer_func Function to execute for each timer
+   * \param[in] waitable_fuinc Function to execute for each waitable
+   */
+  RCLCPP_PUBLIC
+  void
+  collect_all_ptrs(
+    std::function<void(const rclcpp::SubscriptionBase::SharedPtr &)> sub_func,
+    std::function<void(const rclcpp::ServiceBase::SharedPtr &)> service_func,
+    std::function<void(const rclcpp::ClientBase::SharedPtr &)> client_func,
+    std::function<void(const rclcpp::TimerBase::SharedPtr &)> timer_func,
+    std::function<void(const rclcpp::Waitable::SharedPtr &)> waitable_func) const;
+
+  /// Return a reference to the 'associated with executor' atomic boolean.
+  /**
+   * When a callback group is added to an executor this boolean is checked
+   * to ensure it has not already been added to another executor.
+   * If it has not been, then this boolean is set to true to indicate it is
+   * now associated with an executor.
+   *
+   * When the callback group is removed from the executor, this atomic boolean
+   * is set back to false.
+   *
+   * \return the 'associated with executor' atomic boolean
+   */
+  RCLCPP_PUBLIC
+  std::atomic_bool &
+  get_associated_with_executor_atomic();
+
+  /// Return true if this callback group should be automatically added to an executor by the node.
+  /**
+   * \return boolean true if this callback group should be automatically added
+   *   to an executor when the associated node is added, otherwise false.
+   */
+  RCLCPP_PUBLIC
+  bool
+  automatically_add_to_executor_with_node() const;
+
+  /// Retrieve the guard condition used to signal changes to this callback group.
+  /**
+   * \return guard condition if it is valid, otherwise nullptr.
+   */
+  RCLCPP_PUBLIC
+  rclcpp::GuardCondition::SharedPtr
+  get_notify_guard_condition();
+
+  /// Trigger the notify guard condition.
+  RCLCPP_PUBLIC
+  void
+  trigger_notify_guard_condition();
+
 protected:
   RCLCPP_DISABLE_COPY(CallbackGroup)
 
@@ -124,15 +257,36 @@ protected:
   CallbackGroupType type_;
   // Mutex to protect the subsequent vectors of pointers.
   mutable std::mutex mutex_;
+  std::atomic_bool associated_with_executor_;
   std::vector<rclcpp::SubscriptionBase::WeakPtr> subscription_ptrs_;
   std::vector<rclcpp::TimerBase::WeakPtr> timer_ptrs_;
   std::vector<rclcpp::ServiceBase::WeakPtr> service_ptrs_;
   std::vector<rclcpp::ClientBase::WeakPtr> client_ptrs_;
   std::vector<rclcpp::Waitable::WeakPtr> waitable_ptrs_;
   std::atomic_bool can_be_taken_from_;
+  const bool automatically_add_to_executor_with_node_;
+  // defer the creation of the guard condition
+  std::shared_ptr<rclcpp::GuardCondition> notify_guard_condition_ = nullptr;
+  std::recursive_mutex notify_guard_condition_mutex_;
+
+  rclcpp::Context::WeakPtr context_;
+
+private:
+  template<typename TypeT, typename Function>
+  typename TypeT::SharedPtr _find_ptrs_if_impl(
+    Function func, const std::vector<typename TypeT::WeakPtr> & vect_ptrs) const
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    for (auto & weak_ptr : vect_ptrs) {
+      auto ref_ptr = weak_ptr.lock();
+      if (ref_ptr && func(ref_ptr)) {
+        return ref_ptr;
+      }
+    }
+    return typename TypeT::SharedPtr();
+  }
 };
 
-}  // namespace callback_group
 }  // namespace rclcpp
 
 #endif  // RCLCPP__CALLBACK_GROUP_HPP_

rclcpp/include/rclcpp/client.hpp

@@ -15,35 +15,123 @@
 #ifndef RCLCPP__CLIENT_HPP_
 #define RCLCPP__CLIENT_HPP_
 
+#include <atomic>
+#include <functional>
 #include <future>
-#include <map>
 #include <memory>
+#include <mutex>
+#include <optional>
 #include <sstream>
 #include <string>
 #include <tuple>
+#include <unordered_map>
 #include <utility>
+#include <variant>
+#include <vector>
 
 #include "rcl/client.h"
 #include "rcl/error_handling.h"
+#include "rcl/event_callback.h"
+#include "rcl/service_introspection.h"
 #include "rcl/wait.h"
 
+#include "rclcpp/clock.hpp"
+#include "rclcpp/detail/cpp_callback_trampoline.hpp"
 #include "rclcpp/exceptions.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/function_traits.hpp"
+#include "rclcpp/logging.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/utilities.hpp"
-#include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/visibility_control.hpp"
 
-#include "rcutils/logging_macros.h"
-
 #include "rmw/error_handling.h"
+#include "rmw/impl/cpp/demangle.hpp"
 #include "rmw/rmw.h"
 
 namespace rclcpp
 {
 
+namespace detail
+{
+template<typename FutureT>
+struct FutureAndRequestId
+{
+  FutureT future;
+  int64_t request_id;
+
+  FutureAndRequestId(FutureT impl, int64_t req_id)
+  : future(std::move(impl)), request_id(req_id)
+  {}
+
+  /// Allow implicit conversions to `std::future` by reference.
+  operator FutureT &() {return this->future;}
+
+  // delegate future like methods in the std::future impl_
+
+  /// See std::future::get().
+  auto get() {return this->future.get();}
+  /// See std::future::valid().
+  bool valid() const noexcept {return this->future.valid();}
+  /// See std::future::wait().
+  void wait() const {return this->future.wait();}
+  /// See std::future::wait_for().
+  template<class Rep, class Period>
+  std::future_status wait_for(
+    const std::chrono::duration<Rep, Period> & timeout_duration) const
+  {
+    return this->future.wait_for(timeout_duration);
+  }
+  /// See std::future::wait_until().
+  template<class Clock, class Duration>
+  std::future_status wait_until(
+    const std::chrono::time_point<Clock, Duration> & timeout_time) const
+  {
+    return this->future.wait_until(timeout_time);
+  }
+
+  // Rule of five, we could use the rule of zero here, but better be explicit as some of the
+  // methods are deleted.
+
+  /// Move constructor.
+  FutureAndRequestId(FutureAndRequestId && other) noexcept = default;
+  /// Deleted copy constructor, each instance is a unique owner of the future.
+  FutureAndRequestId(const FutureAndRequestId & other) = delete;
+  /// Move assignment.
+  FutureAndRequestId & operator=(FutureAndRequestId && other) noexcept = default;
+  /// Deleted copy assignment, each instance is a unique owner of the future.
+  FutureAndRequestId & operator=(const FutureAndRequestId & other) = delete;
+  /// Destructor.
+  ~FutureAndRequestId() = default;
+};
+
+template<typename PendingRequestsT, typename AllocatorT = std::allocator<int64_t>>
+size_t
+prune_requests_older_than_impl(
+  PendingRequestsT & pending_requests,
+  std::mutex & pending_requests_mutex,
+  std::chrono::time_point<std::chrono::system_clock> time_point,
+  std::vector<int64_t, AllocatorT> * pruned_requests = nullptr)
+{
+  std::lock_guard guard(pending_requests_mutex);
+  auto old_size = pending_requests.size();
+  for (auto it = pending_requests.begin(), last = pending_requests.end(); it != last; ) {
+    if (it->second.first < time_point) {
+      if (pruned_requests) {
+        pruned_requests->push_back(it->first);
+      }
+      it = pending_requests.erase(it);
+    } else {
+      ++it;
+    }
+  }
+  return old_size - pending_requests.size();
+}
+}  // namespace detail
+
 namespace node_interfaces
 {
 class NodeBaseInterface;
@@ -60,24 +148,66 @@ public:
     rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph);
 
   RCLCPP_PUBLIC
-  virtual ~ClientBase();
+  virtual ~ClientBase() = default;
 
+  /// Take the next response for this client as a type erased pointer.
+  /**
+   * The type erased pointer allows for this method to be used in a type
+   * agnostic way along with ClientBase::create_response(),
+   * ClientBase::create_request_header(), and ClientBase::handle_response().
+   * The typed version of this can be used if the Service type is known,
+   * \sa Client::take_response().
+   *
+   * \param[out] response_out The type erased pointer to a Service Response into
+   *   which the middleware will copy the response being taken.
+   * \param[out] request_header_out The request header to be filled by the
+   *   middleware when taking, and which can be used to associte the response
+   *   to a specific request.
+   * \returns true if the response was taken, otherwise false.
+   * \throws rclcpp::exceptions::RCLError based exceptions if the underlying
+   *   rcl function fail.
+   */
+  RCLCPP_PUBLIC
+  bool
+  take_type_erased_response(void * response_out, rmw_request_id_t & request_header_out);
+
+  /// Return the name of the service.
+  /** \return The name of the service. */
   RCLCPP_PUBLIC
   const char *
   get_service_name() const;
 
+  /// Return the rcl_client_t client handle in a std::shared_ptr.
+  /**
+   * This handle remains valid after the Client is destroyed.
+   * The actual rcl client is not finalized until it is out of scope everywhere.
+   */
   RCLCPP_PUBLIC
   std::shared_ptr<rcl_client_t>
   get_client_handle();
 
+  /// Return the rcl_client_t client handle in a std::shared_ptr.
+  /**
+   * This handle remains valid after the Client is destroyed.
+   * The actual rcl client is not finalized until it is out of scope everywhere.
+   */
   RCLCPP_PUBLIC
   std::shared_ptr<const rcl_client_t>
   get_client_handle() const;
 
+  /// Return if the service is ready.
+  /**
+   * \return `true` if the service is ready, `false` otherwise
+   */
   RCLCPP_PUBLIC
   bool
   service_is_ready() const;
 
+  /// Wait for a service to be ready.
+  /**
+   * \param timeout maximum time to wait
+   * \return `true` if the service is ready and the timeout is not over, `false` otherwise
+   */
   template<typename RepT = int64_t, typename RatioT = std::milli>
   bool
   wait_for_service(
@@ -93,6 +223,137 @@ public:
   virtual void handle_response(
     std::shared_ptr<rmw_request_id_t> request_header, std::shared_ptr<void> response) = 0;
 
+  /// Exchange the "in use by wait set" state for this client.
+  /**
+   * This is used to ensure this client is not used by multiple
+   * wait sets at the same time.
+   *
+   * \param[in] in_use_state the new state to exchange into the state, true
+   *   indicates it is now in use by a wait set, and false is that it is no
+   *   longer in use by a wait set.
+   * \returns the previous state.
+   */
+  RCLCPP_PUBLIC
+  bool
+  exchange_in_use_by_wait_set_state(bool in_use_state);
+
+  /// Get the actual request publsher 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 client, 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 request publsher qos settings.
+   * \throws std::runtime_error if failed to get qos settings
+   */
+  RCLCPP_PUBLIC
+  rclcpp::QoS
+  get_request_publisher_actual_qos() const;
+
+  /// Get the actual response subscription 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 client, 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 response subscription qos settings.
+   * \throws std::runtime_error if failed to get qos settings
+   */
+  RCLCPP_PUBLIC
+  rclcpp::QoS
+  get_response_subscription_actual_qos() const;
+
+  /// Set a callback to be called when each new response is received.
+  /**
+   * The callback receives a size_t which is the number of responses received
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if responses were received before any
+   * callback was set.
+   *
+   * Since this callback is called from the middleware, you should aim to make
+   * it fast and not blocking.
+   * If you need to do a lot of work or wait for some other event, you should
+   * spin it off to another thread, otherwise you risk blocking the middleware.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * An exception will be thrown if the callback is not callable.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the client
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rmw_client_set_on_new_response_callback
+   * \sa rcl_client_set_on_new_response_callback
+   *
+   * \param[in] callback functor to be called when a new response is received
+   */
+  void
+  set_on_new_response_callback(std::function<void(size_t)> callback)
+  {
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_new_response_callback "
+              "is not callable.");
+    }
+
+    auto new_callback =
+      [callback, this](size_t number_of_responses) {
+        try {
+          callback(number_of_responses);
+        } catch (const std::exception & exception) {
+          RCLCPP_ERROR_STREAM(
+            node_logger_,
+            "rclcpp::ClientBase@" << this <<
+              " caught " << rmw::impl::cpp::demangle(exception) <<
+              " exception in user-provided callback for the 'on new response' callback: " <<
+              exception.what());
+        } catch (...) {
+          RCLCPP_ERROR_STREAM(
+            node_logger_,
+            "rclcpp::ClientBase@" << this <<
+              " caught unhandled exception in user-provided callback " <<
+              "for the 'on new response' callback");
+        }
+      };
+
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+
+    // Set it temporarily to the new callback, while we replace the old one.
+    // This two-step setting, prevents a gap where the old std::function has
+    // been replaced but the middleware hasn't been told about the new one yet.
+    set_on_new_response_callback(
+      rclcpp::detail::cpp_callback_trampoline<decltype(new_callback), const void *, size_t>,
+      static_cast<const void *>(&new_callback));
+
+    // Store the std::function to keep it in scope, also overwrites the existing one.
+    on_new_response_callback_ = new_callback;
+
+    // Set it again, now using the permanent storage.
+    set_on_new_response_callback(
+      rclcpp::detail::cpp_callback_trampoline<
+        decltype(on_new_response_callback_), const void *, size_t>,
+      static_cast<const void *>(&on_new_response_callback_));
+  }
+
+  /// Unset the callback registered for new responses, if any.
+  void
+  clear_on_new_response_callback()
+  {
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+    if (on_new_response_callback_) {
+      set_on_new_response_callback(nullptr, nullptr);
+      on_new_response_callback_ = nullptr;
+    }
+  }
+
 protected:
   RCLCPP_DISABLE_COPY(ClientBase)
 
@@ -108,17 +369,34 @@ protected:
   const rcl_node_t *
   get_rcl_node_handle() const;
 
+  RCLCPP_PUBLIC
+  void
+  set_on_new_response_callback(rcl_event_callback_t callback, const void * user_data);
+
   rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
   std::shared_ptr<rcl_node_t> node_handle_;
   std::shared_ptr<rclcpp::Context> context_;
+  rclcpp::Logger node_logger_;
 
+  std::recursive_mutex callback_mutex_;
+  // It is important to declare on_new_response_callback_ before
+  // client_handle_, so on destruction the client is
+  // destroyed first. Otherwise, the rmw client callback
+  // would point briefly to a destroyed function.
+  std::function<void(size_t)> on_new_response_callback_{nullptr};
+  // Declare client_handle_ after callback
   std::shared_ptr<rcl_client_t> client_handle_;
+
+  std::atomic<bool> in_use_by_wait_set_{false};
 };
 
 template<typename ServiceT>
 class Client : public ClientBase
 {
 public:
+  using Request = typename ServiceT::Request;
+  using Response = typename ServiceT::Response;
+
   using SharedRequest = typename ServiceT::Request::SharedPtr;
   using SharedResponse = typename ServiceT::Response::SharedPtr;
 
@@ -128,6 +406,7 @@ public:
   using SharedPromise = std::shared_ptr<Promise>;
   using SharedPromiseWithRequest = std::shared_ptr<PromiseWithRequest>;
 
+  using Future = std::future<SharedResponse>;
   using SharedFuture = std::shared_future<SharedResponse>;
   using SharedFutureWithRequest = std::shared_future<std::pair<SharedRequest, SharedResponse>>;
 
@@ -136,20 +415,78 @@ public:
 
   RCLCPP_SMART_PTR_DEFINITIONS(Client)
 
+  /// A convenient Client::Future and request id pair.
+  /**
+   * Public members:
+   * - future: a std::future<SharedResponse>.
+   * - request_id: the request id associated with the future.
+   *
+   * All the other methods are equivalent to the ones std::future provides.
+   */
+  struct FutureAndRequestId
+    : detail::FutureAndRequestId<std::future<SharedResponse>>
+  {
+    using detail::FutureAndRequestId<std::future<SharedResponse>>::FutureAndRequestId;
+
+    // delegate future like methods in the std::future impl_
+
+    /// See std::future::share().
+    SharedFuture share() noexcept {return this->future.share();}
+  };
+
+  /// A convenient Client::SharedFuture and request id pair.
+  /**
+   * Public members:
+   * - future: a std::shared_future<SharedResponse>.
+   * - request_id: the request id associated with the future.
+   *
+   * All the other methods are equivalent to the ones std::shared_future provides.
+   */
+  struct SharedFutureAndRequestId
+    : detail::FutureAndRequestId<std::shared_future<SharedResponse>>
+  {
+    using detail::FutureAndRequestId<std::shared_future<SharedResponse>>::FutureAndRequestId;
+  };
+
+  /// A convenient Client::SharedFutureWithRequest and request id pair.
+  /**
+   * Public members:
+   * - future: a std::shared_future<SharedResponse>.
+   * - request_id: the request id associated with the future.
+   *
+   * All the other methods are equivalent to the ones std::shared_future provides.
+   */
+  struct SharedFutureWithRequestAndRequestId
+    : detail::FutureAndRequestId<std::shared_future<std::pair<SharedRequest, SharedResponse>>>
+  {
+    using detail::FutureAndRequestId<
+      std::shared_future<std::pair<SharedRequest, SharedResponse>>
+    >::FutureAndRequestId;
+  };
+
+  /// Default constructor.
+  /**
+   * The constructor for a Client is almost never called directly.
+   * Instead, clients should be instantiated through the function
+   * rclcpp::create_client().
+   *
+   * \param[in] node_base NodeBaseInterface pointer that is used in part of the setup.
+   * \param[in] node_graph The node graph interface of the corresponding node.
+   * \param[in] service_name Name of the topic to publish to.
+   * \param[in] client_options options for the client.
+   */
   Client(
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
     rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
     const std::string & service_name,
     rcl_client_options_t & client_options)
-  : ClientBase(node_base, node_graph)
+  : ClientBase(node_base, node_graph),
+    srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle<ServiceT>())
   {
-    using rosidl_typesupport_cpp::get_service_type_support_handle;
-    auto service_type_support_handle =
-      get_service_type_support_handle<ServiceT>();
     rcl_ret_t ret = rcl_client_init(
       this->get_client_handle().get(),
       this->get_rcl_node_handle(),
-      service_type_support_handle,
+      srv_type_support_handle_,
       service_name.c_str(),
       &client_options);
     if (ret != RCL_RET_OK) {
@@ -171,12 +508,39 @@ public:
   {
   }
 
+  /// Take the next response for this client.
+  /**
+   * \sa ClientBase::take_type_erased_response().
+   *
+   * \param[out] response_out The reference to a Service Response into
+   *   which the middleware will copy the response being taken.
+   * \param[out] request_header_out The request header to be filled by the
+   *   middleware when taking, and which can be used to associte the response
+   *   to a specific request.
+   * \returns true if the response was taken, otherwise false.
+   * \throws rclcpp::exceptions::RCLError based exceptions if the underlying
+   *   rcl function fail.
+   */
+  bool
+  take_response(typename ServiceT::Response & response_out, rmw_request_id_t & request_header_out)
+  {
+    return this->take_type_erased_response(&response_out, request_header_out);
+  }
+
+  /// Create a shared pointer with the response type
+  /**
+   * \return shared pointer with the response type
+   */
   std::shared_ptr<void>
   create_response() override
   {
     return std::shared_ptr<void>(new typename ServiceT::Response());
   }
 
+  /// Create a shared pointer with a rmw_request_id_t
+  /**
+   * \return shared pointer with a rmw_request_id_t
+   */
   std::shared_ptr<rmw_request_id_t>
   create_request_header() override
   {
@@ -185,39 +549,99 @@ public:
     return std::shared_ptr<rmw_request_id_t>(new rmw_request_id_t);
   }
 
+  /// Handle a server response
+  /**
+    * \param[in] request_header used to check if the secuence number is valid
+    * \param[in] response message with the server response
+   */
   void
   handle_response(
     std::shared_ptr<rmw_request_id_t> request_header,
     std::shared_ptr<void> response) override
   {
-    std::unique_lock<std::mutex> lock(pending_requests_mutex_);
-    auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(response);
-    int64_t sequence_number = request_header->sequence_number;
-    // TODO(esteve) this should throw instead since it is not expected to happen in the first place
-    if (this->pending_requests_.count(sequence_number) == 0) {
-      RCUTILS_LOG_ERROR_NAMED(
-        "rclcpp",
-        "Received invalid sequence number. Ignoring...");
+    std::optional<CallbackInfoVariant>
+    optional_pending_request = this->get_and_erase_pending_request(request_header->sequence_number);
+    if (!optional_pending_request) {
       return;
     }
-    auto tuple = this->pending_requests_[sequence_number];
-    auto call_promise = std::get<0>(tuple);
-    auto callback = std::get<1>(tuple);
-    auto future = std::get<2>(tuple);
-    this->pending_requests_.erase(sequence_number);
-    // Unlock here to allow the service to be called recursively from one of its callbacks.
-    lock.unlock();
-
-    call_promise->set_value(typed_response);
-    callback(future);
+    auto & value = *optional_pending_request;
+    auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(
+      std::move(response));
+    if (std::holds_alternative<Promise>(value)) {
+      auto & promise = std::get<Promise>(value);
+      promise.set_value(std::move(typed_response));
+    } else if (std::holds_alternative<CallbackTypeValueVariant>(value)) {
+      auto & inner = std::get<CallbackTypeValueVariant>(value);
+      const auto & callback = std::get<CallbackType>(inner);
+      auto & promise = std::get<Promise>(inner);
+      auto & future = std::get<SharedFuture>(inner);
+      promise.set_value(std::move(typed_response));
+      callback(std::move(future));
+    } else if (std::holds_alternative<CallbackWithRequestTypeValueVariant>(value)) {
+      auto & inner = std::get<CallbackWithRequestTypeValueVariant>(value);
+      const auto & callback = std::get<CallbackWithRequestType>(inner);
+      auto & promise = std::get<PromiseWithRequest>(inner);
+      auto & future = std::get<SharedFutureWithRequest>(inner);
+      auto & request = std::get<SharedRequest>(inner);
+      promise.set_value(std::make_pair(std::move(request), std::move(typed_response)));
+      callback(std::move(future));
+    }
   }
 
-  SharedFuture
+  /// Send a request to the service server.
+  /**
+   * This method returns a `FutureAndRequestId` instance
+   * that can be passed to Executor::spin_until_future_complete() to
+   * wait until it has been completed.
+   *
+   * If the future never completes,
+   * e.g. the call to Executor::spin_until_future_complete() times out,
+   * Client::remove_pending_request() must be called to clean the client internal state.
+   * Not doing so will make the `Client` instance to use more memory each time a response is not
+   * received from the service server.
+   *
+   * ```cpp
+   * auto future = client->async_send_request(my_request);
+   * if (
+   *   rclcpp::FutureReturnCode::TIMEOUT ==
+   *   executor->spin_until_future_complete(future, timeout))
+   * {
+   *   client->remove_pending_request(future);
+   *   // handle timeout
+   * } else {
+   *   handle_response(future.get());
+   * }
+   * ```
+   *
+   * \param[in] request request to be send.
+   * \return a FutureAndRequestId instance.
+   */
+  FutureAndRequestId
   async_send_request(SharedRequest request)
   {
-    return async_send_request(request, [](SharedFuture) {});
+    Promise promise;
+    auto future = promise.get_future();
+    auto req_id = async_send_request_impl(
+      *request,
+      std::move(promise));
+    return FutureAndRequestId(std::move(future), req_id);
   }
 
+  /// Send a request to the service server and schedule a callback in the executor.
+  /**
+   * Similar to the previous overload, but a callback will automatically be called when a response is received.
+   *
+   * If the callback is never called, because we never got a reply for the service server, remove_pending_request()
+   * has to be called with the returned request id or prune_pending_requests().
+   * Not doing so will make the `Client` instance use more memory each time a response is not
+   * received from the service server.
+   * In this case, it's convenient to setup a timer to cleanup the pending requests.
+   * See for example the `examples_rclcpp_async_client` package in https://github.com/ros2/examples.
+   *
+   * \param[in] request request to be send.
+   * \param[in] cb callback that will be called when we get a response for this request.
+   * \return the request id representing the request just sent.
+   */
   template<
     typename CallbackT,
     typename std::enable_if<
@@ -227,23 +651,28 @@ public:
       >::value
     >::type * = nullptr
   >
-  SharedFuture
+  SharedFutureAndRequestId
   async_send_request(SharedRequest request, CallbackT && cb)
   {
-    std::lock_guard<std::mutex> lock(pending_requests_mutex_);
-    int64_t sequence_number;
-    rcl_ret_t ret = rcl_send_request(get_client_handle().get(), request.get(), &sequence_number);
-    if (RCL_RET_OK != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");
-    }
-
-    SharedPromise call_promise = std::make_shared<Promise>();
-    SharedFuture f(call_promise->get_future());
-    pending_requests_[sequence_number] =
-      std::make_tuple(call_promise, std::forward<CallbackType>(cb), f);
-    return f;
+    Promise promise;
+    auto shared_future = promise.get_future().share();
+    auto req_id = async_send_request_impl(
+      *request,
+      std::make_tuple(
+        CallbackType{std::forward<CallbackT>(cb)},
+        shared_future,
+        std::move(promise)));
+    return SharedFutureAndRequestId{std::move(shared_future), req_id};
   }
 
+  /// Send a request to the service server and schedule a callback in the executor.
+  /**
+   * Similar to the previous method, but you can get both the request and response in the callback.
+   *
+   * \param[in] request request to be send.
+   * \param[in] cb callback that will be called when we get a response for this request.
+   * \return the request id representing the request just sent.
+   */
   template<
     typename CallbackT,
     typename std::enable_if<
@@ -253,28 +682,191 @@ public:
       >::value
     >::type * = nullptr
   >
-  SharedFutureWithRequest
+  SharedFutureWithRequestAndRequestId
   async_send_request(SharedRequest request, CallbackT && cb)
   {
-    SharedPromiseWithRequest promise = std::make_shared<PromiseWithRequest>();
-    SharedFutureWithRequest future_with_request(promise->get_future());
-
-    auto wrapping_cb = [future_with_request, promise, request, &cb](SharedFuture future) {
-        auto response = future.get();
-        promise->set_value(std::make_pair(request, response));
-        cb(future_with_request);
-      };
-
-    async_send_request(request, wrapping_cb);
-
-    return future_with_request;
+    PromiseWithRequest promise;
+    auto shared_future = promise.get_future().share();
+    auto req_id = async_send_request_impl(
+      *request,
+      std::make_tuple(
+        CallbackWithRequestType{std::forward<CallbackT>(cb)},
+        request,
+        shared_future,
+        std::move(promise)));
+    return SharedFutureWithRequestAndRequestId{std::move(shared_future), req_id};
+  }
+
+  /// Cleanup a pending request.
+  /**
+   * This notifies the client that we have waited long enough for a response from the server
+   * to come, we have given up and we are not waiting for a response anymore.
+   *
+   * Not calling this will make the client start using more memory for each request
+   * that never got a reply from the server.
+   *
+   * \param[in] request_id request id returned by async_send_request().
+   * \return true when a pending request was removed, false if not (e.g. a response was received).
+   */
+  bool
+  remove_pending_request(int64_t request_id)
+  {
+    std::lock_guard guard(pending_requests_mutex_);
+    return pending_requests_.erase(request_id) != 0u;
+  }
+
+  /// Cleanup a pending request.
+  /**
+   * Convenient overload, same as:
+   *
+   * `Client::remove_pending_request(this, future.request_id)`.
+   */
+  bool
+  remove_pending_request(const FutureAndRequestId & future)
+  {
+    return this->remove_pending_request(future.request_id);
+  }
+
+  /// Cleanup a pending request.
+  /**
+   * Convenient overload, same as:
+   *
+   * `Client::remove_pending_request(this, future.request_id)`.
+   */
+  bool
+  remove_pending_request(const SharedFutureAndRequestId & future)
+  {
+    return this->remove_pending_request(future.request_id);
+  }
+
+  /// Cleanup a pending request.
+  /**
+   * Convenient overload, same as:
+   *
+   * `Client::remove_pending_request(this, future.request_id)`.
+   */
+  bool
+  remove_pending_request(const SharedFutureWithRequestAndRequestId & future)
+  {
+    return this->remove_pending_request(future.request_id);
+  }
+
+  /// Clean all pending requests.
+  /**
+   * \return number of pending requests that were removed.
+   */
+  size_t
+  prune_pending_requests()
+  {
+    std::lock_guard guard(pending_requests_mutex_);
+    auto ret = pending_requests_.size();
+    pending_requests_.clear();
+    return ret;
+  }
+
+  /// Clean all pending requests older than a time_point.
+  /**
+   * \param[in] time_point Requests that were sent before this point are going to be removed.
+   * \param[inout] pruned_requests Removed requests id will be pushed to the vector
+   *  if a pointer is provided.
+   * \return number of pending requests that were removed.
+   */
+  template<typename AllocatorT = std::allocator<int64_t>>
+  size_t
+  prune_requests_older_than(
+    std::chrono::time_point<std::chrono::system_clock> time_point,
+    std::vector<int64_t, AllocatorT> * pruned_requests = nullptr)
+  {
+    return detail::prune_requests_older_than_impl(
+      pending_requests_,
+      pending_requests_mutex_,
+      time_point,
+      pruned_requests);
+  }
+
+  /// Configure client introspection.
+  /**
+   * \param[in] clock clock to use to generate introspection timestamps
+   * \param[in] qos_service_event_pub QoS settings to use when creating the introspection publisher
+   * \param[in] introspection_state the state to set introspection to
+   *
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw if
+   *   it failed to configure introspection.
+   */
+  void
+  configure_introspection(
+    Clock::SharedPtr clock, const QoS & qos_service_event_pub,
+    rcl_service_introspection_state_t introspection_state)
+  {
+    rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options();
+    pub_opts.qos = qos_service_event_pub.get_rmw_qos_profile();
+
+    rcl_ret_t ret = rcl_client_configure_service_introspection(
+      client_handle_.get(),
+      node_handle_.get(),
+      clock->get_clock_handle(),
+      srv_type_support_handle_,
+      pub_opts,
+      introspection_state);
+
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure client introspection");
+    }
+  }
+
+protected:
+  using CallbackTypeValueVariant = std::tuple<CallbackType, SharedFuture, Promise>;
+  using CallbackWithRequestTypeValueVariant = std::tuple<
+    CallbackWithRequestType, SharedRequest, SharedFutureWithRequest, PromiseWithRequest>;
+
+  using CallbackInfoVariant = std::variant<
+    std::promise<SharedResponse>,
+    CallbackTypeValueVariant,
+    CallbackWithRequestTypeValueVariant>;
+
+  int64_t
+  async_send_request_impl(const Request & request, CallbackInfoVariant value)
+  {
+    int64_t sequence_number;
+    std::lock_guard<std::mutex> lock(pending_requests_mutex_);
+    rcl_ret_t ret = rcl_send_request(get_client_handle().get(), &request, &sequence_number);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");
+    }
+    pending_requests_.try_emplace(
+      sequence_number,
+      std::make_pair(std::chrono::system_clock::now(), std::move(value)));
+    return sequence_number;
+  }
+
+  std::optional<CallbackInfoVariant>
+  get_and_erase_pending_request(int64_t request_number)
+  {
+    std::unique_lock<std::mutex> lock(pending_requests_mutex_);
+    auto it = this->pending_requests_.find(request_number);
+    if (it == this->pending_requests_.end()) {
+      RCUTILS_LOG_DEBUG_NAMED(
+        "rclcpp",
+        "Received invalid sequence number. Ignoring...");
+      return std::nullopt;
+    }
+    std::optional<CallbackInfoVariant> value = std::move(it->second.second);
+    this->pending_requests_.erase(request_number);
+    return value;
   }
 
-private:
   RCLCPP_DISABLE_COPY(Client)
 
-  std::map<int64_t, std::tuple<SharedPromise, CallbackType, SharedFuture>> pending_requests_;
+  std::unordered_map<
+    int64_t,
+    std::pair<
+      std::chrono::time_point<std::chrono::system_clock>,
+      CallbackInfoVariant>>
+  pending_requests_;
   std::mutex pending_requests_mutex_;
+
+private:
+  const rosidl_service_type_support_t * srv_type_support_handle_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/clock.hpp

@@ -16,12 +16,17 @@
 #define RCLCPP__CLOCK_HPP_
 
 #include <functional>
+#include <memory>
+#include <mutex>
 
+#include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/time.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 #include "rcl/time.h"
+#include "rcutils/time.h"
+#include "rcutils/types/rcutils_ret.h"
 
 namespace rclcpp
 {
@@ -55,6 +60,13 @@ public:
   /**
    * Initializes the clock instance with the given clock_type.
    *
+   * WARNING Don't instantiate a clock using RCL_ROS_TIME directly,
+   * unless you really know what you are doing. By default no TimeSource
+   * is attached to a new clock. This will lead to the unexpected behavior,
+   * that your RCL_ROS_TIME will run always on system time. If you want
+   * a RCL_ROS_TIME use Node::get_clock(), or make sure to attach a
+   * TimeSource yourself.
+   *
    * \param clock_type type of the clock.
    * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
    */
@@ -72,7 +84,110 @@ public:
    */
   RCLCPP_PUBLIC
   Time
-  now();
+  now() const;
+
+  /**
+   * Sleep until a specified Time, according to clock type.
+   *
+   * Notes for RCL_ROS_TIME clock type:
+   *   - Can sleep forever if ros time is active and received clock never reaches `until`
+   *   - If ROS time enabled state changes during the sleep, this method will immediately return
+   *     false. There is not a consistent choice of sleeping time when the time source changes,
+   *     so this is up to the caller to call again if needed.
+   *
+   * \warning When using gcc < 10 or when using gcc >= 10 and pthreads lacks the function
+   *    `pthread_cond_clockwait`, steady clocks may sleep using the system clock.
+   *    If so, steady clock sleep times can be affected by system clock time jumps.
+   *    Depending on the steady clock's epoch and resolution in comparison to the system clock's,
+   *    an overflow when converting steady clock durations to system clock times may cause
+   *    undefined behavior.
+   *    For more info see these issues:
+   *    https://gcc.gnu.org/bugzilla/show_bug.cgi?id=41861
+   *    https://gcc.gnu.org/bugzilla/show_bug.cgi?id=58931
+   *
+   * \param until absolute time according to current clock type to sleep until.
+   * \param context the rclcpp context the clock should use to check that ROS is still initialized.
+   * \return true immediately if `until` is in the past
+   * \return true when the time `until` is reached
+   * \return false if time cannot be reached reliably, for example from shutdown or a change
+   *    of time source.
+   * \throws std::runtime_error if the context is invalid
+   * \throws std::runtime_error if `until` has a different clock type from this clock
+   */
+  RCLCPP_PUBLIC
+  bool
+  sleep_until(
+    Time until,
+    Context::SharedPtr context = contexts::get_global_default_context());
+
+  /**
+   * Sleep for a specified Duration.
+   *
+   * Equivalent to
+   *
+   * ```cpp
+   * clock->sleep_until(clock->now() + rel_time, context)
+   * ```
+   *
+   * The function will return immediately if `rel_time` is zero or negative.
+   *
+   * \param rel_time the length of time to sleep for.
+   * \param context the rclcpp context the clock should use to check that ROS is still initialized.
+   * \return true when the end time is reached
+   * \return false if time cannot be reached reliably, for example from shutdown or a change
+   *    of time source.
+   * \throws std::runtime_error if the context is invalid
+   */
+  RCLCPP_PUBLIC
+  bool
+  sleep_for(
+    Duration rel_time,
+    Context::SharedPtr context = contexts::get_global_default_context());
+
+  /**
+   * Check if the clock is started.
+   *
+   * A started clock is a clock that reflects non-zero time.
+   * Typically a clock will be unstarted if it is using RCL_ROS_TIME with ROS time and
+   * nothing has been published on the clock topic yet.
+   *
+   * \return true if clock is started
+   * \throws std::runtime_error if the clock is not rcl_clock_valid
+   */
+  RCLCPP_PUBLIC
+  bool
+  started();
+
+  /**
+   * Wait until clock to start.
+   *
+   * \rclcpp::Clock::started
+   * \param context the context to wait in
+   * \return true if clock was already started or became started
+   * \throws std::runtime_error if the context is invalid or clock is not rcl_clock_valid
+   */
+  RCLCPP_PUBLIC
+  bool
+  wait_until_started(Context::SharedPtr context = contexts::get_global_default_context());
+
+  /**
+   * Wait for clock to start, with timeout.
+   *
+   * The timeout is waited in steady time.
+   *
+   * \rclcpp::Clock::started
+   * \param timeout the maximum time to wait for.
+   * \param context the context to wait in.
+   * \param wait_tick_ns the time to wait between each iteration of the wait loop (in nanoseconds).
+   * \return true if clock was or became valid
+   * \throws std::runtime_error if the context is invalid or clock is not rcl_clock_valid
+   */
+  RCLCPP_PUBLIC
+  bool
+  wait_until_started(
+    const rclcpp::Duration & timeout,
+    Context::SharedPtr context = contexts::get_global_default_context(),
+    const rclcpp::Duration & wait_tick_ns = rclcpp::Duration(0, static_cast<uint32_t>(1e7)));
 
   /**
    * Returns the clock of the type `RCL_ROS_TIME` is active.
@@ -85,6 +200,7 @@ public:
   bool
   ros_time_is_active();
 
+  /// Return the rcl_clock_t clock handle
   RCLCPP_PUBLIC
   rcl_clock_t *
   get_clock_handle() noexcept;
@@ -93,22 +209,27 @@ public:
   rcl_clock_type_t
   get_clock_type() const noexcept;
 
-  // Add a callback to invoke if the jump threshold is exceeded.
+  /// Get the clock's mutex
+  RCLCPP_PUBLIC
+  std::mutex &
+  get_clock_mutex() 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;
+   * callbacks will be executed whose threshold is greater than 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.
+   * \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
+   * than 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
@@ -126,16 +247,127 @@ private:
   RCLCPP_PUBLIC
   static void
   on_time_jump(
-    const struct rcl_time_jump_t * time_jump,
+    const 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_;
+  /// Private internal storage
+  class Impl;
+
+  std::shared_ptr<Impl> impl_;
 };
 
+/**
+ * A synchronization primitive, equal to std::conditional_variable,
+ * that works with the rclcpp::Clock.
+ *
+ * For more information on the API see https://en.cppreference.com/w/cpp/thread/condition_variable.
+ *
+ * Note, this class does not handle shutdowns, if you want to
+ * haven them handles as well, use ClockConditionalVariable.
+ */
+class ClockWaiter
+{
+private:
+  class ClockWaiterImpl;
+  std::unique_ptr<ClockWaiterImpl> impl_;
+
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(ClockWaiter)
+
+  RCLCPP_PUBLIC
+  explicit ClockWaiter(const rclcpp::Clock::SharedPtr & clock);
+
+  RCLCPP_PUBLIC
+  ~ClockWaiter();
+
+  /**
+   * Calling this function will block the current thread, until abs_time is reached,
+   * or pred returns true.
+   * @param lock A locked lock. The lock must be locked at call time, or this method will throw.
+   *             The lock will be atomically released and this thread will blocked.
+   * @param abs_time The time until which this thread shall be blocked.
+   * @param pred may be called in cased of spurious wakeups, but must be called every time
+   *             notify_one() was called. During the call to pred, the given lock will be locked.
+   *             This method will return, if pred returns true.
+   */
+  RCLCPP_PUBLIC
+  bool
+  wait_until(
+    std::unique_lock<std::mutex> & lock,
+    const rclcpp::Time & abs_time, const std::function<bool ()> & pred);
+
+  /**
+   * Notify the blocked thread, that it should reevaluate the wakeup condition.
+   * The given pred function in wait_until will be reevaluated and wait_until
+   * will return if it evaluates to true.
+   */
+  RCLCPP_PUBLIC
+  void
+  notify_one();
+};
+
+
+/**
+ * A synchronization primitive, similar to std::conditional_variable,
+ * that works with the rclcpp::Clock.
+ *
+ * For more information on the API see https://en.cppreference.com/w/cpp/thread/condition_variable.
+ *
+ * This primitive will wake up if the context was shut down.
+ */
+class ClockConditionalVariable
+{
+  class Impl;
+  std::unique_ptr<Impl> impl_;
+
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(ClockConditionalVariable)
+
+  RCLCPP_PUBLIC
+  ClockConditionalVariable(
+    const rclcpp::Clock::SharedPtr & clock,
+    rclcpp::Context::SharedPtr context = rclcpp::contexts::get_global_default_context());
+  RCLCPP_PUBLIC
+  ~ClockConditionalVariable();
+
+  /**
+   * Calling this function will block the current thread, until abs_time is reached,
+   * or pred returns true.
+   * @param lock A locked lock. The lock must be locked at call time, or this method will throw.
+   *             The lock will be atomically released and this thread will blocked.
+   *             The given lock must be created using the mutex returned my mutex().
+   * @param abs_time The time until which this thread shall be blocked.
+   * @param pred may be called in cased of spurious wakeups, but must be called every time
+   *             notify_one() was called. During the call to pred, the given lock will be locked.
+   *             This method will return, if pred returns true.
+   *
+   * @return true if until was reached.
+   */
+  RCLCPP_PUBLIC
+  bool
+  wait_until(
+    std::unique_lock<std::mutex> & lock, rclcpp::Time until,
+    const std::function<bool ()> & pred);
+
+  /**
+   * Notify the blocked thread, that is should reevaluate the wakeup condition.
+   * E.g. the given pred function in wait_until shall be reevaluated.
+   */
+  RCLCPP_PUBLIC
+  void
+  notify_one();
+
+  /**
+   * Returns the internal mutex. In order to be race free with the context shutdown,
+   * this mutex must be used for the wait_until call.
+   */
+  RCLCPP_PUBLIC
+  std::mutex &
+  mutex();
+};
+
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__CLOCK_HPP_

rclcpp/include/rclcpp/context.hpp

@@ -23,8 +23,10 @@
 #include <typeindex>
 #include <typeinfo>
 #include <unordered_map>
+#include <unordered_set>
 #include <utility>
 #include <vector>
+#include <stdexcept>
 
 #include "rcl/context.h"
 #include "rcl/guard_condition.h"
@@ -35,6 +37,10 @@
 
 namespace rclcpp
 {
+namespace graph_listener
+{
+class GraphListener;
+}  // namespace graph_listener
 
 /// Thrown when init is called on an already initialized context.
 class ContextAlreadyInitialized : public std::runtime_error
@@ -44,11 +50,31 @@ public:
   : std::runtime_error("context is already initialized") {}
 };
 
+/// Forward declare WeakContextsWrapper
+class WeakContextsWrapper;
+
+class ShutdownCallbackHandle
+{
+  friend class Context;
+
+public:
+  using ShutdownCallbackType = std::function<void ()>;
+
+private:
+  std::weak_ptr<ShutdownCallbackType> callback;
+};
+
+using OnShutdownCallbackHandle = ShutdownCallbackHandle;
+using PreShutdownCallbackHandle = ShutdownCallbackHandle;
+
 /// 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.
+ * Nodes may be attached to a particular context by passing to the rclcpp::Node
+ * constructor a rclcpp::NodeOptions instance in which the Context is set via
+ * rclcpp::NodeOptions::context.
+ * Nodes will be automatically removed from the context when destructed.
+ * Contexts may be shutdown by calling rclcpp::shutdown.
  */
 class Context : public std::enable_shared_from_this<Context>
 {
@@ -60,7 +86,7 @@ public:
    * 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.
+   * See the shutdown_on_signal option in the InitOptions class.
    */
   RCLCPP_PUBLIC
   Context();
@@ -77,8 +103,8 @@ public:
    *
    * 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
+   * them manually with rclcpp::install_signal_handlers() or use rclcpp::init().
+   * In addition to installing the signal handlers, the shutdown_on_signal
    * of the InitOptions needs to be `true` for this context to be shutdown by
    * the signal handler, otherwise it will be passed over.
    *
@@ -100,13 +126,16 @@ public:
    * \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
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
+   * \throws std::runtime_error if the global logging configure mutex is NULL
+   * \throws exceptions::UnknownROSArgsError if there are unknown ROS arguments
    */
   RCLCPP_PUBLIC
   virtual
   void
   init(
     int argc,
-    char const * const argv[],
+    char const * const * argv,
     const rclcpp::InitOptions & init_options = rclcpp::InitOptions());
 
   /// Return true if the context is valid, otherwise false.
@@ -133,13 +162,18 @@ public:
   rclcpp::InitOptions
   get_init_options();
 
+  /// Return actual domain id.
+  RCLCPP_PUBLIC
+  size_t
+  get_domain_id() const;
+
   /// Return the shutdown reason, or empty string if not shutdown.
   /**
    * This function is thread-safe.
    */
   RCLCPP_PUBLIC
   std::string
-  shutdown_reason();
+  shutdown_reason() const;
 
   /// Shutdown the context, making it uninitialized and therefore invalid for derived entities.
   /**
@@ -157,16 +191,21 @@ public:
    *
    * This function is thread-safe.
    *
+   * Note that if you override this method, but leave shutdown to be called in
+   * the destruction of this base class, it will not call the overridden
+   * version from your base class.
+   * So you need to ensure you call your class's shutdown() in its destructor.
+   *
    * \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
+   * \throw various exceptions derived from rclcpp::exceptions::RCLError, if rcl_shutdown fails
    */
   RCLCPP_PUBLIC
   virtual
   bool
   shutdown(const std::string & reason);
 
-  using OnShutdownCallback = std::function<void ()>;
+  using OnShutdownCallback = OnShutdownCallbackHandle::ShutdownCallbackType;
 
   /// Add a on_shutdown callback to be called when shutdown is called for this context.
   /**
@@ -174,7 +213,7 @@ public:
    * to last step in shutdown().
    *
    * When shutdown occurs due to the signal handler, these callbacks are run
-   * asynchronoulsy in the dedicated singal handling thread.
+   * asynchronously 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.
@@ -192,36 +231,99 @@ public:
   OnShutdownCallback
   on_shutdown(OnShutdownCallback callback);
 
-  /// Return the shutdown callbacks as const.
+  /// Add a on_shutdown callback to be called when shutdown is called for this context.
   /**
-   * Using the returned reference is not thread-safe with calls that modify
-   * the list of "on shutdown" callbacks, i.e. on_shutdown().
+   * 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
+   * asynchronously in the dedicated signal 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 created callback handle
    */
   RCLCPP_PUBLIC
-  const std::vector<OnShutdownCallback> &
-  get_on_shutdown_callbacks() const;
+  virtual
+  OnShutdownCallbackHandle
+  add_on_shutdown_callback(OnShutdownCallback callback);
+
+  /// Remove an registered on_shutdown callbacks.
+  /**
+   * \param[in] callback_handle the on_shutdown callback handle to be removed.
+   * \return true if the callback is found and removed, otherwise false.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle);
+
+  using PreShutdownCallback = PreShutdownCallbackHandle::ShutdownCallbackType;
+
+  /// Add a pre_shutdown callback to be called before shutdown is called for this context.
+  /**
+   * These callbacks will be called in the order they are added.
+   *
+   * When shutdown occurs due to the signal handler, these callbacks are run
+   * asynchronously in the dedicated signal handling thread.
+   *
+   * \param[in] callback the pre_shutdown callback to be registered
+   * \return the created callback handle
+   */
+  RCLCPP_PUBLIC
+  virtual
+  PreShutdownCallbackHandle
+  add_pre_shutdown_callback(PreShutdownCallback callback);
+
+  /// Remove an registered pre_shutdown callback.
+  /**
+   * \param[in] callback_handle the pre_shutdown callback handle to be removed.
+   * \return true if the callback is found and removed, otherwise false.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  remove_pre_shutdown_callback(const PreShutdownCallbackHandle & callback_handle);
 
   /// 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().
+   * Returned callbacks are a copy of the registered callbacks.
    */
   RCLCPP_PUBLIC
-  std::vector<OnShutdownCallback> &
-  get_on_shutdown_callbacks();
+  std::vector<OnShutdownCallback>
+  get_on_shutdown_callbacks() const;
+
+  /// Return the pre-shutdown callbacks.
+  /**
+   * Returned callbacks are a copy of the registered callbacks.
+   */
+  RCLCPP_PUBLIC
+  std::vector<PreShutdownCallback>
+  get_pre_shutdown_callbacks() const;
 
   /// Return the internal rcl context.
   RCLCPP_PUBLIC
   std::shared_ptr<rcl_context_t>
   get_rcl_context();
 
+  RCLCPP_PUBLIC
+  std::shared_ptr<rclcpp::graph_listener::GraphListener>
+  get_graph_listener();
+
   /// 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)
+   *   - this context has shutdown_on_signal=true and SIGINT/SIGTERM occurs (resulting in shutdown)
    *   - interrupt_all_sleep_for() is called
    *
    * \param[in] nanoseconds A std::chrono::duration representing how long to sleep for.
@@ -233,68 +335,9 @@ public:
 
   /// 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>
@@ -324,7 +367,6 @@ 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();
 
@@ -333,28 +375,60 @@ private:
 
   // This mutex is recursive so that the destructor can ensure atomicity
   // between is_initialized and shutdown.
-  std::recursive_mutex init_mutex_;
+  mutable std::recursive_mutex init_mutex_;
   std::shared_ptr<rcl_context_t> rcl_context_;
   rclcpp::InitOptions init_options_;
   std::string shutdown_reason_;
 
+  // Keep shared ownership of the global logging mutex.
+  std::shared_ptr<std::recursive_mutex> logging_mutex_;
+
   std::unordered_map<std::type_index, std::shared_ptr<void>> sub_contexts_;
   // 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_;
+  std::vector<std::shared_ptr<OnShutdownCallback>> on_shutdown_callbacks_;
+  mutable std::recursive_mutex on_shutdown_callbacks_mutex_;
+
+  std::vector<std::shared_ptr<PreShutdownCallback>> pre_shutdown_callbacks_;
+  mutable std::recursive_mutex pre_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_;
+  /// Graph Listener which waits on graph changes for the node and is shared across nodes.
+  std::shared_ptr<rclcpp::graph_listener::GraphListener> graph_listener_;
+
+  /// Keep shared ownership of global vector of weak contexts
+  std::shared_ptr<WeakContextsWrapper> weak_contexts_;
+
+  enum class ShutdownType
+  {
+    pre_shutdown,
+    on_shutdown
+  };
+
+  using ShutdownCallback = ShutdownCallbackHandle::ShutdownCallbackType;
+
+  template<ShutdownType shutdown_type>
+  RCLCPP_LOCAL
+  ShutdownCallbackHandle
+  add_shutdown_callback(
+    ShutdownCallback callback);
+
+  template<ShutdownType shutdown_type>
+  RCLCPP_LOCAL
+  bool
+  remove_shutdown_callback(
+    const ShutdownCallbackHandle & callback_handle);
+
+  template<ShutdownType shutdown_type>
+  RCLCPP_LOCAL
+  std::vector<rclcpp::Context::ShutdownCallback>
+  get_shutdown_callback() const;
 };
 
 /// Return a copy of the list of context shared pointers.

rclcpp/include/rclcpp/contexts/default_context.hpp

@@ -22,8 +22,6 @@ namespace rclcpp
 {
 namespace contexts
 {
-namespace default_context
-{
 
 class DefaultContext : public rclcpp::Context
 {
@@ -38,7 +36,6 @@ RCLCPP_PUBLIC
 DefaultContext::SharedPtr
 get_global_default_context();
 
-}  // namespace default_context
 }  // namespace contexts
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/copy_all_parameter_values.hpp

@@ -0,0 +1,82 @@
+// Copyright 2023 Open Navigation LLC
+//
+// 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__COPY_ALL_PARAMETER_VALUES_HPP_
+#define RCLCPP__COPY_ALL_PARAMETER_VALUES_HPP_
+
+#include <string>
+#include <vector>
+
+#include "rcl_interfaces/srv/list_parameters.hpp"
+#include "rcl_interfaces/msg/parameter_descriptor.hpp"
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
+
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/logger.hpp"
+#include "rclcpp/logging.hpp"
+
+namespace rclcpp
+{
+
+/**
+ * Copy all parameters from one source node to another destination node.
+ * May throw exceptions if parameters from source are uninitialized or undeclared.
+ * \param source Node to copy parameters from
+ * \param destination Node to copy parameters to
+ * \param override_existing_params Default false. Whether to override existing destination params
+ * if both the source and destination contain the same parameter.
+ */
+template<typename NodeT1, typename NodeT2>
+void
+copy_all_parameter_values(
+  const NodeT1 & source, const NodeT2 & destination, const bool override_existing_params = false)
+{
+  using Parameters = std::vector<rclcpp::Parameter>;
+  using Descriptions = std::vector<rcl_interfaces::msg::ParameterDescriptor>;
+  auto source_params = source->get_node_parameters_interface();
+  auto dest_params = destination->get_node_parameters_interface();
+  rclcpp::Logger logger = destination->get_node_logging_interface()->get_logger();
+
+  std::vector<std::string> param_names = source_params->list_parameters({}, 0).names;
+  Parameters params = source_params->get_parameters(param_names);
+  Descriptions descriptions = source_params->describe_parameters(param_names);
+
+  for (unsigned int idx = 0; idx != params.size(); idx++) {
+    if (!dest_params->has_parameter(params[idx].get_name())) {
+      dest_params->declare_parameter(
+        params[idx].get_name(), params[idx].get_parameter_value(), descriptions[idx]);
+    } else if (override_existing_params) {
+      try {
+        rcl_interfaces::msg::SetParametersResult result =
+          dest_params->set_parameters_atomically({params[idx]});
+        if (!result.successful) {
+          // Parameter update rejected or read-only
+          RCLCPP_WARN(
+            logger,
+            "Unable to set parameter (%s): %s!",
+            params[idx].get_name().c_str(), result.reason.c_str());
+        }
+      } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
+        RCLCPP_WARN(
+          logger,
+          "Unable to set parameter (%s): incompatable parameter type (%s)!",
+          params[idx].get_name().c_str(), e.what());
+      }
+    }
+  }
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__COPY_ALL_PARAMETER_VALUES_HPP_

rclcpp/include/rclcpp/create_client.hpp

@@ -0,0 +1,66 @@
+// 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__CREATE_CLIENT_HPP_
+#define RCLCPP__CREATE_CLIENT_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/qos.hpp"
+#include "rmw/rmw.h"
+
+namespace rclcpp
+{
+/// Create a service client with a given type.
+/**
+ * \param[in] node_base NodeBaseInterface implementation of the node on which
+ *  to create the client.
+ * \param[in] node_graph NodeGraphInterface implementation of the node on which
+ *  to create the client.
+ * \param[in] node_services NodeServicesInterface implementation of the node on
+ *  which to create the client.
+ * \param[in] service_name The name on which the service is accessible.
+ * \param[in] qos Quality of service profile for client.
+ * \param[in] group Callback group to handle the reply to service calls.
+ * \return Shared pointer to the created client.
+ */
+template<typename ServiceT>
+typename rclcpp::Client<ServiceT>::SharedPtr
+create_client(
+  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+  std::shared_ptr<node_interfaces::NodeGraphInterface> node_graph,
+  std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+  const std::string & service_name,
+  const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
+  rclcpp::CallbackGroup::SharedPtr group = nullptr)
+{
+  rcl_client_options_t options = rcl_client_get_default_options();
+  options.qos = qos.get_rmw_qos_profile();
+
+  auto cli = rclcpp::Client<ServiceT>::make_shared(
+    node_base.get(),
+    node_graph,
+    service_name,
+    options);
+
+  auto cli_base_ptr = std::dynamic_pointer_cast<rclcpp::ClientBase>(cli);
+  node_services->add_client(cli_base_ptr, group);
+  return cli;
+}
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CREATE_CLIENT_HPP_

rclcpp/include/rclcpp/create_generic_client.hpp

@@ -0,0 +1,90 @@
+// Copyright 2023 Sony Group Corporation.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__CREATE_GENERIC_CLIENT_HPP_
+#define RCLCPP__CREATE_GENERIC_CLIENT_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/generic_client.hpp"
+#include "rclcpp/node_interfaces/get_node_base_interface.hpp"
+#include "rclcpp/node_interfaces/get_node_graph_interface.hpp"
+#include "rclcpp/node_interfaces/get_node_services_interface.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_graph_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/qos.hpp"
+
+namespace rclcpp
+{
+/// Create a generic service client with a name of given type.
+/**
+ * \param[in] node_base NodeBaseInterface implementation of the node on which
+ *  to create the client.
+ * \param[in] node_graph NodeGraphInterface implementation of the node on which
+ *  to create the client.
+ * \param[in] node_services NodeServicesInterface implementation of the node on
+ *  which to create the client.
+ * \param[in] service_name The name on which the service is accessible.
+ * \param[in] service_type The name of service type, e.g. "test_msgs/srv/BasicTypes"
+ * \param[in] qos Quality of service profile for client.
+ * \param[in] group Callback group to handle the reply to service calls.
+ * \return Shared pointer to the created client.
+ */
+RCLCPP_PUBLIC
+rclcpp::GenericClient::SharedPtr
+create_generic_client(
+  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+  std::shared_ptr<node_interfaces::NodeGraphInterface> node_graph,
+  std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+  const std::string & service_name,
+  const std::string & service_type,
+  const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
+  rclcpp::CallbackGroup::SharedPtr group = nullptr);
+
+/// Create a generic service client with a name of given type.
+/**
+ * The NodeT type needs to have NodeBaseInterface implementation, NodeGraphInterface implementation
+ * and NodeServicesInterface implementation of the node which to create the client.
+ *
+ * \param[in] node The node on which to create the client.
+ * \param[in] service_name The name on which the service is accessible.
+ * \param[in] service_type The name of service type, e.g. "test_msgs/srv/BasicTypes"
+ * \param[in] qos Quality of service profile for client.
+ * \param[in] group Callback group to handle the reply to service calls.
+ * \return Shared pointer to the created client.
+ */
+template<typename NodeT>
+rclcpp::GenericClient::SharedPtr
+create_generic_client(
+  NodeT node,
+  const std::string & service_name,
+  const std::string & service_type,
+  const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
+  rclcpp::CallbackGroup::SharedPtr group = nullptr)
+{
+  return create_generic_client(
+    rclcpp::node_interfaces::get_node_base_interface(node),
+    rclcpp::node_interfaces::get_node_graph_interface(node),
+    rclcpp::node_interfaces::get_node_services_interface(node),
+    service_name,
+    service_type,
+    qos,
+    group
+  );
+}
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CREATE_GENERIC_CLIENT_HPP_

rclcpp/include/rclcpp/create_generic_publisher.hpp

@@ -0,0 +1,69 @@
+// Copyright 2020, Apex.AI Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__CREATE_GENERIC_PUBLISHER_HPP_
+#define RCLCPP__CREATE_GENERIC_PUBLISHER_HPP_
+
+#include <memory>
+#include <string>
+#include <utility>
+
+#include "rclcpp/generic_publisher.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/publisher_options.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/typesupport_helpers.hpp"
+
+namespace rclcpp
+{
+
+/// Create and return a GenericPublisher.
+/**
+ * The returned pointer will never be empty, but this function can throw various exceptions, for
+ * instance when the message's package can not be found on the AMENT_PREFIX_PATH.
+ *
+ * \param topics_interface NodeTopicsInterface pointer used in parts of the setup
+ * \param topic_name Topic name
+ * \param topic_type Topic type
+ * \param qos %QoS settings
+ * \param options %Publisher options.
+ * Not all publisher options are currently respected, the only relevant options for this
+ * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`.
+ */
+template<typename AllocatorT = std::allocator<void>>
+std::shared_ptr<GenericPublisher> create_generic_publisher(
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface,
+  const std::string & topic_name,
+  const std::string & topic_type,
+  const rclcpp::QoS & qos,
+  const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options = (
+    rclcpp::PublisherOptionsWithAllocator<AllocatorT>()
+  )
+)
+{
+  auto ts_lib = rclcpp::get_typesupport_library(topic_type, "rosidl_typesupport_cpp");
+  auto pub = std::make_shared<GenericPublisher>(
+    topics_interface->get_node_base_interface(),
+    std::move(ts_lib),
+    topic_name,
+    topic_type,
+    qos,
+    options);
+  topics_interface->add_publisher(pub, options.callback_group);
+  return pub;
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CREATE_GENERIC_PUBLISHER_HPP_

rclcpp/include/rclcpp/create_generic_service.hpp

@@ -0,0 +1,102 @@
+// Copyright 2024 Sony Group Corporation.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__CREATE_GENERIC_SERVICE_HPP_
+#define RCLCPP__CREATE_GENERIC_SERVICE_HPP_
+
+#include <memory>
+#include <string>
+#include <utility>
+
+#include "rclcpp/generic_service.hpp"
+#include "rclcpp/node_interfaces/get_node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/get_node_services_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rmw/rmw.h"
+
+namespace rclcpp
+{
+/// Create a generic service with a given type.
+/**
+ * \param[in] node_base NodeBaseInterface implementation of the node on which
+ *  to create the generic service.
+ * \param[in] node_services NodeServicesInterface implementation of the node on
+ *  which to create the service.
+ * \param[in] service_name The name on which the service is accessible.
+ * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool".
+ * \param[in] callback The callback to call when the service gets a request.
+ * \param[in] qos Quality of service profile for the service.
+ * \param[in] group Callback group to handle the reply to service calls.
+ * \return Shared pointer to the created service.
+ */
+template<typename CallbackT>
+typename rclcpp::GenericService::SharedPtr
+create_generic_service(
+  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+  std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+  const std::string & service_name,
+  const std::string & service_type,
+  CallbackT && callback,
+  const rclcpp::QoS & qos,
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  rclcpp::GenericServiceCallback any_service_callback;
+  any_service_callback.set(std::forward<CallbackT>(callback));
+
+  rcl_service_options_t service_options = rcl_service_get_default_options();
+  service_options.qos = qos.get_rmw_qos_profile();
+
+  auto serv = GenericService::make_shared(
+    node_base->get_shared_rcl_node_handle(),
+    service_name, service_type, any_service_callback, service_options);
+  auto serv_base_ptr = std::dynamic_pointer_cast<ServiceBase>(serv);
+  node_services->add_service(serv_base_ptr, group);
+  return serv;
+}
+
+/// Create a generic service with a given type.
+/**
+ * The NodeT type needs to have NodeBaseInterface implementation and NodeServicesInterface
+ * implementation of the node which to create the generic service.
+ *
+ * \param[in] node The node on which to create the generic service.
+ * \param[in] service_name The name on which the service is accessible.
+ * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool".
+ * \param[in] callback The callback to call when the service gets a request.
+ * \param[in] qos Quality of service profile for the service.
+ * \param[in] group Callback group to handle the reply to service calls.
+ * \return Shared pointer to the created service.
+ */
+template<typename NodeT, typename CallbackT>
+typename rclcpp::GenericService::SharedPtr
+create_generic_service(
+  NodeT node,
+  const std::string & service_name,
+  const std::string & service_type,
+  CallbackT && callback,
+  const rclcpp::QoS & qos,
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  return create_generic_service<CallbackT>(
+    rclcpp::node_interfaces::get_node_base_interface(node),
+    rclcpp::node_interfaces::get_node_services_interface(node),
+    service_name,
+    service_type,
+    std::forward<CallbackT>(callback), qos.get_rmw_qos_profile(), group);
+}
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CREATE_GENERIC_SERVICE_HPP_

rclcpp/include/rclcpp/create_generic_subscription.hpp

@@ -0,0 +1,88 @@
+// Copyright 2020, Apex.AI Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__CREATE_GENERIC_SUBSCRIPTION_HPP_
+#define RCLCPP__CREATE_GENERIC_SUBSCRIPTION_HPP_
+
+#include <functional>
+#include <memory>
+#include <string>
+#include <utility>
+
+#include "rcl/subscription.h"
+#include "rclcpp/generic_subscription.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/serialized_message.hpp"
+#include "rclcpp/subscription_options.hpp"
+#include "rclcpp/typesupport_helpers.hpp"
+
+namespace rclcpp
+{
+
+/// Create and return a GenericSubscription.
+/**
+ * The returned pointer will never be empty, but this function can throw various exceptions, for
+ * instance when the message's package can not be found on the AMENT_PREFIX_PATH.
+ *
+ * \param topics_interface NodeTopicsInterface pointer used in parts of the setup.
+ * \param topic_name Topic name
+ * \param topic_type Topic type
+ * \param qos %QoS settings
+ * \param callback Callback for new messages of serialized form
+ * \param options %Publisher options.
+ * Not all publisher options are currently respected, the only relevant options for this
+ * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`.
+ */
+template<
+  typename CallbackT,
+  typename AllocatorT = std::allocator<void>>
+std::shared_ptr<GenericSubscription> create_generic_subscription(
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface,
+  const std::string & topic_name,
+  const std::string & topic_type,
+  const rclcpp::QoS & qos,
+  CallbackT && callback,
+  const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
+    rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
+  )
+)
+{
+  auto ts_lib = rclcpp::get_typesupport_library(
+    topic_type, "rosidl_typesupport_cpp");
+
+  auto allocator = options.get_allocator();
+
+  using rclcpp::AnySubscriptionCallback;
+  AnySubscriptionCallback<rclcpp::SerializedMessage, AllocatorT>
+  any_subscription_callback(*allocator);
+  any_subscription_callback.set(std::forward<CallbackT>(callback));
+
+  auto subscription = std::make_shared<GenericSubscription>(
+    topics_interface->get_node_base_interface(),
+    std::move(ts_lib),
+    topic_name,
+    topic_type,
+    qos,
+    any_subscription_callback,
+    options);
+
+  topics_interface->add_subscription(subscription, options.callback_group);
+
+  return subscription;
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CREATE_GENERIC_SUBSCRIPTION_HPP_

rclcpp/include/rclcpp/create_publisher.hpp

@@ -17,6 +17,7 @@
 
 #include <memory>
 #include <string>
+#include <utility>
 
 #include "rclcpp/node_interfaces/get_node_topics_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
@@ -24,96 +25,103 @@
 #include "rclcpp/publisher_factory.hpp"
 #include "rclcpp/publisher_options.hpp"
 #include "rclcpp/qos.hpp"
+#include "rclcpp/qos_overriding_options.hpp"
+#include "rclcpp/detail/qos_parameters.hpp"
+
 #include "rmw/qos_profiles.h"
 
 namespace rclcpp
 {
 
+namespace detail
+{
+/// Create and return a publisher of the given MessageT type.
 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")]]
+  typename PublisherT = rclcpp::Publisher<MessageT, AllocatorT>,
+  typename NodeParametersT,
+  typename NodeTopicsT>
 std::shared_ptr<PublisherT>
 create_publisher(
-  rclcpp::node_interfaces::NodeTopicsInterface * node_topics,
+  NodeParametersT & node_parameters,
+  NodeTopicsT & 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)
+  const rclcpp::QoS & qos,
+  const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options = (
+    rclcpp::PublisherOptionsWithAllocator<AllocatorT>()
+  )
+)
 {
-  auto publisher_options = rcl_publisher_get_default_options();
-  publisher_options.qos = qos_profile;
+  auto node_topics_interface = rclcpp::node_interfaces::get_node_topics_interface(node_topics);
+  const rclcpp::QoS & actual_qos = options.qos_overriding_options.get_policy_kinds().size() ?
+    rclcpp::detail::declare_qos_parameters(
+    options.qos_overriding_options, node_parameters,
+    node_topics_interface->resolve_topic_name(topic_name),
+    qos, rclcpp::detail::PublisherQosParametersTraits{}) :
+    qos;
 
-  auto pub = node_topics->create_publisher(
+  // Create the publisher.
+  auto pub = node_topics_interface->create_publisher(
     topic_name,
-    rclcpp::create_publisher_factory<MessageT, AllocatorT, PublisherT>(event_callbacks, allocator),
-    publisher_options,
-    use_intra_process_comms);
+    rclcpp::create_publisher_factory<MessageT, AllocatorT, PublisherT>(options),
+    actual_qos
+  );
 
-  node_topics->add_publisher(pub, group);
+  // Add the publisher to the node topics interface.
+  node_topics_interface->add_publisher(pub, options.callback_group);
 
   return std::dynamic_pointer_cast<PublisherT>(pub);
 }
+}  // namespace detail
+
 
 /// 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.
+ *
+ * In case `options.qos_overriding_options` is enabling qos parameter overrides,
+ * NodeT must also have a method called get_node_parameters_interface()
+ * which returns a shared_ptr to a NodeParametersInterface.
  */
 template<
   typename MessageT,
   typename AllocatorT = std::allocator<void>,
-  typename PublisherT = ::rclcpp::Publisher<MessageT, AllocatorT>,
+  typename PublisherT = rclcpp::Publisher<MessageT, AllocatorT>,
   typename NodeT>
 std::shared_ptr<PublisherT>
 create_publisher(
-  NodeT & node,
+  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);
+  return detail::create_publisher<MessageT, AllocatorT, PublisherT>(
+    node, node, topic_name, qos, options);
+}
 
-  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);
+/// Create and return a publisher of the given MessageT type.
+template<
+  typename MessageT,
+  typename AllocatorT = std::allocator<void>,
+  typename PublisherT = rclcpp::Publisher<MessageT, AllocatorT>>
+std::shared_ptr<PublisherT>
+create_publisher(
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters,
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr & node_topics,
+  const std::string & topic_name,
+  const rclcpp::QoS & qos,
+  const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options = (
+    rclcpp::PublisherOptionsWithAllocator<AllocatorT>()
+  )
+)
+{
+  return detail::create_publisher<MessageT, AllocatorT, PublisherT>(
+    node_parameters, node_topics, topic_name, qos, options);
 }
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/create_service.hpp

@@ -26,6 +26,32 @@
 
 namespace rclcpp
 {
+/// Create a service with a given type.
+/**
+ * \param[in] node_base NodeBaseInterface implementation of the node on which
+ *  to create the service.
+ * \param[in] node_services NodeServicesInterface implementation of the node on
+ *  which to create the service.
+ * \param[in] service_name The name on which the service is accessible.
+ * \param[in] callback The callback to call when the service gets a request.
+ * \param[in] qos Quality of service profile for the service.
+ * \param[in] group Callback group to handle the reply to service calls.
+ * \return Shared pointer to the created service.
+ */
+template<typename ServiceT, typename CallbackT>
+typename rclcpp::Service<ServiceT>::SharedPtr
+create_service(
+  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+  std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+  const std::string & service_name,
+  CallbackT && callback,
+  const rclcpp::QoS & qos,
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  return create_service<ServiceT, CallbackT>(
+    node_base, node_services, service_name,
+    std::forward<CallbackT>(callback), qos.get_rmw_qos_profile(), group);
+}
 
 /// Create a service with a given type.
 /// \internal
@@ -37,7 +63,7 @@ create_service(
   const std::string & service_name,
   CallbackT && callback,
   const rmw_qos_profile_t & qos_profile,
-  rclcpp::callback_group::CallbackGroup::SharedPtr group)
+  rclcpp::CallbackGroup::SharedPtr group)
 {
   rclcpp::AnyServiceCallback<ServiceT> any_service_callback;
   any_service_callback.set(std::forward<CallbackT>(callback));

rclcpp/include/rclcpp/create_subscription.hpp

@@ -15,131 +15,212 @@
 #ifndef RCLCPP__CREATE_SUBSCRIPTION_HPP_
 #define RCLCPP__CREATE_SUBSCRIPTION_HPP_
 
+#include <chrono>
+#include <functional>
 #include <memory>
+#include <stdexcept>
 #include <string>
 #include <utility>
 
+#include "rclcpp/detail/resolve_enable_topic_statistics.hpp"
+
+#include "rclcpp/node_interfaces/get_node_timers_interface.hpp"
 #include "rclcpp/node_interfaces/get_node_topics_interface.hpp"
+#include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
+
+#include "rclcpp/create_publisher.hpp"
+#include "rclcpp/create_timer.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/subscription_factory.hpp"
 #include "rclcpp/subscription_options.hpp"
-#include "rclcpp/qos.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp"
 #include "rmw/qos_profiles.h"
 
 namespace rclcpp
 {
 
+namespace detail
+{
 template<
   typename MessageT,
   typename CallbackT,
   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 SubscriptionT,
+  typename MessageMemoryStrategyT,
+  typename NodeParametersT,
+  typename NodeTopicsT
+>
 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,
-  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
-    CallbackMessageT, AllocatorT>::SharedPtr
-  msg_mem_strat,
-  typename std::shared_ptr<AllocatorT> allocator)
-{
-  auto subscription_options = rcl_subscription_get_default_options();
-  subscription_options.qos = qos_profile;
-  subscription_options.ignore_local_publications = ignore_local_publications;
-
-  auto factory = rclcpp::create_subscription_factory
-    <MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT>(
-    std::forward<CallbackT>(callback),
-    event_callbacks,
-    msg_mem_strat,
-    allocator);
-
-  auto sub = node_topics->create_subscription(
-    topic_name,
-    factory,
-    subscription_options,
-    use_intra_process_comms);
-  node_topics->add_subscription(sub, group);
-  return std::dynamic_pointer_cast<SubscriptionT>(sub);
-}
-
-/// 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,
+  NodeParametersT & node_parameters,
+  NodeTopicsT & node_topics,
   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)
+  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = (
+    MessageMemoryStrategyT::create_default()
+  )
+)
 {
   using rclcpp::node_interfaces::get_node_topics_interface;
-  auto node_topics = get_node_topics_interface(std::forward<NodeT>(node));
+  auto node_topics_interface = get_node_topics_interface(node_topics);
 
-  if (!msg_mem_strat) {
-    using rclcpp::message_memory_strategy::MessageMemoryStrategy;
-    msg_mem_strat = MessageMemoryStrategy<CallbackMessageT, AllocatorT>::create_default();
+  std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics>
+  subscription_topic_stats = nullptr;
+
+  if (rclcpp::detail::resolve_enable_topic_statistics(
+      options,
+      *node_topics_interface->get_node_base_interface()))
+  {
+    if (options.topic_stats_options.publish_period <= std::chrono::milliseconds(0)) {
+      throw std::invalid_argument(
+              "topic_stats_options.publish_period must be greater than 0, specified value of " +
+              std::to_string(options.topic_stats_options.publish_period.count()) + " ms");
+    }
+
+    std::shared_ptr<Publisher<statistics_msgs::msg::MetricsMessage>>
+    publisher = rclcpp::detail::create_publisher<statistics_msgs::msg::MetricsMessage>(
+      node_parameters,
+      node_topics_interface,
+      options.topic_stats_options.publish_topic,
+      options.topic_stats_options.qos);
+
+    subscription_topic_stats =
+      std::make_shared<rclcpp::topic_statistics::SubscriptionTopicStatistics>(
+      node_topics_interface->get_node_base_interface()->get_name(), publisher);
+
+    std::weak_ptr<
+      rclcpp::topic_statistics::SubscriptionTopicStatistics
+    > weak_subscription_topic_stats(subscription_topic_stats);
+    auto sub_call_back = [weak_subscription_topic_stats]() {
+        auto subscription_topic_stats = weak_subscription_topic_stats.lock();
+        if (subscription_topic_stats) {
+          subscription_topic_stats->publish_message_and_reset_measurements();
+        }
+      };
+
+    auto node_timer_interface = node_topics_interface->get_node_timers_interface();
+
+    auto timer = create_wall_timer(
+      std::chrono::duration_cast<std::chrono::nanoseconds>(
+        options.topic_stats_options.publish_period),
+      sub_call_back,
+      options.callback_group,
+      node_topics_interface->get_node_base_interface(),
+      node_timer_interface
+    );
+
+    subscription_topic_stats->set_publisher_timer(timer);
   }
 
-  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);
+  auto factory = rclcpp::create_subscription_factory<MessageT>(
+    std::forward<CallbackT>(callback),
+    options,
+    msg_mem_strat,
+    subscription_topic_stats
+  );
 
-  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;
-  }
+  const rclcpp::QoS & actual_qos = options.qos_overriding_options.get_policy_kinds().size() ?
+    rclcpp::detail::declare_qos_parameters(
+    options.qos_overriding_options, node_parameters,
+    node_topics_interface->resolve_topic_name(topic_name),
+    qos, rclcpp::detail::SubscriptionQosParametersTraits{}) :
+    qos;
+
+  auto sub = node_topics_interface->create_subscription(topic_name, factory, actual_qos);
+  node_topics_interface->add_subscription(sub, options.callback_group);
 
-  // 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 detail
+
+/// 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.
+ *
+ * In case `options.qos_overriding_options` is enabling qos parameter overrides,
+ * NodeT must also have a method called get_node_parameters_interface()
+ * which returns a shared_ptr to a NodeParametersInterface.
+ *
+ * \tparam MessageT
+ * \tparam CallbackT
+ * \tparam AllocatorT
+ * \tparam SubscriptionT
+ * \tparam MessageMemoryStrategyT
+ * \tparam NodeT
+ * \param node
+ * \param topic_name
+ * \param qos
+ * \param callback
+ * \param options
+ * \param msg_mem_strat
+ * \return the created subscription
+ * \throws std::invalid_argument if topic statistics is enabled and the publish period is
+ * less than or equal to zero.
+ */
+template<
+  typename MessageT,
+  typename CallbackT,
+  typename AllocatorT = std::allocator<void>,
+  typename SubscriptionT = rclcpp::Subscription<MessageT, AllocatorT>,
+  typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType,
+  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 MessageMemoryStrategyT::SharedPtr msg_mem_strat = (
+    MessageMemoryStrategyT::create_default()
+  )
+)
+{
+  return rclcpp::detail::create_subscription<
+    MessageT, CallbackT, AllocatorT, SubscriptionT, MessageMemoryStrategyT>(
+    node, node, topic_name, qos, std::forward<CallbackT>(callback), options, msg_mem_strat);
+}
+
+/// Create and return a subscription of the given MessageT type.
+/**
+ * See \ref create_subscription().
+ */
+template<
+  typename MessageT,
+  typename CallbackT,
+  typename AllocatorT = std::allocator<void>,
+  typename SubscriptionT = rclcpp::Subscription<MessageT, AllocatorT>,
+  typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType>
+typename std::shared_ptr<SubscriptionT>
+create_subscription(
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters,
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr & node_topics,
+  const std::string & topic_name,
+  const rclcpp::QoS & qos,
+  CallbackT && callback,
+  const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
+    rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
+  ),
+  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = (
+    MessageMemoryStrategyT::create_default()
+  )
+)
+{
+  return rclcpp::detail::create_subscription<
+    MessageT, CallbackT, AllocatorT, SubscriptionT, MessageMemoryStrategyT>(
+    node_parameters, node_topics, topic_name, qos,
+    std::forward<CallbackT>(callback), options, msg_mem_strat);
+}
 
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/create_timer.hpp

@@ -0,0 +1,217 @@
+// 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__CREATE_TIMER_HPP_
+#define RCLCPP__CREATE_TIMER_HPP_
+
+#include <chrono>
+#include <exception>
+#include <memory>
+#include <string>
+#include <utility>
+
+#include "rclcpp/duration.hpp"
+#include "rclcpp/node_interfaces/get_node_base_interface.hpp"
+#include "rclcpp/node_interfaces/get_node_clock_interface.hpp"
+#include "rclcpp/node_interfaces/get_node_timers_interface.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_clock_interface.hpp"
+#include "rclcpp/node_interfaces/node_timers_interface.hpp"
+
+namespace rclcpp
+{
+namespace detail
+{
+/// Perform a safe cast to a timer period in nanoseconds
+/**
+ *
+ * \tparam DurationRepT
+ * \tparam DurationT
+ * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max()
+ * \return period, expressed as chrono::duration::nanoseconds
+ * \throws std::invalid_argument if period is negative or too large
+ */
+template<typename DurationRepT, typename DurationT>
+std::chrono::nanoseconds
+safe_cast_to_period_in_ns(std::chrono::duration<DurationRepT, DurationT> period)
+{
+  if (period < std::chrono::duration<DurationRepT, DurationT>::zero()) {
+    throw std::invalid_argument{"timer period cannot be negative"};
+  }
+
+  // Casting to a double representation might lose precision and allow the check below to succeed
+  // but the actual cast to nanoseconds fail. Using 1 DurationT worth of nanoseconds less than max.
+  constexpr auto maximum_safe_cast_ns =
+    std::chrono::nanoseconds::max() - std::chrono::duration<DurationRepT, DurationT>(1);
+
+  // If period is greater than nanoseconds::max(), the duration_cast to nanoseconds will overflow
+  // a signed integer, which is undefined behavior. Checking whether any std::chrono::duration is
+  // greater than nanoseconds::max() is a difficult general problem. This is a more conservative
+  // version of Howard Hinnant's (the <chrono> guy>) response here:
+  // https://stackoverflow.com/a/44637334/2089061
+  // However, this doesn't solve the issue for all possible duration types of period.
+  // Follow-up issue: https://github.com/ros2/rclcpp/issues/1177
+  constexpr auto ns_max_as_double =
+    std::chrono::duration_cast<std::chrono::duration<double, std::chrono::nanoseconds::period>>(
+    maximum_safe_cast_ns);
+  if (period > ns_max_as_double) {
+    throw std::invalid_argument{
+            "timer period must be less than std::chrono::nanoseconds::max()"};
+  }
+
+  const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(period);
+  if (period_ns < std::chrono::nanoseconds::zero()) {
+    throw std::runtime_error{
+            "Casting timer period to nanoseconds resulted in integer overflow."};
+  }
+
+  return period_ns;
+}
+}  // namespace detail
+
+/// Create a timer with a given clock
+/// \internal
+template<typename CallbackT>
+typename rclcpp::TimerBase::SharedPtr
+create_timer(
+  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+  std::shared_ptr<node_interfaces::NodeTimersInterface> node_timers,
+  rclcpp::Clock::SharedPtr clock,
+  rclcpp::Duration period,
+  CallbackT && callback,
+  rclcpp::CallbackGroup::SharedPtr group = nullptr,
+  bool autostart = true)
+{
+  return create_timer(
+    clock,
+    period.to_chrono<std::chrono::nanoseconds>(),
+    std::forward<CallbackT>(callback),
+    group,
+    node_base.get(),
+    node_timers.get(),
+    autostart);
+}
+
+/// Create a timer with a given clock
+template<typename NodeT, typename CallbackT>
+typename rclcpp::TimerBase::SharedPtr
+create_timer(
+  NodeT node,
+  rclcpp::Clock::SharedPtr clock,
+  rclcpp::Duration period,
+  CallbackT && callback,
+  rclcpp::CallbackGroup::SharedPtr group = nullptr,
+  bool autostart = true)
+{
+  return create_timer(
+    clock,
+    period.to_chrono<std::chrono::nanoseconds>(),
+    std::forward<CallbackT>(callback),
+    group,
+    rclcpp::node_interfaces::get_node_base_interface(node).get(),
+    rclcpp::node_interfaces::get_node_timers_interface(node).get(),
+    autostart);
+}
+
+/// Convenience method to create a general timer with node resources.
+/**
+ *
+ * \tparam DurationRepT
+ * \tparam DurationT
+ * \tparam CallbackT
+ * \param clock clock to be used
+ * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max()
+ * \param callback callback to execute via the timer period
+ * \param group callback group
+ * \param node_base node base interface
+ * \param node_timers node timer interface
+ * \param autostart defines if the timer should start it's countdown on initialization or not.
+ * \return shared pointer to a generic timer
+ * \throws std::invalid_argument if either clock, node_base or node_timers
+ * are nullptr, or period is negative or too large
+ */
+template<typename DurationRepT, typename DurationT, typename CallbackT>
+typename rclcpp::GenericTimer<CallbackT>::SharedPtr
+create_timer(
+  rclcpp::Clock::SharedPtr clock,
+  std::chrono::duration<DurationRepT, DurationT> period,
+  CallbackT callback,
+  rclcpp::CallbackGroup::SharedPtr group,
+  node_interfaces::NodeBaseInterface * node_base,
+  node_interfaces::NodeTimersInterface * node_timers,
+  bool autostart = true)
+{
+  if (clock == nullptr) {
+    throw std::invalid_argument{"clock cannot be null"};
+  }
+  if (node_base == nullptr) {
+    throw std::invalid_argument{"input node_base cannot be null"};
+  }
+  if (node_timers == nullptr) {
+    throw std::invalid_argument{"input node_timers cannot be null"};
+  }
+
+  const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period);
+
+  // Add a new generic timer.
+  auto timer = rclcpp::GenericTimer<CallbackT>::make_shared(
+    std::move(clock), period_ns, std::move(callback), node_base->get_context(), autostart);
+  node_timers->add_timer(timer, group);
+  return timer;
+}
+
+/// Convenience method to create a wall timer with node resources.
+/**
+ *
+ * \tparam DurationRepT
+ * \tparam DurationT
+ * \tparam CallbackT
+ * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max()
+ * \param callback callback to execute via the timer period
+ * \param group callback group
+ * \param node_base node base interface
+ * \param node_timers node timer interface
+ * \return shared pointer to a wall timer
+ * \throws std::invalid_argument if either node_base or node_timers
+ * are null, or period is negative or too large
+ */
+template<typename DurationRepT, typename DurationT, typename CallbackT>
+typename rclcpp::WallTimer<CallbackT>::SharedPtr
+create_wall_timer(
+  std::chrono::duration<DurationRepT, DurationT> period,
+  CallbackT callback,
+  rclcpp::CallbackGroup::SharedPtr group,
+  node_interfaces::NodeBaseInterface * node_base,
+  node_interfaces::NodeTimersInterface * node_timers,
+  bool autostart = true)
+{
+  if (node_base == nullptr) {
+    throw std::invalid_argument{"input node_base cannot be null"};
+  }
+
+  if (node_timers == nullptr) {
+    throw std::invalid_argument{"input node_timers cannot be null"};
+  }
+
+  const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period);
+
+  // Add a new wall timer.
+  auto timer = rclcpp::WallTimer<CallbackT>::make_shared(
+    period_ns, std::move(callback), node_base->get_context(), autostart);
+  node_timers->add_timer(timer, group);
+  return timer;
+}
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CREATE_TIMER_HPP_

rclcpp/include/rclcpp/detail/README.md

@@ -0,0 +1,3 @@
+Notice that headers in this folder should only provide symbols in the rclcpp::detail namespace.
+
+Also that these headers are not considered part of the public API and are subject to change without notice.

rclcpp/include/rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp

@@ -0,0 +1,39 @@
+// Copyright 2021 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__DETAIL__ADD_GUARD_CONDITION_TO_RCL_WAIT_SET_HPP_
+#define RCLCPP__DETAIL__ADD_GUARD_CONDITION_TO_RCL_WAIT_SET_HPP_
+
+#include "rclcpp/guard_condition.hpp"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+/// Adds the guard condition to a waitset
+/**
+ * \param[in] wait_set reference to a wait set where to add the guard condition
+ * \param[in] guard_condition reference to the guard_condition to be added
+ */
+RCLCPP_PUBLIC
+void
+add_guard_condition_to_rcl_wait_set(
+  rcl_wait_set_t & wait_set,
+  const rclcpp::GuardCondition & guard_condition);
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__ADD_GUARD_CONDITION_TO_RCL_WAIT_SET_HPP_

rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp

@@ -0,0 +1,70 @@
+// Copyright 2021 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__DETAIL__CPP_CALLBACK_TRAMPOLINE_HPP_
+#define RCLCPP__DETAIL__CPP_CALLBACK_TRAMPOLINE_HPP_
+
+#include <functional>
+
+namespace rclcpp
+{
+
+namespace detail
+{
+
+/// Trampoline pattern for wrapping std::function into C-style callbacks.
+/**
+ * A common pattern in C is for a function to take a function pointer and a
+ * void pointer for "user data" which is passed to the function pointer when it
+ * is called from within C.
+ *
+ * It works by using the user data pointer to store a pointer to a
+ * std::function instance.
+ * So when called from C, this function will cast the user data to the right
+ * std::function type and call it.
+ *
+ * This should allow you to use free functions, lambdas with and without
+ * captures, and various kinds of std::bind instances.
+ *
+ * The interior of this function is likely to be executed within a C runtime,
+ * so no exceptions should be thrown at this point, and doing so results in
+ * undefined behavior.
+ *
+ * \tparam UserDataRealT Declared type of the passed function
+ * \tparam UserDataT Deduced type based on what is passed for user data,
+ *   usually this type is either `void *` or `const void *`.
+ * \tparam Args the arguments being passed to the callback
+ * \tparam ReturnT the return type of this function and the callback, default void
+ * \param user_data the function pointer, possibly type erased
+ * \param args the arguments to be forwarded to the callback
+ * \returns whatever the callback returns, if anything
+ */
+template<
+  typename UserDataRealT,
+  typename UserDataT,
+  typename ... Args,
+  typename ReturnT = void
+>
+ReturnT
+cpp_callback_trampoline(UserDataT user_data, Args ... args) noexcept
+{
+  auto & actual_callback = *static_cast<const UserDataRealT *>(user_data);
+  return actual_callback(args ...);
+}
+
+}  // namespace detail
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__CPP_CALLBACK_TRAMPOLINE_HPP_

rclcpp/include/rclcpp/detail/qos_parameters.hpp

@@ -0,0 +1,343 @@
+// Copyright 2020 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__DETAIL__QOS_PARAMETERS_HPP_
+#define RCLCPP__DETAIL__QOS_PARAMETERS_HPP_
+
+#include <algorithm>
+#include <array>
+#include <functional>
+#include <initializer_list>
+#include <map>
+#include <string>
+#include <type_traits>
+#include <vector>
+
+#include "rcl_interfaces/msg/parameter_descriptor.hpp"
+#include "rcpputils/pointer_traits.hpp"
+#include "rmw/qos_string_conversions.h"
+
+#include "rclcpp/duration.hpp"
+#include "rclcpp/node_interfaces/get_node_parameters_interface.hpp"
+#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
+#include "rclcpp/qos_overriding_options.hpp"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+/// \internal Trait used to specialize `declare_qos_parameters()` for publishers.
+struct PublisherQosParametersTraits
+{
+  static constexpr const char * entity_type() {return "publisher";}
+  static constexpr auto allowed_policies()
+  {
+    return std::array<::rclcpp::QosPolicyKind, 9> {
+      QosPolicyKind::AvoidRosNamespaceConventions,
+      QosPolicyKind::Deadline,
+      QosPolicyKind::Durability,
+      QosPolicyKind::History,
+      QosPolicyKind::Depth,
+      QosPolicyKind::Lifespan,
+      QosPolicyKind::Liveliness,
+      QosPolicyKind::LivelinessLeaseDuration,
+      QosPolicyKind::Reliability,
+    };
+  }
+};
+
+/// \internal Trait used to specialize `declare_qos_parameters()` for subscriptions.
+struct SubscriptionQosParametersTraits
+{
+  static constexpr const char * entity_type() {return "subscription";}
+  static constexpr auto allowed_policies()
+  {
+    return std::array<::rclcpp::QosPolicyKind, 8> {
+      QosPolicyKind::AvoidRosNamespaceConventions,
+      QosPolicyKind::Deadline,
+      QosPolicyKind::Durability,
+      QosPolicyKind::History,
+      QosPolicyKind::Depth,
+      QosPolicyKind::Liveliness,
+      QosPolicyKind::LivelinessLeaseDuration,
+      QosPolicyKind::Reliability,
+    };
+  }
+};
+
+/// \internal Returns the given `policy` of the profile `qos` converted to a parameter value.
+inline
+::rclcpp::ParameterValue
+get_default_qos_param_value(rclcpp::QosPolicyKind policy, const rclcpp::QoS & qos);
+
+/// \internal Modify the given `policy` in `qos` to be `value`.
+inline
+void
+apply_qos_override(
+  rclcpp::QosPolicyKind policy, rclcpp::ParameterValue value, rclcpp::QoS & qos);
+
+inline
+rclcpp::ParameterValue
+declare_parameter_or_get(
+  rclcpp::node_interfaces::NodeParametersInterface & parameters_interface,
+  const std::string & param_name,
+  rclcpp::ParameterValue param_value,
+  rcl_interfaces::msg::ParameterDescriptor descriptor)
+{
+  try {
+    // enable parameter modification to make it possible
+    // to declare QoS override parameters during parameter callbacks.
+    parameters_interface.enable_parameter_modification();
+    return parameters_interface.declare_parameter(
+      param_name, param_value, descriptor);
+  } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) {
+    return parameters_interface.get_parameter(param_name).get_parameter_value();
+  }
+}
+
+#ifdef DOXYGEN_ONLY
+/// \internal Declare QoS parameters for the given entity.
+/**
+ * \tparam NodeT Node pointer or reference type.
+ * \tparam EntityQosParametersTraits A class with two static methods: `entity_type()` and
+ *  `allowed_policies()`. See `PublisherQosParametersTraits` and `SubscriptionQosParametersTraits`.
+ * \param options User provided options that indicate if QoS parameter overrides should be
+ *  declared or not, which policy can have overrides, and optionally a callback to validate the profile.
+ * \param node Parameters will be declared using this node.
+ * \param topic_name Name of the topic of the entity.
+ * \param default_qos User provided qos. It will be used as a default for the parameters declared.
+ * \return qos profile based on the user provided parameter overrides.
+ */
+template<typename NodeT, typename EntityQosParametersTraits>
+rclcpp::QoS
+  declare_qos_parameters(
+  const ::rclcpp::QosOverridingOptions & options,
+  NodeT & node,
+  const std::string & topic_name,
+  const ::rclcpp::QoS & default_qos,
+  EntityQosParametersTraits);
+
+#else
+
+template<typename NodeT, typename EntityQosParametersTraits>
+std::enable_if_t<
+  (rclcpp::node_interfaces::has_node_parameters_interface<
+    decltype(std::declval<typename rcpputils::remove_pointer<NodeT>::type>())>::value ||
+  std::is_same<typename std::decay_t<NodeT>,
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr>::value),
+  rclcpp::QoS>
+declare_qos_parameters(
+  const ::rclcpp::QosOverridingOptions & options,
+  NodeT & node,
+  const std::string & topic_name,
+  const ::rclcpp::QoS & default_qos,
+  EntityQosParametersTraits)
+{
+  auto & parameters_interface = *rclcpp::node_interfaces::get_node_parameters_interface(node);
+  std::string param_prefix;
+  const auto & id = options.get_id();
+  {
+    std::ostringstream oss{"qos_overrides.", std::ios::ate};
+    oss << topic_name << "." << EntityQosParametersTraits::entity_type();
+    if (!id.empty()) {
+      oss << "_" << id;
+    }
+    oss << ".";
+    param_prefix = oss.str();
+  }
+  std::string param_description_suffix;
+  {
+    std::ostringstream oss{"} for ", std::ios::ate};
+    oss << EntityQosParametersTraits::entity_type() << " {" << topic_name << "}";
+    if (!id.empty()) {
+      oss << " with id {" << id << "}";
+    }
+    param_description_suffix = oss.str();
+  }
+  rclcpp::QoS qos = default_qos;
+  for (auto policy : EntityQosParametersTraits::allowed_policies()) {
+    if (
+      std::count(options.get_policy_kinds().begin(), options.get_policy_kinds().end(), policy))
+    {
+      std::ostringstream param_name{param_prefix, std::ios::ate};
+      param_name << qos_policy_kind_to_cstr(policy);
+      std::ostringstream param_desciption{"qos policy {", std::ios::ate};
+      param_desciption << qos_policy_kind_to_cstr(policy) << param_description_suffix;
+      rcl_interfaces::msg::ParameterDescriptor descriptor{};
+      descriptor.description = param_desciption.str();
+      descriptor.read_only = true;
+      auto value = declare_parameter_or_get(
+        parameters_interface, param_name.str(),
+        get_default_qos_param_value(policy, qos), descriptor);
+      ::rclcpp::detail::apply_qos_override(policy, value, qos);
+    }
+  }
+  const auto & validation_callback = options.get_validation_callback();
+  if (validation_callback) {
+    auto result = validation_callback(qos);
+    if (!result.successful) {
+      throw rclcpp::exceptions::InvalidQosOverridesException{
+              "validation callback failed: " + result.reason};
+    }
+  }
+  return qos;
+}
+
+// TODO(ivanpauno): This overload cannot declare the QoS parameters, as a node parameters interface
+// was not provided.
+template<typename NodeT, typename EntityQosParametersTraits>
+std::enable_if_t<
+  !(rclcpp::node_interfaces::has_node_parameters_interface<
+    decltype(std::declval<typename rcpputils::remove_pointer<NodeT>::type>())>::value ||
+  std::is_same<typename std::decay_t<NodeT>,
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr>::value),
+  rclcpp::QoS>
+declare_qos_parameters(
+  const ::rclcpp::QosOverridingOptions & options,
+  NodeT &,
+  const std::string &,
+  const ::rclcpp::QoS & default_qos,
+  EntityQosParametersTraits)
+{
+  if (options.get_policy_kinds().size()) {
+    std::runtime_error exc{
+      "passed non-default qos overriding options without providing a parameters interface"};
+    throw exc;
+  }
+  return default_qos;
+}
+
+#endif
+
+/// \internal Helper function to get a rmw qos policy value from a string.
+#define RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING( \
+    kind_lower, kind_upper, parameter_value, rclcpp_qos) \
+  do { \
+    auto policy_string = (parameter_value).get<std::string>(); \
+    auto policy_value = rmw_qos_ ## kind_lower ## _policy_from_str(policy_string.c_str()); \
+    if (RMW_QOS_POLICY_ ## kind_upper ## _UNKNOWN == policy_value) { \
+      throw std::invalid_argument{"unknown QoS policy " #kind_lower " value: " + policy_string}; \
+    } \
+    ((rclcpp_qos).kind_lower)(policy_value); \
+  } while (0)
+
+inline
+void
+apply_qos_override(
+  rclcpp::QosPolicyKind policy, rclcpp::ParameterValue value, rclcpp::QoS & qos)
+{
+  switch (policy) {
+    case QosPolicyKind::AvoidRosNamespaceConventions:
+      qos.avoid_ros_namespace_conventions(value.get<bool>());
+      break;
+    case QosPolicyKind::Deadline:
+      qos.deadline(::rclcpp::Duration::from_nanoseconds(value.get<int64_t>()));
+      break;
+    case QosPolicyKind::Durability:
+      RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING(
+        durability, DURABILITY, value, qos);
+      break;
+    case QosPolicyKind::History:
+      RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING(
+        history, HISTORY, value, qos);
+      break;
+    case QosPolicyKind::Depth:
+      qos.get_rmw_qos_profile().depth = static_cast<size_t>(value.get<int64_t>());
+      break;
+    case QosPolicyKind::Lifespan:
+      qos.lifespan(::rclcpp::Duration::from_nanoseconds(value.get<int64_t>()));
+      break;
+    case QosPolicyKind::Liveliness:
+      RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING(
+        liveliness, LIVELINESS, value, qos);
+      break;
+    case QosPolicyKind::LivelinessLeaseDuration:
+      qos.liveliness_lease_duration(::rclcpp::Duration::from_nanoseconds(value.get<int64_t>()));
+      break;
+    case QosPolicyKind::Reliability:
+      RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING(
+        reliability, RELIABILITY, value, qos);
+      break;
+    default:
+      throw std::invalid_argument{"unknown QosPolicyKind"};
+  }
+}
+
+/// Convert `rmw_time_t` to `int64_t` that can be used as a parameter value.
+inline
+int64_t
+rmw_duration_to_int64_t(rmw_time_t rmw_duration)
+{
+  return ::rclcpp::Duration(
+    static_cast<int32_t>(rmw_duration.sec),
+    static_cast<uint32_t>(rmw_duration.nsec)
+  ).nanoseconds();
+}
+
+/// \internal Throw an exception if `policy_value_stringified` is NULL.
+inline
+const char *
+check_if_stringified_policy_is_null(const char * policy_value_stringified, QosPolicyKind kind)
+{
+  if (!policy_value_stringified) {
+    std::ostringstream oss{"unknown value for policy kind {", std::ios::ate};
+    oss << kind << "}";
+    throw std::invalid_argument{oss.str()};
+  }
+  return policy_value_stringified;
+}
+
+inline
+::rclcpp::ParameterValue
+get_default_qos_param_value(rclcpp::QosPolicyKind kind, const rclcpp::QoS & qos)
+{
+  using ParameterValue = ::rclcpp::ParameterValue;
+  const auto & rmw_qos = qos.get_rmw_qos_profile();
+  switch (kind) {
+    case QosPolicyKind::AvoidRosNamespaceConventions:
+      return ParameterValue(rmw_qos.avoid_ros_namespace_conventions);
+    case QosPolicyKind::Deadline:
+      return ParameterValue(rmw_duration_to_int64_t(rmw_qos.deadline));
+    case QosPolicyKind::Durability:
+      return ParameterValue(
+        check_if_stringified_policy_is_null(
+          rmw_qos_durability_policy_to_str(rmw_qos.durability), kind));
+    case QosPolicyKind::History:
+      return ParameterValue(
+        check_if_stringified_policy_is_null(
+          rmw_qos_history_policy_to_str(rmw_qos.history), kind));
+    case QosPolicyKind::Depth:
+      return ParameterValue(static_cast<int64_t>(rmw_qos.depth));
+    case QosPolicyKind::Lifespan:
+      return ParameterValue(rmw_duration_to_int64_t(rmw_qos.lifespan));
+    case QosPolicyKind::Liveliness:
+      return ParameterValue(
+        check_if_stringified_policy_is_null(
+          rmw_qos_liveliness_policy_to_str(rmw_qos.liveliness), kind));
+    case QosPolicyKind::LivelinessLeaseDuration:
+      return ParameterValue(rmw_duration_to_int64_t(rmw_qos.liveliness_lease_duration));
+    case QosPolicyKind::Reliability:
+      return ParameterValue(
+        check_if_stringified_policy_is_null(
+          rmw_qos_reliability_policy_to_str(rmw_qos.reliability), kind));
+    default:
+      throw std::invalid_argument{"unknown QoS policy kind"};
+  }
+}
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__QOS_PARAMETERS_HPP_

rclcpp/include/rclcpp/detail/resolve_enable_topic_statistics.hpp

@@ -0,0 +1,53 @@
+// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved.
+//
+// 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__DETAIL__RESOLVE_ENABLE_TOPIC_STATISTICS_HPP_
+#define RCLCPP__DETAIL__RESOLVE_ENABLE_TOPIC_STATISTICS_HPP_
+
+#include <stdexcept>
+
+#include "rclcpp/topic_statistics_state.hpp"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+/// Return whether or not topic statistics is enabled, resolving "NodeDefault" if needed.
+template<typename OptionsT, typename NodeBaseT>
+bool
+resolve_enable_topic_statistics(const OptionsT & options, const NodeBaseT & node_base)
+{
+  bool topic_stats_enabled;
+  switch (options.topic_stats_options.state) {
+    case TopicStatisticsState::Enable:
+      topic_stats_enabled = true;
+      break;
+    case TopicStatisticsState::Disable:
+      topic_stats_enabled = false;
+      break;
+    case TopicStatisticsState::NodeDefault:
+      topic_stats_enabled = node_base.get_enable_topic_statistics_default();
+      break;
+    default:
+      throw std::runtime_error("Unrecognized EnableTopicStatistics value");
+  }
+
+  return topic_stats_enabled;
+}
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__RESOLVE_ENABLE_TOPIC_STATISTICS_HPP_

rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp

@@ -0,0 +1,59 @@
+// 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__DETAIL__RESOLVE_INTRA_PROCESS_BUFFER_TYPE_HPP_
+#define RCLCPP__DETAIL__RESOLVE_INTRA_PROCESS_BUFFER_TYPE_HPP_
+
+#include <stdexcept>
+
+#include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/intra_process_buffer_type.hpp"
+
+namespace rclcpp
+{
+
+namespace detail
+{
+
+/// Return the buffer type, resolving the "CallbackDefault" type to an actual type if needed.
+template<typename CallbackMessageT, typename AllocatorT>
+rclcpp::IntraProcessBufferType
+resolve_intra_process_buffer_type(
+  const rclcpp::IntraProcessBufferType buffer_type,
+  const rclcpp::AnySubscriptionCallback<CallbackMessageT, AllocatorT> & any_subscription_callback)
+{
+  rclcpp::IntraProcessBufferType resolved_buffer_type = buffer_type;
+
+  // If the user has not specified a type for the intra-process buffer, use the callback's type.
+  if (resolved_buffer_type == IntraProcessBufferType::CallbackDefault) {
+    if (any_subscription_callback.use_take_shared_method()) {
+      resolved_buffer_type = IntraProcessBufferType::SharedPtr;
+    } else {
+      resolved_buffer_type = IntraProcessBufferType::UniquePtr;
+    }
+  }
+
+  return resolved_buffer_type;
+}
+
+RCLCPP_PUBLIC
+rclcpp::IntraProcessBufferType
+resolve_intra_process_buffer_type(
+  const rclcpp::IntraProcessBufferType buffer_type);
+
+}  // namespace detail
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__RESOLVE_INTRA_PROCESS_BUFFER_TYPE_HPP_

rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp

@@ -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.
+
+#ifndef RCLCPP__DETAIL__RESOLVE_USE_INTRA_PROCESS_HPP_
+#define RCLCPP__DETAIL__RESOLVE_USE_INTRA_PROCESS_HPP_
+
+#include <stdexcept>
+
+#include "rclcpp/intra_process_setting.hpp"
+
+namespace rclcpp
+{
+
+namespace detail
+{
+
+/// Return whether or not intra process is enabled, resolving "NodeDefault" if needed.
+template<typename OptionsT, typename NodeBaseT>
+bool
+resolve_use_intra_process(const OptionsT & options, const NodeBaseT & node_base)
+{
+  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_base.get_use_intra_process_default();
+      break;
+    default:
+      throw std::runtime_error("Unrecognized IntraProcessSetting value");
+      break;
+  }
+
+  return use_intra_process;
+}
+
+}  // namespace detail
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__RESOLVE_USE_INTRA_PROCESS_HPP_

rclcpp/include/rclcpp/detail/rmw_implementation_specific_payload.hpp

@@ -0,0 +1,51 @@
+// 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__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_PAYLOAD_HPP_
+#define RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_PAYLOAD_HPP_
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+/// Mechanism for passing rmw implementation specific settings through the ROS interfaces.
+class RCLCPP_PUBLIC RMWImplementationSpecificPayload
+{
+public:
+  virtual
+  ~RMWImplementationSpecificPayload() = default;
+
+  /// Return false if this class has not been customized, otherwise true.
+  /**
+   * It does this based on the value of the rmw implementation identifier that
+   * this class reports, and so it is important for a specialization of this
+   * class to override the get_rmw_implementation_identifier() method to return
+   * something other than nullptr.
+   */
+  bool
+  has_been_customized() const;
+
+  /// Derrived classes should override this and return the identifier of its rmw implementation.
+  virtual
+  const char *
+  get_implementation_identifier() const;
+};
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_PAYLOAD_HPP_

rclcpp/include/rclcpp/detail/rmw_implementation_specific_publisher_payload.hpp

@@ -0,0 +1,52 @@
+// 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__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_PUBLISHER_PAYLOAD_HPP_
+#define RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_PUBLISHER_PAYLOAD_HPP_
+
+#include "rcl/publisher.h"
+
+#include "rclcpp/detail/rmw_implementation_specific_payload.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+class RCLCPP_PUBLIC RMWImplementationSpecificPublisherPayload
+  : public RMWImplementationSpecificPayload
+{
+public:
+  ~RMWImplementationSpecificPublisherPayload() override = default;
+
+  /// Opportunity for a derived class to inject information into the rcl options.
+  /**
+   * This is called after the rcl_publisher_options_t has been prepared by
+   * rclcpp, but before rcl_publisher_init() is called.
+   *
+   * The passed option is the rmw_publisher_options field of the
+   * rcl_publisher_options_t that will be passed to rcl_publisher_init().
+   *
+   * By default the options are unmodified.
+   */
+  virtual
+  void
+  modify_rmw_publisher_options(rmw_publisher_options_t & rmw_publisher_options) const;
+};
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_PUBLISHER_PAYLOAD_HPP_

rclcpp/include/rclcpp/detail/rmw_implementation_specific_subscription_payload.hpp

@@ -0,0 +1,53 @@
+// 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__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_SUBSCRIPTION_PAYLOAD_HPP_
+#define RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_SUBSCRIPTION_PAYLOAD_HPP_
+
+#include "rcl/subscription.h"
+
+#include "rclcpp/detail/rmw_implementation_specific_payload.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+/// Subscription payload that may be rmw implementation specific.
+class RCLCPP_PUBLIC RMWImplementationSpecificSubscriptionPayload
+  : public RMWImplementationSpecificPayload
+{
+public:
+  ~RMWImplementationSpecificSubscriptionPayload() override = default;
+
+  /// Opportunity for a derived class to inject information into the rcl options.
+  /**
+   * This is called after the rcl_subscription_options_t has been prepared by
+   * rclcpp, but before rcl_subscription_init() is called.
+   *
+   * The passed option is the rmw_subscription_options field of the
+   * rcl_subscription_options_t that will be passed to rcl_subscription_init().
+   *
+   * By default the options are unmodified.
+   */
+  virtual
+  void
+  modify_rmw_subscription_options(rmw_subscription_options_t & rmw_subscription_options) const;
+};
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_SUBSCRIPTION_PAYLOAD_HPP_

rclcpp/include/rclcpp/detail/subscription_callback_type_helper.hpp

@@ -0,0 +1,166 @@
+// Copyright 2021 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__DETAIL__SUBSCRIPTION_CALLBACK_TYPE_HELPER_HPP_
+#define RCLCPP__DETAIL__SUBSCRIPTION_CALLBACK_TYPE_HELPER_HPP_
+
+#include <memory>
+#include <type_traits>
+
+#include "rclcpp/function_traits.hpp"
+#include "rclcpp/message_info.hpp"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+/// Template metaprogramming helper used to resolve the callback argument into a std::function.
+/**
+ * Sometimes the CallbackT is a std::function already, but it could also be a
+ * function pointer, lambda, bind, or some variant of those.
+ * In some cases, like a lambda where the arguments can be converted between one
+ * another, e.g. std::function<void (shared_ptr<...>)> and
+ * std::function<void (unique_ptr<...>)>, you need to make that not ambiguous
+ * by checking the arguments independently using function traits rather than
+ * rely on overloading the two std::function types.
+ *
+ * This issue, with the lambda's, can be demonstrated with this minimal program:
+ *
+ * \code{.cpp}
+ *   #include <functional>
+ *   #include <memory>
+ *
+ *   void f(std::function<void (std::shared_ptr<int>)>) {}
+ *   void f(std::function<void (std::unique_ptr<int>)>) {}
+ *
+ *   int main() {
+ *     // Fails to compile with an "ambiguous call" error.
+ *     f([](std::shared_ptr<int>){});
+ *
+ *     // Works.
+ *     std::function<void (std::shared_ptr<int>)> cb = [](std::shared_ptr<int>){};
+ *     f(cb);
+ *   }
+ * \endcode
+ *
+ * If this program ever starts working in a future version of C++, this class
+ * may become redundant.
+ *
+ * This helper works by using SFINAE with rclcpp::function_traits::same_arguments<>
+ * to narrow down the exact std::function<> type for the given CallbackT.
+ */
+template<typename MessageT, typename CallbackT, typename Enable = void>
+struct SubscriptionCallbackTypeHelper
+{
+  using callback_type = typename rclcpp::function_traits::as_std_function<CallbackT>::type;
+};
+
+template<typename MessageT, typename CallbackT>
+struct SubscriptionCallbackTypeHelper<
+  MessageT,
+  CallbackT,
+  typename std::enable_if_t<
+    rclcpp::function_traits::same_arguments<
+      CallbackT,
+      std::function<void(std::shared_ptr<const MessageT>)>
+    >::value
+  >
+>
+{
+  using callback_type = std::function<void (std::shared_ptr<const MessageT>)>;
+};
+
+template<typename MessageT, typename CallbackT>
+struct SubscriptionCallbackTypeHelper<
+  MessageT,
+  CallbackT,
+  typename std::enable_if_t<
+    rclcpp::function_traits::same_arguments<
+      CallbackT,
+      std::function<void(std::shared_ptr<const MessageT>, const rclcpp::MessageInfo &)>
+    >::value
+  >
+>
+{
+  using callback_type =
+    std::function<void (std::shared_ptr<const MessageT>, const rclcpp::MessageInfo &)>;
+};
+
+template<typename MessageT, typename CallbackT>
+struct SubscriptionCallbackTypeHelper<
+  MessageT,
+  CallbackT,
+  typename std::enable_if_t<
+    rclcpp::function_traits::same_arguments<
+      CallbackT,
+      std::function<void(const std::shared_ptr<const MessageT> &)>
+    >::value
+  >
+>
+{
+  using callback_type = std::function<void (const std::shared_ptr<const MessageT> &)>;
+};
+
+template<typename MessageT, typename CallbackT>
+struct SubscriptionCallbackTypeHelper<
+  MessageT,
+  CallbackT,
+  typename std::enable_if_t<
+    rclcpp::function_traits::same_arguments<
+      CallbackT,
+      std::function<void(const std::shared_ptr<const MessageT> &, const rclcpp::MessageInfo &)>
+    >::value
+  >
+>
+{
+  using callback_type =
+    std::function<void (const std::shared_ptr<const MessageT> &, const rclcpp::MessageInfo &)>;
+};
+
+template<typename MessageT, typename CallbackT>
+struct SubscriptionCallbackTypeHelper<
+  MessageT,
+  CallbackT,
+  typename std::enable_if_t<
+    rclcpp::function_traits::same_arguments<
+      CallbackT,
+      std::function<void(std::shared_ptr<MessageT>)>
+    >::value
+  >
+>
+{
+  using callback_type = std::function<void (std::shared_ptr<MessageT>)>;
+};
+
+template<typename MessageT, typename CallbackT>
+struct SubscriptionCallbackTypeHelper<
+  MessageT,
+  CallbackT,
+  typename std::enable_if_t<
+    rclcpp::function_traits::same_arguments<
+      CallbackT,
+      std::function<void(std::shared_ptr<MessageT>, const rclcpp::MessageInfo &)>
+    >::value
+  >
+>
+{
+  using callback_type =
+    std::function<void (std::shared_ptr<MessageT>, const rclcpp::MessageInfo &)>;
+};
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__SUBSCRIPTION_CALLBACK_TYPE_HELPER_HPP_

rclcpp/include/rclcpp/detail/template_contains.hpp

@@ -0,0 +1,47 @@
+// Copyright 2022 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__DETAIL__TEMPLATE_CONTAINS_HPP_
+#define RCLCPP__DETAIL__TEMPLATE_CONTAINS_HPP_
+
+#include <type_traits>
+
+namespace rclcpp
+{
+namespace detail
+{
+
+/// Template meta-function that checks if a given T is contained in the list Us.
+template<typename T, typename ... Us>
+struct template_contains;
+
+template<typename ... Args>
+inline constexpr bool template_contains_v = template_contains<Args ...>::value;
+
+template<typename T, typename NextT, typename ... Us>
+struct template_contains<T, NextT, Us ...>
+{
+  enum { value = (std::is_same_v<T, NextT>|| template_contains_v<T, Us ...>)};
+};
+
+template<typename T>
+struct template_contains<T>
+{
+  enum { value = false };
+};
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__TEMPLATE_CONTAINS_HPP_

rclcpp/include/rclcpp/detail/template_unique.hpp

@@ -0,0 +1,49 @@
+// Copyright 2022 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__DETAIL__TEMPLATE_UNIQUE_HPP_
+#define RCLCPP__DETAIL__TEMPLATE_UNIQUE_HPP_
+
+#include <type_traits>
+
+#include "rclcpp/detail/template_contains.hpp"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+/// Template meta-function that checks if a given list Ts contains unique types.
+template<typename ... Ts>
+struct template_unique;
+
+template<typename ... Args>
+inline constexpr bool template_unique_v = template_unique<Args ...>::value;
+
+template<typename NextT, typename ... Ts>
+struct template_unique<NextT, Ts ...>
+{
+  enum { value = !template_contains_v<NextT, Ts ...>&& template_unique_v<Ts ...>};
+};
+
+template<typename T>
+struct template_unique<T>
+{
+  enum { value = true };
+};
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__TEMPLATE_UNIQUE_HPP_

rclcpp/include/rclcpp/detail/utilities.hpp

@@ -0,0 +1,40 @@
+// 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__DETAIL__UTILITIES_HPP_
+#define RCLCPP__DETAIL__UTILITIES_HPP_
+
+#include "rclcpp/detail/utilities.hpp"
+
+#include <string>
+#include <vector>
+
+#include "rcl/allocator.h"
+#include "rcl/arguments.h"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+std::vector<std::string>
+get_unparsed_ros_arguments(
+  int argc, char const * const * argv,
+  rcl_arguments_t * arguments,
+  rcl_allocator_t allocator);
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__UTILITIES_HPP_

rclcpp/include/rclcpp/duration.hpp

@@ -18,6 +18,7 @@
 #include <chrono>
 
 #include "builtin_interfaces/msg/duration.hpp"
+#include "builtin_interfaces/msg/time.hpp"
 #include "rcl/time.h"
 #include "rclcpp/visibility_control.hpp"
 
@@ -26,12 +27,22 @@ namespace rclcpp
 class RCLCPP_PUBLIC Duration
 {
 public:
+  /// Duration constructor.
+  /**
+   * Initializes the time values for seconds and nanoseconds individually.
+   * Large values for nsecs are wrapped automatically with the remainder added to secs.
+   * Both inputs must be integers.
+   * Seconds can be negative.
+   *
+   * \param seconds time in seconds
+   * \param nanoseconds time in nanoseconds
+   */
   Duration(int32_t seconds, uint32_t nanoseconds);
 
-  explicit Duration(rcl_duration_value_t nanoseconds);
-
+  /// Construct duration from the specified std::chrono::nanoseconds.
   explicit Duration(std::chrono::nanoseconds nanoseconds);
 
+  // This constructor matches any std::chrono value other than nanoseconds
   // intentionally not using explicit to create a conversion constructor
   template<class Rep, class Period>
   // cppcheck-suppress noExplicitConstructor
@@ -42,6 +53,10 @@ public:
   // cppcheck-suppress noExplicitConstructor
   Duration(const builtin_interfaces::msg::Duration & duration_msg);  // NOLINT(runtime/explicit)
 
+  /// Time constructor
+  /**
+   * \param duration rcl_duration_t structure to copy.
+   */
   explicit Duration(const rcl_duration_t & duration);
 
   Duration(const Duration & rhs);
@@ -55,11 +70,14 @@ public:
   operator=(const Duration & rhs);
 
   Duration &
-  operator=(const builtin_interfaces::msg::Duration & Duration_msg);
+  operator=(const builtin_interfaces::msg::Duration & duration_msg);
 
   bool
   operator==(const rclcpp::Duration & rhs) const;
 
+  bool
+  operator!=(const rclcpp::Duration & rhs) const;
+
   bool
   operator<(const rclcpp::Duration & rhs) const;
 
@@ -75,9 +93,17 @@ public:
   Duration
   operator+(const rclcpp::Duration & rhs) const;
 
+  Duration & operator+=(const rclcpp::Duration & rhs);
+
   Duration
   operator-(const rclcpp::Duration & rhs) const;
 
+  Duration & operator-=(const rclcpp::Duration & rhs);
+
+  /// Get the maximum representable value.
+  /**
+   * \return the maximum representable value
+   */
   static
   Duration
   max();
@@ -85,15 +111,37 @@ public:
   Duration
   operator*(double scale) const;
 
+  Duration &
+  operator*=(double scale);
+
+  /// Get duration in nanosecods
+  /**
+   * \return the duration in nanoseconds as a rcl_duration_value_t.
+   */
   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.
+  /// Get duration in seconds
+  /**
+   * \warning Depending on sizeof(double) there could be significant precision loss.
+   *   When an exact time is required use nanoseconds() instead.
+   * \return the duration in seconds as a floating point number.
+   */
   double
   seconds() const;
 
+  /// Create a duration object from a floating point number representing seconds
+  static Duration
+  from_seconds(double seconds);
+
+  /// Create a duration object from an integer number representing nanoseconds
+  static Duration
+  from_nanoseconds(rcl_duration_value_t nanoseconds);
+
+  static Duration
+  from_rmw_time(rmw_time_t duration);
+
+  /// Convert Duration into a std::chrono::Duration.
   template<class DurationT>
   DurationT
   to_chrono() const
@@ -101,13 +149,24 @@ public:
     return std::chrono::duration_cast<DurationT>(std::chrono::nanoseconds(this->nanoseconds()));
   }
 
+  /// Convert Duration into rmw_time_t.
   rmw_time_t
   to_rmw_time() const;
 
 private:
   rcl_duration_t rcl_duration_;
+
+  Duration() = default;
 };
 
+RCLCPP_PUBLIC
+builtin_interfaces::msg::Time
+operator+(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs);
+
+RCLCPP_PUBLIC
+builtin_interfaces::msg::Time
+operator-(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs);
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__DURATION_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp

@@ -0,0 +1,70 @@
+// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_
+#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_
+
+#include <rcl/allocator.h>
+#include <rcl/types.h>
+#include <rosidl_dynamic_typesupport/types.h>
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace dynamic_typesupport
+{
+
+/// Utility wrapper class for rosidl_dynamic_typesupport_dynamic_data_t
+/// STUBBED OUT
+class DynamicMessage : public std::enable_shared_from_this<DynamicMessage>
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessage)
+
+  RCLCPP_PUBLIC
+  virtual ~DynamicMessage();
+
+protected:
+  // NOTE(methylDragon):
+  // This is just here to extend the lifetime of the serialization support
+  // It isn't actually used by the builder since the builder should compose its own support
+  //
+  // ... Though ideally it should be the exact same support as the one stored in the
+  // DynamicSerializationSupport
+  DynamicSerializationSupport::SharedPtr serialization_support_;
+
+  rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data_;
+  bool is_loaned_;
+
+  // Used for returning the loaned value, and lifetime management
+  DynamicMessage::SharedPtr parent_data_;
+
+private:
+  RCLCPP_DISABLE_COPY(DynamicMessage)
+
+  RCLCPP_PUBLIC
+  DynamicMessage();
+};
+
+}  // namespace dynamic_typesupport
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp

@@ -0,0 +1,64 @@
+// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_
+#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_
+
+#include <rcl/allocator.h>
+#include <rosidl_dynamic_typesupport/types.h>
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace dynamic_typesupport
+{
+
+/// Utility wrapper class for `rosidl_dynamic_typesupport_dynamic_type_t`
+/// STUBBED OUT
+class DynamicMessageType : public std::enable_shared_from_this<DynamicMessageType>
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageType)
+
+  RCLCPP_PUBLIC
+  virtual ~DynamicMessageType();
+
+protected:
+  // NOTE(methylDragon):
+  // This is just here to extend the lifetime of the serialization support
+  // It isn't actually used by the builder since the builder should compose its own support
+  //
+  // ... Though ideally it should be the exact same support as the one stored in the
+  // `DynamicSerializationSupport`
+  DynamicSerializationSupport::SharedPtr serialization_support_;
+
+  rosidl_dynamic_typesupport_dynamic_type_t rosidl_dynamic_type_;
+
+private:
+  RCLCPP_DISABLE_COPY(DynamicMessageType)
+
+  RCLCPP_PUBLIC
+  DynamicMessageType();
+};
+
+}  // namespace dynamic_typesupport
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp

@@ -0,0 +1,65 @@
+// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_
+#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_
+
+#include <rcl/allocator.h>
+#include <rosidl_dynamic_typesupport/types.h>
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace dynamic_typesupport
+{
+
+/// Utility wrapper class for `rosidl_dynamic_typesupport_dynamic_type_builder_t *`
+/// STUBBED OUT
+class DynamicMessageTypeBuilder : public std::enable_shared_from_this<DynamicMessageTypeBuilder>
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageTypeBuilder)
+
+  RCLCPP_PUBLIC
+  virtual ~DynamicMessageTypeBuilder();
+
+protected:
+  // NOTE(methylDragon):
+  // This is just here to extend the lifetime of the serialization support
+  // It isn't actually used by the builder since the builder should compose its own support
+  //
+  // ... Though ideally it should be the exact same support as the one stored in the
+  // `DynamicSerializationSupport`
+  DynamicSerializationSupport::SharedPtr serialization_support_;
+
+  rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder_;
+
+private:
+  RCLCPP_DISABLE_COPY(DynamicMessageTypeBuilder)
+
+  RCLCPP_PUBLIC
+  DynamicMessageTypeBuilder();
+};
+
+}  // namespace dynamic_typesupport
+}  // namespace rclcpp
+
+
+#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp

@@ -0,0 +1,67 @@
+// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_
+#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_
+
+#include <rcl/allocator.h>
+
+#include <rosidl_dynamic_typesupport/dynamic_message_type_support_struct.h>
+#include <rosidl_dynamic_typesupport/types.h>
+#include <rosidl_runtime_c/message_type_support_struct.h>
+#include <rosidl_runtime_c/type_description/type_description__struct.h>
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace dynamic_typesupport
+{
+
+/// Utility wrapper class for `rosidl_message_type_support_t` containing managed
+/// STUBBED OUT
+class DynamicMessageTypeSupport : public std::enable_shared_from_this<DynamicMessageTypeSupport>
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageTypeSupport)
+
+  RCLCPP_PUBLIC
+  virtual ~DynamicMessageTypeSupport();
+
+protected:
+  DynamicSerializationSupport::SharedPtr serialization_support_;
+  DynamicMessageType::SharedPtr dynamic_message_type_;
+  DynamicMessage::SharedPtr dynamic_message_;
+
+  rosidl_message_type_support_t rosidl_message_type_support_;
+
+private:
+  RCLCPP_DISABLE_COPY(DynamicMessageTypeSupport)
+
+  RCLCPP_PUBLIC
+  DynamicMessageTypeSupport();
+};
+
+}  // namespace dynamic_typesupport
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp

@@ -0,0 +1,60 @@
+// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_
+#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_
+
+#include <rcl/allocator.h>
+#include <rosidl_dynamic_typesupport/api/serialization_support.h>
+#include <rosidl_dynamic_typesupport/types.h>
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace dynamic_typesupport
+{
+
+/// Utility wrapper class for rosidl_dynamic_typesupport_serialization_support_t
+class DynamicSerializationSupport : public std::enable_shared_from_this<DynamicSerializationSupport>
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicSerializationSupport)
+
+  RCLCPP_PUBLIC
+  explicit DynamicSerializationSupport(rcl_allocator_t allocator = rcl_get_default_allocator());
+
+  RCLCPP_PUBLIC
+  DynamicSerializationSupport(
+    const std::string & serialization_library_name,
+    rcl_allocator_t allocator = rcl_get_default_allocator());
+
+  RCLCPP_PUBLIC
+  virtual ~DynamicSerializationSupport();
+
+protected:
+  rosidl_dynamic_typesupport_serialization_support_t rosidl_serialization_support_;
+
+private:
+  RCLCPP_DISABLE_COPY(DynamicSerializationSupport)
+};
+
+}  // namespace dynamic_typesupport
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_

rclcpp/include/rclcpp/event.hpp

@@ -29,17 +29,33 @@ class Event
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Event)
 
+  /// Default construct
+  /**
+   * Set the default value to false
+   */
   RCLCPP_PUBLIC
   Event();
 
+  /// Set the Event state value to true
+  /**
+   * \return The state value before the call.
+   */
   RCLCPP_PUBLIC
   bool
   set();
 
+  /// Get the state value of the Event
+  /**
+   * \return the Event state value
+   */
   RCLCPP_PUBLIC
   bool
   check();
 
+  /// Get the state value of the Event and set to false
+  /**
+   * \return the Event state value
+   */
   RCLCPP_PUBLIC
   bool
   check_and_clear();

rclcpp/include/rclcpp/event_handler.hpp

@@ -0,0 +1,376 @@
+// 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__EVENT_HANDLER_HPP_
+#define RCLCPP__EVENT_HANDLER_HPP_
+
+#include <atomic>
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include "rcl/error_handling.h"
+#include "rcl/event_callback.h"
+#include "rmw/impl/cpp/demangle.hpp"
+#include "rmw/incompatible_qos_events_statuses.h"
+#include "rmw/events_statuses/incompatible_type.h"
+
+#include "rcutils/logging_macros.h"
+
+#include "rclcpp/detail/cpp_callback_trampoline.hpp"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/function_traits.hpp"
+#include "rclcpp/logging.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 QOSMessageLostInfo = rmw_message_lost_status_t;
+using QOSOfferedIncompatibleQoSInfo = rmw_offered_qos_incompatible_event_status_t;
+using QOSRequestedIncompatibleQoSInfo = rmw_requested_qos_incompatible_event_status_t;
+
+using IncompatibleTypeInfo = rmw_incompatible_type_status_t;
+using MatchedInfo = rmw_matched_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 &)>;
+using QOSMessageLostCallbackType = std::function<void (QOSMessageLostInfo &)>;
+using QOSOfferedIncompatibleQoSCallbackType = std::function<void (QOSOfferedIncompatibleQoSInfo &)>;
+using QOSRequestedIncompatibleQoSCallbackType =
+  std::function<void (QOSRequestedIncompatibleQoSInfo &)>;
+
+using IncompatibleTypeCallbackType = std::function<void (IncompatibleTypeInfo &)>;
+using PublisherMatchedCallbackType = std::function<void (MatchedInfo &)>;
+using SubscriptionMatchedCallbackType = std::function<void (MatchedInfo &)>;
+
+/// Contains callbacks for various types of events a Publisher can receive from the middleware.
+struct PublisherEventCallbacks
+{
+  QOSDeadlineOfferedCallbackType deadline_callback;
+  QOSLivelinessLostCallbackType liveliness_callback;
+  QOSOfferedIncompatibleQoSCallbackType incompatible_qos_callback;
+  IncompatibleTypeCallbackType incompatible_type_callback;
+  PublisherMatchedCallbackType matched_callback;
+};
+
+/// Contains callbacks for non-message events that a Subscription can receive from the middleware.
+struct SubscriptionEventCallbacks
+{
+  QOSDeadlineRequestedCallbackType deadline_callback;
+  QOSLivelinessChangedCallbackType liveliness_callback;
+  QOSRequestedIncompatibleQoSCallbackType incompatible_qos_callback;
+  QOSMessageLostCallbackType message_lost_callback;
+  IncompatibleTypeCallbackType incompatible_type_callback;
+  SubscriptionMatchedCallbackType matched_callback;
+};
+
+class UnsupportedEventTypeException : public exceptions::RCLErrorBase, public std::runtime_error
+{
+public:
+  RCLCPP_PUBLIC
+  UnsupportedEventTypeException(
+    rcl_ret_t ret,
+    const rcl_error_state_t * error_state,
+    const std::string & prefix);
+
+  RCLCPP_PUBLIC
+  UnsupportedEventTypeException(
+    const exceptions::RCLErrorBase & base_exc,
+    const std::string & prefix);
+};
+
+class EventHandlerBase : public Waitable
+{
+public:
+  enum class EntityType : std::size_t
+  {
+    Event,
+  };
+
+  RCLCPP_PUBLIC
+  virtual ~EventHandlerBase();
+
+  RCLCPP_PUBLIC
+  virtual
+  void enable() = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  void disable() = 0;
+
+  /// 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
+  void
+  add_to_wait_set(rcl_wait_set_t & wait_set) override;
+
+  /// Check if the Waitable is ready.
+  RCLCPP_PUBLIC
+  bool
+  is_ready(const rcl_wait_set_t & wait_set) override;
+
+  /// Set a callback to be called when each new event instance occurs.
+  /**
+   * The callback receives a size_t which is the number of events that occurred
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if events occurred before any
+   * callback was set.
+   *
+   * The callback also receives an int identifier argument.
+   * This is needed because a Waitable may be composed of several distinct entities,
+   * such as subscriptions, services, etc.
+   * The application should provide a generic callback function that will be then
+   * forwarded by the waitable to all of its entities.
+   * Before forwarding, a different value for the identifier argument will be
+   * bond to the function.
+   * This implies that the provided callback can use the identifier to behave
+   * differently depending on which entity triggered the waitable to become ready.
+   *
+   * Since this callback is called from the middleware, you should aim to make
+   * it fast and not blocking.
+   * If you need to do a lot of work or wait for some other event, you should
+   * spin it off to another thread, otherwise you risk blocking the middleware.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * An exception will be thrown if the callback is not callable.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the qos event
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rmw_event_set_callback
+   * \sa rcl_event_set_callback
+   *
+   * \param[in] callback functor to be called when a new event occurs
+   */
+  void
+  set_on_ready_callback(std::function<void(size_t, int)> callback) override
+  {
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_ready_callback "
+              "is not callable.");
+    }
+
+    // Note: we bind the int identifier argument to this waitable's entity types
+    auto new_callback =
+      [callback, this](size_t number_of_events) {
+        try {
+          callback(number_of_events, static_cast<int>(EntityType::Event));
+        } catch (const std::exception & exception) {
+          RCLCPP_ERROR_STREAM(
+            // TODO(wjwwood): get this class access to the node logger it is associated with
+            rclcpp::get_logger("rclcpp"),
+            "rclcpp::EventHandlerBase@" << this <<
+              " caught " << rmw::impl::cpp::demangle(exception) <<
+              " exception in user-provided callback for the 'on ready' callback: " <<
+              exception.what());
+        } catch (...) {
+          RCLCPP_ERROR_STREAM(
+            rclcpp::get_logger("rclcpp"),
+            "rclcpp::EventHandlerBase@" << this <<
+              " caught unhandled exception in user-provided callback " <<
+              "for the 'on ready' callback");
+        }
+      };
+
+    std::lock_guard<std::recursive_mutex> lock(on_new_event_callback_mutex_);
+
+    // Set it temporarily to the new callback, while we replace the old one.
+    // This two-step setting, prevents a gap where the old std::function has
+    // been replaced but the middleware hasn't been told about the new one yet.
+    set_on_new_event_callback(
+      rclcpp::detail::cpp_callback_trampoline<decltype(new_callback), const void *, size_t>,
+      static_cast<const void *>(&new_callback));
+
+    // Store the std::function to keep it in scope, also overwrites the existing one.
+    on_new_event_callback_ = new_callback;
+
+    // Set it again, now using the permanent storage.
+    set_on_new_event_callback(
+      rclcpp::detail::cpp_callback_trampoline<
+        decltype(on_new_event_callback_), const void *, size_t>,
+      static_cast<const void *>(&on_new_event_callback_));
+  }
+
+  /// Unset the callback registered for new events, if any.
+  void
+  clear_on_ready_callback() override
+  {
+    std::lock_guard<std::recursive_mutex> lock(on_new_event_callback_mutex_);
+    if (on_new_event_callback_) {
+      set_on_new_event_callback(nullptr, nullptr);
+      on_new_event_callback_ = nullptr;
+    }
+  }
+
+  RCLCPP_PUBLIC
+  std::vector<std::shared_ptr<rclcpp::TimerBase>>
+  get_timers() const override
+  {
+    return {};
+  }
+
+protected:
+  RCLCPP_PUBLIC
+  void
+  set_on_new_event_callback(rcl_event_callback_t callback, const void * user_data);
+
+  std::recursive_mutex on_new_event_callback_mutex_;
+  std::function<void(size_t)> on_new_event_callback_{nullptr};
+
+  rcl_event_t event_handle_;
+  size_t wait_set_event_index_;
+};
+
+template<typename EventCallbackT, typename ParentHandleT>
+class EventHandler : public EventHandlerBase
+{
+public:
+  template<typename InitFuncT, typename EventTypeEnum>
+  EventHandler(
+    const EventCallbackT & callback,
+    InitFuncT init_func,
+    ParentHandleT parent_handle,
+    EventTypeEnum event_type)
+  : parent_handle_(parent_handle), event_callback_(callback)
+  {
+    event_handle_ = rcl_get_zero_initialized_event();
+    rcl_ret_t ret = init_func(&event_handle_, parent_handle.get(), event_type);
+    if (ret != RCL_RET_OK) {
+      if (ret == RCL_RET_UNSUPPORTED) {
+        UnsupportedEventTypeException exc(ret, rcl_get_error_state(), "Failed to initialize event");
+        rcl_reset_error();
+        throw exc;
+      } else {
+        rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to initialize event");
+      }
+    }
+  }
+
+  ~EventHandler()
+  {
+    // Since the rmw event listener holds a reference to the
+    // "on ready" callback, we need to clear it on destruction of this class.
+    // This clearing is not needed for other rclcpp entities like pub/subs, since
+    // they do own the underlying rmw entities, which are destroyed
+    // on their rclcpp destructors, thus no risk of dangling pointers.
+    clear_on_ready_callback();
+  }
+
+  /// Take data so that the callback cannot be scheduled again
+  std::shared_ptr<void>
+  take_data() 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);
+      rcl_reset_error();
+      return nullptr;
+    }
+    return std::static_pointer_cast<void>(std::make_shared<EventCallbackInfoT>(callback_info));
+  }
+
+  std::shared_ptr<void>
+  take_data_by_entity_id([[maybe_unused]] size_t id) override
+  {
+    return take_data();
+  }
+
+  /// Execute any entities of the Waitable that are ready.
+  void
+  execute(const std::shared_ptr<void> & data) override
+  {
+    std::unique_lock<std::mutex> event_callback_lock(event_callback_mutex_);
+    if (disabled_.load()) {
+      return;
+    }
+    if (!data) {
+      throw std::runtime_error("'data' is empty");
+    }
+    auto callback_ptr = std::static_pointer_cast<EventCallbackInfoT>(data);
+    event_callback_(*callback_ptr);
+    callback_ptr.reset();
+  }
+
+  /// Disable the event callback from being called when execute(..) invoked
+  /**
+   * This will also temporarily remove the on_new_event_callback from the underlying rmw layer,
+   *  so that it is not called from the middleware while disabled.
+   */
+  void disable() override
+  {
+    {
+      // Temporary remove the on_new_event_callback_ to prevent it from being called
+      std::lock_guard<std::recursive_mutex> on_new_event_lock(on_new_event_callback_mutex_);
+      if (on_new_event_callback_) {
+        set_on_new_event_callback(nullptr, nullptr);
+      }
+    }
+    std::lock_guard<std::mutex> event_callback_lock(event_callback_mutex_);
+    disabled_.store(true);
+  }
+
+  /// Enable the event callback to be called when execute(..) invoked
+  /**
+   * This will also set back the on_new_event_callback to the underlying rmw layer, if it was
+   * previously removed with disable().
+   */
+  void enable() override
+  {
+    {
+      // Set callback again if it was previously removed in disable()
+      std::lock_guard<std::recursive_mutex> on_new_event_lock(on_new_event_callback_mutex_);
+      if (on_new_event_callback_) {
+        set_on_new_event_callback(
+          rclcpp::detail::cpp_callback_trampoline<
+            decltype(on_new_event_callback_), const void *, size_t>,
+          static_cast<const void *>(&on_new_event_callback_));
+      }
+    }
+    std::lock_guard<std::mutex> event_callback_lock(event_callback_mutex_);
+    disabled_.store(false);
+  }
+
+private:
+  using EventCallbackInfoT = typename std::remove_reference<typename
+      rclcpp::function_traits::function_traits<EventCallbackT>::template argument_type<0>>::type;
+
+  ParentHandleT parent_handle_;
+  EventCallbackT event_callback_;
+  std::mutex event_callback_mutex_;
+  std::atomic_bool disabled_{false};
+};
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EVENT_HANDLER_HPP_

rclcpp/include/rclcpp/exceptions.hpp

@@ -15,211 +15,6 @@
 #ifndef RCLCPP__EXCEPTIONS_HPP_
 #define RCLCPP__EXCEPTIONS_HPP_
 
-#include <stdexcept>
-#include <string>
-
-#include "rcl/error_handling.h"
-#include "rcl/types.h"
-#include "rclcpp/visibility_control.hpp"
-
-namespace rclcpp
-{
-namespace exceptions
-{
-
-/// Thrown when a method is trying to use a node, but it is invalid.
-class InvalidNodeError : public std::runtime_error
-{
-public:
-  InvalidNodeError()
-  : std::runtime_error("node is invalid") {}
-};
-
-/// Thrown when a any kind of name (node, namespace, topic, etc.) is invalid.
-class NameValidationError : public std::invalid_argument
-{
-public:
-  NameValidationError(
-    const char * name_type_,
-    const char * name_,
-    const char * error_msg_,
-    size_t invalid_index_)
-  : std::invalid_argument(format_error(name_type_, name_, error_msg_, invalid_index_)),
-    name_type(name_type_), name(name_), error_msg(error_msg_), invalid_index(invalid_index_)
-  {}
-
-  static std::string
-  format_error(
-    const char * name_type,
-    const char * name,
-    const char * error_msg,
-    size_t invalid_index);
-
-  const std::string name_type;
-  const std::string name;
-  const std::string error_msg;
-  const size_t invalid_index;
-};
-
-/// Thrown when a node name is invalid.
-class InvalidNodeNameError : public NameValidationError
-{
-public:
-  InvalidNodeNameError(const char * node_name, const char * error_msg, size_t invalid_index)
-  : NameValidationError("node name", node_name, error_msg, invalid_index)
-  {}
-};
-
-/// Thrown when a node namespace is invalid.
-class InvalidNamespaceError : public NameValidationError
-{
-public:
-  InvalidNamespaceError(const char * namespace_, const char * error_msg, size_t invalid_index)
-  : NameValidationError("namespace", namespace_, error_msg, invalid_index)
-  {}
-};
-
-/// Thrown when a topic name is invalid.
-class InvalidTopicNameError : public NameValidationError
-{
-public:
-  InvalidTopicNameError(const char * namespace_, const char * error_msg, size_t invalid_index)
-  : NameValidationError("topic name", namespace_, error_msg, invalid_index)
-  {}
-};
-
-/// Thrown when a service name is invalid.
-class InvalidServiceNameError : public NameValidationError
-{
-public:
-  InvalidServiceNameError(const char * namespace_, const char * error_msg, size_t invalid_index)
-  : NameValidationError("service name", namespace_, error_msg, invalid_index)
-  {}
-};
-
-/// Throw a C++ std::exception which was created based on an rcl error.
-/**
- * Passing nullptr for reset_error is safe and will avoid calling any function
- * to reset the error.
- *
- * \param ret the return code for the current error state
- * \param prefix string to prefix to the error if applicable (not all errors have custom messages)
- * \param error_state error state to create exception from, if nullptr rcl_get_error_state is used
- * \param reset_error function to be called before throwing which whill clear the error state
- * \throws std::invalid_argument if ret is RCL_RET_OK
- * \throws std::runtime_error if the rcl_get_error_state returns 0
- * \throws RCLErrorBase some child class exception based on ret
- */
-/* *INDENT-OFF* */  // Uncrustify cannot yet understand [[noreturn]] properly
-RCLCPP_PUBLIC
-void
-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);
-/* *INDENT-ON* */
-
-class RCLErrorBase
-{
-public:
-  RCLCPP_PUBLIC
-  RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state);
-  virtual ~RCLErrorBase() {}
-
-  rcl_ret_t ret;
-  std::string message;
-  std::string file;
-  size_t line;
-  std::string formatted_message;
-};
-
-/// Created when the return code does not match one of the other specialized exceptions.
-class RCLError : public RCLErrorBase, public std::runtime_error
-{
-public:
-  RCLCPP_PUBLIC
-  RCLError(rcl_ret_t ret, const rcl_error_state_t * error_state, const std::string & prefix);
-  RCLCPP_PUBLIC
-  RCLError(const RCLErrorBase & base_exc, const std::string & prefix);
-};
-
-/// Created when the ret is RCL_RET_BAD_ALLOC.
-class RCLBadAlloc : public RCLErrorBase, public std::bad_alloc
-{
-public:
-  RCLCPP_PUBLIC
-  RCLBadAlloc(rcl_ret_t ret, const rcl_error_state_t * error_state);
-  RCLCPP_PUBLIC
-  explicit RCLBadAlloc(const RCLErrorBase & base_exc);
-};
-
-/// Created when the ret is RCL_RET_INVALID_ARGUMENT.
-class RCLInvalidArgument : public RCLErrorBase, public std::invalid_argument
-{
-public:
-  RCLCPP_PUBLIC
-  RCLInvalidArgument(
-    rcl_ret_t ret,
-    const rcl_error_state_t * error_state,
-    const std::string & prefix);
-  RCLCPP_PUBLIC
-  RCLInvalidArgument(const RCLErrorBase & base_exc, const std::string & prefix);
-};
-
-/// Thrown when an invalid rclcpp::Event object or SharedPtr is encountered.
-class InvalidEventError : public std::runtime_error
-{
-public:
-  InvalidEventError()
-  : std::runtime_error("event is invalid") {}
-};
-
-/// Thrown when an unregistered rclcpp::Event is encountered where a registered one was expected.
-class EventNotRegisteredError : public std::runtime_error
-{
-public:
-  EventNotRegisteredError()
-  : std::runtime_error("event already registered") {}
-};
-
-/// Thrown if passed parameters are inconsistent or invalid
-class InvalidParametersException : public std::runtime_error
-{
-public:
-  // Inherit constructors from runtime_error.
-  using std::runtime_error::runtime_error;
-};
-
-/// Thrown if passed parameter value is invalid.
-class InvalidParameterValueException : public std::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;
-};
-
-}  // namespace exceptions
-}  // namespace rclcpp
+#include "rclcpp/exceptions/exceptions.hpp"
 
 #endif  // RCLCPP__EXCEPTIONS_HPP_

rclcpp/include/rclcpp/exceptions/exceptions.hpp

@@ -0,0 +1,359 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__EXCEPTIONS__EXCEPTIONS_HPP_
+#define RCLCPP__EXCEPTIONS__EXCEPTIONS_HPP_
+
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include "rcl/error_handling.h"
+#include "rcl/types.h"
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcpputils/join.hpp"
+
+namespace rclcpp
+{
+namespace exceptions
+{
+
+/// Thrown when a method is trying to use a node, but it is invalid.
+class InvalidNodeError : public std::runtime_error
+{
+public:
+  InvalidNodeError()
+  : std::runtime_error("node is invalid") {}
+};
+
+/// Thrown when a any kind of name (node, namespace, topic, etc.) is invalid.
+class NameValidationError : public std::invalid_argument
+{
+public:
+  NameValidationError(
+    const char * name_type_,
+    const char * name_,
+    const char * error_msg_,
+    size_t invalid_index_)
+  : std::invalid_argument(format_error(name_type_, name_, error_msg_, invalid_index_)),
+    name_type(name_type_), name(name_), error_msg(error_msg_), invalid_index(invalid_index_)
+  {}
+
+  static std::string
+  format_error(
+    const char * name_type,
+    const char * name,
+    const char * error_msg,
+    size_t invalid_index);
+
+  const std::string name_type;
+  const std::string name;
+  const std::string error_msg;
+  const size_t invalid_index;
+};
+
+/// Thrown when a node name is invalid.
+class InvalidNodeNameError : public NameValidationError
+{
+public:
+  InvalidNodeNameError(const char * node_name, const char * error_msg, size_t invalid_index)
+  : NameValidationError("node name", node_name, error_msg, invalid_index)
+  {}
+};
+
+/// Thrown when a node namespace is invalid.
+class InvalidNamespaceError : public NameValidationError
+{
+public:
+  InvalidNamespaceError(const char * namespace_, const char * error_msg, size_t invalid_index)
+  : NameValidationError("namespace", namespace_, error_msg, invalid_index)
+  {}
+};
+
+/// Thrown when a topic name is invalid.
+class InvalidTopicNameError : public NameValidationError
+{
+public:
+  InvalidTopicNameError(const char * namespace_, const char * error_msg, size_t invalid_index)
+  : NameValidationError("topic name", namespace_, error_msg, invalid_index)
+  {}
+};
+
+/// Thrown when a service name is invalid.
+class InvalidServiceNameError : public NameValidationError
+{
+public:
+  InvalidServiceNameError(const char * namespace_, const char * error_msg, size_t invalid_index)
+  : NameValidationError("service name", namespace_, error_msg, invalid_index)
+  {}
+};
+
+class InvalidServiceTypeError : public std::runtime_error
+{
+public:
+  InvalidServiceTypeError()
+  : std::runtime_error("Service type is invalid.") {}
+  explicit InvalidServiceTypeError(const std::string & msg)
+  : std::runtime_error(msg) {}
+};
+
+class UnimplementedError : public std::runtime_error
+{
+public:
+  UnimplementedError()
+  : std::runtime_error("This code is unimplemented.") {}
+  explicit UnimplementedError(const std::string & msg)
+  : std::runtime_error(msg) {}
+};
+
+typedef void (* reset_error_function_t)();
+
+/// Throw a C++ std::exception which was created based on an rcl error.
+/**
+ * Passing nullptr for reset_error is safe and will avoid calling any function
+ * to reset the error.
+ *
+ * \param ret the return code for the current error state
+ * \param prefix string to prefix to the error if applicable (not all errors have custom messages)
+ * \param error_state error state to create exception from, if nullptr rcl_get_error_state is used
+ * \param reset_error function to be called before throwing which whill clear the error state
+ * \throws std::invalid_argument if ret is RCL_RET_OK
+ * \throws std::runtime_error if the rcl_get_error_state returns 0
+ * \throws RCLErrorBase some child class exception based on ret
+ */
+/* *INDENT-OFF* */  // Uncrustify cannot yet understand [[noreturn]] properly
+RCLCPP_PUBLIC
+void
+throw_from_rcl_error [[noreturn]] (
+  rcl_ret_t ret,
+  const std::string & prefix = "",
+  const rcl_error_state_t * error_state = nullptr,
+  reset_error_function_t reset_error = rcl_reset_error);
+/* *INDENT-ON* */
+
+class RCLErrorBase
+{
+public:
+  RCLCPP_PUBLIC
+  RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state);
+  virtual ~RCLErrorBase() {}
+
+  rcl_ret_t ret;
+  std::string message;
+  std::string file;
+  size_t line;
+  std::string formatted_message;
+};
+
+/// Created when the return code does not match one of the other specialized exceptions.
+class RCLError : public RCLErrorBase, public std::runtime_error
+{
+public:
+  RCLCPP_PUBLIC
+  RCLError(rcl_ret_t ret, const rcl_error_state_t * error_state, const std::string & prefix);
+  RCLCPP_PUBLIC
+  RCLError(const RCLErrorBase & base_exc, const std::string & prefix);
+};
+
+/// Created when the ret is RCL_RET_BAD_ALLOC.
+class RCLBadAlloc : public RCLErrorBase, public std::bad_alloc
+{
+public:
+  RCLCPP_PUBLIC
+  RCLBadAlloc(rcl_ret_t ret, const rcl_error_state_t * error_state);
+  RCLCPP_PUBLIC
+  explicit RCLBadAlloc(const RCLErrorBase & base_exc);
+};
+
+/// Created when the ret is RCL_RET_INVALID_ARGUMENT.
+class RCLInvalidArgument : public RCLErrorBase, public std::invalid_argument
+{
+public:
+  RCLCPP_PUBLIC
+  RCLInvalidArgument(
+    rcl_ret_t ret,
+    const rcl_error_state_t * error_state,
+    const std::string & prefix);
+  RCLCPP_PUBLIC
+  RCLInvalidArgument(const RCLErrorBase & base_exc, const std::string & prefix);
+};
+
+/// Created when the ret is RCL_RET_INVALID_ROS_ARGS.
+class RCLInvalidROSArgsError : public RCLErrorBase, public std::runtime_error
+{
+public:
+  RCLCPP_PUBLIC
+  RCLInvalidROSArgsError(
+    rcl_ret_t ret, const rcl_error_state_t * error_state, const std::string & prefix);
+  RCLCPP_PUBLIC
+  RCLInvalidROSArgsError(const RCLErrorBase & base_exc, const std::string & prefix);
+};
+
+/// Thrown when unparsed ROS specific arguments are found.
+class UnknownROSArgsError : public std::runtime_error
+{
+public:
+  explicit UnknownROSArgsError(std::vector<std::string> && unknown_ros_args_in)
+  : std::runtime_error(
+      "found unknown ROS arguments: '" + rcpputils::join(unknown_ros_args_in, "', '") + "'"),
+    unknown_ros_args(unknown_ros_args_in)
+  {
+  }
+
+  const std::vector<std::string> unknown_ros_args;
+};
+
+/// Thrown when an unknown type is passed
+class UnknownTypeError : public std::runtime_error
+{
+public:
+  explicit UnknownTypeError(const std::string & type)
+  : std::runtime_error("Unknown type: " + type) {}
+};
+
+/// Thrown when an invalid rclcpp::Event object or SharedPtr is encountered.
+class InvalidEventError : public std::runtime_error
+{
+public:
+  InvalidEventError()
+  : std::runtime_error("event is invalid") {}
+};
+
+/// Thrown when an unregistered rclcpp::Event is encountered where a registered one was expected.
+class EventNotRegisteredError : public std::runtime_error
+{
+public:
+  EventNotRegisteredError()
+  : std::runtime_error("event already registered") {}
+};
+
+/// Thrown when a callback group is missing from the node, when it wants to utilize the group.
+class MissingGroupNodeException : public std::runtime_error
+{
+public:
+  explicit MissingGroupNodeException(const std::string & obj_type)
+  : std::runtime_error("cannot create: " + obj_type + " , callback group not in node") {}
+};
+
+/// Thrown if passed parameters are inconsistent or invalid
+class InvalidParametersException : public std::runtime_error
+{
+public:
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
+/// Thrown if passed parameter value is invalid.
+class InvalidParameterValueException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
+/// Thrown if requested parameter type is invalid.
+/**
+ * Essentially the same as rclcpp::ParameterTypeException, but with parameter
+ * name in the error message.
+ */
+class InvalidParameterTypeException : public std::runtime_error
+{
+public:
+  /// Construct an instance.
+  /**
+   * \param[in] name the name of the parameter.
+   * \param[in] message custom exception message.
+   */
+  RCLCPP_PUBLIC
+  InvalidParameterTypeException(const std::string & name, const std::string message)
+  : std::runtime_error("parameter '" + name + "' has invalid type: " + message)
+  {}
+};
+
+/// Thrown if user attempts to create an uninitialized statically typed parameter
+/**
+ * (see https://github.com/ros2/rclcpp/issues/1691)
+ */
+class UninitializedStaticallyTypedParameterException : public std::runtime_error
+{
+public:
+  /// Construct an instance.
+  /**
+   * \param[in] name the name of the parameter.
+   */
+  RCLCPP_PUBLIC
+  explicit UninitializedStaticallyTypedParameterException(const std::string & name)
+  : std::runtime_error("Statically typed parameter '" + name + "' must be initialized.")
+  {}
+};
+
+/// 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;
+};
+
+/// Thrown if parameter is modified while in a set callback.
+class ParameterModifiedInCallbackException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
+/// Thrown when an uninitialized parameter is accessed.
+class ParameterUninitializedException : public std::runtime_error
+{
+public:
+  /// Construct an instance.
+  /**
+   * \param[in] name the name of the parameter.
+   */
+  explicit ParameterUninitializedException(const std::string & name)
+  : std::runtime_error("parameter '" + name + "' is not initialized")
+  {}
+};
+
+/// Thrown if the QoS overrides provided aren't valid.
+class InvalidQosOverridesException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
+/// Thrown if a QoS compatibility check fails.
+class QoSCheckCompatibleException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
+}  // namespace exceptions
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXCEPTIONS__EXCEPTIONS_HPP_

rclcpp/include/rclcpp/executor.hpp

@@ -21,66 +21,35 @@
 #include <cstdlib>
 #include <iostream>
 #include <list>
+#include <map>
 #include <memory>
+#include <mutex>
 #include <string>
 #include <vector>
 
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
+#include "rclcpp/executors/executor_notify_waitable.hpp"
+#include "rcpputils/scope_exit.hpp"
 
+#include "rclcpp/context.hpp"
 #include "rclcpp/contexts/default_context.hpp"
-#include "rclcpp/memory_strategies.hpp"
-#include "rclcpp/memory_strategy.hpp"
+#include "rclcpp/guard_condition.hpp"
+#include "rclcpp/executor_options.hpp"
+#include "rclcpp/executors/executor_entities_collection.hpp"
+#include "rclcpp/executors/executor_entities_collector.hpp"
+#include "rclcpp/future_return_code.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/wait_set.hpp"
 
 namespace rclcpp
 {
 
 // Forward declaration is used in convenience method signature.
 class Node;
-
-namespace executor
-{
-
-/// Return codes to be used with spin_until_future_complete.
-/**
- * SUCCESS: The future is complete and can be accessed with "get" without blocking.
- * INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error.
- * TIMEOUT: Spinning timed out.
- */
-enum class FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT};
-
-RCLCPP_PUBLIC
-std::ostream &
-operator<<(std::ostream & os, const FutureReturnCode & future_return_code);
-
-RCLCPP_PUBLIC
-std::string
-to_string(const FutureReturnCode & future_return_code);
-
-///
-/**
- * Options to be passed to the executor constructor.
- */
-struct ExecutorArgs
-{
-  ExecutorArgs()
-  : memory_strategy(memory_strategies::create_default_strategy()),
-    context(rclcpp::contexts::default_context::get_global_default_context()),
-    max_conditions(0)
-  {}
-
-  memory_strategy::MemoryStrategy::SharedPtr memory_strategy;
-  std::shared_ptr<rclcpp::Context> context;
-  size_t max_conditions;
-};
-
-static inline ExecutorArgs create_default_executor_arguments()
-{
-  return ExecutorArgs();
-}
+class ExecutorImplementation;
 
 /// Coordinate the order and timing of available communication tasks.
 /**
@@ -98,9 +67,11 @@ public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Executor)
 
   /// Default constructor.
-  // \param[in] ms The memory strategy to be used with this executor.
+  /**
+   * \param[in] options Options used to configure the executor.
+   */
   RCLCPP_PUBLIC
-  explicit Executor(const ExecutorArgs & args = ExecutorArgs());
+  explicit Executor(const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
 
   /// Default destructor.
   RCLCPP_PUBLIC
@@ -111,35 +82,155 @@ public:
   virtual void
   spin() = 0;
 
+  /// Add a callback group to an executor.
+  /**
+   * An executor can have zero or more callback groups which provide work during `spin` functions.
+   * When an executor attempts to add a callback group, the executor checks to see if it is already
+   * associated with another executor, and if it has been, then an exception is thrown.
+   * Otherwise, the callback group is added to the executor.
+   *
+   * Adding a callback group with this method does not associate its node with this executor
+   * in any way
+   *
+   * \param[in] group_ptr a shared ptr that points to a callback group
+   * \param[in] node_ptr a shared pointer that points to a node base interface
+   * \param[in] notify True to trigger the interrupt guard condition during this function. If
+   * the executor is blocked at the rmw layer while waiting for work and it is notified that a new
+   * callback group was added, it will wake up.
+   * \throw std::runtime_error if the callback group is associated to an executor
+   */
+  RCLCPP_PUBLIC
+  virtual void
+  add_callback_group(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    bool notify = true);
+
+  /// Get callback groups that belong to executor.
+  /**
+   * This function returns a vector of weak pointers that point to callback groups that were
+   * associated with the executor.
+   * The callback groups associated with this executor may have been added with
+   * `add_callback_group`, or added when a node was added to the executor with `add_node`, or
+   * automatically added when it created by a node already associated with this executor and the
+   * automatically_add_to_executor_with_node parameter was true.
+   *
+   * \return a vector of weak pointers that point to callback groups that are associated with
+   * the executor
+   */
+  RCLCPP_PUBLIC
+  virtual std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_all_callback_groups();
+
+  /// Get callback groups that belong to executor.
+  /**
+   * This function returns a vector of weak pointers that point to callback groups that were
+   * associated with the executor.
+   * The callback groups associated with this executor have been added with
+   * `add_callback_group`.
+   *
+   * \return a vector of weak pointers that point to callback groups that are associated with
+   * the executor
+   */
+  RCLCPP_PUBLIC
+  virtual std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_manually_added_callback_groups();
+
+  /// Get callback groups that belong to executor.
+  /**
+   * This function returns a vector of weak pointers that point to callback groups that were
+   * added from a node that is associated with the executor.
+   * The callback groups are added when a node is added to the executor with `add_node`, or
+   * automatically if they are created in the future by that node and have the
+   * automatically_add_to_executor_with_node argument set to true.
+   *
+   * \return a vector of weak pointers that point to callback groups from a node associated with
+   * the executor
+   */
+  RCLCPP_PUBLIC
+  virtual std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_automatically_added_callback_groups_from_nodes();
+
+  /// Remove a callback group from the executor.
+  /**
+   * The callback group is removed from and disassociated with the executor.
+   * If the callback group removed was the last callback group from the node
+   * that is associated with the executor, the interrupt guard condition
+   * is triggered and node's guard condition is removed from the executor.
+   *
+   * This function only removes a callback group that was manually added with
+   * rclcpp::Executor::add_callback_group.
+   * To remove callback groups that were added from a node using
+   * rclcpp::Executor::add_node, use rclcpp::Executor::remove_node instead.
+   *
+   * \param[in] group_ptr Shared pointer to the callback group to be added.
+   * \param[in] notify True to trigger the interrupt guard condition during this function. If
+   * the executor is blocked at the rmw layer while waiting for work and it is notified that a
+   * callback group was removed, it will wake up.
+   * \throw std::runtime_error if node is deleted before callback group
+   * \throw std::runtime_error if the callback group is not associated with the executor
+   */
+  RCLCPP_PUBLIC
+  virtual void
+  remove_callback_group(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    bool notify = true);
+
   /// Add a node to the executor.
   /**
-   * An executor can have zero or more nodes which provide work during `spin` functions.
+   * Nodes have associated callback groups, and this method adds any of those callback groups
+   * to this executor which have their automatically_add_to_executor_with_node parameter true.
+   * The node is also associated with the executor so that future callback groups which are
+   * created on the node with the automatically_add_to_executor_with_node parameter set to true
+   * are also automatically associated with this executor.
+   *
+   * Callback groups with the automatically_add_to_executor_with_node parameter set to false must
+   * be manually added to an executor using the rclcpp::Executor::add_callback_group method.
+   *
+   * If a node is already associated with an executor, this method throws an exception.
+   *
    * \param[in] node_ptr Shared pointer to the node to be added.
    * \param[in] notify True to trigger the interrupt guard condition during this function. If
    * the executor is blocked at the rmw layer while waiting for work and it is notified that a new
    * node was added, it will wake up.
+   * \throw std::runtime_error if a node is already associated to an executor
    */
   RCLCPP_PUBLIC
   virtual void
   add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true);
 
   /// Convenience function which takes Node and forwards NodeBaseInterface.
+  /**
+   * \see rclcpp::Executor::add_node
+   */
   RCLCPP_PUBLIC
   virtual void
   add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true);
 
   /// Remove a node from the executor.
   /**
+   * Any callback groups automatically added when this node was added with
+   * rclcpp::Executor::add_node are automatically removed, and the node is no longer associated
+   * with this executor.
+   *
+   * This also means that future callback groups created by the given node are no longer
+   * automatically added to this executor.
+   *
    * \param[in] node_ptr Shared pointer to the node to remove.
    * \param[in] notify True to trigger the interrupt guard condition and wake up the executor.
    * This is useful if the last node was removed from the executor while the executor was blocked
    * waiting for work in another thread, because otherwise the executor would never be notified.
+   * \throw std::runtime_error if the node is not associated with an executor.
+   * \throw std::runtime_error if the node is not associated with this executor.
    */
   RCLCPP_PUBLIC
   virtual void
   remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true);
 
   /// Convenience function which takes Node and forwards NodeBaseInterface.
+  /**
+   * \see rclcpp::Executor::remove_node
+   */
   RCLCPP_PUBLIC
   virtual void
   remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true);
@@ -181,193 +272,347 @@ public:
    * \param[in] node Shared pointer to the node to add.
    */
   RCLCPP_PUBLIC
-  void
+  virtual void
   spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node);
 
   /// Convenience function which takes Node and forwards NodeBaseInterface.
   RCLCPP_PUBLIC
-  void
+  virtual void
   spin_node_some(std::shared_ptr<rclcpp::Node> node);
 
-  /// Complete all available queued work without blocking.
+  /// Collect work once and execute all available work, optionally within a max duration.
   /**
-   * This function can be overridden. The default implementation is suitable for a
-   * single-threaded model of execution.
-   * Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function
-   * to block (which may have unintended consequences).
+   * This function can be overridden.
+   * The default implementation is suitable for a single-threaded model of execution.
+   * Adding subscriptions, timers, services, etc. with blocking or long running
+   * callbacks may cause the function exceed the max_duration significantly.
+   *
+   * Work that is ready to be done is collected only once, and when collecting that work
+   * entities which may have multiple pieces of work ready will only be executed at most
+   * one time.
+   * The reason for this is that it is not possible to tell if, for example, a ready
+   * subscription has only one message ready or multiple without checking again.
+   * Because, in order to find out if there are multiple messages, one message must
+   * be taken and executed before checking again if that subscription is still ready.
+   * However, this function only checks for ready entities to work on once,
+   * and so it will never execute a single entity more than once per call to this function.
+   * See spin_all() variants for a function that will repeatedly work on a single entity
+   * in a single call.
+   *
+   * If there is no work to be done when this called, it will return immediately
+   * because the collecting of available work is non-blocking.
+   * Before each piece of ready work is executed this function checks if the
+   * max_duration has been exceeded, and if it has it returns without starting
+   * the execution of the next piece of work.
+   *
+   * If a max_duration of 0 is given, then all of the collected work will be
+   * executed before the function returns.
    *
    * \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(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0));
 
+  /// Add a node, complete all immediately available work exhaustively, and remove the node.
+  /**
+   * \param[in] node Shared pointer to the node to add.
+   */
+  RCLCPP_PUBLIC
+  virtual void
+  spin_node_all(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
+    std::chrono::nanoseconds max_duration);
+
+  /// Convenience function which takes Node and forwards NodeBaseInterface.
+  RCLCPP_PUBLIC
+  virtual void
+  spin_node_all(std::shared_ptr<rclcpp::Node> node, std::chrono::nanoseconds max_duration);
+
+  /// Collect and execute work repeatedly within a duration or until no more work is available.
+  /**
+   * This function can be overridden. The default implementation is suitable for a
+   * single-threaded model of execution.
+   * Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function
+   * to block (which may have unintended consequences).
+   * If the time that waitables take to be executed is longer than the period on which new waitables
+   * become ready, this method will execute work repeatedly until `max_duration` has elapsed.
+   *
+   * \param[in] max_duration The maximum amount of time to spend executing work, must be >= 0.
+   *   `0` is potentially block forever until no more work is available.
+   * \throw std::invalid_argument if max_duration is less than 0.
+   * Note that spin_all() may take longer than this time as it only returns once max_duration has
+   * been exceeded.
+   */
+  RCLCPP_PUBLIC
+  virtual void
+  spin_all(std::chrono::nanoseconds max_duration);
+
+
+  /// Collect work once and execute the next available work, optionally within a duration.
+  /**
+   * This function can be overridden.  The default implementation is suitable for
+   * a single-thread model of execution.
+   * Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function
+   * to block (which may have unintended consequences).
+   * \param[in] timeout The maximum amount of time to spend waiting for work.
+   *   `-1` is potentially block forever waiting for work.
+   */
   RCLCPP_PUBLIC
   virtual void
   spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
   /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
   /**
-   * \param[in] future The future to wait on. If SUCCESS, the future is safe to access after this
-   *   function.
+   * \param[in] future The future to wait on. If this function returns SUCCESS, the future can be
+   *   accessed without blocking (though it may still throw an exception).
    * \param[in] timeout Optional timeout parameter, which gets passed to Executor::spin_node_once.
    *   `-1` is block forever, `0` is non-blocking.
    *   If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return
    *   code.
    * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
+   * \note This method will check the future and the timeout only when the executor is woken up.
+   *   If this future is unrelated to an executor's entity, this method will not correctly detect
+   *   when it's completed and therefore may wait forever and never time out.
    */
-  template<typename ResponseT, typename TimeRepT = int64_t, typename TimeT = std::milli>
+  template<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::milli>
   FutureReturnCode
   spin_until_future_complete(
-    std::shared_future<ResponseT> & future,
+    const FutureT & future,
     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.
-
-    // Check the future before entering the while loop.
-    // If the future is already complete, don't try to spin.
-    std::future_status status = future.wait_for(std::chrono::seconds(0));
-    if (status == std::future_status::ready) {
-      return FutureReturnCode::SUCCESS;
-    }
-
-    auto end_time = std::chrono::steady_clock::now();
-    std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
-      timeout);
-    if (timeout_ns > std::chrono::nanoseconds::zero()) {
-      end_time += timeout_ns;
-    }
-    std::chrono::nanoseconds timeout_left = timeout_ns;
-
-    while (rclcpp::ok(this->context_)) {
-      // Do one item of work.
-      spin_once(timeout_left);
-      // Check if the future is set, return SUCCESS if it is.
-      status = future.wait_for(std::chrono::seconds(0));
-      if (status == std::future_status::ready) {
-        return FutureReturnCode::SUCCESS;
+    return spin_until_future_complete_impl(
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout),
+      [&future](std::chrono::nanoseconds wait_time) {
+        return future.wait_for(wait_time);
       }
-      // If the original timeout is < 0, then this is blocking, never TIMEOUT.
-      if (timeout_ns < std::chrono::nanoseconds::zero()) {
-        continue;
-      }
-      // Otherwise check if we still have time to wait, return TIMEOUT if not.
-      auto now = std::chrono::steady_clock::now();
-      if (now >= end_time) {
-        return FutureReturnCode::TIMEOUT;
-      }
-      // Subtract the elapsed time from the original timeout.
-      timeout_left = std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - now);
-    }
-
-    // The future did not complete before ok() returned false, return INTERRUPTED.
-    return FutureReturnCode::INTERRUPTED;
+    );
   }
 
   /// Cancel any running spin* function, causing it to return.
-  /* This function can be called asynchonously from any thread. */
-  RCLCPP_PUBLIC
-  void
-  cancel();
-
-  /// Support dynamic switching of the memory strategy.
   /**
-   * Switching the memory strategy while the executor is spinning in another threading could have
-   * unintended consequences.
-   * \param[in] memory_strategy Shared pointer to the memory strategy to set.
+   * This function can be called asynchonously from any thread.
+   * \throws std::runtime_error if there is an issue triggering the guard condition
    */
   RCLCPP_PUBLIC
-  void
-  set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy);
+  virtual void
+  cancel();
+
+  /// Returns true if the executor is currently spinning.
+  /**
+   * This function can be called asynchronously from any thread.
+   * \return True if the executor is currently spinning.
+   */
+  RCLCPP_PUBLIC
+  bool
+  is_spinning();
 
 protected:
+  /// Constructor that will not initialize any non-trivial members.
+  /**
+  * This constructor is intended to be used by any derived executor
+  * that explicitly does not want to use the default implementation provided
+  * by this class.
+  */
+  explicit Executor(const std::shared_ptr<rclcpp::Context> & context);
+
+  /// Add a node to executor, execute the next available unit of work, and remove the node.
+  /**
+   * Implementation of spin_node_once using std::chrono::nanoseconds
+   * \param[in] node Shared pointer to the node to add.
+   * \param[in] timeout How long to wait for work to become available. Negative values cause
+   *   spin_node_once to block indefinitely (the default behavior). A timeout of 0 causes this
+   *   function to be non-blocking.
+   */
   RCLCPP_PUBLIC
   void
   spin_node_once_nanoseconds(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
     std::chrono::nanoseconds timeout);
 
+  /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
+  /**
+   * \sa spin_until_future_complete()
+   * The only difference with spin_until_future_complete() is that the future's
+   * type is obscured through a std::function which lets you wait on it
+   * reguardless of type.
+   *
+   * \param[in] timeout see spin_until_future_complete() for details
+   * \param[in] wait_for_future function to wait on the future and get the
+   *   status after waiting
+   */
+  RCLCPP_PUBLIC
+  virtual FutureReturnCode
+  spin_until_future_complete_impl(
+    std::chrono::nanoseconds timeout,
+    const std::function<std::future_status(std::chrono::nanoseconds wait_time)> & wait_for_future);
+
+  /// Collect work and execute available work, optionally within a duration.
+  /**
+   * Implementation of spin_some and spin_all.
+   * The exhaustive flag controls if the function will re-collect available work within the duration.
+   *
+   * \param[in] max_duration The maximum amount of time to spend executing work, or 0 for no limit.
+   * \param[in] exhaustive when set to true, continue to collect work and execute (spin_all)
+   *                       when set to false, return when all collected work is executed (spin_some)
+   */
+  RCLCPP_PUBLIC
+  void
+  spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive);
+
   /// Find the next available executable and do the work associated with it.
-  /** \param[in] any_exec Union structure that can hold any executable type (timer, subscription,
+  /**
+   * \param[in] any_exec Union structure that can hold any executable type (timer, subscription,
    * service, client).
+   * \throws std::runtime_error if there is an issue triggering the guard condition
    */
   RCLCPP_PUBLIC
   void
   execute_any_executable(AnyExecutable & any_exec);
 
+  /// Run subscription executable.
+  /**
+   * Do necessary setup and tear-down as well as executing the subscription.
+   * \param[in] subscription Subscription to execute
+   */
   RCLCPP_PUBLIC
   static void
   execute_subscription(
     rclcpp::SubscriptionBase::SharedPtr subscription);
 
+  /// Run timer executable.
+  /**
+   * Do necessary setup and tear-down as well as executing the timer callback.
+   * \param[in] timer Timer to execute
+   */
   RCLCPP_PUBLIC
   static void
-  execute_intra_process_subscription(
-    rclcpp::SubscriptionBase::SharedPtr subscription);
-
-  RCLCPP_PUBLIC
-  static void
-  execute_timer(rclcpp::TimerBase::SharedPtr timer);
+  execute_timer(rclcpp::TimerBase::SharedPtr timer, const std::shared_ptr<void> & data_ptr);
 
+  /// Run service server executable.
+  /**
+   * Do necessary setup and tear-down as well as executing the service server callback.
+   * \param[in] service Service to execute
+   */
   RCLCPP_PUBLIC
   static void
   execute_service(rclcpp::ServiceBase::SharedPtr service);
 
+  /// Run service client executable.
+  /**
+   * Do necessary setup and tear-down as well as executing the service client callback.
+   * \param[in] service Service to execute
+   */
   RCLCPP_PUBLIC
   static void
   execute_client(rclcpp::ClientBase::SharedPtr client);
 
+  /// Gather all of the waitable entities from associated nodes and callback groups.
+  RCLCPP_PUBLIC
+  void
+  collect_entities();
+
+  /// Block until more work becomes avilable or timeout is reached.
+  /**
+   * Builds a set of waitable entities, which are passed to the middleware.
+   * After building wait set, waits on middleware to notify.
+   * \param[in] timeout duration to wait for new work to become available.
+   * \throws std::runtime_error if the wait set can be cleared
+   */
   RCLCPP_PUBLIC
   void
   wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
-  RCLCPP_PUBLIC
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
-  get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group);
-
-  RCLCPP_PUBLIC
-  rclcpp::callback_group::CallbackGroup::SharedPtr
-  get_group_by_timer(rclcpp::TimerBase::SharedPtr timer);
-
-  RCLCPP_PUBLIC
-  void
-  get_next_timer(AnyExecutable & any_exec);
-
+  /// Check for executable in ready state and populate union structure.
+  /**
+   * \param[out] any_executable populated union structure of ready executable
+   * \return true if an executable was ready and any_executable was populated,
+   *   otherwise false
+   */
   RCLCPP_PUBLIC
   bool
   get_next_ready_executable(AnyExecutable & any_executable);
 
+  /// Wait for executable in ready state and populate union structure.
+  /**
+   * If an executable is ready, it will return immediately, otherwise
+   * block based on the timeout for work to become ready.
+   *
+   * \param[out] any_executable populated union structure of ready executable
+   * \param[in] timeout duration of time to wait for work, a negative value
+   *   (the defualt behavior), will make this function block indefinitely
+   * \return true if an executable was ready and any_executable was populated,
+   *   otherwise false
+   */
   RCLCPP_PUBLIC
   bool
   get_next_executable(
     AnyExecutable & any_executable,
     std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
+  /// This function triggers a recollect of all entities that are registered to the executor.
+  /**
+   * Calling this function is thread safe.
+   *
+   * \param[in] notify if true will execute a trigger that will wake up a waiting executor
+   */
+  RCLCPP_PUBLIC
+  virtual void
+  handle_updated_entities(bool notify);
+
   /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins.
   std::atomic_bool spinning;
 
   /// Guard condition for signaling the rmw layer to wake up for special events.
-  rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
+  std::shared_ptr<rclcpp::GuardCondition> interrupt_guard_condition_;
 
-  /// Wait set for managing entities that the rmw layer waits on.
-  rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
+  /// Guard condition for signaling the rmw layer to wake up for system shutdown.
+  std::shared_ptr<rclcpp::GuardCondition> shutdown_guard_condition_;
 
-  /// The memory strategy: an interface for handling user-defined memory allocation strategies.
-  memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;
+  mutable std::mutex mutex_;
 
   /// The context associated with this executor.
   std::shared_ptr<rclcpp::Context> context_;
 
-private:
   RCLCPP_DISABLE_COPY(Executor)
 
-  std::vector<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
+  RCLCPP_PUBLIC
+  virtual void
+  spin_once_impl(std::chrono::nanoseconds timeout);
+
+  /// Waitable containing guard conditions controlling the executor flow.
+  /**
+   * This waitable contains the interrupt and shutdown guard condition, as well
+   * as the guard condition associated with each node and callback group.
+   * By default, if any change is detected in the monitored entities, the notify
+   * waitable will awake the executor and rebuild the collections.
+   */
+  std::shared_ptr<rclcpp::executors::ExecutorNotifyWaitable> notify_waitable_;
+
+  std::atomic_bool entities_need_rebuild_;
+
+  /// Collector used to associate executable entities from nodes and guard conditions
+  rclcpp::executors::ExecutorEntitiesCollector collector_;
+
+  /// WaitSet to be waited on.
+  rclcpp::WaitSet wait_set_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+  std::optional<rclcpp::WaitResult<rclcpp::WaitSet>> wait_result_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
+  /// Hold the current state of the collection being waited on by the waitset
+  rclcpp::executors::ExecutorEntitiesCollection current_collection_ RCPPUTILS_TSA_GUARDED_BY(
+    mutex_);
+
+  /// Hold the current state of the notify waitable being waited on by the waitset
+  std::shared_ptr<rclcpp::executors::ExecutorNotifyWaitable> current_notify_waitable_
+  RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
+  /// shutdown callback handle registered to Context
+  rclcpp::OnShutdownCallbackHandle shutdown_callback_handle_;
+
+  /// Pointer to implementation
+  std::unique_ptr<ExecutorImplementation> impl_;
 };
 
-}  // namespace executor
 }  // namespace rclcpp
 
 #endif  // RCLCPP__EXECUTOR_HPP_

rclcpp/include/rclcpp/executor_options.hpp

@@ -0,0 +1,57 @@
+// Copyright 2014-2020 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__EXECUTOR_OPTIONS_HPP_
+#define RCLCPP__EXECUTOR_OPTIONS_HPP_
+
+#include <memory>
+
+#include "rclcpp/context.hpp"
+#include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/memory_strategies.hpp"
+#include "rclcpp/memory_strategy.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+class ExecutorOptionsImplementation;
+
+/// Options to be passed to the executor constructor.
+struct ExecutorOptions
+{
+  RCLCPP_PUBLIC
+  ExecutorOptions();
+
+  RCLCPP_PUBLIC
+  virtual ~ExecutorOptions();
+
+  RCLCPP_PUBLIC
+  ExecutorOptions(const ExecutorOptions &);
+
+  RCLCPP_PUBLIC
+  ExecutorOptions & operator=(const ExecutorOptions &);
+
+  rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy;
+  rclcpp::Context::SharedPtr context;
+  size_t max_conditions;
+
+private:
+  /// Pointer to implementation
+  std::unique_ptr<ExecutorOptionsImplementation> impl_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXECUTOR_OPTIONS_HPP_

rclcpp/include/rclcpp/executors.hpp

@@ -20,6 +20,7 @@
 
 #include "rclcpp/executors/multi_threaded_executor.hpp"
 #include "rclcpp/executors/single_threaded_executor.hpp"
+#include "rclcpp/experimental/executors/events_executor/events_executor.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -27,12 +28,82 @@
 namespace rclcpp
 {
 
-/// Create a default single-threaded executor and execute any immediately available work.
-/** \param[in] node_ptr Shared pointer to the node to spin. */
+/**
+ * @brief Create a default single-threaded executor and execute all available work exhaustively.
+ * @param node_ptr Shared pointer to the base interface of the node to spin.
+ * @param max_duration max duration to spin
+ *
+ * This method is deprecated because it can lead to very bad performance if used in a loop:
+ * each call will create a new executor and register the node, which is an expensive operation.
+ * It's recommended to always manually instantiate an executor and call the methods with
+ * the same name on it.
+ * For example:
+ *     SingleThreadedExecutor executor;
+ *     executor.add_node(node_ptr);
+ *     executor.spin_all(max_duration);
+ * If you are using a non-default context, this should be passed to the executor's constructor.
+ */
+[[deprecated("use SingleThreadedExecutor::spin_all instead")]]
+RCLCPP_PUBLIC
+void
+spin_all(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  std::chrono::nanoseconds max_duration);
+
+/**
+ * @brief Create a default single-threaded executor and execute all available work exhaustively.
+ * @param node_ptr Shared pointer to the node to spin.
+ * @param max_duration max duration to spin
+ *
+ * This method is deprecated because it can lead to very bad performance if used in a loop:
+ * each call will create a new executor and register the node, which is an expensive operation.
+ * It's recommended to always manually instantiate an executor and call the methods with
+ * the same name on it.
+ * For example:
+ *     SingleThreadedExecutor executor;
+ *     executor.add_node(node_ptr);
+ *     executor.spin_all(max_duration);
+ * If you are using a non-default context, this should be passed to the executor's constructor.
+ */
+[[deprecated("use SingleThreadedExecutor::spin_all instead")]]
+RCLCPP_PUBLIC
+void
+spin_all(rclcpp::Node::SharedPtr node_ptr, std::chrono::nanoseconds max_duration);
+
+/**
+ * @brief Create a default single-threaded executor and execute any immediately available work.
+ * @param node_ptr Shared pointer to the base interface of the node to spin.
+ *
+ * This method is deprecated because it can lead to very bad performance if used in a loop:
+ * each call will create a new executor and register the node, which is an expensive operation.
+ * It's recommended to always manually instantiate an executor and call the methods with
+ * the same name on it.
+ * For example:
+ *     SingleThreadedExecutor executor;
+ *     executor.add_node(node_ptr);
+ *     executor.spin_some();
+ * If you are using a non-default context, this should be passed to the executor's constructor.
+ */
+[[deprecated("use SingleThreadedExecutor::spin_some instead")]]
 RCLCPP_PUBLIC
 void
 spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
 
+/**
+ * @brief Create a default single-threaded executor and execute any immediately available work.
+ * @param node_ptr Shared pointer to the node to spin.
+ *
+ * This method is deprecated because it can lead to very bad performance if used in a loop:
+ * each call will create a new executor and register the node, which is an expensive operation.
+ * It's recommended to always manually instantiate an executor and call the methods with
+ * the same name on it.
+ * For example:
+ *     SingleThreadedExecutor executor;
+ *     executor.add_node(node_ptr);
+ *     executor.spin_some();
+ * If you are using a non-default context, this should be passed to the executor's constructor.
+ */
+[[deprecated("use SingleThreadedExecutor::spin_some instead")]]
 RCLCPP_PUBLIC
 void
 spin_some(rclcpp::Node::SharedPtr node_ptr);
@@ -65,12 +136,12 @@ 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 TimeRepT = int64_t, typename TimeT = std::milli>
-rclcpp::executor::FutureReturnCode
+template<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::milli>
+rclcpp::FutureReturnCode
 spin_node_until_future_complete(
-  rclcpp::executor::Executor & executor,
+  rclcpp::Executor & executor,
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  std::shared_future<ResponseT> & future,
+  const FutureT & future,
   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
@@ -81,13 +152,13 @@ spin_node_until_future_complete(
   return retcode;
 }
 
-template<typename NodeT = rclcpp::Node, typename ResponseT, typename TimeRepT = int64_t,
+template<typename NodeT = rclcpp::Node, typename FutureT, typename TimeRepT = int64_t,
   typename TimeT = std::milli>
-rclcpp::executor::FutureReturnCode
+rclcpp::FutureReturnCode
 spin_node_until_future_complete(
-  rclcpp::executor::Executor & executor,
+  rclcpp::Executor & executor,
   std::shared_ptr<NodeT> node_ptr,
-  std::shared_future<ResponseT> & future,
+  const FutureT & future,
   std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   return rclcpp::executors::spin_node_until_future_complete(
@@ -100,22 +171,24 @@ spin_node_until_future_complete(
 }  // namespace executors
 
 template<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::milli>
-rclcpp::executor::FutureReturnCode
+rclcpp::FutureReturnCode
 spin_until_future_complete(
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  std::shared_future<FutureT> & future,
+  const FutureT & future,
   std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
-  rclcpp::executors::SingleThreadedExecutor executor;
+  rclcpp::ExecutorOptions options;
+  options.context = node_ptr->get_context();
+  rclcpp::executors::SingleThreadedExecutor executor(options);
   return executors::spin_node_until_future_complete<FutureT>(executor, node_ptr, future, timeout);
 }
 
 template<typename NodeT = rclcpp::Node, typename FutureT, typename TimeRepT = int64_t,
   typename TimeT = std::milli>
-rclcpp::executor::FutureReturnCode
+rclcpp::FutureReturnCode
 spin_until_future_complete(
   std::shared_ptr<NodeT> node_ptr,
-  std::shared_future<FutureT> & future,
+  const FutureT & future,
   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/executor_entities_collection.hpp

@@ -0,0 +1,219 @@
+// Copyright 2023 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__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_
+#define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_
+
+#include <deque>
+#include <functional>
+#include <unordered_map>
+#include <vector>
+
+#include <rclcpp/any_executable.hpp>
+#include <rclcpp/node_interfaces/node_base.hpp>
+#include <rclcpp/callback_group.hpp>
+#include <rclcpp/executors/executor_notify_waitable.hpp>
+#include <rclcpp/visibility_control.hpp>
+#include <rclcpp/wait_result.hpp>
+#include <rclcpp/wait_set.hpp>
+
+namespace rclcpp
+{
+namespace executors
+{
+
+/// Structure to represent a single entity's entry in a collection
+template<typename EntityValueType>
+struct CollectionEntry
+{
+  /// Weak pointer to entity type
+  using EntityWeakPtr = typename EntityValueType::WeakPtr;
+  /// Shared pointer to entity type
+  using EntitySharedPtr = typename EntityValueType::SharedPtr;
+
+  /// The entity
+  EntityWeakPtr entity;
+
+  /// If relevant, the entity's corresponding callback_group
+  rclcpp::CallbackGroup::WeakPtr callback_group;
+};
+
+/// Update a collection based on another collection
+/*
+ * Iterates update_from and update_to to see which entities have been added/removed between
+ * the two collections.
+ *
+ * For each new entry (in update_from, but not in update_to),
+ *   add the entity and fire the on_added callback
+ * For each removed entry (in update_to, but not in update_from),
+ *   remove the entity and fire the on_removed callback.
+ *
+ *  \param[in] update_from The collection representing the next iteration's state
+ *  \param[inout] update_to The collection representing the current iteration's state
+ *  \param[in] on_added Callback fired when a new entity is detected
+ *  \param[in] on_removed Callback fired when an entity is removed
+ */
+template<typename CollectionType>
+void update_entities(
+  const CollectionType & update_from,
+  CollectionType & update_to,
+  std::function<void(const typename CollectionType::EntitySharedPtr &)> on_added,
+  std::function<void(const typename CollectionType::EntitySharedPtr &)> on_removed
+)
+{
+  for (auto it = update_to.begin(); it != update_to.end(); ) {
+    if (update_from.count(it->first) == 0) {
+      auto entity = it->second.entity.lock();
+      if (entity) {
+        on_removed(entity);
+      }
+      it = update_to.erase(it);
+    } else {
+      ++it;
+    }
+  }
+  for (auto it = update_from.begin(); it != update_from.end(); ++it) {
+    if (update_to.count(it->first) == 0) {
+      auto entity = it->second.entity.lock();
+      if (entity) {
+        on_added(entity);
+      }
+      update_to.insert(*it);
+    }
+  }
+}
+
+/// A collection of entities, indexed by their corresponding handles
+template<typename EntityKeyType, typename EntityValueType>
+class EntityCollection
+  : public std::unordered_map<const EntityKeyType *, CollectionEntry<EntityValueType>>
+{
+public:
+  /// Key type of the map
+  using Key = const EntityKeyType *;
+
+  /// Weak pointer to entity type
+  using EntityWeakPtr = typename EntityValueType::WeakPtr;
+
+  /// Shared pointer to entity type
+  using EntitySharedPtr = typename EntityValueType::SharedPtr;
+
+  /// Update this collection based on the contents of another collection
+  /**
+   * Update the internal state of this collection, firing callbacks when entities have been
+   * added or removed.
+   *
+   * \param[in] other Collection to compare to
+   * \param[in] on_added Callback for when entities have been added
+   * \param[in] on_removed Callback for when entities have been removed
+   */
+  void update(
+    const EntityCollection<EntityKeyType, EntityValueType> & other,
+    std::function<void(const EntitySharedPtr &)> on_added,
+    std::function<void(const EntitySharedPtr &)> on_removed)
+  {
+    update_entities(other, *this, on_added, on_removed);
+  }
+};
+
+/// Represent the total set of entities for a single executor
+/**
+ * This allows the entities to be stored from ExecutorEntitiesCollector.
+ * The structure also makes in convenient to re-evaluate when entities have been added or removed.
+ */
+struct ExecutorEntitiesCollection
+{
+  /// Collection type for timer entities
+  using TimerCollection = EntityCollection<rcl_timer_t, rclcpp::TimerBase>;
+
+  /// Collection type for subscription entities
+  using SubscriptionCollection = EntityCollection<rcl_subscription_t, rclcpp::SubscriptionBase>;
+
+  /// Collection type for client entities
+  using ClientCollection = EntityCollection<rcl_client_t, rclcpp::ClientBase>;
+
+  /// Collection type for service entities
+  using ServiceCollection = EntityCollection<rcl_service_t, rclcpp::ServiceBase>;
+
+  /// Collection type for waitable entities
+  using WaitableCollection = EntityCollection<rclcpp::Waitable, rclcpp::Waitable>;
+
+  /// Collection type for guard condition entities
+  using GuardConditionCollection = EntityCollection<rcl_guard_condition_t, rclcpp::GuardCondition>;
+
+  /// Collection of timers currently in use by the executor.
+  TimerCollection timers;
+
+  /// Collection of subscriptions currently in use by the executor.
+  SubscriptionCollection subscriptions;
+
+  /// Collection of clients currently in use by the executor.
+  ClientCollection clients;
+
+  /// Collection of services currently in use by the executor.
+  ServiceCollection services;
+
+  /// Collection of guard conditions currently in use by the executor.
+  GuardConditionCollection guard_conditions;
+
+  /// Collection of waitables currently in use by the executor.
+  WaitableCollection waitables;
+
+  /// Check if the entities collection is empty
+  /**
+   * \return true if all member collections are empty, false otherwise
+  */
+  bool empty() const;
+
+  /// Clear the entities collection
+  void clear();
+
+  /// Remove entities that have expired weak ownership
+  /**
+   * \return The total number of removed entities
+   */
+  size_t remove_expired_entities();
+};
+
+/// Build an entities collection from callback groups
+/**
+ * Iterates a list of callback groups and adds entities from each valid group
+ *
+ * \param[in] callback_groups List of callback groups to check for entities
+ * \param[inout] colletion Entities collection to populate with found entities
+ */
+void
+build_entities_collection(
+  const std::vector<rclcpp::CallbackGroup::WeakPtr> & callback_groups,
+  ExecutorEntitiesCollection & collection);
+
+/// Build a queue of executables ready to be executed
+/**
+ * Iterates a list of entities and adds them to a queue if they are ready.
+ *
+ * \param[in] collection Collection of entities corresponding to the current wait set.
+ * \param[in] wait_result Result of rclcpp::WaitSet::wait corresponding to the collection.
+ * \param[inout] queue of executables to append new ready executables to
+ * \return number of new ready executables
+ */
+size_t
+ready_executables(
+  const ExecutorEntitiesCollection & collection,
+  rclcpp::WaitResult<rclcpp::WaitSet> & wait_result,
+  std::deque<rclcpp::AnyExecutable> & executables
+);
+}  // namespace executors
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_

rclcpp/include/rclcpp/executors/executor_entities_collector.hpp

@@ -0,0 +1,270 @@
+// Copyright 2023 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__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_
+#define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_
+
+#include <map>
+#include <memory>
+#include <mutex>
+#include <set>
+#include <vector>
+
+#include "rcpputils/thread_safety_annotations.hpp"
+
+#include <rclcpp/any_executable.hpp>
+#include <rclcpp/node_interfaces/node_base.hpp>
+#include <rclcpp/callback_group.hpp>
+#include <rclcpp/executors/executor_notify_waitable.hpp>
+#include <rclcpp/visibility_control.hpp>
+#include <rclcpp/wait_set.hpp>
+#include <rclcpp/wait_result.hpp>
+
+namespace rclcpp
+{
+namespace executors
+{
+
+/// Class to monitor a set of nodes and callback groups for changes in entity membership
+/**
+ * This is to be used with an executor to track the membership of various nodes, groups,
+ * and entities (timers, subscriptions, clients, services, etc) and report status to the
+ * executor.
+ *
+ * In general, users will add either nodes or callback groups to an executor.
+ * Each node may have callback groups that are automatically associated with executors,
+ * or callback groups that must be manually associated with an executor.
+ *
+ * This object tracks both types of callback groups as well as nodes that have been
+ * previously added to the executor.
+ * When a new callback group is added/removed or new entities are added/removed, the
+ * corresponding node or callback group will signal this to the executor so that the
+ * entity collection may be rebuilt according to that executor's implementation.
+ *
+ */
+class ExecutorEntitiesCollector
+{
+public:
+  /// Constructor
+  /**
+   * \param[in] notify_waitable Waitable that is used to signal to the executor
+   *   when nodes or callback groups have been added or removed.
+   */
+  RCLCPP_PUBLIC
+  explicit ExecutorEntitiesCollector(
+    std::shared_ptr<ExecutorNotifyWaitable> notify_waitable);
+
+  /// Destructor
+  RCLCPP_PUBLIC
+  ~ExecutorEntitiesCollector();
+
+  /// Indicate if the entities collector has pending additions or removals.
+  /**
+   * \return true if there are pending additions or removals
+   */
+  bool has_pending() const;
+
+  /// Add a node to the entity collector
+  /**
+   * \param[in] node_ptr a shared pointer that points to a node base interface
+   * \throw std::runtime_error if the node is associated with an executor
+   */
+  RCLCPP_PUBLIC
+  void
+  add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+  /// Remove a node from the entity collector
+  /**
+   * \param[in] node_ptr a shared pointer that points to a node base interface
+   * \throw std::runtime_error if the node is associated with an executor
+   * \throw std::runtime_error if the node is associated with this executor
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+  /// Add a callback group to the entity collector
+  /**
+   * \param[in] group_ptr a shared pointer that points to a callback group
+   * \throw std::runtime_error if the callback_group is associated with an executor
+   */
+  RCLCPP_PUBLIC
+  void
+  add_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr);
+
+  /// Remove a callback group from the entity collector
+  /**
+   * \param[in] group_ptr a shared pointer that points to a callback group
+   * \throw std::runtime_error if the callback_group is not associated with an executor
+   * \throw std::runtime_error if the callback_group is not associated with this executor
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr);
+
+  /// Get all callback groups known to this entity collector
+  /**
+   * This will include manually added and automatically added (node associated) groups
+   * \return vector of all callback groups
+   */
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_all_callback_groups() const;
+
+  /// Get manually-added callback groups known to this entity collector
+  /**
+   * This will include callback groups that have been added via add_callback_group
+   * \return vector of manually-added callback groups
+   */
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_manually_added_callback_groups() const;
+
+  /// Get automatically-added callback groups known to this entity collector
+  /**
+   * This will include callback groups that are associated with nodes added via add_node
+   * \return vector of automatically-added callback groups
+   */
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_automatically_added_callback_groups() const;
+
+  /// Update the underlying collections
+  /**
+   * This will prune nodes and callback groups that are no longer valid as well
+   * as add new callback groups from any associated nodes.
+   */
+  RCLCPP_PUBLIC
+  void
+  update_collections();
+
+protected:
+  using NodeCollection = std::set<
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+    std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>;
+
+  using CallbackGroupCollection = std::set<
+    rclcpp::CallbackGroup::WeakPtr,
+    std::owner_less<rclcpp::CallbackGroup::WeakPtr>>;
+
+  using WeakNodesToGuardConditionsMap = std::map<
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+    rclcpp::GuardCondition::WeakPtr,
+    std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>;
+
+  using WeakGroupsToGuardConditionsMap = std::map<
+    rclcpp::CallbackGroup::WeakPtr,
+    rclcpp::GuardCondition::WeakPtr,
+    std::owner_less<rclcpp::CallbackGroup::WeakPtr>>;
+
+  /// Implementation of removing a node from the collector.
+  /**
+   * This will disassociate the node from the collector and remove any
+   * automatically-added callback groups
+   *
+   * This takes and returns an iterator so it may be used as:
+   *
+   * it = remove_weak_node(it);
+   *
+   * \param[in] weak_node iterator to the weak node to be removed
+   * \return Valid updated iterator in the same collection
+   */
+  RCLCPP_PUBLIC
+  NodeCollection::iterator
+  remove_weak_node(NodeCollection::iterator weak_node) RCPPUTILS_TSA_REQUIRES(mutex_);
+
+  /// Implementation of removing a callback group from the collector.
+  /**
+   * This will disassociate the callback group from the collector
+   *
+   * This takes and returns an iterator so it may be used as:
+   *
+   * it = remove_weak_callback_group(it);
+   *
+   * \param[in] weak_group_it iterator to the weak group to be removed
+   * \param[in] collection the collection to remove the group from
+   *   (manually or automatically added)
+   * \return Valid updated iterator in the same collection
+   */
+  RCLCPP_PUBLIC
+  CallbackGroupCollection::iterator
+  remove_weak_callback_group(
+    CallbackGroupCollection::iterator weak_group_it,
+    CallbackGroupCollection & collection) RCPPUTILS_TSA_REQUIRES(mutex_);
+
+  /// Implementation of adding a callback group
+  /**
+   * \param[in] group_ptr the group to add
+   * \param[in] collection the collection to add the group to
+   */
+  RCLCPP_PUBLIC
+  void
+  add_callback_group_to_collection(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    CallbackGroupCollection & collection)  RCPPUTILS_TSA_REQUIRES(mutex_);
+
+  /// Iterate over queued added/remove nodes and callback_groups
+  RCLCPP_PUBLIC
+  void
+  process_queues() RCPPUTILS_TSA_REQUIRES(mutex_);
+
+  /// Check a collection of nodes and add any new callback_groups that
+  /// are set to be automatically associated via the node.
+  RCLCPP_PUBLIC
+  void
+  add_automatically_associated_callback_groups(
+    const NodeCollection & nodes_to_check) RCPPUTILS_TSA_REQUIRES(mutex_);
+
+  /// Check all nodes and group for expired weak pointers and remove them.
+  RCLCPP_PUBLIC
+  void
+  prune_invalid_nodes_and_groups() RCPPUTILS_TSA_REQUIRES(mutex_);
+
+  /// mutex to protect collections and pending queues
+  mutable std::mutex mutex_;
+
+  /// Callback groups that were added via `add_callback_group`
+  CallbackGroupCollection manually_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
+  /// Callback groups that were added by their association with added nodes
+  CallbackGroupCollection automatically_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
+  /// nodes that are associated with the executor
+  NodeCollection weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
+  /// Track guard conditions associated with added nodes
+  WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
+  /// Track guard conditions associated with added callback groups
+  WeakGroupsToGuardConditionsMap weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
+  /// nodes that have been added since the last update.
+  NodeCollection pending_added_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
+  /// nodes that have been removed since the last update.
+  NodeCollection pending_removed_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
+  /// callback groups that have been added since the last update.
+  CallbackGroupCollection pending_manually_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
+  /// callback groups that have been removed since the last update.
+  CallbackGroupCollection pending_manually_removed_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
+  /// Waitable to add guard conditions to
+  std::shared_ptr<ExecutorNotifyWaitable> notify_waitable_;
+};
+}  // namespace executors
+}  // namespace rclcpp
+//
+#endif  // RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_

rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp

@@ -0,0 +1,188 @@
+// Copyright 2023 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__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_
+#define RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_
+
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <set>
+#include <vector>
+
+#include "rclcpp/guard_condition.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace executors
+{
+
+/// Maintain a collection of guard conditions from associated nodes and callback groups
+/// to signal to the executor when associated entities have changed.
+class ExecutorNotifyWaitable : public rclcpp::Waitable
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(ExecutorNotifyWaitable)
+
+  // Constructor
+  /**
+   * \param[in] on_execute_callback Callback to execute when one of the conditions
+   *   of this waitable has signaled the wait_set.
+   */
+  RCLCPP_PUBLIC
+  explicit ExecutorNotifyWaitable(
+    std::function<void(void)> on_execute_callback = {}, const rclcpp::Context::SharedPtr & context =
+    rclcpp::contexts::get_global_default_context());
+
+  // Destructor
+  RCLCPP_PUBLIC
+  ~ExecutorNotifyWaitable() override = default;
+
+  RCLCPP_PUBLIC
+  ExecutorNotifyWaitable(ExecutorNotifyWaitable & other);
+
+  RCLCPP_PUBLIC
+  ExecutorNotifyWaitable & operator=(ExecutorNotifyWaitable & other);
+
+  /// Add conditions to the wait set
+  /**
+   * \param[inout] wait_set structure that conditions will be added to
+   */
+  RCLCPP_PUBLIC
+  void
+  add_to_wait_set(rcl_wait_set_t & wait_set) override;
+
+  /// Check conditions against the wait set
+  /**
+   * \param[inout] wait_set structure that internal elements will be checked against.
+   * \return true if this waitable is ready to be executed, false otherwise.
+   */
+  RCLCPP_PUBLIC
+  bool
+  is_ready(const rcl_wait_set_t & wait_set) override;
+
+  /// Perform work associated with the waitable.
+  /**
+   * This will call the callback provided in the constructor.
+   * \param[in] data Data to be use for the execute, if available, else nullptr.
+   */
+  RCLCPP_PUBLIC
+  void
+  execute(const std::shared_ptr<void> & data) override;
+
+  /// Retrieve data to be used in the next execute call.
+  /**
+   * \return If available, data to be used, otherwise nullptr
+   */
+  RCLCPP_PUBLIC
+  std::shared_ptr<void>
+  take_data() override;
+
+  /// Take the data from an entity ID so that it can be consumed with `execute`.
+  /**
+   * \param[in] id ID of the entity to take data from.
+   * \return If available, data to be used, otherwise nullptr
+   * \sa rclcpp::Waitable::take_data_by_entity_id
+   */
+  RCLCPP_PUBLIC
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id) override;
+
+  /// Set a callback to be called whenever the waitable becomes ready.
+  /**
+   * \param[in] callback callback to set
+   * \sa rclcpp::Waitable::set_on_ready_callback
+   */
+  RCLCPP_PUBLIC
+  void
+  set_on_ready_callback(std::function<void(size_t, int)> callback) override;
+
+  /// Add a guard condition to be waited on.
+  /**
+   * \param[in] guard_condition The guard condition to add.
+   */
+  RCLCPP_PUBLIC
+  void
+  add_guard_condition(rclcpp::GuardCondition::WeakPtr guard_condition);
+
+  /// Unset any callback registered via set_on_ready_callback.
+  /**
+   * \sa rclcpp::Waitable::clear_on_ready_callback
+   */
+  RCLCPP_PUBLIC
+  void
+  clear_on_ready_callback() override;
+
+  /// Set a new callback to be called whenever this waitable is executed.
+  /**
+   * \param[in] on_execute_callback The new callback
+   */
+  RCLCPP_PUBLIC
+  void
+  set_execute_callback(std::function<void(void)> on_execute_callback);
+
+  /// Remove a guard condition from being waited on.
+  /**
+   * \param[in] weak_guard_condition The guard condition to remove.
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition);
+
+  /// Get the number of ready guard_conditions
+  /**
+   * \return The number of guard_conditions associated with the Waitable.
+   */
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_ready_guard_conditions() override;
+
+  /// Returns the number of used Timers
+  /**
+   * Will always return an empty vector.
+   */
+  RCLCPP_PUBLIC
+  std::vector<std::shared_ptr<rclcpp::TimerBase>>
+  get_timers() const override;
+
+private:
+  /// Callback to run when waitable executes
+  std::function<void(void)> execute_callback_;
+
+  /// Mutex to procetect the guard conditions
+  std::mutex guard_condition_mutex_;
+  /// Mutex to protect the execute callback
+  std::mutex execute_mutex_;
+
+  std::function<void(size_t)> on_ready_callback_;
+
+  /// The collection of guard conditions to be waited on.
+  std::set<rclcpp::GuardCondition::SharedPtr> notify_guard_conditions_;
+
+  /// The indixes were our guard conditions were stored in the
+  /// rcl waitset
+  std::vector<size_t> idxs_of_added_guard_condition_;
+
+  /// set to true, if we got a pending trigger
+  bool needs_processing = false;
+
+  /// A guard condition needed to generate wakeups
+  rclcpp::GuardCondition::SharedPtr guard_condition_;
+};
+
+}  // namespace executors
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
 #define RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
 
+#include <chrono>
 #include <memory>
 #include <mutex>
 #include <set>
@@ -31,7 +32,7 @@ namespace rclcpp
 namespace executors
 {
 
-class MultiThreadedExecutor : public executor::Executor
+class MultiThreadedExecutor : public rclcpp::Executor
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(MultiThreadedExecutor)
@@ -44,23 +45,29 @@ public:
    * This is useful for reproducing some bugs related to taking work more than
    * once.
    *
-   * \param args common arguments for all executors
+   * \param options common options 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
+   *   the default 0 will use the number of cpu cores found (minimum of 2)
    * \param yield_before_execute if true std::this_thread::yield() is called
+   * \param timeout maximum time to wait
    */
   RCLCPP_PUBLIC
-  MultiThreadedExecutor(
-    const executor::ExecutorArgs & args = executor::ExecutorArgs(),
+  explicit MultiThreadedExecutor(
+    const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions(),
     size_t number_of_threads = 0,
-    bool yield_before_execute = false);
+    bool yield_before_execute = false,
+    std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
   RCLCPP_PUBLIC
   virtual ~MultiThreadedExecutor();
 
+  /**
+   * \sa rclcpp::Executor:spin() for more details
+   * \throws std::runtime_error when spin() called while already spinning
+   */
   RCLCPP_PUBLIC
   void
-  spin();
+  spin() override;
 
   RCLCPP_PUBLIC
   size_t
@@ -77,8 +84,7 @@ private:
   std::mutex wait_mutex_;
   size_t number_of_threads_;
   bool yield_before_execute_;
-
-  std::set<TimerBase::SharedPtr> scheduled_timers_;
+  std::chrono::nanoseconds next_exec_timeout_;
 };
 
 }  // namespace executors

rclcpp/include/rclcpp/executors/single_threaded_executor.hpp

@@ -35,28 +35,35 @@ namespace rclcpp
 namespace executors
 {
 
-/// Single-threaded executor implementation
-// This is the default executor created by rclcpp::spin.
-class SingleThreadedExecutor : public executor::Executor
+/// Single-threaded executor implementation.
+/**
+ * This is the default executor created by rclcpp::spin.
+ */
+class SingleThreadedExecutor : public rclcpp::Executor
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(SingleThreadedExecutor)
 
   /// Default constructor. See the default constructor for Executor.
   RCLCPP_PUBLIC
-  SingleThreadedExecutor(
-    const executor::ExecutorArgs & args = executor::ExecutorArgs());
+  explicit SingleThreadedExecutor(
+    const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
 
-  /// Default destrcutor.
+  /// Default destructor.
   RCLCPP_PUBLIC
   virtual ~SingleThreadedExecutor();
 
   /// Single-threaded implementation of spin.
-  // This function will block until work comes in, execute it, and keep blocking.
-  // It will only be interrupt by a CTRL-C (managed by the global signal handler).
+  /**
+   * This function will block until work comes in, execute it, and then repeat
+   * the process until canceled.
+   * It may be interrupt by a call to rclcpp::Executor::cancel() or by ctrl-c
+   * if the associated context is configured to shutdown on SIGINT.
+   * \throws std::runtime_error when spin() called while already spinning
+   */
   RCLCPP_PUBLIC
   void
-  spin();
+  spin() override;
 
 private:
   RCLCPP_DISABLE_COPY(SingleThreadedExecutor)

rclcpp/include/rclcpp/expand_topic_or_service_name.hpp

@@ -49,6 +49,8 @@ namespace rclcpp
  * \throws InvalidServiceNameError if name is invalid and is_service is true
  * \throws std::bad_alloc if memory cannot be allocated
  * \throws RCLError if an unexpect error occurs
+ * \throws std::runtime_error if the topic name is unexpectedly valid or,
+ *    if the rcl name is invalid or if the rcl namespace is invalid
  */
 RCLCPP_PUBLIC
 std::string

rclcpp/include/rclcpp/experimental/README.md

@@ -0,0 +1,4 @@
+Notice that headers in this folder should only provide symbols in the rclcpp::experimental namespace.
+
+Also notice that these headers are not considered part of the public API as they have not yet been stabilized.
+And therefore they are subject to change without notice.

rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp

@@ -0,0 +1,47 @@
+// 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__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_
+#define RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_
+
+#include <vector>
+
+namespace rclcpp
+{
+namespace experimental
+{
+namespace buffers
+{
+
+template<typename BufferT>
+class BufferImplementationBase
+{
+public:
+  virtual ~BufferImplementationBase() {}
+
+  virtual BufferT dequeue() = 0;
+  virtual void enqueue(BufferT request) = 0;
+
+  virtual std::vector<BufferT> get_all_data() = 0;
+
+  virtual void clear() = 0;
+  virtual bool has_data() const = 0;
+  virtual size_t available_capacity() const = 0;
+};
+
+}  // namespace buffers
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_

rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp

@@ -0,0 +1,332 @@
+// 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__EXPERIMENTAL__BUFFERS__INTRA_PROCESS_BUFFER_HPP_
+#define RCLCPP__EXPERIMENTAL__BUFFERS__INTRA_PROCESS_BUFFER_HPP_
+
+#include <memory>
+#include <stdexcept>
+#include <type_traits>
+#include <utility>
+#include <vector>
+
+#include "rclcpp/allocator/allocator_common.hpp"
+#include "rclcpp/allocator/allocator_deleter.hpp"
+#include "rclcpp/experimental/buffers/buffer_implementation_base.hpp"
+#include "rclcpp/macros.hpp"
+#include "tracetools/tracetools.h"
+
+namespace rclcpp
+{
+namespace experimental
+{
+namespace buffers
+{
+
+class IntraProcessBufferBase
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(IntraProcessBufferBase)
+
+  virtual ~IntraProcessBufferBase() {}
+
+  virtual void clear() = 0;
+
+  virtual bool has_data() const = 0;
+  virtual bool use_take_shared_method() const = 0;
+  virtual size_t available_capacity() const = 0;
+};
+
+template<
+  typename MessageT,
+  typename Alloc = std::allocator<void>,
+  typename MessageDeleter = std::default_delete<MessageT>>
+class IntraProcessBuffer : public IntraProcessBufferBase
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(IntraProcessBuffer)
+
+  virtual ~IntraProcessBuffer() {}
+
+  using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
+  using MessageSharedPtr = std::shared_ptr<const MessageT>;
+
+  virtual void add_shared(MessageSharedPtr msg) = 0;
+  virtual void add_unique(MessageUniquePtr msg) = 0;
+
+  virtual MessageSharedPtr consume_shared() = 0;
+  virtual MessageUniquePtr consume_unique() = 0;
+
+  virtual std::vector<MessageSharedPtr> get_all_data_shared() = 0;
+  virtual std::vector<MessageUniquePtr> get_all_data_unique() = 0;
+};
+
+template<
+  typename MessageT,
+  typename Alloc = std::allocator<void>,
+  typename MessageDeleter = std::default_delete<MessageT>,
+  typename BufferT = std::unique_ptr<MessageT>>
+class TypedIntraProcessBuffer : public IntraProcessBuffer<MessageT, Alloc, MessageDeleter>
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(TypedIntraProcessBuffer)
+
+  using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
+  using MessageAlloc = typename MessageAllocTraits::allocator_type;
+  using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
+  using MessageSharedPtr = std::shared_ptr<const MessageT>;
+
+  explicit
+  TypedIntraProcessBuffer(
+    std::unique_ptr<BufferImplementationBase<BufferT>> buffer_impl,
+    std::shared_ptr<Alloc> allocator = nullptr)
+  {
+    bool valid_type = (std::is_same<BufferT, MessageSharedPtr>::value ||
+      std::is_same<BufferT, MessageUniquePtr>::value);
+    if (!valid_type) {
+      throw std::runtime_error("Creating TypedIntraProcessBuffer with not valid BufferT");
+    }
+
+    buffer_ = std::move(buffer_impl);
+
+    TRACETOOLS_TRACEPOINT(
+      rclcpp_buffer_to_ipb,
+      static_cast<const void *>(buffer_.get()),
+      static_cast<const void *>(this));
+    if (!allocator) {
+      message_allocator_ = std::make_shared<MessageAlloc>();
+    } else {
+      message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
+    }
+  }
+
+  virtual ~TypedIntraProcessBuffer() {}
+
+  void add_shared(MessageSharedPtr msg) override
+  {
+    add_shared_impl<BufferT>(std::move(msg));
+  }
+
+  void add_unique(MessageUniquePtr msg) override
+  {
+    buffer_->enqueue(std::move(msg));
+  }
+
+  MessageSharedPtr consume_shared() override
+  {
+    return consume_shared_impl<BufferT>();
+  }
+
+  MessageUniquePtr consume_unique() override
+  {
+    return consume_unique_impl<BufferT>();
+  }
+
+  std::vector<MessageSharedPtr> get_all_data_shared() override
+  {
+    return get_all_data_shared_impl();
+  }
+
+  std::vector<MessageUniquePtr> get_all_data_unique() override
+  {
+    return get_all_data_unique_impl();
+  }
+
+  bool has_data() const override
+  {
+    return buffer_->has_data();
+  }
+
+  void clear() override
+  {
+    buffer_->clear();
+  }
+
+  bool use_take_shared_method() const override
+  {
+    return std::is_same<BufferT, MessageSharedPtr>::value;
+  }
+
+  size_t available_capacity() const override
+  {
+    return buffer_->available_capacity();
+  }
+
+private:
+  std::unique_ptr<BufferImplementationBase<BufferT>> buffer_;
+
+  std::shared_ptr<MessageAlloc> message_allocator_;
+
+  // MessageSharedPtr to MessageSharedPtr
+  template<typename DestinationT>
+  typename std::enable_if<
+    std::is_same<DestinationT, MessageSharedPtr>::value
+  >::type
+  add_shared_impl(MessageSharedPtr shared_msg)
+  {
+    buffer_->enqueue(std::move(shared_msg));
+  }
+
+  // MessageSharedPtr to MessageUniquePtr
+  template<typename DestinationT>
+  typename std::enable_if<
+    std::is_same<DestinationT, MessageUniquePtr>::value
+  >::type
+  add_shared_impl(MessageSharedPtr shared_msg)
+  {
+    // This should not happen: here a copy is unconditionally made, while the intra-process manager
+    // can decide whether a copy is needed depending on the number and the type of buffers
+
+    MessageUniquePtr unique_msg;
+    MessageDeleter * deleter = std::get_deleter<MessageDeleter, const MessageT>(shared_msg);
+    auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
+    MessageAllocTraits::construct(*message_allocator_.get(), ptr, *shared_msg);
+    if (deleter) {
+      unique_msg = MessageUniquePtr(ptr, *deleter);
+    } else {
+      unique_msg = MessageUniquePtr(ptr);
+    }
+
+    buffer_->enqueue(std::move(unique_msg));
+  }
+
+  // MessageSharedPtr to MessageSharedPtr
+  template<typename OriginT>
+  typename std::enable_if<
+    std::is_same<OriginT, MessageSharedPtr>::value,
+    MessageSharedPtr
+  >::type
+  consume_shared_impl()
+  {
+    return buffer_->dequeue();
+  }
+
+  // MessageUniquePtr to MessageSharedPtr
+  template<typename OriginT>
+  typename std::enable_if<
+    (std::is_same<OriginT, MessageUniquePtr>::value),
+    MessageSharedPtr
+  >::type
+  consume_shared_impl()
+  {
+    // automatic cast from unique ptr to shared ptr
+    return buffer_->dequeue();
+  }
+
+  // MessageSharedPtr to MessageUniquePtr
+  template<typename OriginT>
+  typename std::enable_if<
+    (std::is_same<OriginT, MessageSharedPtr>::value),
+    MessageUniquePtr
+  >::type
+  consume_unique_impl()
+  {
+    MessageSharedPtr buffer_msg = buffer_->dequeue();
+
+    MessageUniquePtr unique_msg;
+    MessageDeleter * deleter = std::get_deleter<MessageDeleter, const MessageT>(buffer_msg);
+    auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
+    MessageAllocTraits::construct(*message_allocator_.get(), ptr, *buffer_msg);
+    if (deleter) {
+      unique_msg = MessageUniquePtr(ptr, *deleter);
+    } else {
+      unique_msg = MessageUniquePtr(ptr);
+    }
+
+    return unique_msg;
+  }
+
+  // MessageUniquePtr to MessageUniquePtr
+  template<typename OriginT>
+  typename std::enable_if<
+    (std::is_same<OriginT, MessageUniquePtr>::value),
+    MessageUniquePtr
+  >::type
+  consume_unique_impl()
+  {
+    return buffer_->dequeue();
+  }
+
+  // MessageSharedPtr to MessageSharedPtr
+  template<typename T = BufferT>
+  typename std::enable_if<
+    std::is_same<T, MessageSharedPtr>::value,
+    std::vector<MessageSharedPtr>
+  >::type
+  get_all_data_shared_impl()
+  {
+    return buffer_->get_all_data();
+  }
+
+  // MessageUniquePtr to MessageSharedPtr
+  template<typename T = BufferT>
+  typename std::enable_if<
+    std::is_same<T, MessageUniquePtr>::value,
+    std::vector<MessageSharedPtr>
+  >::type
+  get_all_data_shared_impl()
+  {
+    std::vector<MessageSharedPtr> result;
+    auto uni_ptr_vec = buffer_->get_all_data();
+    result.reserve(uni_ptr_vec.size());
+    for (MessageUniquePtr & uni_ptr : uni_ptr_vec) {
+      result.emplace_back(std::move(uni_ptr));
+    }
+    return result;
+  }
+
+  // MessageSharedPtr to MessageUniquePtr
+  template<typename T = BufferT>
+  typename std::enable_if<
+    std::is_same<T, MessageSharedPtr>::value,
+    std::vector<MessageUniquePtr>
+  >::type
+  get_all_data_unique_impl()
+  {
+    std::vector<MessageUniquePtr> result;
+    auto shared_ptr_vec = buffer_->get_all_data();
+    result.reserve(shared_ptr_vec.size());
+    for (MessageSharedPtr shared_msg : shared_ptr_vec) {
+      MessageUniquePtr unique_msg;
+      MessageDeleter * deleter = std::get_deleter<MessageDeleter, const MessageT>(shared_msg);
+      auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
+      MessageAllocTraits::construct(*message_allocator_.get(), ptr, *shared_msg);
+      if (deleter) {
+        unique_msg = MessageUniquePtr(ptr, *deleter);
+      } else {
+        unique_msg = MessageUniquePtr(ptr);
+      }
+      result.push_back(std::move(unique_msg));
+    }
+    return result;
+  }
+
+  // MessageUniquePtr to MessageUniquePtr
+  template<typename T = BufferT>
+  typename std::enable_if<
+    std::is_same<T, MessageUniquePtr>::value,
+    std::vector<MessageUniquePtr>
+  >::type
+  get_all_data_unique_impl()
+  {
+    return buffer_->get_all_data();
+  }
+};
+
+}  // namespace buffers
+}  // namespace experimental
+}  // namespace rclcpp
+
+
+#endif  // RCLCPP__EXPERIMENTAL__BUFFERS__INTRA_PROCESS_BUFFER_HPP_

rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp

@@ -0,0 +1,324 @@
+// 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__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_
+#define RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_
+
+#include <memory>
+#include <mutex>
+#include <stdexcept>
+#include <utility>
+#include <vector>
+
+#include "rclcpp/experimental/buffers/buffer_implementation_base.hpp"
+#include "rclcpp/logger.hpp"
+#include "rclcpp/logging.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "tracetools/tracetools.h"
+
+namespace rclcpp
+{
+namespace experimental
+{
+namespace buffers
+{
+
+/// Store elements in a fixed-size, FIFO buffer
+/**
+ * All public member functions are thread-safe.
+ */
+template<typename BufferT>
+class RingBufferImplementation : public BufferImplementationBase<BufferT>
+{
+public:
+  explicit RingBufferImplementation(size_t capacity)
+  : capacity_(capacity),
+    ring_buffer_(capacity),
+    write_index_(capacity_ - 1),
+    read_index_(0),
+    size_(0)
+  {
+    if (capacity == 0) {
+      throw std::invalid_argument("capacity must be a positive, non-zero value");
+    }
+    TRACETOOLS_TRACEPOINT(
+      rclcpp_construct_ring_buffer,
+      static_cast<const void *>(this),
+      capacity_);
+  }
+
+  virtual ~RingBufferImplementation() {}
+
+  /// Add a new element to store in the ring buffer
+  /**
+   * This member function is thread-safe.
+   *
+   * \param request the element to be stored in the ring buffer
+   */
+  void enqueue(BufferT request) override
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+
+    write_index_ = next_(write_index_);
+    ring_buffer_[write_index_] = std::move(request);
+    TRACETOOLS_TRACEPOINT(
+      rclcpp_ring_buffer_enqueue,
+      static_cast<const void *>(this),
+      write_index_,
+      size_ + 1,
+      is_full_());
+
+    if (is_full_()) {
+      read_index_ = next_(read_index_);
+    } else {
+      size_++;
+    }
+  }
+
+  /// Remove the oldest element from ring buffer
+  /**
+   * This member function is thread-safe.
+   *
+   * \return the element that is being removed from the ring buffer
+   */
+  BufferT dequeue() override
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+
+    if (!has_data_()) {
+      return BufferT();
+    }
+
+    auto request = std::move(ring_buffer_[read_index_]);
+    TRACETOOLS_TRACEPOINT(
+      rclcpp_ring_buffer_dequeue,
+      static_cast<const void *>(this),
+      read_index_,
+      size_ - 1);
+    read_index_ = next_(read_index_);
+
+    size_--;
+
+    return request;
+  }
+
+  /// Get all the elements from the ring buffer
+  /**
+   * This member function is thread-safe.
+   *
+   * \return a vector containing all the elements from the ring buffer
+   */
+  std::vector<BufferT> get_all_data() override
+  {
+    return get_all_data_impl();
+  }
+
+  /// Get the next index value for the ring buffer
+  /**
+   * This member function is thread-safe.
+   *
+   * \param val the current index value
+   * \return the next index value
+   */
+  inline size_t next(size_t val)
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return next_(val);
+  }
+
+  /// Get if the ring buffer has at least one element stored
+  /**
+   * This member function is thread-safe.
+   *
+   * \return `true` if there is data and `false` otherwise
+   */
+  inline bool has_data() const override
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return has_data_();
+  }
+
+  /// Get if the size of the buffer is equal to its capacity
+  /**
+   * This member function is thread-safe.
+   *
+   * \return `true` if the size of the buffer is equal is capacity
+   * and `false` otherwise
+   */
+  inline bool is_full() const
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return is_full_();
+  }
+
+  /// Get the remaining capacity to store messages
+  /**
+   * This member function is thread-safe.
+   *
+   * \return the number of free capacity for new messages
+   */
+  size_t available_capacity() const override
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return available_capacity_();
+  }
+
+  void clear() override
+  {
+    TRACETOOLS_TRACEPOINT(rclcpp_ring_buffer_clear, static_cast<const void *>(this));
+    std::lock_guard<std::mutex> lock(mutex_);
+    clear_();
+  }
+
+private:
+  /// Get the next index value for the ring buffer
+  /**
+   * This member function is not thread-safe.
+   *
+   * \param val the current index value
+   * \return the next index value
+   */
+  inline size_t next_(size_t val)
+  {
+    return (val + 1) % capacity_;
+  }
+
+  /// Get if the ring buffer has at least one element stored
+  /**
+   * This member function is not thread-safe.
+   *
+   * \return `true` if there is data and `false` otherwise
+   */
+  inline bool has_data_() const
+  {
+    return size_ != 0;
+  }
+
+  /// Get if the size of the buffer is equal to its capacity
+  /**
+   * This member function is not thread-safe.
+   *
+   * \return `true` if the size of the buffer is equal is capacity
+   * and `false` otherwise
+   */
+  inline bool is_full_() const
+  {
+    return size_ == capacity_;
+  }
+
+  /// Get the remaining capacity to store messages
+  /**
+   * This member function is not thread-safe.
+   *
+   * \return the number of free capacity for new messages
+   */
+  inline size_t available_capacity_() const
+  {
+    return capacity_ - size_;
+  }
+
+  inline void clear_()
+  {
+    ring_buffer_.clear();
+    size_ = 0;
+    read_index_ = 0;
+    write_index_ = capacity_ - 1;
+  }
+
+  /// Traits for checking if a type is std::unique_ptr
+  template<typename ...>
+  struct is_std_unique_ptr final : std::false_type {};
+  template<class T, typename ... Args>
+  struct is_std_unique_ptr<std::unique_ptr<T, Args...>> final : std::true_type
+  {
+    typedef T Ptr_type;
+  };
+
+  /// Get all the elements from the ring buffer
+  /**
+   * This member function is thread-safe.
+   * Two versions for the implementation of the function.
+   * One for buffer containing unique_ptr and the other for other types
+   *
+   * \return a vector containing all the elements from the ring buffer
+   */
+  template<typename T = BufferT, std::enable_if_t<is_std_unique_ptr<T>::value &&
+    std::is_copy_constructible<
+      typename is_std_unique_ptr<T>::Ptr_type
+    >::value,
+    void> * = nullptr>
+  std::vector<BufferT> get_all_data_impl()
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    std::vector<BufferT> result_vtr;
+    result_vtr.reserve(size_);
+    for (size_t id = 0; id < size_; ++id) {
+      const auto & elem(ring_buffer_[(read_index_ + id) % capacity_]);
+      if (elem != nullptr) {
+        result_vtr.emplace_back(new typename is_std_unique_ptr<T>::Ptr_type(
+          *elem));
+      } else {
+        result_vtr.emplace_back(nullptr);
+      }
+    }
+    return result_vtr;
+  }
+
+  template<typename T = BufferT, std::enable_if_t<
+      std::is_copy_constructible<T>::value, void> * = nullptr>
+  std::vector<BufferT> get_all_data_impl()
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    std::vector<BufferT> result_vtr;
+    result_vtr.reserve(size_);
+    for (size_t id = 0; id < size_; ++id) {
+      result_vtr.emplace_back(ring_buffer_[(read_index_ + id) % capacity_]);
+    }
+    return result_vtr;
+  }
+
+  template<typename T = BufferT, std::enable_if_t<!is_std_unique_ptr<T>::value &&
+    !std::is_copy_constructible<T>::value, void> * = nullptr>
+  std::vector<BufferT> get_all_data_impl()
+  {
+    throw std::logic_error("Underlined type results in invalid get_all_data_impl()");
+    return {};
+  }
+
+  template<typename T = BufferT, std::enable_if_t<is_std_unique_ptr<T>::value &&
+    !std::is_copy_constructible<typename is_std_unique_ptr<T>::Ptr_type>::value,
+    void> * = nullptr>
+  std::vector<BufferT> get_all_data_impl()
+  {
+    throw std::logic_error("Underlined type in unique_ptr results in invalid get_all_data_impl()");
+    return {};
+  }
+
+  const size_t capacity_;
+
+  std::vector<BufferT> ring_buffer_;
+
+  size_t write_index_;
+  size_t read_index_;
+  size_t size_;
+
+  mutable std::mutex mutex_;
+};
+
+}  // namespace buffers
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_

rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp

@@ -0,0 +1,98 @@
+// 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__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_
+#define RCLCPP__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_
+
+#include <memory>
+#include <stdexcept>
+#include <utility>
+
+#include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
+#include "rclcpp/experimental/buffers/ring_buffer_implementation.hpp"
+#include "rclcpp/intra_process_buffer_type.hpp"
+#include "rclcpp/qos.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+
+template<
+  typename MessageT,
+  typename Alloc = std::allocator<void>,
+  typename Deleter = std::default_delete<MessageT>>
+typename rclcpp::experimental::buffers::IntraProcessBuffer<MessageT, Alloc, Deleter>::UniquePtr
+create_intra_process_buffer(
+  IntraProcessBufferType buffer_type,
+  const rclcpp::QoS & qos,
+  std::shared_ptr<Alloc> allocator)
+{
+  using MessageSharedPtr = std::shared_ptr<const MessageT>;
+  using MessageUniquePtr = std::unique_ptr<MessageT, Deleter>;
+
+  size_t buffer_size = qos.depth();
+
+  using rclcpp::experimental::buffers::IntraProcessBuffer;
+  typename IntraProcessBuffer<MessageT, Alloc, Deleter>::UniquePtr buffer;
+
+  switch (buffer_type) {
+    case IntraProcessBufferType::SharedPtr:
+      {
+        using BufferT = MessageSharedPtr;
+
+        auto buffer_implementation =
+          std::make_unique<rclcpp::experimental::buffers::RingBufferImplementation<BufferT>>(
+          buffer_size);
+
+        // Construct the intra_process_buffer
+        buffer =
+          std::make_unique<rclcpp::experimental::buffers::TypedIntraProcessBuffer<MessageT, Alloc,
+            Deleter, BufferT>>(
+          std::move(buffer_implementation),
+          allocator);
+
+        break;
+      }
+    case IntraProcessBufferType::UniquePtr:
+      {
+        using BufferT = MessageUniquePtr;
+
+        auto buffer_implementation =
+          std::make_unique<rclcpp::experimental::buffers::RingBufferImplementation<BufferT>>(
+          buffer_size);
+
+        // Construct the intra_process_buffer
+        buffer =
+          std::make_unique<rclcpp::experimental::buffers::TypedIntraProcessBuffer<MessageT, Alloc,
+            Deleter, BufferT>>(
+          std::move(buffer_implementation),
+          allocator);
+
+        break;
+      }
+    default:
+      {
+        throw std::runtime_error("Unrecognized IntraProcessBufferType value");
+        break;
+      }
+  }
+
+  return buffer;
+}
+
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_

rclcpp/include/rclcpp/experimental/executable_list.hpp

@@ -0,0 +1,91 @@
+// Copyright 2019 Nobleo Technology
+//
+// 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__EXPERIMENTAL__EXECUTABLE_LIST_HPP_
+#define RCLCPP__EXPERIMENTAL__EXECUTABLE_LIST_HPP_
+
+#include <vector>
+
+#include "rclcpp/client.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+
+/// This class contains subscriptionbase, timerbase, etc. which can be used to run callbacks.
+class ExecutableList final
+{
+public:
+  RCLCPP_PUBLIC
+  ExecutableList();
+
+  RCLCPP_PUBLIC
+  ~ExecutableList();
+
+  RCLCPP_PUBLIC
+  void
+  clear();
+
+  RCLCPP_PUBLIC
+  void
+  add_subscription(rclcpp::SubscriptionBase::SharedPtr subscription);
+
+  RCLCPP_PUBLIC
+  void
+  add_timer(rclcpp::TimerBase::SharedPtr timer);
+
+  RCLCPP_PUBLIC
+  void
+  add_service(rclcpp::ServiceBase::SharedPtr service);
+
+  RCLCPP_PUBLIC
+  void
+  add_client(rclcpp::ClientBase::SharedPtr client);
+
+  RCLCPP_PUBLIC
+  void
+  add_waitable(rclcpp::Waitable::SharedPtr waitable);
+
+  // Vector containing the SubscriptionBase of all the subscriptions added to the executor.
+  std::vector<rclcpp::SubscriptionBase::SharedPtr> subscription;
+  // Contains the count of added subscriptions
+  size_t number_of_subscriptions;
+  // Vector containing the TimerBase of all the timers added to the executor.
+  std::vector<rclcpp::TimerBase::SharedPtr> timer;
+  // Contains the count of added timers
+  size_t number_of_timers;
+  // Vector containing the ServiceBase of all the services added to the executor.
+  std::vector<rclcpp::ServiceBase::SharedPtr> service;
+  // Contains the count of added services
+  size_t number_of_services;
+  // Vector containing the ClientBase of all the clients added to the executor.
+  std::vector<rclcpp::ClientBase::SharedPtr> client;
+  // Contains the count of added clients
+  size_t number_of_clients;
+  // Vector containing all the waitables added to the executor.
+  std::vector<rclcpp::Waitable::SharedPtr> waitable;
+  // Contains the count of added waitables
+  size_t number_of_waitables;
+};
+
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__EXECUTABLE_LIST_HPP_

rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp

@@ -0,0 +1,211 @@
+// Copyright 2023 iRobot Corporation.
+//
+// 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__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_HPP_
+#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_HPP_
+
+#include <atomic>
+#include <chrono>
+#include <memory>
+#include <vector>
+
+#include "rclcpp/executor.hpp"
+#include "rclcpp/executors/executor_entities_collection.hpp"
+#include "rclcpp/executors/executor_entities_collector.hpp"
+#include "rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp"
+#include "rclcpp/experimental/executors/events_executor/events_queue.hpp"
+#include "rclcpp/experimental/executors/events_executor/simple_events_queue.hpp"
+#include "rclcpp/experimental/timers_manager.hpp"
+#include "rclcpp/node.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+namespace executors
+{
+
+/// Events executor implementation
+/**
+ * This executor uses an events queue and a timers manager to execute entities from its
+ * associated nodes and callback groups.
+ * ROS 2 entities allow to set callback functions that are invoked when the entity is triggered
+ * or has work to do. The events-executor sets these callbacks such that they push an
+ * event into its queue.
+ *
+ * This executor tries to reduce as much as possible the amount of maintenance operations.
+ * This allows to use customized `EventsQueue` classes to achieve different goals such
+ * as very low CPU usage, bounded memory requirement, determinism, etc.
+ *
+ * The executor uses a weak ownership model and it locks entities only while executing
+ * their related events.
+ *
+ * To run this executor:
+ * rclcpp::experimental::executors::EventsExecutor executor;
+ * executor.add_node(node);
+ * executor.spin();
+ * executor.remove_node(node);
+ */
+class EventsExecutor : public rclcpp::Executor
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(EventsExecutor)
+
+  /// Default constructor. See the default constructor for Executor.
+  /**
+   * \param[in] options Options used to configure the executor.
+   * \param[in] events_queue The queue used to store events.
+   * \param[in] execute_timers_separate_thread If true, timers are executed in a separate
+   * thread. If false, timers are executed in the same thread as all other entities.
+   */
+  RCLCPP_PUBLIC
+  EventsExecutor(
+    const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions(),
+    rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue = std::make_unique<
+      rclcpp::experimental::executors::SimpleEventsQueue>(),
+    bool execute_timers_separate_thread = false);
+
+  /// Default destructor.
+  RCLCPP_PUBLIC
+  virtual ~EventsExecutor();
+
+  /// Events executor implementation of spin.
+  /**
+   * This function will block until work comes in, execute it, and keep blocking.
+   * It will only be interrupted by a CTRL-C (managed by the global signal handler).
+   * \throws std::runtime_error when spin() called while already spinning
+   */
+  RCLCPP_PUBLIC
+  void
+  spin() override;
+
+  /// Events executor implementation of spin some
+  /**
+   * This non-blocking function will execute the timers and events
+   * that were ready when this API was called, until timeout or no
+   * more work available. New ready-timers/events arrived while
+   * executing work, won't be taken into account here.
+   *
+   * Example:
+   *   while(condition) {
+   *     spin_some();
+   *     sleep(); // User should have some sync work or
+   *              // sleep to avoid a 100% CPU usage
+   *   }
+   */
+  RCLCPP_PUBLIC
+  void
+  spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)) override;
+
+  /// Events executor implementation of spin all
+  /**
+   * This non-blocking function will execute timers and events
+   * until timeout or no more work available. If new ready-timers/events
+   * arrive while executing work available, they will be executed
+   * as long as the timeout hasn't expired.
+   *
+   * Example:
+   *   while(condition) {
+   *     spin_all();
+   *     sleep(); // User should have some sync work or
+   *              // sleep to avoid a 100% CPU usage
+   *   }
+   */
+  RCLCPP_PUBLIC
+  void
+  spin_all(std::chrono::nanoseconds max_duration) override;
+
+protected:
+  /// Internal implementation of spin_once
+  RCLCPP_PUBLIC
+  void
+  spin_once_impl(std::chrono::nanoseconds timeout) override;
+
+  /// Internal implementation of spin_some
+  RCLCPP_PUBLIC
+  void
+  spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive);
+
+  /// Collect entities from callback groups and refresh the current collection with them
+  RCLCPP_PUBLIC
+  void
+  handle_updated_entities(bool notify) override;
+
+private:
+  RCLCPP_DISABLE_COPY(EventsExecutor)
+
+  /// Execute a provided executor event if its associated entities are available
+  void
+  execute_event(const ExecutorEvent & event);
+
+  /// Rebuilds the executor's notify waitable, as we can't use the one built in the base class
+  void
+  setup_notify_waitable();
+
+  /// Refresh the current collection using the provided new_collection
+  void
+  refresh_current_collection(const rclcpp::executors::ExecutorEntitiesCollection & new_collection);
+
+  /// Create a listener callback function for the provided entity
+  std::function<void(size_t)>
+  create_entity_callback(void * entity_key, ExecutorEventType type);
+
+  /// Create a listener callback function for the provided waitable entity
+  std::function<void(size_t, int)>
+  create_waitable_callback(const rclcpp::Waitable * waitable_id);
+
+  /// Utility to add the notify waitable to an entities collection
+  void
+  add_notify_waitable_to_collection(
+    rclcpp::executors::ExecutorEntitiesCollection::WaitableCollection & collection);
+
+  /// Searches for the provided entity_id in the collection and returns the entity if valid
+  template<typename CollectionType>
+  typename CollectionType::EntitySharedPtr
+  retrieve_entity(typename CollectionType::Key entity_id, CollectionType & collection)
+  {
+    // Note: we lock the mutex because we assume that you are trying to get an element from the
+    // current collection... If there will be a use-case to retrieve elements also from other
+    // collections, we can move the mutex back to the calling codes.
+    std::lock_guard<std::mutex> guard(mutex_);
+
+    // Check if the entity_id is in the collection
+    auto it = collection.find(entity_id);
+    if (it == collection.end()) {
+      return nullptr;
+    }
+
+    // Check if the entity associated with the entity_id is valid
+    // and remove it from the collection if it isn't
+    auto entity = it->second.entity.lock();
+    if (!entity) {
+      collection.erase(it);
+    }
+
+    // Return the retrieved entity (this can be a nullptr if the entity was not valid)
+    return entity;
+  }
+
+  /// Queue where entities can push events
+  rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue_;
+
+  /// Timers manager used to track and/or execute associated timers
+  std::shared_ptr<rclcpp::experimental::TimersManager> timers_manager_;
+};
+
+}  // namespace executors
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_HPP_

rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp

@@ -0,0 +1,49 @@
+// Copyright 2023 iRobot Corporation.
+//
+// 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__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_EVENT_TYPES_HPP_
+#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_EVENT_TYPES_HPP_
+
+#include <memory>
+
+namespace rclcpp
+{
+namespace experimental
+{
+namespace executors
+{
+
+enum ExecutorEventType
+{
+  CLIENT_EVENT,
+  SUBSCRIPTION_EVENT,
+  SERVICE_EVENT,
+  TIMER_EVENT,
+  WAITABLE_EVENT
+};
+
+struct ExecutorEvent
+{
+  const void * entity_key;
+  std::shared_ptr<void> data;
+  int waitable_data;
+  ExecutorEventType type;
+  size_t num_events;
+};
+
+}  // namespace executors
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_EVENT_TYPES_HPP_

rclcpp/include/rclcpp/experimental/executors/events_executor/events_queue.hpp

@@ -0,0 +1,100 @@
+// Copyright 2023 iRobot Corporation.
+//
+// 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__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_QUEUE_HPP_
+#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_QUEUE_HPP_
+
+#include <queue>
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include "rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+namespace executors
+{
+
+/**
+ * @brief This abstract class can be used to implement different types of queues
+ * where `ExecutorEvent` can be stored.
+ * The derived classes should choose which underlying container to use and
+ * the strategy for pushing and popping events.
+ * For example a queue implementation may be bounded or unbounded and have
+ * different pruning strategies.
+ * Implementations may or may not check the validity of events and decide how to handle
+ * the situation where an event is not valid anymore (e.g. a subscription history cache overruns)
+ */
+class EventsQueue
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(EventsQueue)
+
+  RCLCPP_PUBLIC
+  EventsQueue() = default;
+
+  /**
+   * @brief Destruct the object.
+   */
+  RCLCPP_PUBLIC
+  virtual ~EventsQueue() = default;
+
+  /**
+   * @brief push event into the queue
+   * @param event The event to push into the queue
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  enqueue(const rclcpp::experimental::executors::ExecutorEvent & event) = 0;
+
+  /**
+   * @brief Extracts an event from the queue, eventually waiting until timeout
+   * if none is available.
+   * @return true if event has been found, false if timeout
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  dequeue(
+    rclcpp::experimental::executors::ExecutorEvent & event,
+    std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max()) = 0;
+
+  /**
+   * @brief Test whether queue is empty
+   * @return true if the queue's size is 0, false otherwise.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  empty() const = 0;
+
+  /**
+   * @brief Returns the number of elements in the queue.
+   * @return the number of elements in the queue.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  size() const = 0;
+};
+
+}  // namespace executors
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_QUEUE_HPP_

rclcpp/include/rclcpp/experimental/executors/events_executor/simple_events_queue.hpp

@@ -0,0 +1,134 @@
+// Copyright 2023 iRobot Corporation.
+//
+// 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__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__SIMPLE_EVENTS_QUEUE_HPP_
+#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__SIMPLE_EVENTS_QUEUE_HPP_
+
+#include <condition_variable>
+#include <mutex>
+#include <queue>
+#include <utility>
+
+#include "rclcpp/experimental/executors/events_executor/events_queue.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+namespace executors
+{
+
+/**
+ * @brief This class implements an EventsQueue as a simple wrapper around a std::queue.
+ * It does not perform any checks about the size of queue, which can grow
+ * unbounded without being pruned.
+ * The simplicity of this implementation makes it suitable for optimizing CPU usage.
+ */
+class SimpleEventsQueue : public EventsQueue
+{
+public:
+  RCLCPP_PUBLIC
+  ~SimpleEventsQueue() override = default;
+
+  /**
+   * @brief enqueue event into the queue
+   * Thread safe
+   * @param event The event to enqueue into the queue
+   */
+  RCLCPP_PUBLIC
+  void
+  enqueue(const rclcpp::experimental::executors::ExecutorEvent & event) override
+  {
+    rclcpp::experimental::executors::ExecutorEvent single_event = event;
+    single_event.num_events = 1;
+    {
+      std::unique_lock<std::mutex> lock(mutex_);
+      for (size_t ev = 0; ev < event.num_events; ev++) {
+        event_queue_.push(single_event);
+      }
+    }
+    events_queue_cv_.notify_one();
+  }
+
+  /**
+   * @brief waits for an event until timeout, gets a single event
+   * Thread safe
+   * @return true if event, false if timeout
+   */
+  RCLCPP_PUBLIC
+  bool
+  dequeue(
+    rclcpp::experimental::executors::ExecutorEvent & event,
+    std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max()) override
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+
+    // Initialize to true because it's only needed if we have a valid timeout
+    bool has_data = true;
+    if (timeout != std::chrono::nanoseconds::max()) {
+      has_data =
+        events_queue_cv_.wait_for(lock, timeout, [this]() {return !event_queue_.empty();});
+    } else {
+      events_queue_cv_.wait(lock, [this]() {return !event_queue_.empty();});
+    }
+
+    if (has_data) {
+      event = event_queue_.front();
+      event_queue_.pop();
+      return true;
+    }
+
+    return false;
+  }
+
+  /**
+   * @brief Test whether queue is empty
+   * Thread safe
+   * @return true if the queue's size is 0, false otherwise.
+   */
+  RCLCPP_PUBLIC
+  bool
+  empty() const override
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+    return event_queue_.empty();
+  }
+
+  /**
+   * @brief Returns the number of elements in the queue.
+   * Thread safe
+   * @return the number of elements in the queue.
+   */
+  RCLCPP_PUBLIC
+  size_t
+  size() const override
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+    return event_queue_.size();
+  }
+
+private:
+  // The underlying queue implementation
+  std::queue<rclcpp::experimental::executors::ExecutorEvent> event_queue_;
+  // Mutex to protect read/write access to the queue
+  mutable std::mutex mutex_;
+  // Variable used to notify when an event is added to the queue
+  std::condition_variable events_queue_cv_;
+};
+
+}  // namespace executors
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__SIMPLE_EVENTS_QUEUE_HPP_

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

@@ -0,0 +1,650 @@
+// 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__EXPERIMENTAL__INTRA_PROCESS_MANAGER_HPP_
+#define RCLCPP__EXPERIMENTAL__INTRA_PROCESS_MANAGER_HPP_
+
+#include <rmw/types.h>
+
+#include <shared_mutex>
+
+#include <iterator>
+#include <memory>
+#include <stdexcept>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+#include <typeinfo>
+
+#include "rclcpp/allocator/allocator_deleter.hpp"
+#include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
+#include "rclcpp/experimental/ros_message_intra_process_buffer.hpp"
+#include "rclcpp/experimental/subscription_intra_process.hpp"
+#include "rclcpp/experimental/subscription_intra_process_base.hpp"
+#include "rclcpp/experimental/subscription_intra_process_buffer.hpp"
+#include "rclcpp/logger.hpp"
+#include "rclcpp/logging.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/publisher_base.hpp"
+#include "rclcpp/type_adapter.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+namespace experimental
+{
+
+/// This class performs intra process communication between nodes.
+/**
+ * This class is used in the creation of publishers and subscriptions.
+ * A singleton instance of this class is owned by a rclcpp::Context and a
+ * rclcpp::Node can use an associated Context to get an instance of this class.
+ * Nodes which do not have a common Context will not exchange intra process
+ * messages because they do not share access to the same instance of this class.
+ *
+ * When a Node creates a subscription, it can also create a helper class,
+ * called SubscriptionIntraProcess, meant to receive intra process messages.
+ * It can be registered with this class.
+ * It is also allocated an id which is unique among all publishers
+ * and subscriptions in this process and that is associated to the subscription.
+ *
+ * When a Node creates a publisher, as with subscriptions, a helper class can
+ * be registered with this class.
+ * This is required in order to publish intra-process messages.
+ * It is also allocated an id which is unique among all publishers
+ * and subscriptions in this process and that is associated to the publisher.
+ *
+ * When a publisher or a subscription are registered, this class checks to see
+ * which other subscriptions or publishers it will communicate with,
+ * i.e. they have the same topic and compatible QoS.
+ *
+ * When the user publishes a message, if intra-process communication is enabled
+ * on the publisher, the message is given to this class.
+ * Using the publisher id, a list of recipients for the message is selected.
+ * For each subscription in the list, this class stores the message, whether
+ * sharing ownership or making a copy, in a buffer associated with the
+ * subscription helper class.
+ *
+ * The subscription helper class contains a buffer where published
+ * intra-process messages are stored until they are taken from the subscription.
+ * Depending on the data type stored in the buffer, the subscription helper
+ * class can request either shared or exclusive ownership on the message.
+ *
+ * Thus, when an intra-process message is published, this class knows how many
+ * intra-process subscriptions needs it and how many require ownership.
+ * This information allows this class to operate efficiently by performing the
+ * fewest number of copies of the message required.
+ *
+ * This class is neither CopyConstructable nor CopyAssignable.
+ */
+class IntraProcessManager
+{
+private:
+  RCLCPP_DISABLE_COPY(IntraProcessManager)
+
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager)
+
+  RCLCPP_PUBLIC
+  IntraProcessManager();
+
+  RCLCPP_PUBLIC
+  virtual ~IntraProcessManager();
+
+  /// Register a subscription with the manager, returns subscriptions unique id.
+  /**
+   * This method stores the subscription intra process object, together with
+   * the information of its wrapped subscription (i.e. topic name and QoS).
+   *
+   * In addition this generates a unique intra process id for the subscription.
+   *
+   * \param subscription the SubscriptionIntraProcess to register.
+   * \return an unsigned 64-bit integer which is the subscription's unique id.
+   */
+  template<
+    typename ROSMessageType,
+    typename Alloc = std::allocator<ROSMessageType>
+  >
+  uint64_t
+  add_subscription(rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription)
+  {
+    std::unique_lock<std::shared_timed_mutex> lock(mutex_);
+
+    uint64_t sub_id = IntraProcessManager::get_next_unique_id();
+
+    subscriptions_[sub_id] = subscription;
+
+    // adds the subscription id to all the matchable publishers
+    for (auto & pair : publishers_) {
+      auto publisher = pair.second.lock();
+      if (!publisher) {
+        continue;
+      }
+      if (can_communicate(publisher, subscription)) {
+        uint64_t pub_id = pair.first;
+        insert_sub_id_for_pub(sub_id, pub_id, subscription->use_take_shared_method());
+        if (publisher->is_durability_transient_local() &&
+          subscription->is_durability_transient_local())
+        {
+          do_transient_local_publish<ROSMessageType, Alloc>(
+            pub_id, sub_id,
+            subscription->use_take_shared_method());
+        }
+      }
+    }
+
+    return sub_id;
+  }
+
+  /// Unregister a subscription using the subscription's unique id.
+  /**
+   * This method does not allocate memory.
+   *
+   * \param intra_process_subscription_id id of the subscription to remove.
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_subscription(uint64_t intra_process_subscription_id);
+
+  /// Register a publisher with the manager, returns the publisher unique id.
+  /**
+   * This method stores the publisher intra process object, together with
+   * the information of its wrapped publisher (i.e. topic name and QoS).
+   *
+   * If the publisher's durability is transient local, its buffer pointer should
+   * be passed and the method will store it as well.
+   *
+   * In addition this generates a unique intra process id for the publisher.
+   *
+   * \param publisher publisher to be registered with the manager.
+   * \param buffer publisher's buffer to be stored if its duability is transient local.
+   * \return an unsigned 64-bit integer which is the publisher's unique id.
+   */
+  RCLCPP_PUBLIC
+  uint64_t
+  add_publisher(
+    rclcpp::PublisherBase::SharedPtr publisher,
+    rclcpp::experimental::buffers::IntraProcessBufferBase::SharedPtr buffer =
+    rclcpp::experimental::buffers::IntraProcessBufferBase::SharedPtr());
+
+  /// Unregister a publisher using the publisher's unique id.
+  /**
+   * This method does not allocate memory.
+   *
+   * \param intra_process_publisher_id id of the publisher to remove.
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_publisher(uint64_t intra_process_publisher_id);
+
+  /// Publishes an intra-process message, passed as a unique pointer.
+  /**
+   * This is one of the two methods for publishing intra-process.
+   *
+   * Using the intra-process publisher id, a list of recipients is obtained.
+   * This list is split in half, depending whether they require ownership or not.
+   *
+   * This particular method takes a unique pointer as input.
+   * The pointer can be promoted to a shared pointer and passed to all the subscriptions
+   * that do not require ownership.
+   * In case of subscriptions requiring ownership, the message will be copied for all of
+   * them except the last one, when ownership can be transferred.
+   *
+   * This method can save an additional copy compared to the shared pointer one.
+   *
+   * This method can throw an exception if the publisher id is not found or
+   * if the publisher shared_ptr given to add_publisher has gone out of scope.
+   *
+   * This method does allocate memory.
+   *
+   * \param intra_process_publisher_id the id of the publisher of this message.
+   * \param message the message that is being stored.
+   * \param allocator for allocations when buffering messages.
+   */
+  template<
+    typename MessageT,
+    typename ROSMessageType,
+    typename Alloc,
+    typename Deleter = std::default_delete<MessageT>
+  >
+  void
+  do_intra_process_publish(
+    uint64_t intra_process_publisher_id,
+    std::unique_ptr<MessageT, Deleter> message,
+    typename allocator::AllocRebind<MessageT, Alloc>::allocator_type & allocator)
+  {
+    using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
+    using MessageAllocatorT = typename MessageAllocTraits::allocator_type;
+
+    std::shared_lock<std::shared_timed_mutex> lock(mutex_);
+
+    auto publisher_it = pub_to_subs_.find(intra_process_publisher_id);
+    if (publisher_it == pub_to_subs_.end()) {
+      // Publisher is either invalid or no longer exists.
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling do_intra_process_publish for invalid or no longer existing publisher id");
+      return;
+    }
+    const auto & sub_ids = publisher_it->second;
+
+    if (sub_ids.take_ownership_subscriptions.empty()) {
+      // None of the buffers require ownership, so we promote the pointer
+      std::shared_ptr<MessageT> msg = std::move(message);
+
+      this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
+        msg, sub_ids.take_shared_subscriptions);
+    } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT
+      sub_ids.take_shared_subscriptions.size() <= 1)
+    {
+      // There is at maximum 1 buffer that does not require ownership.
+      // So this case is equivalent to all the buffers requiring ownership
+
+      // Merge the two vector of ids into a unique one
+      std::vector<uint64_t> concatenated_vector(
+        sub_ids.take_shared_subscriptions.begin(), sub_ids.take_shared_subscriptions.end());
+      concatenated_vector.insert(
+        concatenated_vector.end(),
+        sub_ids.take_ownership_subscriptions.begin(),
+        sub_ids.take_ownership_subscriptions.end());
+      this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
+        std::move(message),
+        concatenated_vector,
+        allocator);
+    } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT
+      sub_ids.take_shared_subscriptions.size() > 1)
+    {
+      // Construct a new shared pointer from the message
+      // for the buffers that do not require ownership
+      auto shared_msg = std::allocate_shared<MessageT, MessageAllocatorT>(allocator, *message);
+
+      this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
+        shared_msg, sub_ids.take_shared_subscriptions);
+      this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
+        std::move(message), sub_ids.take_ownership_subscriptions, allocator);
+    }
+  }
+
+  template<
+    typename MessageT,
+    typename ROSMessageType,
+    typename Alloc,
+    typename Deleter = std::default_delete<MessageT>
+  >
+  std::shared_ptr<const MessageT>
+  do_intra_process_publish_and_return_shared(
+    uint64_t intra_process_publisher_id,
+    std::unique_ptr<MessageT, Deleter> message,
+    typename allocator::AllocRebind<MessageT, Alloc>::allocator_type & allocator)
+  {
+    using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
+    using MessageAllocatorT = typename MessageAllocTraits::allocator_type;
+
+    std::shared_lock<std::shared_timed_mutex> lock(mutex_);
+
+    auto publisher_it = pub_to_subs_.find(intra_process_publisher_id);
+    if (publisher_it == pub_to_subs_.end()) {
+      // Publisher is either invalid or no longer exists.
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling do_intra_process_publish for invalid or no longer existing publisher id");
+      return nullptr;
+    }
+    const auto & sub_ids = publisher_it->second;
+
+    if (sub_ids.take_ownership_subscriptions.empty()) {
+      // If there are no owning, just convert to shared.
+      std::shared_ptr<MessageT> shared_msg = std::move(message);
+      if (!sub_ids.take_shared_subscriptions.empty()) {
+        this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
+          shared_msg, sub_ids.take_shared_subscriptions);
+      }
+      return shared_msg;
+    } else {
+      // Construct a new shared pointer from the message for the buffers that
+      // do not require ownership and to return.
+      auto shared_msg = std::allocate_shared<MessageT, MessageAllocatorT>(allocator, *message);
+
+      if (!sub_ids.take_shared_subscriptions.empty()) {
+        this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
+          shared_msg,
+          sub_ids.take_shared_subscriptions);
+      }
+      if (!sub_ids.take_ownership_subscriptions.empty()) {
+        this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
+          std::move(message),
+          sub_ids.take_ownership_subscriptions,
+          allocator);
+      }
+      return shared_msg;
+    }
+  }
+
+  template<
+    typename MessageT,
+    typename Alloc,
+    typename Deleter,
+    typename ROSMessageType>
+  void
+  add_shared_msg_to_buffer(
+    std::shared_ptr<const MessageT> message,
+    uint64_t subscription_id)
+  {
+    add_shared_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(message, {subscription_id});
+  }
+
+  template<
+    typename MessageT,
+    typename Alloc,
+    typename Deleter,
+    typename ROSMessageType>
+  void
+  add_owned_msg_to_buffer(
+    std::unique_ptr<MessageT, Deleter> message,
+    uint64_t subscription_id,
+    typename allocator::AllocRebind<MessageT, Alloc>::allocator_type & allocator)
+  {
+    add_owned_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
+      std::move(message), {subscription_id}, allocator);
+  }
+
+  /// Return true if the given rmw_gid_t matches any stored Publishers.
+  RCLCPP_PUBLIC
+  bool
+  matches_any_publishers(const rmw_gid_t * id) const;
+
+  /// Return the number of intraprocess subscriptions that are matched with a given publisher id.
+  RCLCPP_PUBLIC
+  size_t
+  get_subscription_count(uint64_t intra_process_publisher_id) const;
+
+  RCLCPP_PUBLIC
+  rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr
+  get_subscription_intra_process(uint64_t intra_process_subscription_id);
+
+  /// Return the lowest available capacity for all subscription buffers for a publisher id.
+  RCLCPP_PUBLIC
+  size_t
+  lowest_available_capacity(const uint64_t intra_process_publisher_id) const;
+
+private:
+  struct SplittedSubscriptions
+  {
+    std::vector<uint64_t> take_shared_subscriptions;
+    std::vector<uint64_t> take_ownership_subscriptions;
+  };
+
+  using SubscriptionMap =
+    std::unordered_map<uint64_t, rclcpp::experimental::SubscriptionIntraProcessBase::WeakPtr>;
+
+  using PublisherMap =
+    std::unordered_map<uint64_t, rclcpp::PublisherBase::WeakPtr>;
+
+  using PublisherBufferMap =
+    std::unordered_map<uint64_t, rclcpp::experimental::buffers::IntraProcessBufferBase::WeakPtr>;
+
+  using PublisherToSubscriptionIdsMap =
+    std::unordered_map<uint64_t, SplittedSubscriptions>;
+
+  RCLCPP_PUBLIC
+  static
+  uint64_t
+  get_next_unique_id();
+
+  RCLCPP_PUBLIC
+  void
+  insert_sub_id_for_pub(uint64_t sub_id, uint64_t pub_id, bool use_take_shared_method);
+
+  RCLCPP_PUBLIC
+  bool
+  can_communicate(
+    rclcpp::PublisherBase::SharedPtr pub,
+    rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr sub) const;
+
+  template<
+    typename ROSMessageType,
+    typename Alloc = std::allocator<ROSMessageType>
+  >
+  void do_transient_local_publish(
+    const uint64_t pub_id, const uint64_t sub_id,
+    const bool use_take_shared_method)
+  {
+    using ROSMessageTypeAllocatorTraits = allocator::AllocRebind<ROSMessageType, Alloc>;
+    using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
+    using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>;
+
+    auto publisher_buffer = publisher_buffers_[pub_id].lock();
+    if (!publisher_buffer) {
+      throw std::runtime_error("publisher buffer has unexpectedly gone out of scope");
+    }
+    auto buffer = std::dynamic_pointer_cast<
+      rclcpp::experimental::buffers::IntraProcessBuffer<
+        ROSMessageType,
+        ROSMessageTypeAllocator,
+        ROSMessageTypeDeleter
+      >
+      >(publisher_buffer);
+    if (!buffer) {
+      throw std::runtime_error(
+              "failed to dynamic cast publisher's IntraProcessBufferBase to "
+              "IntraProcessBuffer<ROSMessageType,ROSMessageTypeAllocator,"
+              "ROSMessageTypeDeleter> which can happen when the publisher and "
+              "subscription use different allocator types, which is not supported");
+    }
+    if (use_take_shared_method) {
+      auto data_vec = buffer->get_all_data_shared();
+      for (auto shared_data : data_vec) {
+        this->template add_shared_msg_to_buffer<
+          ROSMessageType, ROSMessageTypeAllocator, ROSMessageTypeDeleter, ROSMessageType>(
+          shared_data, sub_id);
+      }
+    } else {
+      auto data_vec = buffer->get_all_data_unique();
+      for (auto & owned_data : data_vec) {
+        auto allocator = ROSMessageTypeAllocator();
+        this->template add_owned_msg_to_buffer<
+          ROSMessageType, ROSMessageTypeAllocator, ROSMessageTypeDeleter, ROSMessageType>(
+          std::move(owned_data), sub_id, allocator);
+      }
+    }
+  }
+
+  template<
+    typename MessageT,
+    typename Alloc,
+    typename Deleter,
+    typename ROSMessageType>
+  void
+  add_shared_msg_to_buffers(
+    std::shared_ptr<const MessageT> message,
+    std::vector<uint64_t> subscription_ids)
+  {
+    using ROSMessageTypeAllocatorTraits = allocator::AllocRebind<ROSMessageType, Alloc>;
+    using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
+    using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>;
+
+    using PublishedType = typename rclcpp::TypeAdapter<MessageT>::custom_type;
+    using PublishedTypeAllocatorTraits = allocator::AllocRebind<PublishedType, Alloc>;
+    using PublishedTypeAllocator = typename PublishedTypeAllocatorTraits::allocator_type;
+    using PublishedTypeDeleter = allocator::Deleter<PublishedTypeAllocator, PublishedType>;
+
+    for (auto id : subscription_ids) {
+      auto subscription_it = subscriptions_.find(id);
+      if (subscription_it == subscriptions_.end()) {
+        throw std::runtime_error("subscription has unexpectedly gone out of scope");
+      }
+      auto subscription_base = subscription_it->second.lock();
+      if (subscription_base == nullptr) {
+        subscriptions_.erase(id);
+        continue;
+      }
+
+      auto subscription = std::dynamic_pointer_cast<
+        rclcpp::experimental::SubscriptionIntraProcessBuffer<PublishedType,
+        PublishedTypeAllocator, PublishedTypeDeleter, ROSMessageType>
+        >(subscription_base);
+      if (subscription != nullptr) {
+        subscription->provide_intra_process_data(message);
+        continue;
+      }
+
+      auto ros_message_subscription = std::dynamic_pointer_cast<
+        rclcpp::experimental::SubscriptionROSMsgIntraProcessBuffer<ROSMessageType,
+        ROSMessageTypeAllocator, ROSMessageTypeDeleter>
+        >(subscription_base);
+      if (nullptr == ros_message_subscription) {
+        throw std::runtime_error(
+                "failed to dynamic cast SubscriptionIntraProcessBase to "
+                "SubscriptionIntraProcessBuffer<MessageT, Alloc, Deleter>, or to "
+                "SubscriptionROSMsgIntraProcessBuffer<ROSMessageType,ROSMessageTypeAllocator,"
+                "ROSMessageTypeDeleter> which can happen when the publisher and "
+                "subscription use different allocator types, which is not supported");
+      }
+
+      if constexpr (rclcpp::TypeAdapter<MessageT>::is_specialized::value) {
+        ROSMessageType ros_msg;
+        rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*message, ros_msg);
+        ros_message_subscription->provide_intra_process_message(
+          std::make_shared<ROSMessageType>(ros_msg));
+      } else {
+        if constexpr (std::is_same<MessageT, ROSMessageType>::value) {
+          ros_message_subscription->provide_intra_process_message(message);
+        } else {
+          if constexpr (std::is_same<typename rclcpp::TypeAdapter<MessageT,
+            ROSMessageType>::ros_message_type, ROSMessageType>::value)
+          {
+            ROSMessageType ros_msg;
+            rclcpp::TypeAdapter<MessageT, ROSMessageType>::convert_to_ros_message(
+              *message, ros_msg);
+            ros_message_subscription->provide_intra_process_message(
+              std::make_shared<ROSMessageType>(ros_msg));
+          }
+        }
+      }
+    }
+  }
+
+  template<
+    typename MessageT,
+    typename Alloc,
+    typename Deleter,
+    typename ROSMessageType>
+  void
+  add_owned_msg_to_buffers(
+    std::unique_ptr<MessageT, Deleter> message,
+    std::vector<uint64_t> subscription_ids,
+    typename allocator::AllocRebind<MessageT, Alloc>::allocator_type & allocator)
+  {
+    using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
+    using MessageUniquePtr = std::unique_ptr<MessageT, Deleter>;
+
+    using ROSMessageTypeAllocatorTraits = allocator::AllocRebind<ROSMessageType, Alloc>;
+    using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
+    using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>;
+
+    using PublishedType = typename rclcpp::TypeAdapter<MessageT>::custom_type;
+    using PublishedTypeAllocatorTraits = allocator::AllocRebind<PublishedType, Alloc>;
+    using PublishedTypeAllocator = typename PublishedTypeAllocatorTraits::allocator_type;
+    using PublishedTypeDeleter = allocator::Deleter<PublishedTypeAllocator, PublishedType>;
+
+    for (auto it = subscription_ids.begin(); it != subscription_ids.end(); it++) {
+      auto subscription_it = subscriptions_.find(*it);
+      if (subscription_it == subscriptions_.end()) {
+        throw std::runtime_error("subscription has unexpectedly gone out of scope");
+      }
+      auto subscription_base = subscription_it->second.lock();
+      if (subscription_base == nullptr) {
+        subscriptions_.erase(subscription_it);
+        continue;
+      }
+
+      auto subscription = std::dynamic_pointer_cast<
+        rclcpp::experimental::SubscriptionIntraProcessBuffer<PublishedType,
+        PublishedTypeAllocator, PublishedTypeDeleter, ROSMessageType>
+        >(subscription_base);
+      if (subscription != nullptr) {
+        if (std::next(it) == subscription_ids.end()) {
+          // If this is the last subscription, give up ownership
+          subscription->provide_intra_process_data(std::move(message));
+          // Last message delivered, break from for loop
+          break;
+        } else {
+          // Copy the message since we have additional subscriptions to serve
+          Deleter deleter = message.get_deleter();
+          auto ptr = MessageAllocTraits::allocate(allocator, 1);
+          MessageAllocTraits::construct(allocator, ptr, *message);
+
+          subscription->provide_intra_process_data(MessageUniquePtr(ptr, deleter));
+        }
+
+        continue;
+      }
+
+      auto ros_message_subscription = std::dynamic_pointer_cast<
+        rclcpp::experimental::SubscriptionROSMsgIntraProcessBuffer<ROSMessageType,
+        ROSMessageTypeAllocator, ROSMessageTypeDeleter>
+        >(subscription_base);
+      if (nullptr == ros_message_subscription) {
+        throw std::runtime_error(
+                "failed to dynamic cast SubscriptionIntraProcessBase to "
+                "SubscriptionIntraProcessBuffer<MessageT, Alloc, Deleter>, or to "
+                "SubscriptionROSMsgIntraProcessBuffer<ROSMessageType,ROSMessageTypeAllocator,"
+                "ROSMessageTypeDeleter> which can happen when the publisher and "
+                "subscription use different allocator types, which is not supported");
+      }
+
+      if constexpr (rclcpp::TypeAdapter<MessageT, ROSMessageType>::is_specialized::value) {
+        ROSMessageTypeAllocator ros_message_alloc(allocator);
+        auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_alloc, 1);
+        ROSMessageTypeAllocatorTraits::construct(ros_message_alloc, ptr);
+        ROSMessageTypeDeleter deleter;
+        allocator::set_allocator_for_deleter(&deleter, &allocator);
+        rclcpp::TypeAdapter<MessageT, ROSMessageType>::convert_to_ros_message(*message, *ptr);
+        auto ros_msg = std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>(ptr, deleter);
+        ros_message_subscription->provide_intra_process_message(std::move(ros_msg));
+      } else {
+        if constexpr (std::is_same<MessageT, ROSMessageType>::value) {
+          if (std::next(it) == subscription_ids.end()) {
+            // If this is the last subscription, give up ownership
+            ros_message_subscription->provide_intra_process_message(std::move(message));
+            // Last message delivered, break from for loop
+            break;
+          } else {
+            // Copy the message since we have additional subscriptions to serve
+            Deleter deleter = message.get_deleter();
+            allocator::set_allocator_for_deleter(&deleter, &allocator);
+            auto ptr = MessageAllocTraits::allocate(allocator, 1);
+            MessageAllocTraits::construct(allocator, ptr, *message);
+
+            ros_message_subscription->provide_intra_process_message(
+              MessageUniquePtr(ptr, deleter));
+          }
+        }
+      }
+    }
+  }
+
+  PublisherToSubscriptionIdsMap pub_to_subs_;
+  SubscriptionMap subscriptions_;
+  PublisherMap publishers_;
+  PublisherBufferMap publisher_buffers_;
+
+  mutable std::shared_timed_mutex mutex_;
+};
+
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__INTRA_PROCESS_MANAGER_HPP_

rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp

@@ -0,0 +1,68 @@
+// Copyright 2021 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__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_
+#define RCLCPP__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rcl/error_handling.h"
+
+#include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/context.hpp"
+#include "rclcpp/experimental/subscription_intra_process_base.hpp"
+#include "tracetools/tracetools.h"
+
+namespace rclcpp
+{
+namespace experimental
+{
+
+template<
+  typename RosMessageT,
+  typename Alloc = std::allocator<void>,
+  typename Deleter = std::default_delete<void>
+>
+class SubscriptionROSMsgIntraProcessBuffer : public SubscriptionIntraProcessBase
+{
+public:
+  using ROSMessageTypeAllocatorTraits = allocator::AllocRebind<RosMessageT, Alloc>;
+  using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
+  using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, RosMessageT>;
+
+  using ConstMessageSharedPtr = std::shared_ptr<const RosMessageT>;
+  using MessageUniquePtr = std::unique_ptr<RosMessageT, ROSMessageTypeDeleter>;
+
+  SubscriptionROSMsgIntraProcessBuffer(
+    rclcpp::Context::SharedPtr context,
+    const std::string & topic_name,
+    const rclcpp::QoS & qos_profile)
+  : SubscriptionIntraProcessBase(context, topic_name, qos_profile)
+  {}
+
+  virtual ~SubscriptionROSMsgIntraProcessBuffer()
+  {}
+
+  virtual void
+  provide_intra_process_message(ConstMessageSharedPtr message) = 0;
+
+  virtual void
+  provide_intra_process_message(MessageUniquePtr message) = 0;
+};
+
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -0,0 +1,219 @@
+// 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__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_
+#define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_
+
+#include <rmw/types.h>
+
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <type_traits>
+#include <utility>
+
+#include "rcl/types.h"
+
+#include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/context.hpp"
+#include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
+#include "rclcpp/experimental/subscription_intra_process_buffer.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/type_support_decl.hpp"
+#include "tracetools/tracetools.h"
+
+namespace rclcpp
+{
+namespace experimental
+{
+
+template<
+  typename MessageT,
+  typename SubscribedType,
+  typename SubscribedTypeAlloc = std::allocator<SubscribedType>,
+  typename SubscribedTypeDeleter = std::default_delete<SubscribedType>,
+  typename ROSMessageType = SubscribedType,
+  typename Alloc = std::allocator<void>
+>
+class SubscriptionIntraProcess
+  : public SubscriptionIntraProcessBuffer<
+    SubscribedType,
+    SubscribedTypeAlloc,
+    SubscribedTypeDeleter,
+    ROSMessageType
+  >
+{
+  using SubscriptionIntraProcessBufferT = SubscriptionIntraProcessBuffer<
+    SubscribedType,
+    SubscribedTypeAlloc,
+    SubscribedTypeDeleter,
+    ROSMessageType
+  >;
+
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess)
+
+  using MessageAllocTraits =
+    typename SubscriptionIntraProcessBufferT::SubscribedTypeAllocatorTraits;
+  using MessageAlloc = typename SubscriptionIntraProcessBufferT::SubscribedTypeAllocator;
+  using ConstMessageSharedPtr = typename SubscriptionIntraProcessBufferT::ConstDataSharedPtr;
+  using MessageUniquePtr = typename SubscriptionIntraProcessBufferT::SubscribedTypeUniquePtr;
+  using BufferUniquePtr = typename SubscriptionIntraProcessBufferT::BufferUniquePtr;
+
+  SubscriptionIntraProcess(
+    AnySubscriptionCallback<MessageT, Alloc> callback,
+    std::shared_ptr<Alloc> allocator,
+    rclcpp::Context::SharedPtr context,
+    const std::string & topic_name,
+    const rclcpp::QoS & qos_profile,
+    rclcpp::IntraProcessBufferType buffer_type)
+  : SubscriptionIntraProcessBuffer<SubscribedType, SubscribedTypeAlloc,
+      SubscribedTypeDeleter, ROSMessageType>(
+      std::make_shared<SubscribedTypeAlloc>(*allocator),
+      context,
+      topic_name,
+      qos_profile,
+      buffer_type),
+    any_callback_(callback)
+  {
+    TRACETOOLS_TRACEPOINT(
+      rclcpp_subscription_callback_added,
+      static_cast<const void *>(this),
+      static_cast<const void *>(&any_callback_));
+    // The callback object gets copied, so if registration is done too early/before this point
+    // (e.g. in `AnySubscriptionCallback::set()`), its address won't match any address used later
+    // in subsequent tracepoints.
+#ifndef TRACETOOLS_DISABLED
+    any_callback_.register_callback_for_tracing();
+#endif
+  }
+
+  virtual ~SubscriptionIntraProcess() = default;
+
+  void
+  add_to_wait_set(rcl_wait_set_t & wait_set) override
+  {
+    // This block is necessary when the guard condition wakes the wait set, but
+    // the intra process waitable was not handled before the wait set is waited
+    // on again.
+    // Basically we're keeping the guard condition triggered so long as there is
+    // data in the buffer.
+    if (this->buffer_->has_data()) {
+      // If there is data still to be processed, indicate to the
+      // executor or waitset by triggering the guard condition.
+      this->trigger_guard_condition();
+    }
+    // Let the parent classes handle the rest of the work:
+    return SubscriptionIntraProcessBufferT::add_to_wait_set(wait_set);
+  }
+
+  std::shared_ptr<void>
+  take_data() override
+  {
+    ConstMessageSharedPtr shared_msg;
+    MessageUniquePtr unique_msg;
+
+    if (any_callback_.use_take_shared_method()) {
+      shared_msg = this->buffer_->consume_shared();
+      if (!shared_msg) {
+        return nullptr;
+      }
+    } else {
+      unique_msg = this->buffer_->consume_unique();
+      if (!unique_msg) {
+        return nullptr;
+      }
+    }
+
+    if (this->buffer_->has_data()) {
+      // If there is data still to be processed, indicate to the
+      // executor or waitset by triggering the guard condition.
+      this->trigger_guard_condition();
+    }
+
+    return std::static_pointer_cast<void>(
+      std::make_shared<std::pair<ConstMessageSharedPtr, MessageUniquePtr>>(
+        std::pair<ConstMessageSharedPtr, MessageUniquePtr>(
+          shared_msg, std::move(unique_msg)))
+    );
+  }
+
+  void execute(const std::shared_ptr<void> & data) override
+  {
+    execute_impl<SubscribedType>(data);
+  }
+
+  /// Disable callbacks from being called
+  /**
+    * This method will block, until any subscription's callbacks currently being executed are
+    * finished.
+    * This method is thread safe, and provides a safe way to atomically disable the callbacks.
+    */
+  void disable_callbacks() override
+  {
+    SubscriptionIntraProcessBase::disable_callbacks();
+    any_callback_.disable();
+  }
+
+  /// Enable the callbacks to be called
+  /**
+    * This method is thread safe, and provides a safe way to atomically enable the callbacks
+    * in a multithreaded environment.
+    */
+  void enable_callbacks() override
+  {
+    SubscriptionIntraProcessBase::enable_callbacks();
+    any_callback_.enable();
+  }
+
+protected:
+  template<typename T>
+  typename std::enable_if<std::is_same<T, rcl_serialized_message_t>::value, void>::type
+  execute_impl(const std::shared_ptr<void> &)
+  {
+    throw std::runtime_error("Subscription intra-process can't handle serialized messages");
+  }
+
+  template<class T>
+  typename std::enable_if<!std::is_same<T, rcl_serialized_message_t>::value, void>::type
+  execute_impl(const std::shared_ptr<void> & data)
+  {
+    if (nullptr == data) {
+      return;
+    }
+
+    rmw_message_info_t msg_info;
+    msg_info.publisher_gid = {0, {0}};
+    msg_info.from_intra_process = true;
+
+    auto shared_ptr = std::static_pointer_cast<std::pair<ConstMessageSharedPtr, MessageUniquePtr>>(
+      data);
+
+    if (any_callback_.use_take_shared_method()) {
+      ConstMessageSharedPtr shared_msg = shared_ptr->first;
+      any_callback_.dispatch_intra_process(shared_msg, msg_info);
+    } else {
+      MessageUniquePtr unique_msg = std::move(shared_ptr->second);
+      any_callback_.dispatch_intra_process(std::move(unique_msg), msg_info);
+    }
+    shared_ptr.reset();
+  }
+
+  AnySubscriptionCallback<MessageT, Alloc> any_callback_;
+};
+
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_

rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp

@@ -0,0 +1,237 @@
+// 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__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_
+#define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_
+
+#include <algorithm>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+#include "rcl/wait.h"
+#include "rmw/impl/cpp/demangle.hpp"
+
+#include "rclcpp/guard_condition.hpp"
+#include "rclcpp/logging.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+
+class SubscriptionIntraProcessBase : public rclcpp::Waitable
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase)
+
+  enum class EntityType : std::size_t
+  {
+    Subscription,
+  };
+
+  RCLCPP_PUBLIC
+  SubscriptionIntraProcessBase(
+    rclcpp::Context::SharedPtr context,
+    const std::string & topic_name,
+    const rclcpp::QoS & qos_profile)
+  : gc_(context), topic_name_(topic_name), qos_profile_(qos_profile)
+  {}
+
+  RCLCPP_PUBLIC
+  virtual ~SubscriptionIntraProcessBase() = default;
+
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_ready_guard_conditions() override {return 1;}
+
+  RCLCPP_PUBLIC
+  void
+  add_to_wait_set(rcl_wait_set_t & wait_set) override;
+
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  available_capacity() const = 0;
+
+  RCLCPP_PUBLIC
+  bool
+  is_durability_transient_local() const;
+
+  bool
+  is_ready(const rcl_wait_set_t & wait_set) override = 0;
+
+  std::shared_ptr<void>
+  take_data() override = 0;
+
+  std::shared_ptr<void>
+  take_data_by_entity_id([[maybe_unused]] size_t id) override
+  {
+    return take_data();
+  }
+
+  void
+  execute(const std::shared_ptr<void> & data) override = 0;
+
+  /// Disable callbacks from being called
+  /**
+   * This function temporary disable on_new_message_callback to prevent it from being called.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void disable_callbacks();
+
+  /// Enable the callbacks to be called
+  /**
+    * This function enable the on_new_message_callback if it was previously set.
+    */
+  RCLCPP_PUBLIC
+  virtual
+  void enable_callbacks();
+
+  virtual
+  bool
+  use_take_shared_method() const = 0;
+
+  RCLCPP_PUBLIC
+  const char *
+  get_topic_name() const;
+
+  RCLCPP_PUBLIC
+  QoS
+  get_actual_qos() const;
+
+  /// Set a callback to be called when each new message arrives.
+  /**
+   * The callback receives a size_t which is the number of messages received
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if messages were received before any
+   * callback was set.
+   *
+   * The callback also receives an int identifier argument.
+   * This is needed because a Waitable may be composed of several distinct entities,
+   * such as subscriptions, services, etc.
+   * The application should provide a generic callback function that will be then
+   * forwarded by the waitable to all of its entities.
+   * Before forwarding, a different value for the identifier argument will be
+   * bound to the function.
+   * This implies that the provided callback can use the identifier to behave
+   * differently depending on which entity triggered the waitable to become ready.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * An exception will be thrown if the callback is not callable.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the subscription
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \param[in] callback functor to be called when a new message is received.
+   */
+  void
+  set_on_ready_callback(std::function<void(size_t, int)> callback) override
+  {
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_ready_callback "
+              "is not callable.");
+    }
+
+    // Note: we bind the int identifier argument to this waitable's entity types
+    auto new_callback =
+      [callback, this](size_t number_of_events) {
+        try {
+          callback(number_of_events, static_cast<int>(EntityType::Subscription));
+        } catch (const std::exception & exception) {
+          RCLCPP_ERROR_STREAM(
+            // TODO(wjwwood): get this class access to the node logger it is associated with
+            rclcpp::get_logger("rclcpp"),
+            "rclcpp::SubscriptionIntraProcessBase@" << this <<
+              " caught " << rmw::impl::cpp::demangle(exception) <<
+              " exception in user-provided callback for the 'on ready' callback: " <<
+              exception.what());
+        } catch (...) {
+          RCLCPP_ERROR_STREAM(
+            rclcpp::get_logger("rclcpp"),
+            "rclcpp::SubscriptionIntraProcessBase@" << this <<
+              " caught unhandled exception in user-provided callback " <<
+              "for the 'on ready' callback");
+        }
+      };
+
+    std::lock_guard<std::recursive_mutex> lock(on_new_message_callback_mutex_);
+    on_new_message_callback_ = new_callback;
+
+    if (unread_count_ > 0) {
+      if (qos_profile_.history() == HistoryPolicy::KeepAll) {
+        on_new_message_callback_(unread_count_);
+      } else {
+        // Use qos profile depth as upper bound for unread_count_
+        on_new_message_callback_(std::min(unread_count_, qos_profile_.depth()));
+      }
+      unread_count_ = 0;
+    }
+  }
+
+  /// Unset the callback registered for new messages, if any.
+  void
+  clear_on_ready_callback() override
+  {
+    std::lock_guard<std::recursive_mutex> lock(on_new_message_callback_mutex_);
+    on_new_message_callback_ = nullptr;
+  }
+
+  RCLCPP_PUBLIC
+  std::vector<std::shared_ptr<rclcpp::TimerBase>>
+  get_timers() const override
+  {
+    return {};
+  }
+
+protected:
+  std::recursive_mutex on_new_message_callback_mutex_;
+  std::function<void(size_t)> on_new_message_callback_ {nullptr};
+  bool on_new_message_callback_disabled_{false};
+  size_t unread_count_{0};
+  rclcpp::GuardCondition gc_;
+
+  virtual void
+  trigger_guard_condition() = 0;
+
+  void
+  invoke_on_new_message()
+  {
+    std::lock_guard<std::recursive_mutex> lock(this->on_new_message_callback_mutex_);
+    if (!on_new_message_callback_disabled_) {
+      if (this->on_new_message_callback_) {
+        this->on_new_message_callback_(1);
+      } else {
+        this->unread_count_++;
+      }
+    }
+  }
+
+private:
+  std::string topic_name_;
+  QoS qos_profile_;
+};
+
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_

rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp

@@ -0,0 +1,201 @@
+// Copyright 2021 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__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_
+#define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_
+
+#include <memory>
+#include <string>
+#include <stdexcept>
+#include <utility>
+
+#include "rcl/error_handling.h"
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
+
+#include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
+#include "rclcpp/experimental/create_intra_process_buffer.hpp"
+#include "rclcpp/experimental/subscription_intra_process_base.hpp"
+#include "rclcpp/experimental/ros_message_intra_process_buffer.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp"
+
+#include "tracetools/tracetools.h"
+
+namespace rclcpp
+{
+namespace experimental
+{
+
+template<
+  typename SubscribedType,
+  typename Alloc = std::allocator<SubscribedType>,
+  typename Deleter = std::default_delete<SubscribedType>,
+  /// MessageT::ros_message_type if MessageT is a TypeAdapter,
+  /// otherwise just MessageT.
+  typename ROSMessageType = SubscribedType
+>
+class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuffer<ROSMessageType,
+    typename allocator::AllocRebind<ROSMessageType, Alloc>::allocator_type,
+    allocator::Deleter<typename allocator::AllocRebind<ROSMessageType, Alloc>::allocator_type,
+    ROSMessageType>>
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcessBuffer)
+
+  using SubscribedTypeAllocatorTraits = allocator::AllocRebind<SubscribedType, Alloc>;
+  using SubscribedTypeAllocator = typename SubscribedTypeAllocatorTraits::allocator_type;
+  using SubscribedTypeDeleter = allocator::Deleter<SubscribedTypeAllocator, SubscribedType>;
+
+  using ROSMessageTypeAllocatorTraits = allocator::AllocRebind<ROSMessageType, Alloc>;
+  using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
+  using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>;
+
+  using ConstMessageSharedPtr = std::shared_ptr<const ROSMessageType>;
+  using MessageUniquePtr = std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>;
+
+  using ConstDataSharedPtr = std::shared_ptr<const SubscribedType>;
+  using SubscribedTypeUniquePtr = std::unique_ptr<SubscribedType, SubscribedTypeDeleter>;
+
+  using BufferUniquePtr = typename rclcpp::experimental::buffers::IntraProcessBuffer<
+    SubscribedType,
+    Alloc,
+    SubscribedTypeDeleter
+    >::UniquePtr;
+
+  SubscriptionIntraProcessBuffer(
+    std::shared_ptr<Alloc> allocator,
+    rclcpp::Context::SharedPtr context,
+    const std::string & topic_name,
+    const rclcpp::QoS & qos_profile,
+    rclcpp::IntraProcessBufferType buffer_type)
+  : SubscriptionROSMsgIntraProcessBuffer<ROSMessageType, ROSMessageTypeAllocator,
+      ROSMessageTypeDeleter>(
+      context, topic_name, qos_profile),
+    subscribed_type_allocator_(*allocator)
+  {
+    allocator::set_allocator_for_deleter(&subscribed_type_deleter_, &subscribed_type_allocator_);
+
+    // Create the intra-process buffer.
+    buffer_ = rclcpp::experimental::create_intra_process_buffer<SubscribedType, Alloc,
+        SubscribedTypeDeleter>(
+      buffer_type,
+      qos_profile,
+      std::make_shared<Alloc>(subscribed_type_allocator_));
+    TRACETOOLS_TRACEPOINT(
+      rclcpp_ipb_to_subscription,
+      static_cast<const void *>(buffer_.get()),
+      static_cast<const void *>(this));
+  }
+
+  void
+  add_to_wait_set(rcl_wait_set_t & wait_set) override
+  {
+    if (this->buffer_->has_data()) {
+      this->trigger_guard_condition();
+    }
+    detail::add_guard_condition_to_rcl_wait_set(wait_set, this->gc_);
+  }
+
+  bool
+  is_ready([[maybe_unused]] const rcl_wait_set_t & wait_set) override
+  {
+    return buffer_->has_data();
+  }
+
+  SubscribedTypeUniquePtr
+  convert_ros_message_to_subscribed_type_unique_ptr(const ROSMessageType & msg)
+  {
+    if constexpr (!std::is_same<SubscribedType, ROSMessageType>::value) {
+      auto ptr = SubscribedTypeAllocatorTraits::allocate(subscribed_type_allocator_, 1);
+      SubscribedTypeAllocatorTraits::construct(subscribed_type_allocator_, ptr);
+      rclcpp::TypeAdapter<SubscribedType, ROSMessageType>::convert_to_custom(msg, *ptr);
+      return SubscribedTypeUniquePtr(ptr, subscribed_type_deleter_);
+    } else {
+      throw std::runtime_error(
+              "convert_ros_message_to_subscribed_type_unique_ptr "
+              "unexpectedly called without TypeAdapter");
+    }
+  }
+
+  void
+  provide_intra_process_message(ConstMessageSharedPtr message) override
+  {
+    if constexpr (std::is_same<SubscribedType, ROSMessageType>::value) {
+      buffer_->add_shared(std::move(message));
+      trigger_guard_condition();
+    } else {
+      buffer_->add_shared(convert_ros_message_to_subscribed_type_unique_ptr(*message));
+      trigger_guard_condition();
+    }
+    this->invoke_on_new_message();
+  }
+
+  void
+  provide_intra_process_message(MessageUniquePtr message) override
+  {
+    if constexpr (std::is_same<SubscribedType, ROSMessageType>::value) {
+      buffer_->add_unique(std::move(message));
+      trigger_guard_condition();
+    } else {
+      buffer_->add_unique(convert_ros_message_to_subscribed_type_unique_ptr(*message));
+      trigger_guard_condition();
+    }
+    this->invoke_on_new_message();
+  }
+
+  void
+  provide_intra_process_data(ConstDataSharedPtr message)
+  {
+    buffer_->add_shared(std::move(message));
+    trigger_guard_condition();
+    this->invoke_on_new_message();
+  }
+
+  void
+  provide_intra_process_data(SubscribedTypeUniquePtr message)
+  {
+    buffer_->add_unique(std::move(message));
+    trigger_guard_condition();
+    this->invoke_on_new_message();
+  }
+
+  bool
+  use_take_shared_method() const override
+  {
+    return buffer_->use_take_shared_method();
+  }
+
+  size_t available_capacity() const override
+  {
+    return buffer_->available_capacity();
+  }
+
+protected:
+  void
+  trigger_guard_condition() override
+  {
+    this->gc_.trigger();
+  }
+
+  BufferUniquePtr buffer_;
+  SubscribedTypeAllocator subscribed_type_allocator_;
+  SubscribedTypeDeleter subscribed_type_deleter_;
+};
+
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_

rclcpp/include/rclcpp/experimental/timers_manager.hpp

@@ -0,0 +1,558 @@
+// Copyright 2023 iRobot Corporation.
+//
+// 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__EXPERIMENTAL__TIMERS_MANAGER_HPP_
+#define RCLCPP__EXPERIMENTAL__TIMERS_MANAGER_HPP_
+
+#include <algorithm>
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <optional>
+#include <thread>
+#include <utility>
+#include <vector>
+#include "rclcpp/context.hpp"
+#include "rclcpp/timer.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+
+/**
+ * @brief This class provides a way for storing and executing timer objects.
+ * It provides APIs to suit the needs of different applications and execution models.
+ * All public APIs provided by this class are thread-safe.
+ *
+ * Timers management
+ * This class provides APIs to add/remove timers to/from an internal storage.
+ * It keeps a list of weak pointers from added timers, and locks them only when
+ * they need to be executed or modified.
+ * Timers are kept ordered in a binary-heap priority queue.
+ * Calls to add/remove APIs will temporarily block the execution of the timers and
+ * will require to reorder the internal priority queue.
+ * Because of this, they have a not-negligible impact on the performance.
+ *
+ * Timers execution
+ * The most efficient use of this class consists in letting a TimersManager object
+ * to spawn a thread where timers are monitored and optionally executed.
+ * This can be controlled via the `start` and `stop` methods.
+ * Ready timers can either be executed or an on_ready_callback can be used to notify
+ * other entities that they are ready and need to be executed.
+ * Other APIs allow to directly execute a given timer.
+ *
+ * This class assumes that the `execute_callback()` API of the stored timers is never
+ * called by other entities, but it can only be called from here.
+ * If this assumption is not respected, the heap property may be invalidated,
+ * so timers may be executed out of order, without this object noticing it.
+ *
+ */
+class TimersManager
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimersManager)
+
+  /**
+   * @brief Construct a new TimersManager object
+   *
+   * @param context custom context to be used.
+   * Shared ownership of the context is held until destruction.
+   * @param on_ready_callback The timers on ready callback. The presence of this function
+   * indicates what to do when the TimersManager is running and a timer becomes ready.
+   * The TimersManager is considered "running" when the `start` method has been called.
+   * If it's callable, it will be invoked instead of the timer callback.
+   * If it's not callable, then the TimersManager will
+   * directly execute timers when they are ready.
+   * All the methods that execute a given timer (e.g. `execute_head_timer`
+   * or `execute_ready_timer`) without the TimersManager being `running`, i.e.
+   * without actually explicitly waiting for the timer to become ready, will ignore this
+   * callback.
+   */
+  RCLCPP_PUBLIC
+  TimersManager(
+    std::shared_ptr<rclcpp::Context> context,
+    std::function<void(const rclcpp::TimerBase *,
+    const std::shared_ptr<void> &)> on_ready_callback = nullptr);
+
+  /**
+   * @brief Destruct the TimersManager object making sure to stop thread and release memory.
+   */
+  RCLCPP_PUBLIC
+  ~TimersManager();
+
+  /**
+   * @brief Adds a new timer to the storage, maintaining weak ownership of it.
+   * Function is thread safe and it can be called regardless of the state of the timers thread.
+   *
+   * @param timer the timer to add.
+   * @throws std::invalid_argument if timer is a nullptr.
+   */
+  RCLCPP_PUBLIC
+  void add_timer(rclcpp::TimerBase::SharedPtr timer);
+
+  /**
+   * @brief Remove a single timer from the object storage.
+   * Will do nothing if the timer was not being stored here.
+   * Function is thread safe and it can be called regardless of the state of the timers thread.
+   *
+   * @param timer the timer to remove.
+   */
+  RCLCPP_PUBLIC
+  void remove_timer(rclcpp::TimerBase::SharedPtr timer);
+
+  /**
+   * @brief Remove all the timers stored in the object.
+   * Function is thread safe and it can be called regardless of the state of the timers thread.
+   */
+  RCLCPP_PUBLIC
+  void clear();
+
+  /**
+   * @brief Starts a thread that takes care of executing the timers stored in this object.
+   * Function will throw an error if the timers thread was already running.
+   */
+  RCLCPP_PUBLIC
+  void start();
+
+  /**
+   * @brief Stops the timers thread.
+   * Will do nothing if the timer thread was not running.
+   */
+  RCLCPP_PUBLIC
+  void stop();
+
+  /**
+   * @brief Get the number of timers that are currently ready.
+   * This function is thread safe.
+   *
+   * @return size_t number of ready timers.
+   * @throws std::runtime_error if the timers thread was already running.
+   */
+  RCLCPP_PUBLIC
+  size_t get_number_ready_timers();
+
+  /**
+   * @brief Executes head timer if ready.
+   * This function is thread safe.
+   * This function will try to execute the timer callback regardless of whether
+   * the TimersManager on_ready_callback was passed during construction.
+   *
+   * @return true if head timer was ready.
+   * @throws std::runtime_error if the timers thread was already running.
+   */
+  RCLCPP_PUBLIC
+  bool execute_head_timer();
+
+  /**
+   * @brief Executes timer identified by its ID.
+   * This function is thread safe.
+   * This function will try to execute the timer callback regardless of whether
+   * the TimersManager on_ready_callback was passed during construction.
+   *
+   * @param timer_id the timer ID of the timer to execute
+   * @param data internal data of the timer
+   */
+  RCLCPP_PUBLIC
+  void execute_ready_timer(const rclcpp::TimerBase * timer_id, const std::shared_ptr<void> & data);
+
+  /**
+   * @brief Get the amount of time before the next timer triggers.
+   * This function is thread safe.
+   *
+   * @return std::optional<std::chrono::nanoseconds> to wait,
+   * the returned value could be negative if the timer is already expired
+   * or std::chrono::nanoseconds::max() if there are no timers stored in the object.
+   * If the head timer was cancelled, then this will return a nullopt.
+   * @throws std::runtime_error if the timers thread was already running.
+   */
+  RCLCPP_PUBLIC
+  std::optional<std::chrono::nanoseconds> get_head_timeout();
+
+private:
+  RCLCPP_DISABLE_COPY(TimersManager)
+
+  using TimerPtr = rclcpp::TimerBase::SharedPtr;
+  using WeakTimerPtr = rclcpp::TimerBase::WeakPtr;
+
+  // Forward declaration
+  class TimersHeap;
+
+  /**
+   * @brief This class allows to store weak pointers to timers in a heap-like data structure.
+   * The root of the heap is the timer that triggers first.
+   * Since this class uses weak ownership, it is not guaranteed that it represents a valid heap
+   * at any point in time as timers could go out of scope, thus invalidating it.
+   * The "validate_and_lock" API allows to restore the heap property and also returns a locked version
+   * of the timers heap.
+   * This class is not thread safe and requires external mutexes to protect its usage.
+   */
+  class WeakTimersHeap
+  {
+public:
+    /**
+     * @brief Add a new timer to the heap. After the addition, the heap property is enforced.
+     *
+     * @param timer new timer to add.
+     * @return true if timer has been added, false if it was already there.
+     */
+    bool add_timer(TimerPtr timer)
+    {
+      TimersHeap locked_heap = this->validate_and_lock();
+      bool added = locked_heap.add_timer(std::move(timer));
+
+      if (added) {
+        // Re-create the weak heap with the new timer added
+        this->store(locked_heap);
+      }
+
+      return added;
+    }
+
+    /**
+     * @brief Remove a timer from the heap. After the removal, the heap property is enforced.
+     *
+     * @param timer timer to remove.
+     * @return true if timer has been removed, false if it was not there.
+     */
+    bool remove_timer(TimerPtr timer)
+    {
+      TimersHeap locked_heap = this->validate_and_lock();
+      bool removed = locked_heap.remove_timer(std::move(timer));
+
+      if (removed) {
+        // Re-create the weak heap with the timer removed
+        this->store(locked_heap);
+      }
+
+      return removed;
+    }
+
+    /**
+     * @brief Retrieve the timer identified by the key
+     * @param timer_id The ID of the timer to retrieve.
+     * @return TimerPtr if there's a timer associated with the ID, nullptr otherwise
+     */
+    TimerPtr get_timer(const rclcpp::TimerBase * timer_id)
+    {
+      for (auto & weak_timer : weak_heap_) {
+        auto timer = weak_timer.lock();
+        if (timer.get() == timer_id) {
+          return timer;
+        }
+      }
+      return nullptr;
+    }
+
+    /**
+     * @brief Returns a const reference to the front element.
+     */
+    const WeakTimerPtr & front() const
+    {
+      return weak_heap_.front();
+    }
+
+    /**
+     * @brief Returns whether the heap is empty or not.
+     */
+    bool empty() const
+    {
+      return weak_heap_.empty();
+    }
+
+    /**
+     * @brief This function restores the current object as a valid heap
+     * and it returns a locked version of it.
+     * Timers that went out of scope are removed from the container.
+     * It is the only public API to access and manipulate the stored timers.
+     *
+     * @return TimersHeap owned timers corresponding to the current object
+     */
+    TimersHeap validate_and_lock()
+    {
+      TimersHeap locked_heap;
+      bool any_timer_destroyed = false;
+
+      for (auto weak_timer : weak_heap_) {
+        auto timer = weak_timer.lock();
+        if (timer) {
+          // This timer is valid, so add it to the locked heap
+          // Note: we access friend private `owned_heap_` member field.
+          locked_heap.owned_heap_.push_back(std::move(timer));
+        } else {
+          // This timer went out of scope, so we don't add it to locked heap
+          // and we mark the corresponding flag.
+          // It's not needed to erase it from weak heap, as we are going to re-heapify.
+          // Note: we can't exit from the loop here, as we need to find all valid timers.
+          any_timer_destroyed = true;
+        }
+      }
+
+      // If a timer has gone out of scope, then the remaining elements do not represent
+      // a valid heap anymore. We need to re-heapify the timers heap.
+      if (any_timer_destroyed) {
+        locked_heap.heapify();
+        // Re-create the weak heap now that elements have been heapified again
+        this->store(locked_heap);
+      }
+
+      return locked_heap;
+    }
+
+    /**
+     * @brief This function allows to recreate the heap of weak pointers
+     * from an heap of owned pointers.
+     * It is required to be called after a locked TimersHeap generated from this object
+     * has been modified in any way (e.g. timers triggered, added, removed).
+     *
+     * @param heap timers heap to store as weak pointers
+     */
+    void store(const TimersHeap & heap)
+    {
+      weak_heap_.clear();
+      // Note: we access friend private `owned_heap_` member field.
+      for (auto t : heap.owned_heap_) {
+        weak_heap_.push_back(t);
+      }
+    }
+
+    /**
+     * @brief Remove all timers from the heap.
+     */
+    void clear()
+    {
+      weak_heap_.clear();
+    }
+
+private:
+    std::vector<WeakTimerPtr> weak_heap_;
+  };
+
+  /**
+   * @brief This class is the equivalent of WeakTimersHeap but with ownership of the timers.
+   * It can be generated by locking the weak version.
+   * It provides operations to manipulate the heap.
+   * This class is not thread safe and requires external mutexes to protect its usage.
+   */
+  class TimersHeap
+  {
+public:
+    /**
+     * @brief Try to add a new timer to the heap.
+     * After the addition, the heap property is preserved.
+     * @param timer new timer to add.
+     * @return true if timer has been added, false if it was already there.
+     */
+    bool add_timer(TimerPtr timer)
+    {
+      // Nothing to do if the timer is already stored here
+      auto it = std::find(owned_heap_.begin(), owned_heap_.end(), timer);
+      if (it != owned_heap_.end()) {
+        return false;
+      }
+
+      owned_heap_.push_back(std::move(timer));
+      std::push_heap(owned_heap_.begin(), owned_heap_.end(), timer_greater);
+
+      return true;
+    }
+
+    /**
+     * @brief Try to remove a timer from the heap.
+     * After the removal, the heap property is preserved.
+     * @param timer timer to remove.
+     * @return true if timer has been removed, false if it was not there.
+     */
+    bool remove_timer(TimerPtr timer)
+    {
+      // Nothing to do if the timer is not stored here
+      auto it = std::find(owned_heap_.begin(), owned_heap_.end(), timer);
+      if (it == owned_heap_.end()) {
+        return false;
+      }
+
+      owned_heap_.erase(it);
+      this->heapify();
+
+      return true;
+    }
+
+    /**
+     * @brief Returns a reference to the front element.
+     * @return reference to front element.
+     */
+    TimerPtr & front()
+    {
+      return owned_heap_.front();
+    }
+
+    /**
+     * @brief Returns a const reference to the front element.
+     * @return const reference to front element.
+     */
+    const TimerPtr & front() const
+    {
+      return owned_heap_.front();
+    }
+
+    /**
+     * @brief Returns whether the heap is empty or not.
+     * @return true if the heap is empty.
+     */
+    bool empty() const
+    {
+      return owned_heap_.empty();
+    }
+
+    /**
+     * @brief Returns the size of the heap.
+     * @return the number of valid timers in the heap.
+     */
+    size_t size() const
+    {
+      return owned_heap_.size();
+    }
+
+    /**
+     * @brief Get the number of timers that are currently ready.
+     * @return size_t number of ready timers.
+     */
+    size_t get_number_ready_timers() const
+    {
+      size_t ready_timers = 0;
+
+      for (TimerPtr t : owned_heap_) {
+        if (t->is_ready()) {
+          ready_timers++;
+        }
+      }
+
+      return ready_timers;
+    }
+
+    /**
+    * @brief Restore a valid heap after the root value has been replaced (e.g. timer triggered).
+    */
+    void heapify_root()
+    {
+      // The following code is a more efficient version than doing
+      // pop_heap, pop_back, push_back, push_heap
+      // as it removes the need for the last push_heap
+
+      // Push the modified element (i.e. the current root) at the bottom of the heap
+      owned_heap_.push_back(owned_heap_[0]);
+      // Exchange first and last-1 elements and reheapify
+      std::pop_heap(owned_heap_.begin(), owned_heap_.end(), timer_greater);
+      // Remove last element
+      owned_heap_.pop_back();
+    }
+
+    /**
+     * @brief Completely restores the structure to a valid heap
+     */
+    void heapify()
+    {
+      std::make_heap(owned_heap_.begin(), owned_heap_.end(), timer_greater);
+    }
+
+    /**
+     * @brief Helper function to clear the "on_reset_callback" on all associated timers.
+     */
+    void clear_timers_on_reset_callbacks()
+    {
+      for (TimerPtr & t : owned_heap_) {
+        t->clear_on_reset_callback();
+      }
+    }
+
+    /**
+     * @brief Friend declaration to allow the `validate_and_lock()` function to access the
+     * underlying heap container
+     */
+    friend TimersHeap WeakTimersHeap::validate_and_lock();
+
+    /**
+     * @brief Friend declaration to allow the `store()` function to access the
+     * underlying heap container
+     */
+    friend void WeakTimersHeap::store(const TimersHeap & heap);
+
+private:
+    /**
+     * @brief Comparison function between timers.
+     * @return true if `a` triggers after `b`.
+     */
+    static bool timer_greater(TimerPtr a, TimerPtr b)
+    {
+      // TODO(alsora): this can cause an error if timers are using different clocks
+      return a->time_until_trigger() > b->time_until_trigger();
+    }
+
+    std::vector<TimerPtr> owned_heap_;
+  };
+
+  /**
+   * @brief Implements a loop that keeps executing ready timers.
+   * This function is executed in the timers thread.
+   */
+  void run_timers();
+
+  /**
+   * @brief Get the amount of time before the next timer triggers.
+   * This function is not thread safe, acquire a mutex before calling it.
+   *
+   * @return std::optional<std::chrono::nanoseconds> to wait,
+   * the returned value could be negative if the timer is already expired
+   * or std::chrono::nanoseconds::max() if the heap is empty.
+   * If the head timer was cancelled, then this will return a nullopt.
+   * This function is not thread safe, acquire the timers_mutex_ before calling it.
+   */
+  std::optional<std::chrono::nanoseconds> get_head_timeout_unsafe();
+
+  /**
+   * @brief Executes all the timers currently ready when the function is invoked
+   * while keeping the heap correctly sorted.
+   * This function is not thread safe, acquire the timers_mutex_ before calling it.
+   */
+  void execute_ready_timers_unsafe();
+
+  // Callback to be called when timer is ready
+  std::function<void(const rclcpp::TimerBase *,
+    const std::shared_ptr<void> &)> on_ready_callback_ = nullptr;
+
+  // Thread used to run the timers execution task
+  std::thread timers_thread_;
+  // Protects access to timers
+  std::mutex timers_mutex_;
+  // Protects access to stop()
+  std::mutex stop_mutex_;
+  // Notifies the timers thread whenever timers are added/removed
+  std::condition_variable timers_cv_;
+  // Flag used as predicate by timers_cv_ that denotes one or more timers being added/removed
+  bool timers_updated_ {false};
+  // Indicates whether the timers thread is currently running or not
+  std::atomic<bool> running_ {false};
+  // Parent context used to understand if ROS is still active
+  std::shared_ptr<rclcpp::Context> context_;
+  // Timers heap storage with weak ownership
+  WeakTimersHeap weak_timers_heap_;
+};
+
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__TIMERS_MANAGER_HPP_

rclcpp/include/rclcpp/function_traits.hpp

@@ -80,16 +80,36 @@ 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)
+#if defined DOXYGEN_ONLY
+struct function_traits<std::bind<ReturnTypeT (ClassT::*)(Args ...), FArgs ...>>
+#elif defined _LIBCPP_VERSION  // libc++ (Clang)
 struct function_traits<std::__bind<ReturnTypeT (ClassT::*)(Args ...), FArgs ...>>
 #elif defined _GLIBCXX_RELEASE  // glibc++ (GNU C++ >= 7.1)
-struct function_traits<std::_Bind<ReturnTypeT(ClassT::*(FArgs ...))(Args ...)>>
+struct function_traits<std::_Bind<ReturnTypeT(ClassT::* (FArgs ...))(Args ...)>>
 #elif defined __GLIBCXX__  // glibc++ (GNU C++)
 struct function_traits<std::_Bind<std::_Mem_fn<ReturnTypeT (ClassT::*)(Args ...)>(FArgs ...)>>
 #elif defined _MSC_VER  // MS Visual Studio
 struct function_traits<
-  std::_Binder<std::_Unforced, ReturnTypeT(__cdecl ClassT::*)(Args ...), FArgs ...>
->
+  std::_Binder<std::_Unforced, ReturnTypeT (ClassT::*)(Args ...), FArgs ...>>
+#else
+#error "Unsupported C++ compiler / standard library"
+#endif
+  : function_traits<ReturnTypeT(Args ...)>
+{};
+
+// std::bind for object const methods
+template<typename ClassT, typename ReturnTypeT, typename ... Args, typename ... FArgs>
+#if defined DOXYGEN_ONLY
+struct function_traits<std::bind<ReturnTypeT (ClassT::*)(Args ...) const, FArgs ...>>
+#elif defined _LIBCPP_VERSION  // libc++ (Clang)
+struct function_traits<std::__bind<ReturnTypeT (ClassT::*)(Args ...) const, FArgs ...>>
+#elif defined _GLIBCXX_RELEASE  // glibc++ (GNU C++ >= 7.1)
+struct function_traits<std::_Bind<ReturnTypeT(ClassT::* (FArgs ...))(Args ...) const>>
+#elif defined __GLIBCXX__  // glibc++ (GNU C++)
+struct function_traits<std::_Bind<std::_Mem_fn<ReturnTypeT (ClassT::*)(Args ...) const>(FArgs ...)>>
+#elif defined _MSC_VER  // MS Visual Studio
+struct function_traits<
+  std::_Binder<std::_Unforced, ReturnTypeT (ClassT::*)(Args ...) const, FArgs ...>>
 #else
 #error "Unsupported C++ compiler / standard library"
 #endif
@@ -98,12 +118,14 @@ struct function_traits<
 
 // std::bind for free functions
 template<typename ReturnTypeT, typename ... Args, typename ... FArgs>
-#if defined _LIBCPP_VERSION  // libc++ (Clang)
+#if defined DOXYGEN_ONLY
+struct function_traits<std::bind<ReturnTypeT( &)(Args ...), FArgs ...>>
+#elif defined _LIBCPP_VERSION  // libc++ (Clang)
 struct function_traits<std::__bind<ReturnTypeT( &)(Args ...), FArgs ...>>
 #elif defined __GLIBCXX__  // glibc++ (GNU C++)
 struct function_traits<std::_Bind<ReturnTypeT(*(FArgs ...))(Args ...)>>
 #elif defined _MSC_VER  // MS Visual Studio
-struct function_traits<std::_Binder<std::_Unforced, ReturnTypeT(__cdecl &)(Args ...), FArgs ...>>
+struct function_traits<std::_Binder<std::_Unforced, ReturnTypeT( &)(Args ...), FArgs ...>>
 #else
 #error "Unsupported C++ compiler / standard library"
 #endif
@@ -146,6 +168,32 @@ struct same_arguments : std::is_same<
 >
 {};
 
+namespace detail
+{
+
+template<typename ReturnTypeT, typename ... Args>
+struct as_std_function_helper;
+
+template<typename ReturnTypeT, typename ... Args>
+struct as_std_function_helper<ReturnTypeT, std::tuple<Args ...>>
+{
+  using type = std::function<ReturnTypeT(Args ...)>;
+};
+
+}  // namespace detail
+
+template<
+  typename FunctorT,
+  typename FunctionTraits = function_traits<FunctorT>
+>
+struct as_std_function
+{
+  using type = typename detail::as_std_function_helper<
+    typename FunctionTraits::return_type,
+    typename FunctionTraits::arguments
+    >::type;
+};
+
 }  // namespace function_traits
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/future_return_code.hpp

@@ -0,0 +1,47 @@
+// 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__FUTURE_RETURN_CODE_HPP_
+#define RCLCPP__FUTURE_RETURN_CODE_HPP_
+
+#include <iostream>
+#include <string>
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Return codes to be used with spin_until_future_complete.
+/**
+ * SUCCESS: The future is complete and can be accessed with "get" without blocking.
+ *          This does not indicate that the operation succeeded; "get" may still throw an exception.
+ * INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error.
+ * TIMEOUT: Spinning timed out.
+ */
+enum class FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT};
+
+/// Stream operator for FutureReturnCode.
+RCLCPP_PUBLIC
+std::ostream &
+operator<<(std::ostream & os, const FutureReturnCode & future_return_code);
+
+/// String conversion function for FutureReturnCode.
+RCLCPP_PUBLIC
+std::string
+to_string(const FutureReturnCode & future_return_code);
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__FUTURE_RETURN_CODE_HPP_

rclcpp/include/rclcpp/generic_client.hpp

@@ -0,0 +1,303 @@
+// Copyright 2023 Sony Group Corporation.
+//
+// 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__GENERIC_CLIENT_HPP_
+#define RCLCPP__GENERIC_CLIENT_HPP_
+
+#include <map>
+#include <memory>
+#include <future>
+#include <string>
+#include <tuple>
+#include <vector>
+#include <utility>
+
+#include "rcl/client.h"
+
+#include "rclcpp/client.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rcpputils/shared_library.hpp"
+
+#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp"
+
+namespace rclcpp
+{
+class GenericClient : public ClientBase
+{
+public:
+  using Request = void *;   // Deserialized data pointer of request message
+  using Response = void *;  // Deserialized data pointer of response message
+
+  using SharedResponse = std::shared_ptr<void>;
+
+  using Promise = std::promise<SharedResponse>;
+  using SharedPromise = std::shared_ptr<Promise>;
+
+  using Future = std::future<SharedResponse>;
+  using SharedFuture = std::shared_future<SharedResponse>;
+
+  using CallbackType = std::function<void (SharedFuture)>;
+
+  RCLCPP_SMART_PTR_DEFINITIONS(GenericClient)
+
+  /// A convenient GenericClient::Future and request id pair.
+  /**
+   * Public members:
+   * - future: a std::future<void *>.
+   * - request_id: the request id associated with the future.
+   *
+   * All the other methods are equivalent to the ones std::future provides.
+   */
+  struct FutureAndRequestId
+    : detail::FutureAndRequestId<Future>
+  {
+    using detail::FutureAndRequestId<Future>::FutureAndRequestId;
+
+    /// See std::future::share().
+    SharedFuture share() noexcept {return this->future.share();}
+
+    /// Move constructor.
+    FutureAndRequestId(FutureAndRequestId && other) noexcept = default;
+    /// Deleted copy constructor, each instance is a unique owner of the future.
+    FutureAndRequestId(const FutureAndRequestId & other) = delete;
+    /// Move assignment.
+    FutureAndRequestId & operator=(FutureAndRequestId && other) noexcept = default;
+    /// Deleted copy assignment, each instance is a unique owner of the future.
+    FutureAndRequestId & operator=(const FutureAndRequestId & other) = delete;
+    /// Destructor.
+    ~FutureAndRequestId() = default;
+  };
+
+  /// A convenient GenericClient::SharedFuture and request id pair.
+  /**
+   * Public members:
+   * - future: a std::shared_future<SharedResponse>.
+   * - request_id: the request id associated with the future.
+   *
+   * All the other methods are equivalent to the ones std::shared_future provides.
+   */
+  struct SharedFutureAndRequestId
+    : detail::FutureAndRequestId<std::shared_future<SharedResponse>>
+  {
+    using detail::FutureAndRequestId<std::shared_future<SharedResponse>>::FutureAndRequestId;
+  };
+
+  GenericClient(
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
+    const std::string & service_name,
+    const std::string & service_type,
+    rcl_client_options_t & client_options);
+
+  RCLCPP_PUBLIC
+  SharedResponse
+  create_response() override;
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<rmw_request_id_t>
+  create_request_header() override;
+
+  RCLCPP_PUBLIC
+  void
+  handle_response(
+    std::shared_ptr<rmw_request_id_t> request_header,
+    std::shared_ptr<void> response) override;
+
+  /// Send a request to the service server.
+  /**
+   * This method returns a `FutureAndRequestId` instance
+   * that can be passed to Executor::spin_until_future_complete() to
+   * wait until it has been completed.
+   *
+   * If the future never completes,
+   * e.g. the call to Executor::spin_until_future_complete() times out,
+   * GenericClient::remove_pending_request() must be called to clean the client internal state.
+   * Not doing so will make the `GenericClient` instance to use more memory each time a response is
+   * not received from the service server.
+   *
+   * ```cpp
+   * auto future = generic_client->async_send_request(my_request);
+   * if (
+   *   rclcpp::FutureReturnCode::TIMEOUT ==
+   *   executor->spin_until_future_complete(future, timeout))
+   * {
+   *   generic_client->remove_pending_request(future);
+   *   // handle timeout
+   * } else {
+   *   handle_response(future.get());
+   * }
+   * ```
+   *
+   * \param[in] request request to be send.
+   * \return a FutureAndRequestId instance.
+   */
+  RCLCPP_PUBLIC
+  FutureAndRequestId
+  async_send_request(const Request request);
+
+  /// Send a request to the service server and schedule a callback in the executor.
+  /**
+   * Similar to the previous overload, but a callback will automatically be called when a response
+   * is received.
+   *
+   * If the callback is never called, because we never got a reply for the service server,
+   * remove_pending_request() has to be called with the returned request id or
+   * prune_pending_requests().
+   * Not doing so will make the `GenericClient` instance use more memory each time a response is not
+   * received from the service server.
+   * In this case, it's convenient to setup a timer to cleanup the pending requests.
+   *
+   * \param[in] request request to be send.
+   * \param[in] cb callback that will be called when we get a response for this request.
+   * \return the request id representing the request just sent.
+   */
+  template<
+    typename CallbackT,
+    typename std::enable_if<
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        CallbackType
+      >::value
+    >::type * = nullptr
+  >
+  SharedFutureAndRequestId
+  async_send_request(const Request request, CallbackT && cb)
+  {
+    Promise promise;
+    auto shared_future = promise.get_future().share();
+    auto req_id = async_send_request_impl(
+      request,
+      std::make_tuple(
+        CallbackType{std::forward<CallbackT>(cb)},
+        shared_future,
+        std::move(promise)));
+    return SharedFutureAndRequestId{std::move(shared_future), req_id};
+  }
+
+  /// Clean all pending requests older than a time_point.
+  /**
+   * \param[in] time_point Requests that were sent before this point are going to be removed.
+   * \param[inout] pruned_requests Removed requests id will be pushed to the vector
+   *  if a pointer is provided.
+   * \return number of pending requests that were removed.
+   */
+  template<typename AllocatorT = std::allocator<int64_t>>
+  size_t
+  prune_requests_older_than(
+    std::chrono::time_point<std::chrono::system_clock> time_point,
+    std::vector<int64_t, AllocatorT> * pruned_requests = nullptr)
+  {
+    return detail::prune_requests_older_than_impl(
+      pending_requests_,
+      pending_requests_mutex_,
+      time_point,
+      pruned_requests);
+  }
+
+  /// Clean all pending requests.
+  /**
+   * \return number of pending requests that were removed.
+   */
+  RCLCPP_PUBLIC
+  size_t
+  prune_pending_requests();
+
+  /// Cleanup a pending request.
+  /**
+   * This notifies the client that we have waited long enough for a response from the server
+   * to come, we have given up and we are not waiting for a response anymore.
+   *
+   * Not calling this will make the client start using more memory for each request
+   * that never got a reply from the server.
+   *
+   * \param[in] request_id request id returned by async_send_request().
+   * \return true when a pending request was removed, false if not (e.g. a response was received).
+   */
+  RCLCPP_PUBLIC
+  bool
+  remove_pending_request(
+    int64_t request_id);
+
+  /// Cleanup a pending request.
+  /**
+   * Convenient overload, same as:
+   *
+   * `GenericClient::remove_pending_request(this, future.request_id)`.
+   */
+  RCLCPP_PUBLIC
+  bool
+  remove_pending_request(
+    const FutureAndRequestId & future);
+
+  /// Cleanup a pending request.
+  /**
+   * Convenient overload, same as:
+   *
+   * `GenericClient::remove_pending_request(this, future.request_id)`.
+   */
+  RCLCPP_PUBLIC
+  bool
+  remove_pending_request(
+    const SharedFutureAndRequestId & future);
+
+  /// Take the next response for this client.
+  /**
+   * \sa ClientBase::take_type_erased_response().
+   *
+   * \param[out] response_out The reference to a Service Response into
+   *   which the middleware will copy the response being taken.
+   * \param[out] request_header_out The request header to be filled by the
+   *   middleware when taking, and which can be used to associate the response
+   *   to a specific request.
+   * \returns true if the response was taken, otherwise false.
+   * \throws rclcpp::exceptions::RCLError based exceptions if the underlying
+   *   rcl function fail.
+   */
+  RCLCPP_PUBLIC
+  bool
+  take_response(Response response_out, rmw_request_id_t & request_header_out)
+  {
+    return this->take_type_erased_response(response_out, request_header_out);
+  }
+
+protected:
+  using CallbackTypeValueVariant = std::tuple<CallbackType, SharedFuture, Promise>;
+  using CallbackInfoVariant = std::variant<
+    std::promise<SharedResponse>,
+    CallbackTypeValueVariant>;  // Use variant for extension
+
+  RCLCPP_PUBLIC
+  int64_t
+  async_send_request_impl(
+    const Request request,
+    CallbackInfoVariant value);
+
+  std::optional<CallbackInfoVariant>
+  get_and_erase_pending_request(
+    int64_t request_number);
+
+  RCLCPP_DISABLE_COPY(GenericClient)
+
+  std::map<int64_t, std::pair<
+      std::chrono::time_point<std::chrono::system_clock>,
+      CallbackInfoVariant>> pending_requests_;
+  std::mutex pending_requests_mutex_;
+
+private:
+  std::shared_ptr<rcpputils::SharedLibrary> ts_lib_;
+  const rosidl_typesupport_introspection_cpp::MessageMembers * response_members_;
+};
+}  // namespace rclcpp
+
+#endif  // RCLCPP__GENERIC_CLIENT_HPP_

rclcpp/include/rclcpp/generic_publisher.hpp

@@ -0,0 +1,117 @@
+// Copyright 2018, Bosch Software Innovations GmbH.
+// Copyright 2021, Apex.AI 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__GENERIC_PUBLISHER_HPP_
+#define RCLCPP__GENERIC_PUBLISHER_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rcpputils/shared_library.hpp"
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/publisher_base.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/serialized_message.hpp"
+#include "rclcpp/typesupport_helpers.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include "rmw/rmw.h"
+
+namespace rclcpp
+{
+
+/// %Publisher for serialized messages whose type is not known at compile time.
+/**
+ * Since the type is not known at compile time, this is not a template, and the dynamic library
+ * containing type support information has to be identified and loaded based on the type name.
+ *
+ * It does not support intra-process handling.
+ */
+class GenericPublisher : public rclcpp::PublisherBase
+{
+public:
+  // cppcheck-suppress unknownMacro
+  RCLCPP_SMART_PTR_DEFINITIONS(GenericPublisher)
+
+  /// Constructor.
+  /**
+   * In order to properly publish to a topic, this publisher needs to be added to
+   * the node_topic_interface of the node passed into this constructor.
+   *
+   * \sa rclcpp::Node::create_generic_publisher() or rclcpp::create_generic_publisher() for
+   * creating an instance of this class and adding it to the node_topic_interface.
+   *
+   * \param node_base Pointer to parent node's NodeBaseInterface
+   * \param ts_lib Type support library, needs to correspond to topic_type
+   * \param topic_name Topic name
+   * \param topic_type Topic type
+   * \param qos %QoS settings
+   * \param options %Publisher options.
+   * Not all publisher options are currently respected, the only relevant options for this
+   * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`.
+   */
+  template<typename AllocatorT = std::allocator<void>>
+  GenericPublisher(
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    std::shared_ptr<rcpputils::SharedLibrary> ts_lib,
+    const std::string & topic_name,
+    const std::string & topic_type,
+    const rclcpp::QoS & qos,
+    const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options)
+  : rclcpp::PublisherBase(
+      node_base,
+      topic_name,
+      *rclcpp::get_message_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib),
+      options.template to_rcl_publisher_options<rclcpp::SerializedMessage>(qos),
+      // NOTE(methylDragon): Passing these args separately is necessary for event binding
+      options.event_callbacks,
+      options.use_default_callbacks),
+    ts_lib_(ts_lib)
+  {}
+
+  RCLCPP_PUBLIC
+  virtual ~GenericPublisher() = default;
+
+  /// Publish a rclcpp::SerializedMessage.
+  RCLCPP_PUBLIC
+  void publish(const rclcpp::SerializedMessage & message);
+
+  /**
+   * Publish a rclcpp::SerializedMessage via loaned message after de-serialization.
+   *
+   * \param message a serialized message
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can show
+   */
+  RCLCPP_PUBLIC
+  void publish_as_loaned_msg(const rclcpp::SerializedMessage & message);
+
+private:
+  // The type support library should stay loaded, so it is stored in the GenericPublisher
+  std::shared_ptr<rcpputils::SharedLibrary> ts_lib_;
+
+  void * borrow_loaned_message();
+  void deserialize_message(
+    const rmw_serialized_message_t & serialized_message,
+    void * deserialized_msg);
+  void publish_loaned_message(void * loaned_message);
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__GENERIC_PUBLISHER_HPP_

rclcpp/include/rclcpp/generic_service.hpp

@@ -0,0 +1,308 @@
+// Copyright 2024 Sony Group Corporation.
+//
+// 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__GENERIC_SERVICE_HPP_
+#define RCLCPP__GENERIC_SERVICE_HPP_
+
+#include <cstdlib>
+#include <functional>
+#include <memory>
+#include <string>
+#include <type_traits>
+#include <utility>
+#include <variant>
+
+#include "rclcpp/typesupport_helpers.hpp"
+
+#include "rosidl_runtime_c/service_type_support_struct.h"
+#include "rosidl_typesupport_introspection_cpp/identifier.hpp"
+#include "rosidl_typesupport_introspection_cpp/service_introspection.hpp"
+
+#include "service.hpp"
+
+namespace rclcpp
+{
+class GenericService;
+
+class GenericServiceCallback
+{
+public:
+  using SharedRequest = std::shared_ptr<void>;
+  using SharedResponse = std::shared_ptr<void>;
+
+  GenericServiceCallback()
+  : callback_(std::monostate{})
+  {}
+
+  template<
+    typename CallbackT,
+    typename std::enable_if_t<!detail::can_be_nullptr<CallbackT>::value, int> = 0>
+  void
+  set(CallbackT && callback)
+  {
+    // Workaround Windows issue with std::bind
+    if constexpr (
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrCallback
+      >::value)
+    {
+      callback_.template emplace<SharedPtrCallback>(callback);
+    } else if constexpr (  // NOLINT, can't satisfy both cpplint and uncrustify
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrWithRequestHeaderCallback
+      >::value)
+    {
+      callback_.template emplace<SharedPtrWithRequestHeaderCallback>(callback);
+    } else if constexpr (  // NOLINT
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrDeferResponseCallback
+      >::value)
+    {
+      callback_.template emplace<SharedPtrDeferResponseCallback>(callback);
+    } else if constexpr (  // NOLINT
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrDeferResponseCallbackWithServiceHandle
+      >::value)
+    {
+      callback_.template emplace<SharedPtrDeferResponseCallbackWithServiceHandle>(callback);
+    } else {
+      // the else clause is not needed, but anyways we should only be doing this instead
+      // of all the above workaround ...
+      callback_ = std::forward<CallbackT>(callback);
+    }
+  }
+
+  template<
+    typename CallbackT,
+    typename std::enable_if_t<detail::can_be_nullptr<CallbackT>::value, int> = 0>
+  void
+  set(CallbackT && callback)
+  {
+    if (!callback) {
+      throw std::invalid_argument("AnyServiceCallback::set(): callback cannot be nullptr");
+    }
+    // Workaround Windows issue with std::bind
+    if constexpr (
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrCallback
+      >::value)
+    {
+      callback_.template emplace<SharedPtrCallback>(callback);
+    } else if constexpr (  // NOLINT
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrWithRequestHeaderCallback
+      >::value)
+    {
+      callback_.template emplace<SharedPtrWithRequestHeaderCallback>(callback);
+    } else if constexpr (  // NOLINT
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrDeferResponseCallback
+      >::value)
+    {
+      callback_.template emplace<SharedPtrDeferResponseCallback>(callback);
+    } else if constexpr (  // NOLINT
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrDeferResponseCallbackWithServiceHandle
+      >::value)
+    {
+      callback_.template emplace<SharedPtrDeferResponseCallbackWithServiceHandle>(callback);
+    } else {
+      // the else clause is not needed, but anyways we should only be doing this instead
+      // of all the above workaround ...
+      callback_ = std::forward<CallbackT>(callback);
+    }
+  }
+
+  SharedResponse
+  dispatch(
+    const std::shared_ptr<rclcpp::GenericService> & service_handle,
+    const std::shared_ptr<rmw_request_id_t> & request_header,
+    SharedRequest request,
+    SharedRequest response)
+  {
+    TRACETOOLS_TRACEPOINT(callback_start, static_cast<const void *>(this), false);
+    if (std::holds_alternative<std::monostate>(callback_)) {
+      // TODO(ivanpauno): Remove the set method, and force the users of this class
+      // to pass a callback at construnciton.
+      throw std::runtime_error{"unexpected request without any callback set"};
+    }
+    if (std::holds_alternative<SharedPtrDeferResponseCallback>(callback_)) {
+      const auto & cb = std::get<SharedPtrDeferResponseCallback>(callback_);
+      cb(request_header, std::move(request));
+      return nullptr;
+    }
+    if (std::holds_alternative<SharedPtrDeferResponseCallbackWithServiceHandle>(callback_)) {
+      const auto & cb = std::get<SharedPtrDeferResponseCallbackWithServiceHandle>(callback_);
+      cb(service_handle, request_header, std::move(request));
+      return nullptr;
+    }
+
+    if (std::holds_alternative<SharedPtrCallback>(callback_)) {
+      (void)request_header;
+      const auto & cb = std::get<SharedPtrCallback>(callback_);
+      cb(std::move(request), std::move(response));
+    } else if (std::holds_alternative<SharedPtrWithRequestHeaderCallback>(callback_)) {
+      const auto & cb = std::get<SharedPtrWithRequestHeaderCallback>(callback_);
+      cb(request_header, std::move(request), std::move(response));
+    }
+    TRACETOOLS_TRACEPOINT(callback_end, static_cast<const void *>(this));
+    return response;
+  }
+
+  void register_callback_for_tracing()
+  {
+#ifndef TRACETOOLS_DISABLED
+    std::visit(
+      [this](auto && arg) {
+        if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) {
+          char * symbol = tracetools::get_symbol(arg);
+          TRACETOOLS_DO_TRACEPOINT(
+            rclcpp_callback_register,
+            static_cast<const void *>(this),
+            symbol);
+          std::free(symbol);
+        }
+      }, callback_);
+#endif  // TRACETOOLS_DISABLED
+  }
+
+private:
+  using SharedPtrCallback = std::function<void (SharedRequest, SharedResponse)>;
+  using SharedPtrWithRequestHeaderCallback = std::function<
+    void (
+      std::shared_ptr<rmw_request_id_t>,
+      SharedRequest,
+      SharedResponse
+    )>;
+  using SharedPtrDeferResponseCallback = std::function<
+    void (
+      std::shared_ptr<rmw_request_id_t>,
+      SharedRequest
+    )>;
+  using SharedPtrDeferResponseCallbackWithServiceHandle = std::function<
+    void (
+      std::shared_ptr<rclcpp::GenericService>,
+      std::shared_ptr<rmw_request_id_t>,
+      SharedRequest
+    )>;
+
+  std::variant<
+    std::monostate,
+    SharedPtrCallback,
+    SharedPtrWithRequestHeaderCallback,
+    SharedPtrDeferResponseCallback,
+    SharedPtrDeferResponseCallbackWithServiceHandle> callback_;
+};
+
+class GenericService
+  : public ServiceBase,
+  public std::enable_shared_from_this<GenericService>
+{
+public:
+  using Request = void *;  // Serialized/Deserialized data pointer of request message
+  using Response = void *;  // Serialized/Deserialized data pointer of response message
+  using SharedRequest = std::shared_ptr<void>;
+  using SharedResponse = std::shared_ptr<void>;
+  using CallbackType = std::function<void (const SharedRequest, SharedResponse)>;
+
+  using CallbackWithHeaderType =
+    std::function<void (const std::shared_ptr<rmw_request_id_t>,
+      const SharedRequest,
+      SharedResponse)>;
+
+  RCLCPP_SMART_PTR_DEFINITIONS(GenericService)
+
+  /// Default constructor.
+  /**
+   * The constructor for a Service is almost never called directly.
+   * Instead, services should be instantiated through the function
+   * rclcpp::create_service().
+   *
+   * \param[in] node_handle NodeBaseInterface pointer that is used in part of the setup.
+   * \param[in] service_name Name of the topic to publish to.
+   * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool".
+   * \param[in] any_callback User defined callback to call when a client request is received.
+   * \param[in] service_options options for the service.
+   */
+  RCLCPP_PUBLIC
+  GenericService(
+    std::shared_ptr<rcl_node_t> node_handle,
+    const std::string & service_name,
+    const std::string & service_type,
+    GenericServiceCallback any_callback,
+    rcl_service_options_t & service_options);
+
+  GenericService() = delete;
+
+  RCLCPP_PUBLIC
+  virtual ~GenericService() {}
+
+  /// Take the next request from the service.
+  /**
+   * \sa ServiceBase::take_type_erased_request().
+   *
+   * \param[out] request_out The reference to a service deserialized request object
+   *   into which the middleware will copy the taken request.
+   * \param[out] request_id_out The output id for the request which can be used
+   *   to associate response with this request in the future.
+   * \returns true if the request was taken, otherwise false.
+   * \throws rclcpp::exceptions::RCLError based exceptions if the underlying
+   *   rcl calls fail.
+   */
+  RCLCPP_PUBLIC
+  bool
+  take_request(SharedRequest request_out, rmw_request_id_t & request_id_out);
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<void>
+  create_request() override;
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<void>
+  create_response();
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<rmw_request_id_t>
+  create_request_header() override;
+
+  RCLCPP_PUBLIC
+  void
+  handle_request(
+    std::shared_ptr<rmw_request_id_t> request_header,
+    std::shared_ptr<void> request) override;
+
+  RCLCPP_PUBLIC
+  void
+  send_response(rmw_request_id_t & req_id, SharedResponse & response);
+
+private:
+  RCLCPP_DISABLE_COPY(GenericService)
+
+  GenericServiceCallback any_callback_;
+
+  std::shared_ptr<rcpputils::SharedLibrary> ts_lib_;
+  const rosidl_typesupport_introspection_cpp::MessageMembers * request_members_;
+  const rosidl_typesupport_introspection_cpp::MessageMembers * response_members_;
+};
+
+}  // namespace rclcpp
+#endif  // RCLCPP__GENERIC_SERVICE_HPP_

rclcpp/include/rclcpp/generic_subscription.hpp

@@ -0,0 +1,192 @@
+// Copyright 2018, Bosch Software Innovations GmbH.
+// Copyright 2021, Apex.AI 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__GENERIC_SUBSCRIPTION_HPP_
+#define RCLCPP__GENERIC_SUBSCRIPTION_HPP_
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include "rcpputils/shared_library.hpp"
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/serialized_message.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/typesupport_helpers.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// %Subscription for serialized messages whose type is not known at compile time.
+/**
+ * Since the type is not known at compile time, this is not a template, and the dynamic library
+ * containing type support information has to be identified and loaded based on the type name.
+ *
+ * It does not support intra-process handling.
+ */
+class GenericSubscription : public rclcpp::SubscriptionBase
+{
+public:
+  // cppcheck-suppress unknownMacro
+  RCLCPP_SMART_PTR_DEFINITIONS(GenericSubscription)
+
+  /// Constructor.
+  /**
+   * In order to properly subscribe to a topic, this subscription needs to be added to
+   * the node_topic_interface of the node passed into this constructor.
+   *
+   * \sa rclcpp::Node::create_generic_subscription() or rclcpp::create_generic_subscription() for
+   * creating an instance of this class and adding it to the node_topic_interface.
+   *
+   * \param node_base Pointer to parent node's NodeBaseInterface
+   * \param ts_lib Type support library, needs to correspond to topic_type
+   * \param topic_name Topic name
+   * \param topic_type Topic type
+   * \param qos %QoS settings
+   * \param callback Callback for new messages of serialized form
+   * \param options %Subscription options.
+   * Not all subscription options are currently respected, the only relevant options for this
+   * subscription are `event_callbacks`, `use_default_callbacks`, `ignore_local_publications`, and
+   * `%callback_group`.
+   */
+  template<typename AllocatorT = std::allocator<void>>
+  GenericSubscription(
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    const std::shared_ptr<rcpputils::SharedLibrary> ts_lib,
+    const std::string & topic_name,
+    const std::string & topic_type,
+    const rclcpp::QoS & qos,
+    AnySubscriptionCallback<rclcpp::SerializedMessage, AllocatorT> callback,
+    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options)
+  : SubscriptionBase(
+      node_base,
+      *rclcpp::get_message_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib),
+      topic_name,
+      options.to_rcl_subscription_options(qos),
+      options.event_callbacks,
+      options.use_default_callbacks,
+      DeliveredMessageKind::SERIALIZED_MESSAGE),
+    any_callback_(callback),
+    ts_lib_(ts_lib)
+  {
+    TRACETOOLS_TRACEPOINT(
+      rclcpp_subscription_init,
+      static_cast<const void *>(get_subscription_handle().get()),
+      static_cast<const void *>(this));
+    TRACETOOLS_TRACEPOINT(
+      rclcpp_subscription_callback_added,
+      static_cast<const void *>(this),
+      static_cast<const void *>(&any_callback_));
+
+#ifndef TRACETOOLS_DISABLED
+    any_callback_.register_callback_for_tracing();
+#endif
+  }
+
+  RCLCPP_PUBLIC
+  virtual ~GenericSubscription() = default;
+
+  // Same as create_serialized_message() as the subscription is to serialized_messages only
+  RCLCPP_PUBLIC
+  std::shared_ptr<void> create_message() override;
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<rclcpp::SerializedMessage> create_serialized_message() override;
+
+  /// Disable callbacks from being called
+  /**
+    * This method will block, until any subscription's callbacks provided during construction
+    * currently being executed are finished.
+    * \note This method also temporary removes the on new message callback and all
+    * on new event callbacks from the rmw layer to prevent them from being called. However, this
+    * method will not block and wait until the currently executing on_new_[message]event callbacks
+    * are finished.
+    */
+  RCLCPP_PUBLIC
+  void disable_callbacks() override;
+
+  /// Enable the callbacks to be called
+  /**
+    * This method is thread safe, and provides a safe way to atomically enable the callbacks
+    * in a multithreaded environment.
+    */
+  RCLCPP_PUBLIC
+  void enable_callbacks() override;
+
+  /// Cast the message to a rclcpp::SerializedMessage and call the callback.
+  RCLCPP_PUBLIC
+  void handle_message(
+    std::shared_ptr<void> & message, const rclcpp::MessageInfo & message_info) override;
+
+  /// Handle dispatching rclcpp::SerializedMessage to user callback.
+  RCLCPP_PUBLIC
+  void
+  handle_serialized_message(
+    const std::shared_ptr<rclcpp::SerializedMessage> & serialized_message,
+    const rclcpp::MessageInfo & message_info) override;
+
+  /// This function is currently not implemented.
+  RCLCPP_PUBLIC
+  void handle_loaned_message(
+    void * loaned_message, const rclcpp::MessageInfo & message_info) override;
+
+  // Same as return_serialized_message() as the subscription is to serialized_messages only
+  RCLCPP_PUBLIC
+  void return_message(std::shared_ptr<void> & message) override;
+
+  RCLCPP_PUBLIC
+  void return_serialized_message(std::shared_ptr<rclcpp::SerializedMessage> & message) override;
+
+
+  // DYNAMIC TYPE ==================================================================================
+  RCLCPP_PUBLIC
+  rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr get_shared_dynamic_message_type()
+  override;
+
+  RCLCPP_PUBLIC
+  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr get_shared_dynamic_message() override;
+
+  RCLCPP_PUBLIC
+  rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
+  get_shared_dynamic_serialization_support() override;
+
+  RCLCPP_PUBLIC
+  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr create_dynamic_message() override;
+
+  RCLCPP_PUBLIC
+  void return_dynamic_message(
+    rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) override;
+
+  RCLCPP_PUBLIC
+  void handle_dynamic_message(
+    const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
+    const rclcpp::MessageInfo & message_info) override;
+
+private:
+  RCLCPP_DISABLE_COPY(GenericSubscription)
+  AnySubscriptionCallback<rclcpp::SerializedMessage, std::allocator<void>> any_callback_;
+  // The type support library should stay loaded, so it is stored in the GenericSubscription
+  std::shared_ptr<rcpputils::SharedLibrary> ts_lib_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__GENERIC_SUBSCRIPTION_HPP_

rclcpp/include/rclcpp/get_message_type_support_handle.hpp

@@ -0,0 +1,98 @@
+// Copyright 2021 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__GET_MESSAGE_TYPE_SUPPORT_HANDLE_HPP_
+#define RCLCPP__GET_MESSAGE_TYPE_SUPPORT_HANDLE_HPP_
+
+#include <type_traits>
+
+#include "rosidl_runtime_cpp/traits.hpp"
+#include "rosidl_runtime_cpp/message_type_support_decl.hpp"
+#include "rosidl_typesupport_cpp/message_type_support.hpp"
+
+#include "rclcpp/type_adapter.hpp"
+
+/// Versions of rosidl_typesupport_cpp::get_message_type_support_handle that handle adapted types.
+
+namespace rclcpp
+{
+
+#ifdef DOXYGEN_ONLY
+
+/// Returns the message type support for the given `MessageT` type.
+/**
+ * \tparam MessageT an actual ROS message type or an adapted type using `rclcpp::TypeAdapter`
+ */
+template<typename MessageT>
+constexpr const rosidl_message_type_support_t & get_message_type_support_handle();
+
+#else
+
+template<typename MessageT>
+constexpr
+typename std::enable_if_t<
+  rosidl_generator_traits::is_message<MessageT>::value,
+  const rosidl_message_type_support_t &
+>
+get_message_type_support_handle()
+{
+  auto handle = rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>();
+  if (!handle) {
+    throw std::runtime_error("Type support handle unexpectedly nullptr");
+  }
+  return *handle;
+}
+
+template<typename AdaptedType>
+constexpr
+typename std::enable_if_t<
+  !rosidl_generator_traits::is_message<AdaptedType>::value &&
+  rclcpp::TypeAdapter<AdaptedType>::is_specialized::value,
+  const rosidl_message_type_support_t &
+>
+get_message_type_support_handle()
+{
+  auto handle = rosidl_typesupport_cpp::get_message_type_support_handle<
+    typename TypeAdapter<AdaptedType>::ros_message_type
+    >();
+  if (!handle) {
+    throw std::runtime_error("Type support handle unexpectedly nullptr");
+  }
+  return *handle;
+}
+
+// This specialization is a pass through runtime check, which allows a better
+// static_assert to catch this issue further down the line.
+// This should never get to be called in practice, and is purely defensive.
+template<
+  typename AdaptedType
+>
+constexpr
+typename std::enable_if_t<
+  !rosidl_generator_traits::is_message<AdaptedType>::value &&
+  !TypeAdapter<AdaptedType>::is_specialized::value,
+  const rosidl_message_type_support_t &
+>
+get_message_type_support_handle()
+{
+  throw std::runtime_error(
+          "this specialization of rclcpp::get_message_type_support_handle() "
+          "should never be called");
+}
+
+#endif
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__GET_MESSAGE_TYPE_SUPPORT_HANDLE_HPP_

rclcpp/include/rclcpp/graph_listener.hpp

@@ -24,6 +24,7 @@
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
 #include "rclcpp/context.hpp"
+#include "rclcpp/guard_condition.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -63,7 +64,7 @@ class GraphListener : public std::enable_shared_from_this<GraphListener>
 {
 public:
   RCLCPP_PUBLIC
-  explicit GraphListener(std::shared_ptr<rclcpp::Context> parent_context);
+  explicit GraphListener(const rclcpp::Context::SharedPtr & parent_context);
 
   RCLCPP_PUBLIC
   virtual ~GraphListener();
@@ -73,6 +74,8 @@ public:
    * This function is thread-safe.
    *
    * \throws GraphListenerShutdownError if the GraphListener is shutdown
+   * \throws std::runtime if the parent context was destroyed
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
    */
   RCLCPP_PUBLIC
   virtual
@@ -125,6 +128,11 @@ public:
    * If start_if_not_started() was never called, this function still succeeds,
    * but start_if_not_started() still cannot be called after this function.
    *
+   * Note that if you override this method, but leave shutdown to be called in
+   * the destruction of this base class, it will not call the overridden
+   * version from your base class.
+   * So you need to ensure you call your class's shutdown() in its destructor.
+   *
    * \throws rclcpp::execptions::RCLError from rcl_guard_condition_fini()
    * \throws rclcpp::execptions::RCLError from rcl_wait_set_fini()
    * \throws std::system_error anything std::mutex::lock() throws
@@ -146,6 +154,12 @@ public:
   bool
   is_shutdown();
 
+  /// Return true if the graph listener was started.
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  is_started();
+
 protected:
   /// Main function for the listening thread.
   RCLCPP_PUBLIC
@@ -158,14 +172,22 @@ protected:
   void
   run_loop();
 
+  RCLCPP_PUBLIC
+  void
+  init_wait_set();
+
+  RCLCPP_PUBLIC
+  void
+  cleanup_wait_set();
+
 private:
   RCLCPP_DISABLE_COPY(GraphListener)
 
   /** \internal */
   void
-  __shutdown(bool);
+  __shutdown();
 
-  rclcpp::Context::SharedPtr parent_context_;
+  std::shared_ptr<rcl_context_t> rcl_parent_context_;
 
   std::thread listener_thread_;
   bool is_started_;
@@ -176,9 +198,7 @@ private:
   mutable std::mutex node_graph_interfaces_mutex_;
   std::vector<rclcpp::node_interfaces::NodeGraphInterface *> node_graph_interfaces_;
 
-  rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
-  std::shared_ptr<rcl_context_t> interrupt_guard_condition_context_;
-  rcl_guard_condition_t * shutdown_guard_condition_;
+  rclcpp::GuardCondition interrupt_guard_condition_;
   rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
 };
 

rclcpp/include/rclcpp/guard_condition.hpp

@@ -0,0 +1,138 @@
+// Copyright 2020 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__GUARD_CONDITION_HPP_
+#define RCLCPP__GUARD_CONDITION_HPP_
+
+#include <atomic>
+
+#include "rcl/guard_condition.h"
+
+#include "rclcpp/context.hpp"
+#include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// A condition that can be waited on in a single wait set and asynchronously triggered.
+class GuardCondition
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(GuardCondition)
+
+  // TODO(wjwwood): support custom allocator, maybe restrict to polymorphic allocator
+  /// Construct the guard condition, optionally specifying which Context to use.
+  /**
+   * \param[in] context Optional custom context to be used.
+   *   Defaults to using the global default context singleton.
+   *   Shared ownership of the context is held with the guard condition until
+   *   destruction.
+   * \param[in] guard_condition_options Optional guard condition options to be used.
+   *   Defaults to using the default guard condition options.
+   * \throws std::invalid_argument if the context is nullptr.
+   * \throws rclcpp::exceptions::RCLError based exceptions when underlying
+   *   rcl functions fail.
+   */
+  RCLCPP_PUBLIC
+  explicit GuardCondition(
+    const rclcpp::Context::SharedPtr & context =
+    rclcpp::contexts::get_global_default_context(),
+    rcl_guard_condition_options_t guard_condition_options =
+    rcl_guard_condition_get_default_options());
+
+  RCLCPP_PUBLIC
+  virtual
+  ~GuardCondition();
+
+  /// Return the underlying rcl guard condition structure.
+  RCLCPP_PUBLIC
+  rcl_guard_condition_t &
+  get_rcl_guard_condition();
+
+  /// Return the underlying rcl guard condition structure.
+  RCLCPP_PUBLIC
+  const rcl_guard_condition_t &
+  get_rcl_guard_condition() const;
+
+  /// Signal that the condition has been met, notifying both the wait set and listeners, if any.
+  /**
+   * This function is thread-safe, and may be called concurrently with waiting
+   * on this guard condition in a wait set.
+   *
+   * \throws rclcpp::exceptions::RCLError based exceptions when underlying
+   *   rcl functions fail.
+   */
+  RCLCPP_PUBLIC
+  void
+  trigger();
+
+  /// Exchange the "in use by wait set" state for this guard condition.
+  /**
+   * This is used to ensure this guard condition is not used by multiple
+   * wait sets at the same time.
+   *
+   * \param[in] in_use_state the new state to exchange into the state, true
+   *   indicates it is now in use by a wait set, and false is that it is no
+   *   longer in use by a wait set.
+   * \returns the previous state.
+   */
+  RCLCPP_PUBLIC
+  bool
+  exchange_in_use_by_wait_set_state(bool in_use_state);
+
+  /// Adds the guard condition to a waitset
+  /**
+   * This function is thread-safe.
+   * \param[in] wait_set pointer to a wait set where to add the guard condition
+   */
+  RCLCPP_PUBLIC
+  void
+  add_to_wait_set(rcl_wait_set_t & wait_set);
+
+  /// Set a callback to be called whenever the guard condition is triggered.
+  /**
+   * The callback receives a size_t which is the number of times the guard condition was triggered
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if the guard condition was triggered before any
+   * callback was set.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the guard condition
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \param[in] callback functor to be called when the guard condition is triggered
+   */
+  RCLCPP_PUBLIC
+  void
+  set_on_trigger_callback(std::function<void(size_t)> callback);
+
+protected:
+  rcl_guard_condition_t rcl_guard_condition_;
+  std::atomic<bool> in_use_by_wait_set_{false};
+  std::recursive_mutex reentrant_mutex_;
+  std::function<void(size_t)> on_trigger_callback_{nullptr};
+  size_t unread_count_{0};
+  // the type of wait_set_ is actually rcl_wait_set_t *, but it's never
+  // dereferenced, only compared to, so make it void * to avoid accidental use
+  void * wait_set_{nullptr};
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__GUARD_CONDITION_HPP_

rclcpp/include/rclcpp/init_options.hpp

@@ -16,6 +16,7 @@
 #define RCLCPP__INIT_OPTIONS_HPP_
 
 #include <memory>
+#include <mutex>
 
 #include "rcl/init_options.h"
 #include "rclcpp/visibility_control.hpp"
@@ -28,13 +29,23 @@ 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;
+  bool shutdown_on_signal = true;
 
-  /// Constructor which allows you to specify the allocator used within the init options.
+  /// Constructor
+  /**
+   * It allows you to specify the allocator used within the init options.
+   * \param[in] allocator used allocate memory within the init options
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
+   */
   RCLCPP_PUBLIC
   explicit InitOptions(rcl_allocator_t allocator = rcl_get_default_allocator());
 
   /// Constructor which is initialized by an existing init_options.
+  /**
+   * Initialized by an existing init_options.
+   * \param[in] init_options rcl_init_options_t to initialized
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
+   */
   RCLCPP_PUBLIC
   explicit InitOptions(const rcl_init_options_t & init_options);
 
@@ -42,6 +53,16 @@ public:
   RCLCPP_PUBLIC
   InitOptions(const InitOptions & other);
 
+  /// Return `true` if logging should be initialized when `rclcpp::Context::init` is called.
+  RCLCPP_PUBLIC
+  bool
+  auto_initialize_logging() const;
+
+  /// Set flag indicating if logging should be initialized or not.
+  RCLCPP_PUBLIC
+  InitOptions &
+  auto_initialize_logging(bool initialize_logging);
+
   /// Assignment operator.
   RCLCPP_PUBLIC
   InitOptions &
@@ -52,16 +73,40 @@ public:
   ~InitOptions();
 
   /// Return the rcl init options.
+  /**
+   * \return the rcl init options.
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
+   */
   RCLCPP_PUBLIC
   const rcl_init_options_t *
   get_rcl_init_options() const;
 
+  /// Retrieve default domain id and set.
+  RCLCPP_PUBLIC
+  void
+  use_default_domain_id();
+
+  /// Set the domain id.
+  RCLCPP_PUBLIC
+  void
+  set_domain_id(size_t domain_id);
+
+  /// Return domain id.
+  RCLCPP_PUBLIC
+  size_t
+  get_domain_id() const;
+
 protected:
   void
   finalize_init_options();
 
 private:
+  void
+  finalize_init_options_impl();
+
+  mutable std::mutex init_options_mutex_;
   std::unique_ptr<rcl_init_options_t> init_options_;
+  bool initialize_logging_{true};
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/intra_process_buffer_type.hpp

@@ -0,0 +1,35 @@
+// 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_BUFFER_TYPE_HPP_
+#define RCLCPP__INTRA_PROCESS_BUFFER_TYPE_HPP_
+
+namespace rclcpp
+{
+
+/// Used as argument in create_publisher and create_subscriber
+/// when intra-process communication is enabled
+enum class IntraProcessBufferType
+{
+  /// Set the data type used in the intra-process buffer as std::shared_ptr<MessageT>
+  SharedPtr,
+  /// Set the data type used in the intra-process buffer as std::unique_ptr<MessageT>
+  UniquePtr,
+  /// Set the data type used in the intra-process buffer as the same used in the callback
+  CallbackDefault
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__INTRA_PROCESS_BUFFER_TYPE_HPP_

rclcpp/include/rclcpp/intra_process_manager.hpp

@@ -1,420 +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.
-
-#ifndef RCLCPP__INTRA_PROCESS_MANAGER_HPP_
-#define RCLCPP__INTRA_PROCESS_MANAGER_HPP_
-
-#include <rmw/types.h>
-
-#include <algorithm>
-#include <atomic>
-#include <cstdint>
-#include <exception>
-#include <map>
-#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_base.hpp"
-#include "rclcpp/subscription_base.hpp"
-#include "rclcpp/visibility_control.hpp"
-
-namespace rclcpp
-{
-namespace intra_process_manager
-{
-
-/// This class facilitates intra process communication between nodes.
-/**
- * This class is used in the creation of publishers and subscriptions.
- * A singleton instance of this class is owned by a rclcpp::Context and a
- * rclcpp::Node can use an associated Context to get an instance of this class.
- * Nodes which do not have a common Context will not exchange intra process
- * messages because they will not share access to an instance of this class.
- *
- * When a Node creates a publisher or subscription, it will register them
- * with this class.
- * The node will also hook into the publisher's publish call
- * in order to do intra process related work.
- *
- * When a publisher is created, it advertises on the topic the user provided,
- * as well as a "shadowing" topic of type rcl_interfaces/IntraProcessMessage.
- * For instance, if the user specified the topic '/namespace/chatter', then the
- * corresponding intra process topic might be '/namespace/chatter/_intra'.
- * The publisher is also allocated an id which is unique among all publishers
- * and subscriptions in this process.
- * Additionally, when registered with this class a ring buffer is created and
- * owned by this class as a temporary place to hold messages destined for intra
- * process subscriptions.
- *
- * When a subscription is created, it subscribes to the topic provided by the
- * user as well as to the corresponding intra process topic.
- * It is also gets a unique id from the singleton instance of this class which
- * is unique among publishers and subscriptions.
- *
- * When the user publishes a message, the message is stored by calling
- * store_intra_process_message on this class.
- * The instance of that message is uniquely identified by a publisher id and a
- * message sequence number.
- * The publisher id, message sequence pair is unique with in the process.
- * At that point a list of the id's of intra process subscriptions which have
- * been registered with the singleton instance of this class are stored with
- * the message instance so that delivery is only made to those subscriptions.
- * Then an instance of rcl_interfaces/IntraProcessMessage is published to the
- * intra process topic which is specific to the topic specified by the user.
- *
- * When an instance of rcl_interfaces/IntraProcessMessage is received by a
- * subscription, then it is handled by calling take_intra_process_message
- * on a singleton of this class.
- * The subscription passes a publisher id, message sequence pair which
- * uniquely identifies the message instance it was suppose to receive as well
- * as the subscriptions unique id.
- * If the message is still being held by this class and the subscription's id
- * is in the list of intended subscriptions then the message is returned.
- * If either of those predicates are not satisfied then the message is not
- * returned and the subscription does not call the users callback.
- *
- * Since the publisher builds a list of destined subscriptions on publish, and
- * other requests are ignored, this class knows how many times a message
- * instance should be requested.
- * The final time a message is requested, the ownership is passed out of this
- * class and passed to the final subscription, effectively freeing space in
- * this class's internal storage.
- *
- * Since a topic is being used to ferry notifications about new intra process
- * messages between publishers and subscriptions, it is possible for that
- * notification to be lost.
- * It is also possible that a subscription which was available when publish was
- * called will no longer exist once the notification gets posted.
- * In both cases this might result in a message instance getting requested
- * fewer times than expected.
- * This is why the internal storage of this class is a ring buffer.
- * That way if a message is orphaned it will eventually be dropped from storage
- * when a new message instance is stored and will not result in a memory leak.
- *
- * However, since the storage system is finite, this also means that a message
- * instance might get displaced by an incoming message instance before all
- * interested parties have called take_intra_process_message.
- * Because of this the size of the internal storage should be carefully
- * considered.
- *
- * /TODO(wjwwood): update to include information about handling latching.
- * /TODO(wjwwood): consider thread safety of the class.
- *
- * This class is neither CopyConstructable nor CopyAssignable.
- */
-class IntraProcessManager
-{
-private:
-  RCLCPP_DISABLE_COPY(IntraProcessManager)
-
-public:
-  RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager)
-
-  RCLCPP_PUBLIC
-  explicit IntraProcessManager(
-    IntraProcessManagerImplBase::SharedPtr state = create_default_impl());
-
-  RCLCPP_PUBLIC
-  virtual ~IntraProcessManager();
-
-  /// Register a subscription with the manager, returns subscriptions unique id.
-  /**
-   * In addition to generating a unique intra process id for the subscription,
-   * this method also stores the topic name of the subscription.
-   *
-   * This method is normally called during the creation of a subscription,
-   * but after it creates the internal intra process rmw_subscription_t.
-   *
-   * This method will allocate memory.
-   *
-   * \param subscription the Subscription to register.
-   * \return an unsigned 64-bit integer which is the subscription's unique id.
-   */
-  RCLCPP_PUBLIC
-  uint64_t
-  add_subscription(SubscriptionBase::SharedPtr subscription);
-
-  /// Unregister a subscription using the subscription's unique id.
-  /**
-   * This method does not allocate memory.
-   *
-   * \param intra_process_subscription_id id of the subscription to remove.
-   */
-  RCLCPP_PUBLIC
-  void
-  remove_subscription(uint64_t intra_process_subscription_id);
-
-  /// Register a publisher with the manager, returns the publisher unique id.
-  /**
-   * In addition to generating and returning a unique id for the publisher,
-   * this method creates internal ring buffer storage for "in-flight" intra
-   * process messages which are stored when store_intra_process_message is
-   * called with this publisher's unique id.
-   *
-   * The buffer_size must be less than or equal to the max uint64_t value.
-   * If the buffer_size is 0 then a buffer size is calculated using the
-   * publisher's QoS settings.
-   * The default is to use the depth field of the publisher's QoS.
-   * TODO(wjwwood): Consider doing depth *= 1.2, round up, or similar.
-   * TODO(wjwwood): Consider what to do for keep all.
-   *
-   * This method is templated on the publisher's message type so that internal
-   * storage of the same type can be allocated.
-   *
-   * This method will allocate memory.
-   *
-   * \param publisher publisher to be registered with the manager.
-   * \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.
-   */
-  RCLCPP_PUBLIC
-  uint64_t
-  add_publisher(
-    rclcpp::PublisherBase::SharedPtr publisher,
-    size_t buffer_size = 0);
-
-  /// Unregister a publisher using the publisher's unique id.
-  /**
-   * This method does not allocate memory.
-   *
-   * \param intra_process_publisher_id id of the publisher to remove.
-   */
-  RCLCPP_PUBLIC
-  void
-  remove_publisher(uint64_t intra_process_publisher_id);
-
-  /// Store a message in the manager, and return the message sequence number.
-  /**
-   * The given message is stored in internal storage using the given publisher
-   * id and the newly generated message sequence, which is also returned.
-   * The combination of publisher id and message sequence number can later
-   * be used with a subscription id to retrieve the message by calling
-   * take_intra_process_message.
-   * The number of times take_intra_process_message can be called with this
-   * unique pair of id's is determined by the number of subscriptions currently
-   * subscribed to the same topic and which share the same Context, i.e. once
-   * for each subscription which should receive the intra process message.
-   *
-   * The ownership of the incoming message is transfered to the internal
-   * storage in order to avoid copying the message data.
-   * Therefore, the message parameter will no longer contain the original
-   * message after calling this method.
-   * Instead it will either be a nullptr or it will contain the ownership of
-   * the message instance which was displaced.
-   * If the message parameter is not equal to nullptr after calling this method
-   * then a message was prematurely displaced, i.e. take_intra_process_message
-   * had not been called on it as many times as was expected.
-   *
-   * This method can throw an exception if the publisher id is not found or
-   * if the publisher shared_ptr given to add_publisher has gone out of scope.
-   *
-   * This method does allocate memory.
-   *
-   * \param intra_process_publisher_id the id of the publisher of this message.
-   * \param message the message that is being stored.
-   * \return the message sequence number.
-   */
-  template<
-    typename MessageT, typename Alloc = std::allocator<void>>
-  uint64_t
-  store_intra_process_message(
-    uint64_t intra_process_publisher_id,
-    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>;
-    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, 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;
-  }
-
-  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
-   * uniquely identify a message instance, which should be taken.
-   *
-   * The requesting_subscriptions_intra_process_id parameter is used to make
-   * sure the requesting subscription was intended to receive this message
-   * instance.
-   * This check is made because it could happen that the requester
-   * comes up after the publish event, so it still receives the notification of
-   * a new intra process message, but it wasn't registered with the manager at
-   * the time of publishing, causing it to take when it wasn't intended.
-   * This should be avioded unless latching-like behavior is involved.
-   *
-   * The message parameter is used to store the taken message.
-   * On the last expected call to this method, the ownership is transfered out
-   * of internal storage and into the message parameter.
-   * On all previous calls a copy of the internally stored message is made and
-   * the ownership of the copy is transfered to the message parameter.
-   * TODO(wjwwood): update this documentation when latching is supported.
-   *
-   * The message parameter can be set to nullptr if:
-   *
-   * - The publisher id is not found.
-   * - The message sequence is not found for the given publisher id.
-   * - The requesting subscription's id is not in the list of intended takers.
-   * - The requesting subscription's id has been used before with this message.
-   *
-   * This method may allocate memory to copy the stored message.
-   *
-   * \param intra_process_publisher_id the id of the message's publisher.
-   * \param message_sequence_number the sequence number of the message.
-   * \param requesting_subscriptions_intra_process_id the subscription's id.
-   * \param message the message typed unique_ptr used to return the message.
-   */
-  template<
-    typename MessageT, typename Alloc = std::allocator<void>,
-    typename Deleter = std::default_delete<MessageT>>
-  void
-  take_intra_process_message(
-    uint64_t intra_process_publisher_id,
-    uint64_t message_sequence_number,
-    uint64_t requesting_subscriptions_intra_process_id,
-    std::unique_ptr<MessageT, Deleter> & 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);
-    }
-  }
-
-  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);
-    }
-  }
-
-  /// Return true if the given rmw_gid_t matches any stored Publishers.
-  RCLCPP_PUBLIC
-  bool
-  matches_any_publishers(const rmw_gid_t * id) const;
-
-  /// Return 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
-  get_next_unique_id();
-
-  IntraProcessManagerImplBase::SharedPtr impl_;
-  std::mutex take_mutex_;
-};
-
-}  // namespace intra_process_manager
-}  // namespace rclcpp
-
-#endif  // RCLCPP__INTRA_PROCESS_MANAGER_HPP_

rclcpp/include/rclcpp/intra_process_manager_impl.hpp

@@ -1,358 +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.
-
-#ifndef RCLCPP__INTRA_PROCESS_MANAGER_IMPL_HPP_
-#define RCLCPP__INTRA_PROCESS_MANAGER_IMPL_HPP_
-
-#include <algorithm>
-#include <array>
-#include <atomic>
-#include <cstring>
-#include <functional>
-#include <limits>
-#include <map>
-#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_base.hpp"
-#include "rclcpp/subscription_base.hpp"
-#include "rclcpp/visibility_control.hpp"
-
-namespace rclcpp
-{
-namespace intra_process_manager
-{
-
-class IntraProcessManagerImplBase
-{
-public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(IntraProcessManagerImplBase)
-
-  IntraProcessManagerImplBase() = default;
-  virtual ~IntraProcessManagerImplBase() = default;
-
-  virtual void
-  add_subscription(uint64_t id, SubscriptionBase::SharedPtr subscription) = 0;
-
-  virtual void
-  remove_subscription(uint64_t intra_process_subscription_id) = 0;
-
-  virtual void add_publisher(
-    uint64_t id,
-    PublisherBase::WeakPtr publisher,
-    mapped_ring_buffer::MappedRingBufferBase::SharedPtr mrb,
-    size_t size) = 0;
-
-  virtual void
-  remove_publisher(uint64_t intra_process_publisher_id) = 0;
-
-  virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr
-  get_publisher_info_for_id(
-    uint64_t intra_process_publisher_id,
-    uint64_t & message_seq) = 0;
-
-  virtual void
-  store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq) = 0;
-
-  virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr
-  take_intra_process_message(
-    uint64_t intra_process_publisher_id,
-    uint64_t message_sequence_number,
-    uint64_t requesting_subscriptions_intra_process_id,
-    size_t & size) = 0;
-
-  virtual bool
-  matches_any_publishers(const rmw_gid_t * id) const = 0;
-
-  virtual size_t
-  get_subscription_count(uint64_t intra_process_publisher_id) const = 0;
-
-private:
-  RCLCPP_DISABLE_COPY(IntraProcessManagerImplBase)
-};
-
-template<typename Allocator = std::allocator<void>>
-class IntraProcessManagerImpl : public IntraProcessManagerImplBase
-{
-public:
-  IntraProcessManagerImpl() = default;
-  ~IntraProcessManagerImpl() = default;
-
-  void
-  add_subscription(uint64_t id, SubscriptionBase::SharedPtr subscription)
-  {
-    subscriptions_[id] = subscription;
-    subscription_ids_by_topic_[fixed_size_string(subscription->get_topic_name())].insert(id);
-  }
-
-  void
-  remove_subscription(uint64_t intra_process_subscription_id)
-  {
-    subscriptions_.erase(intra_process_subscription_id);
-    for (auto & pair : subscription_ids_by_topic_) {
-      pair.second.erase(intra_process_subscription_id);
-    }
-    // Iterate over all publisher infos and all stored subscription id's and
-    // remove references to this subscription's id.
-    for (auto & publisher_pair : publishers_) {
-      for (auto & sub_pair : publisher_pair.second.target_subscriptions_by_message_sequence) {
-        sub_pair.second.erase(intra_process_subscription_id);
-      }
-    }
-  }
-
-  void add_publisher(
-    uint64_t id,
-    PublisherBase::WeakPtr publisher,
-    mapped_ring_buffer::MappedRingBufferBase::SharedPtr mrb,
-    size_t size)
-  {
-    publishers_[id].publisher = publisher;
-    // As long as the size of the ring buffer is less than the max sequence number, we're safe.
-    if (size > std::numeric_limits<uint64_t>::max()) {
-      throw std::invalid_argument("the calculated buffer size is too large");
-    }
-    publishers_[id].sequence_number.store(0);
-
-    publishers_[id].buffer = mrb;
-    publishers_[id].target_subscriptions_by_message_sequence.reserve(size);
-  }
-
-  void
-  remove_publisher(uint64_t intra_process_publisher_id)
-  {
-    publishers_.erase(intra_process_publisher_id);
-  }
-
-  // return message_seq and mrb
-  mapped_ring_buffer::MappedRingBufferBase::SharedPtr
-  get_publisher_info_for_id(
-    uint64_t intra_process_publisher_id,
-    uint64_t & message_seq)
-  {
-    std::lock_guard<std::mutex> lock(runtime_mutex_);
-    auto it = publishers_.find(intra_process_publisher_id);
-    if (it == publishers_.end()) {
-      throw std::runtime_error("get_publisher_info_for_id called with invalid publisher id");
-    }
-    PublisherInfo & info = it->second;
-    // Calculate the next message sequence number.
-    message_seq = info.sequence_number.fetch_add(1);
-
-    return info.buffer;
-  }
-
-  void
-  store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq)
-  {
-    std::lock_guard<std::mutex> lock(runtime_mutex_);
-    auto it = publishers_.find(intra_process_publisher_id);
-    if (it == publishers_.end()) {
-      throw std::runtime_error("store_intra_process_message called with invalid publisher id");
-    }
-    PublisherInfo & info = it->second;
-    auto publisher = info.publisher.lock();
-    if (!publisher) {
-      throw std::runtime_error("publisher has unexpectedly gone out of scope");
-    }
-
-    // Figure out what subscriptions should receive the message.
-    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(
-        message_seq, AllocSet(std::less<uint64_t>(), uint64_allocator));
-    } else {
-      info.target_subscriptions_by_message_sequence[message_seq].clear();
-    }
-    std::copy(
-      destined_subscriptions.begin(), destined_subscriptions.end(),
-      // Memory allocation occurs in info.target_subscriptions_by_message_sequence[message_seq]
-      std::inserter(
-        info.target_subscriptions_by_message_sequence[message_seq],
-        // This ends up only being a hint to std::set, could also be .begin().
-        info.target_subscriptions_by_message_sequence[message_seq].end()
-      )
-    );
-  }
-
-  mapped_ring_buffer::MappedRingBufferBase::SharedPtr
-  take_intra_process_message(
-    uint64_t intra_process_publisher_id,
-    uint64_t message_sequence_number,
-    uint64_t requesting_subscriptions_intra_process_id,
-    size_t & size
-  )
-  {
-    std::lock_guard<std::mutex> lock(runtime_mutex_);
-    PublisherInfo * info;
-    {
-      auto it = publishers_.find(intra_process_publisher_id);
-      if (it == publishers_.end()) {
-        // Publisher is either invalid or no longer exists.
-        return 0;
-      }
-      info = &it->second;
-    }
-    // Figure out how many subscriptions are left.
-    AllocSet * target_subs;
-    {
-      auto it = info->target_subscriptions_by_message_sequence.find(message_sequence_number);
-      if (it == info->target_subscriptions_by_message_sequence.end()) {
-        // Message is no longer being stored by this publisher.
-        return 0;
-      }
-      target_subs = &it->second;
-    }
-    {
-      auto it = std::find(
-        target_subs->begin(), target_subs->end(),
-        requesting_subscriptions_intra_process_id);
-      if (it == target_subs->end()) {
-        // This publisher id/message seq pair was not intended for this subscription.
-        return 0;
-      }
-      target_subs->erase(it);
-    }
-    size = target_subs->size();
-    return info->buffer;
-  }
-
-  bool
-  matches_any_publishers(const rmw_gid_t * id) const
-  {
-    for (auto & publisher_pair : publishers_) {
-      auto publisher = publisher_pair.second.publisher.lock();
-      if (!publisher) {
-        continue;
-      }
-      if (*publisher.get() == id) {
-        return true;
-      }
-    }
-    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>;
-
-  RebindAlloc<uint64_t> uint64_allocator;
-
-  using AllocSet = std::set<uint64_t, std::less<uint64_t>, RebindAlloc<uint64_t>>;
-  using SubscriptionMap = std::unordered_map<
-    uint64_t, SubscriptionBase::WeakPtr,
-    std::hash<uint64_t>, std::equal_to<uint64_t>,
-    RebindAlloc<std::pair<const uint64_t, SubscriptionBase::WeakPtr>>>;
-
-  using IDTopicMap = std::map<
-    FixedSizeString,
-    AllocSet,
-    strcmp_wrapper,
-    RebindAlloc<std::pair<const FixedSizeString, AllocSet>>>;
-
-  SubscriptionMap subscriptions_;
-
-  IDTopicMap subscription_ids_by_topic_;
-
-  struct PublisherInfo
-  {
-    RCLCPP_DISABLE_COPY(PublisherInfo)
-
-    PublisherInfo() = default;
-
-    PublisherBase::WeakPtr publisher;
-    std::atomic<uint64_t> sequence_number;
-    mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer;
-
-    using TargetSubscriptionsMap = std::unordered_map<
-      uint64_t, AllocSet,
-      std::hash<uint64_t>, std::equal_to<uint64_t>,
-      RebindAlloc<std::pair<const uint64_t, AllocSet>>>;
-    TargetSubscriptionsMap target_subscriptions_by_message_sequence;
-  };
-
-  using PublisherMap = std::unordered_map<
-    uint64_t, PublisherInfo,
-    std::hash<uint64_t>, std::equal_to<uint64_t>,
-    RebindAlloc<std::pair<const uint64_t, PublisherInfo>>>;
-
-  PublisherMap publishers_;
-
-  std::mutex runtime_mutex_;
-};
-
-RCLCPP_PUBLIC
-IntraProcessManagerImplBase::SharedPtr
-create_default_impl();
-
-}  // namespace intra_process_manager
-}  // namespace rclcpp
-
-#endif  // RCLCPP__INTRA_PROCESS_MANAGER_IMPL_HPP_

rclcpp/include/rclcpp/is_ros_compatible_type.hpp

@@ -0,0 +1,35 @@
+// Copyright 2021 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__IS_ROS_COMPATIBLE_TYPE_HPP_
+#define RCLCPP__IS_ROS_COMPATIBLE_TYPE_HPP_
+
+#include "rosidl_runtime_cpp/traits.hpp"
+
+#include "rclcpp/type_adapter.hpp"
+
+namespace rclcpp
+{
+
+template<typename T>
+struct is_ros_compatible_type
+{
+  static constexpr bool value =
+    rosidl_generator_traits::is_message<T>::value ||
+    rclcpp::TypeAdapter<T>::is_specialized::value;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__IS_ROS_COMPATIBLE_TYPE_HPP_

rclcpp/include/rclcpp/loaned_message.hpp

@@ -0,0 +1,199 @@
+// 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__LOANED_MESSAGE_HPP_
+#define RCLCPP__LOANED_MESSAGE_HPP_
+
+#include <memory>
+#include <utility>
+
+#include "rclcpp/allocator/allocator_common.hpp"
+#include "rclcpp/logging.hpp"
+#include "rclcpp/publisher_base.hpp"
+
+#include "rcl/allocator.h"
+#include "rcl/publisher.h"
+
+namespace rclcpp
+{
+
+template<typename MessageT, typename AllocatorT = std::allocator<void>>
+class LoanedMessage
+{
+public:
+  using MessageAllocatorTraits = rclcpp::allocator::AllocRebind<MessageT, AllocatorT>;
+  using MessageAllocator = typename MessageAllocatorTraits::allocator_type;
+
+  /// Constructor of the LoanedMessage class.
+  /**
+   * The constructor of this class allocates memory for a given message type
+   * and associates this with a given publisher.
+   *
+   * The underlying middleware is queried to determine whether it is able to allocate the
+   * appropriate memory for this message type or not.
+   * In the case that the middleware cannot loan messages, the passed in allocator instance
+   * is used to allocate the message within the scope of this class.
+   * Otherwise, the allocator is ignored and the allocation is solely performed
+   * in the underlying middleware with its appropriate allocation strategy.
+   * The need for this arises as the user code can be written explicitly targeting a middleware
+   * capable of loaning messages.
+   * However, this user code is ought to be usable even when dynamically linked against
+   * a middleware which doesn't support message loaning in which case the allocator will be used.
+   *
+   * \param[in] pub rclcpp::Publisher instance to which the memory belongs
+   * \param[in] allocator Allocator instance in case middleware cannot allocate messages
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
+   */
+  LoanedMessage(
+    const rclcpp::PublisherBase & pub,
+    MessageAllocator allocator)
+  : pub_(pub),
+    message_(nullptr),
+    message_allocator_(std::move(allocator))
+  {
+    if (pub_.can_loan_messages()) {
+      void * message_ptr = nullptr;
+      auto ret = rcl_borrow_loaned_message(
+        pub_.get_publisher_handle().get(),
+        rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>(),
+        &message_ptr);
+      if (RCL_RET_OK != ret) {
+        rclcpp::exceptions::throw_from_rcl_error(ret);
+      }
+      message_ = static_cast<MessageT *>(message_ptr);
+    } else {
+      RCLCPP_INFO_ONCE(
+        rclcpp::get_logger("rclcpp"),
+        "Currently used middleware can't loan messages. Local allocator will be used.");
+      message_ = message_allocator_.allocate(1);
+      new (message_) MessageT();
+    }
+  }
+
+  /// Move semantic for RVO
+  LoanedMessage(LoanedMessage<MessageT> && other)
+  : pub_(std::move(other.pub_)),
+    message_(std::move(other.message_)),
+    message_allocator_(std::move(other.message_allocator_))
+  {
+    other.message_ = nullptr;
+  }
+
+  /// Destructor of the LoanedMessage class.
+  /**
+   * The destructor has the explicit task to return the allocated memory for its message
+   * instance.
+   * If the message was previously allocated via the middleware, the message is getting
+   * returned to the middleware to cleanly destroy the allocation.
+   * In the case that the local allocator instance was used, the same instance is then
+   * being used to destroy the allocated memory.
+   *
+   * The contract here is that the memory for this message is valid as long as this instance
+   * of the LoanedMessage class is alive.
+   */
+  virtual ~LoanedMessage()
+  {
+    auto error_logger = rclcpp::get_logger("LoanedMessage");
+
+    if (message_ == nullptr) {
+      return;
+    }
+
+    if (pub_.can_loan_messages()) {
+      // return allocated memory to the middleware
+      auto ret =
+        rcl_return_loaned_message_from_publisher(pub_.get_publisher_handle().get(), message_);
+      if (ret != RCL_RET_OK) {
+        RCLCPP_ERROR(
+          error_logger, "rcl_deallocate_loaned_message failed: %s", rcl_get_error_string().str);
+        rcl_reset_error();
+      }
+    } else {
+      // call destructor before deallocating
+      message_->~MessageT();
+      message_allocator_.deallocate(message_, 1);
+    }
+    message_ = nullptr;
+  }
+
+  /// Validate if the message was correctly allocated.
+  /**
+   * The allocated memory might not be always consistent and valid.
+   * Reasons why this could fail is that an allocation step was failing,
+   * e.g. just like malloc could fail or a maximum amount of previously allocated
+   * messages is exceeded in which case the loaned messages have to be returned
+   * to the middleware prior to be able to allocate a new one.
+   */
+  bool is_valid() const
+  {
+    return message_ != nullptr;
+  }
+
+  /// Access the ROS message instance.
+  /**
+   * A call to `get()` will return a mutable reference to the underlying ROS message instance.
+   * This allows a user to modify the content of the message prior to publishing it.
+   *
+   * If this reference is copied, the memory for this copy is no longer managed
+   * by the LoanedMessage instance and has to be cleanup individually.
+   */
+  MessageT & get() const
+  {
+    return *message_;
+  }
+
+  /// Release ownership of the ROS message instance.
+  /**
+   * A call to `release()` will unmanage the memory for the ROS message.
+   * That means that the destructor of this class will not free the memory on scope exit.
+   * If the message is loaned from the middleware but not be published, the user needs to call
+   * `rcl_return_loaned_message_from_publisher` manually.
+   * If the memory is from the local allocator, the memory is freed when the unique pointer
+   * goes out instead.
+   *
+   * \return std::unique_ptr to the message instance.
+   */
+  std::unique_ptr<MessageT, std::function<void(MessageT *)>>
+  release()
+  {
+    auto msg = message_;
+    message_ = nullptr;
+
+    if (pub_.can_loan_messages()) {
+      return std::unique_ptr<MessageT, std::function<void(MessageT *)>>(msg, [](MessageT *) {});
+    }
+
+    return std::unique_ptr<MessageT, std::function<void(MessageT *)>>(
+      msg,
+      [allocator = message_allocator_](MessageT * msg_ptr) mutable {
+        // call destructor before deallocating
+        msg_ptr->~MessageT();
+        allocator.deallocate(msg_ptr, 1);
+      });
+  }
+
+protected:
+  const rclcpp::PublisherBase & pub_;
+
+  MessageT * message_;
+
+  MessageAllocator message_allocator_;
+
+  /// Deleted copy constructor to preserve memory integrity.
+  LoanedMessage(const LoanedMessage<MessageT> & other) = delete;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__LOANED_MESSAGE_HPP_

rclcpp/include/rclcpp/logger.hpp

@@ -15,12 +15,16 @@
 #ifndef RCLCPP__LOGGER_HPP_
 #define RCLCPP__LOGGER_HPP_
 
+#include <filesystem>
 #include <memory>
 #include <string>
+#include <utility>
 
 #include "rclcpp/visibility_control.hpp"
 
 #include "rcl/node.h"
+#include "rcutils/logging.h"
+#include "rcpputils/filesystem_helper.hpp"
 
 /**
  * \def RCLCPP_LOGGING_ENABLED
@@ -74,8 +78,32 @@ RCLCPP_PUBLIC
 Logger
 get_node_logger(const rcl_node_t * node);
 
+/// Get the current logging directory.
+/**
+ * For more details of how the logging directory is determined,
+ * see rcl_logging_get_logging_directory().
+ *
+ * \returns the logging directory being used.
+ * \throws rclcpp::exceptions::RCLError if an unexpected error occurs.
+ */
+RCLCPP_PUBLIC
+std::filesystem::path
+get_log_directory();
+
 class Logger
 {
+public:
+  /// An enum for the type of logger level.
+  enum class Level
+  {
+    Unset = RCUTILS_LOG_SEVERITY_UNSET,  ///< The unset log level
+    Debug = RCUTILS_LOG_SEVERITY_DEBUG,  ///< The debug log level
+    Info = RCUTILS_LOG_SEVERITY_INFO,    ///< The info log level
+    Warn = RCUTILS_LOG_SEVERITY_WARN,    ///< The warn log level
+    Error = RCUTILS_LOG_SEVERITY_ERROR,  ///< The error log level
+    Fatal = RCUTILS_LOG_SEVERITY_FATAL,  ///< The fatal log level
+  };
+
 private:
   friend Logger rclcpp::get_logger(const std::string & name);
   friend ::rclcpp::node_interfaces::NodeLogging;
@@ -96,11 +124,9 @@ private:
   : name_(new std::string(name)) {}
 
   std::shared_ptr<const std::string> name_;
+  std::shared_ptr<std::pair<std::string, std::string>> logger_sublogger_pairname_ = nullptr;
 
 public:
-  RCLCPP_PUBLIC
-  Logger(const Logger &) = default;
-
   /// Get the name of this logger.
   /**
    * \return the full name of the logger including any prefixes, or
@@ -131,13 +157,35 @@ public:
    */
   RCLCPP_PUBLIC
   Logger
-  get_child(const std::string & suffix)
-  {
-    if (!name_) {
-      return Logger();
-    }
-    return Logger(*name_ + "." + suffix);
-  }
+  get_child(const std::string & suffix);
+
+  /// Set level for current logger.
+  /**
+   * \param[in] level the logger's level
+   * \throws rclcpp::exceptions::RCLInvalidArgument if level is invalid.
+   * \throws rclcpp::exceptions::RCLError if other error happens.
+   */
+  RCLCPP_PUBLIC
+  void
+  set_level(Level level);
+
+  /// Get effective level for current logger.
+  /**
+   * The effective level is determined as the severity level of
+   * the logger if it is set, otherwise it is the first specified severity
+   * level of the logger's ancestors, starting with its closest ancestor.
+   * The ancestor hierarchy is signified by logger names being separated by dots:
+   * a logger named `x` is an ancestor of `x.y`, and both `x` and `x.y` are
+   * ancestors of `x.y.z`, etc.
+   * If the level has not been set for the logger nor any of its
+   * ancestors, the default level is used.
+   *
+   * \throws rclcpp::exceptions::RCLError if any error happens.
+   * \return Level for the current logger.
+   */
+  RCLCPP_PUBLIC
+  Level
+  get_effective_level() const;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/logging.hpp

@@ -0,0 +1,983 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__LOGGING_HPP_
+#define RCLCPP__LOGGING_HPP_
+
+#include <sstream>
+#include <type_traits>
+
+#include "rclcpp/logger.hpp"
+#include "rcutils/logging_macros.h"
+
+// These are used for compiling out logging macros lower than a minimum severity.
+#define RCLCPP_LOG_MIN_SEVERITY_DEBUG 0
+#define RCLCPP_LOG_MIN_SEVERITY_INFO 1
+#define RCLCPP_LOG_MIN_SEVERITY_WARN 2
+#define RCLCPP_LOG_MIN_SEVERITY_ERROR 3
+#define RCLCPP_LOG_MIN_SEVERITY_FATAL 4
+#define RCLCPP_LOG_MIN_SEVERITY_NONE 5
+
+#define RCLCPP_STATIC_ASSERT_LOGGER(logger) \
+  do { \
+    static_assert( \
+      ::std::is_convertible_v<decltype(logger), ::rclcpp::Logger>, \
+      "First argument to logging macros must be an rclcpp::Logger"); \
+  } while (0)
+
+/**
+ * \def RCLCPP_LOG
+ * Log a message with given severity.
+ * \param logger The `rclcpp::Logger` to use
+ * \param ... The format string, followed by the variable arguments for the format string.
+ */
+#define RCLCPP_LOG(severity, logger, ...) \
+  do { \
+    RCLCPP_STATIC_ASSERT_LOGGER(logger); \
+    RCUTILS_LOG_NAMED(severity, (logger).get_name(), __VA_ARGS__); \
+  } while (0)
+
+/**
+ * \def RCLCPP_LOG_ONCE
+ * Log a message with given severity with the following condition:
+ * - All log calls except the first one are ignored.
+ *
+ * \param logger The `rclcpp::Logger` to use
+ * \param ... The format string, followed by the variable arguments for the format string.
+ */
+#define RCLCPP_LOG_ONCE(severity, logger, ...) \
+  do { \
+    RCLCPP_STATIC_ASSERT_LOGGER(logger); \
+    RCUTILS_LOG_ONCE_NAMED(severity, (logger).get_name(), __VA_ARGS__); \
+  } while (0)
+
+/**
+ * \def RCLCPP_LOG_EXPRESSION
+ * Log a message with given severity with the following condition:
+ * - Log calls are ignored when the expression evaluates to false.
+ *
+ * \param logger The `rclcpp::Logger` to use
+ * \param expression The expression determining if the message should be logged
+ * \param ... The format string, followed by the variable arguments for the format string.
+ */
+#define RCLCPP_LOG_EXPRESSION(severity, logger, expression, ...) \
+  do { \
+    RCLCPP_STATIC_ASSERT_LOGGER(logger); \
+    RCUTILS_LOG_EXPRESSION_NAMED(severity, expression, (logger).get_name(), __VA_ARGS__); \
+  } while (0)
+
+/**
+ * \def RCLCPP_LOG_FUNCTION
+ * Log a message with given severity with the following condition:
+ * - Log calls are ignored when the function returns false.
+ *
+ * \param logger The `rclcpp::Logger` to use
+ * \param function The functions return value determines if the message should be logged
+ * \param ... The format string, followed by the variable arguments for the format string.
+ */
+#define RCLCPP_LOG_FUNCTION(severity, logger, function, ...) \
+  do { \
+    RCLCPP_STATIC_ASSERT_LOGGER(logger); \
+    RCUTILS_LOG_FUNCTION_NAMED(severity, function, (logger).get_name(), __VA_ARGS__); \
+  } while (0)
+
+/**
+ * \def RCLCPP_LOG_SKIPFIRST
+ * Log a message with given severity with the following condition:
+ * - The first log call is ignored but all subsequent calls are processed.
+ *
+ * \param logger The `rclcpp::Logger` to use
+ * \param ... The format string, followed by the variable arguments for the format string.
+ */
+#define RCLCPP_LOG_SKIPFIRST(severity, logger, ...) \
+  do { \
+    RCLCPP_STATIC_ASSERT_LOGGER(logger); \
+    RCUTILS_LOG_SKIPFIRST_NAMED(severity, (logger).get_name(), __VA_ARGS__); \
+  } while (0)
+
+#define RCLCPP_LOG_TIME_POINT_FUNC(clock) \
+  [&c = clock](rcutils_time_point_value_t * time_point)->rcutils_ret_t { \
+    try { \
+      *time_point = c.now().nanoseconds(); \
+    } catch (...) { \
+      RCUTILS_SAFE_FWRITE_TO_STDERR( \
+      "[rclcpp|logging.hpp] RCLCPP_DEBUG_THROTTLE could not get current time stamp\n"); \
+      return RCUTILS_RET_ERROR; \
+    } \
+    return RCUTILS_RET_OK; \
+  }
+
+/**
+ * \def RCLCPP_LOG_THROTTLE
+ * Log a message with given severity with the following condition:
+ * - Log calls are ignored if the last logged message is not longer ago than the specified duration.
+ *
+ * \param logger The `rclcpp::Logger` to use
+ * \param clock rclcpp::Clock that will be used to get the time point.
+ * \param duration The duration of the throttle interval as an integral value in milliseconds.
+ * \param ... The format string, followed by the variable arguments for the format string.
+ */
+#define RCLCPP_LOG_THROTTLE(severity, logger, clock, duration, ...) \
+  do { \
+    RCLCPP_STATIC_ASSERT_LOGGER(logger); \
+    RCUTILS_LOG_THROTTLE_NAMED( \
+      severity, \
+      RCLCPP_LOG_TIME_POINT_FUNC(clock), \
+      duration, \
+      (logger).get_name(), \
+      __VA_ARGS__); \
+  } while (0)
+
+/**
+ * \def RCLCPP_LOG_SKIPFIRST_THROTTLE
+ * Log a message with given severity with the following conditions:
+ * - The first log call is ignored but all subsequent calls are processed.
+ * - Log calls are ignored if the last logged message is not longer ago than the specified duration.
+ *
+ * \param logger The `rclcpp::Logger` to use
+ * \param clock rclcpp::Clock that will be used to get the time point.
+ * \param duration The duration of the throttle interval as an integral value in milliseconds.
+ * \param ... The format string, followed by the variable arguments for the format string.
+ */
+#define RCLCPP_LOG_SKIPFIRST_THROTTLE(severity, logger, clock, duration, ...) \
+  do { \
+    RCLCPP_STATIC_ASSERT_LOGGER(logger); \
+    RCUTILS_LOG_SKIPFIRST_THROTTLE_NAMED( \
+      severity, \
+      RCLCPP_LOG_TIME_POINT_FUNC(clock), \
+      duration, \
+      (logger).get_name(), \
+      __VA_ARGS__); \
+  } while (0)
+
+/**
+ * \def RCLCPP_LOG_STREAM
+ * Log a message with given severity.
+ *
+ * \param logger The `rclcpp::Logger` to use
+ * \param stream_arg The argument << into a stringstream
+ */
+#define RCLCPP_LOG_STREAM(severity, logger, stream_arg) \
+  do { \
+    RCLCPP_STATIC_ASSERT_LOGGER(logger); \
+    std::stringstream rclcpp_stream_ss_; \
+    rclcpp_stream_ss_ << stream_arg; \
+    RCUTILS_LOG_NAMED(severity, (logger).get_name(), "%s", rclcpp_stream_ss_.str().c_str()); \
+  } while (0)
+
+/**
+ * \def RCLCPP_LOG_STREAM_ONCE
+ * Log a message with given severity with the following condition:
+ * - All log calls except the first one are ignored.
+ *
+ * \param logger The `rclcpp::Logger` to use
+ * \param stream_arg The argument << into a stringstream
+ */
+#define RCLCPP_LOG_STREAM_ONCE(severity, logger, stream_arg) \
+  do { \
+    RCLCPP_STATIC_ASSERT_LOGGER(logger); \
+    std::stringstream rclcpp_stream_ss_; \
+    rclcpp_stream_ss_ << stream_arg; \
+    RCUTILS_LOG_ONCE_NAMED(severity, (logger).get_name(), "%s", rclcpp_stream_ss_.str().c_str()); \
+  } while (0)
+
+/**
+ * \def RCLCPP_LOG_STREAM_EXPRESSION
+ * Log a message with given severity with the following condition:
+ * - Log calls are being ignored when the expression evaluates to false.
+ *
+ * \param logger The `rclcpp::Logger` to use
+ * \param expression The expression determining if the message should be logged
+ * \param stream_arg The argument << into a stringstream
+ */
+#define RCLCPP_LOG_STREAM_EXPRESSION(severity, logger, expression, stream_arg) \
+  do { \
+    RCLCPP_STATIC_ASSERT_LOGGER(logger); \
+    std::stringstream rclcpp_stream_ss_; \
+    rclcpp_stream_ss_ << stream_arg; \
+    RCUTILS_LOG_EXPRESSION_NAMED( \
+      severity, \
+      expression, \
+      (logger).get_name(), \
+      "%s", rclcpp_stream_ss_.str().c_str()); \
+  } while (0)
+
+/**
+ * \def RCLCPP_LOG_STREAM_FUNCTION
+ * Log a message with given severity with the following condition:
+ * - Log calls are being ignored when the function returns false.
+ *
+ * \param logger The `rclcpp::Logger` to use
+ * \param function The functions return value determines if the message should be logged
+ * \param stream_arg The argument << into a stringstream
+ */
+#define RCLCPP_LOG_STREAM_FUNCTION(severity, logger, function, stream_arg) \
+  do { \
+    RCLCPP_STATIC_ASSERT_LOGGER(logger); \
+    std::stringstream rclcpp_stream_ss_; \
+    rclcpp_stream_ss_ << stream_arg; \
+    RCUTILS_LOG_FUNCTION_NAMED( \
+      severity, \
+      function, \
+      (logger).get_name(), \
+      "%s", rclcpp_stream_ss_.str().c_str()); \
+  } while (0)
+
+/**
+ * \def RCLCPP_LOG_STREAM_SKIPFIRST
+ * Log a message with given severity with the following condition:
+ * - The first log call is ignored but all subsequent calls are processed.
+ *
+ * \param logger The `rclcpp::Logger` to use
+ * \param stream_arg The argument << into a stringstream
+ */
+#define RCLCPP_LOG_STREAM_SKIPFIRST(severity, logger, stream_arg) \
+  do { \
+    RCLCPP_STATIC_ASSERT_LOGGER(logger); \
+    std::stringstream rclcpp_stream_ss_; \
+    rclcpp_stream_ss_ << stream_arg; \
+    RCUTILS_LOG_SKIPFIRST_NAMED( \
+      severity, \
+      (logger).get_name(), \
+      "%s", rclcpp_stream_ss_.str().c_str()); \
+  } while (0)
+
+/**
+ * \def RCLCPP_LOG_STREAM_THROTTLE
+ * Log a message with given severity with the following condition:
+ * - Log calls are ignored if the last logged message is not longer ago than the specified duration.
+ *
+ * \param logger The `rclcpp::Logger` to use
+ * \param clock rclcpp::Clock that will be used to get the time point.
+ * \param duration The duration of the throttle interval as an integral value in milliseconds.
+ * \param stream_arg The argument << into a stringstream
+ */
+#define RCLCPP_LOG_STREAM_THROTTLE(severity, logger, clock, duration, stream_arg) \
+  do { \
+    RCLCPP_STATIC_ASSERT_LOGGER(logger); \
+    std::stringstream rclcpp_stream_ss_; \
+    rclcpp_stream_ss_ << stream_arg; \
+    RCUTILS_LOG_THROTTLE_NAMED( \
+      severity, \
+      RCLCPP_LOG_TIME_POINT_FUNC(clock), \
+      duration, \
+      (logger).get_name(), \
+      "%s", rclcpp_stream_ss_.str().c_str()); \
+  } while (0)
+
+/**
+ * \def RCLCPP_LOG_STREAM_SKIPFIRST_THROTTLE
+ * Log a message with given severity with the following conditions:
+ * - The first log call is ignored but all subsequent calls are processed.
+ * - Log calls are ignored if the last logged message is not longer ago than the specified duration.
+ *
+ * \param logger The `rclcpp::Logger` to use
+ * \param clock rclcpp::Clock that will be used to get the time point.
+ * \param duration The duration of the throttle interval as an integral value in milliseconds.
+ * \param stream_arg The argument << into a stringstream
+ */
+#define RCLCPP_LOG_STREAM_SKIPFIRST_THROTTLE(severity, logger, clock, duration, stream_arg) \
+  do { \
+    RCLCPP_STATIC_ASSERT_LOGGER(logger); \
+    std::stringstream rclcpp_stream_ss_; \
+    rclcpp_stream_ss_ << stream_arg; \
+    RCUTILS_LOG_SKIPFIRST_THROTTLE_NAMED( \
+      severity, \
+      RCLCPP_LOG_TIME_POINT_FUNC(clock), \
+      duration, \
+      (logger).get_name(), \
+      "%s", rclcpp_stream_ss_.str().c_str()); \
+  } while (0)
+
+/**
+ * \def RCLCPP_LOG_MIN_SEVERITY
+ * Define RCLCPP_LOG_MIN_SEVERITY=RCLCPP_LOG_MIN_SEVERITY_[DEBUG|INFO|WARN|ERROR|FATAL]
+ * in your build options to compile out anything below that severity.
+ * Use RCLCPP_LOG_MIN_SEVERITY_NONE to compile out all macros.
+ */
+#ifndef RCLCPP_LOG_MIN_SEVERITY
+#define RCLCPP_LOG_MIN_SEVERITY RCLCPP_LOG_MIN_SEVERITY_DEBUG
+#endif
+
+/** @name Logging macros for severity DEBUG.
+ */
+#if (RCLCPP_LOG_MIN_SEVERITY > RCLCPP_LOG_MIN_SEVERITY_DEBUG)
+// empty logging macros for severity DEBUG when being disabled at compile time
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_DEBUG(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_DEBUG_ONCE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_DEBUG_EXPRESSION(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_DEBUG_FUNCTION(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_DEBUG_SKIPFIRST(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_DEBUG_THROTTLE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_DEBUG_SKIPFIRST_THROTTLE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_DEBUG_STREAM(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_DEBUG_STREAM_ONCE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_DEBUG_STREAM_EXPRESSION(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_DEBUG_STREAM_FUNCTION(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_DEBUG_STREAM_SKIPFIRST(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_DEBUG_STREAM_THROTTLE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_DEBUG_STREAM_SKIPFIRST_THROTTLE(...)
+
+#else
+/**
+ * \def RCLCPP_DEBUG
+ * \copydoc RCLCPP_LOG
+ */
+#define RCLCPP_DEBUG(logger, ...) RCLCPP_LOG(RCUTILS_LOG_SEVERITY_DEBUG, logger, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_DEBUG_ONCE
+ * \copydoc RCLCPP_LOG_ONCE
+ */
+#define RCLCPP_DEBUG_ONCE(logger, ...) \
+  RCLCPP_LOG_ONCE(RCUTILS_LOG_SEVERITY_DEBUG, logger, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_DEBUG_EXPRESSION
+ * \copydoc RCLCPP_LOG_EXPRESSION
+ */
+#define RCLCPP_DEBUG_EXPRESSION(logger, expression, ...) \
+  RCLCPP_LOG_EXPRESSION(RCUTILS_LOG_SEVERITY_DEBUG, logger, expression, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_DEBUG_FUNCTION
+ * \copydoc RCLCPP_LOG_FUNCTION
+ */
+#define RCLCPP_DEBUG_FUNCTION(logger, function, ...) \
+  RCLCPP_LOG_FUNCTION(RCUTILS_LOG_SEVERITY_DEBUG, logger, function, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_DEBUG_SKIPFIRST
+ * \copydoc RCLCPP_LOG_SKIPFIRST
+ */
+#define RCLCPP_DEBUG_SKIPFIRST(logger, ...) \
+  RCLCPP_LOG_SKIPFIRST(RCUTILS_LOG_SEVERITY_DEBUG, logger, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_DEBUG_THROTTLE
+ * \copydoc RCLCPP_LOG_THROTTLE
+ */
+#define RCLCPP_DEBUG_THROTTLE(logger, clock, duration, ...) \
+  RCLCPP_LOG_THROTTLE(RCUTILS_LOG_SEVERITY_DEBUG, logger, clock, duration, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_DEBUG_SKIPFIRST_THROTTLE
+ * \copydoc RCLCPP_LOG_SKIPFIRST_THROTTLE
+ */
+#define RCLCPP_DEBUG_SKIPFIRST_THROTTLE(logger, clock, duration, ...) \
+  RCLCPP_LOG_SKIPFIRST_THROTTLE(RCUTILS_LOG_SEVERITY_DEBUG, logger, clock, duration, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_DEBUG_STREAM
+ * \copydoc RCLCPP_LOG_STREAM
+ */
+#define RCLCPP_DEBUG_STREAM(logger, stream_arg) \
+  RCLCPP_LOG_STREAM(RCUTILS_LOG_SEVERITY_DEBUG, logger, stream_arg)
+
+/**
+ * \def RCLCPP_DEBUG_STREAM_ONCE
+ * \copydoc RCLCPP_LOG_STREAM_ONCE
+ */
+#define RCLCPP_DEBUG_STREAM_ONCE(logger, stream_arg) \
+  RCLCPP_LOG_STREAM_ONCE(RCUTILS_LOG_SEVERITY_DEBUG, logger, stream_arg)
+
+/**
+ * \def RCLCPP_DEBUG_STREAM_EXPRESSION
+ * \copydoc RCLCPP_LOG_STREAM_EXPRESSION
+ */
+#define RCLCPP_DEBUG_STREAM_EXPRESSION(logger, expression, stream_arg) \
+  RCLCPP_LOG_STREAM_EXPRESSION(RCUTILS_LOG_SEVERITY_DEBUG, logger, expression, stream_arg)
+
+/**
+ * \def RCLCPP_DEBUG_STREAM_FUNCTION
+ * \copydoc RCLCPP_LOG_STREAM_FUNCTION
+ */
+#define RCLCPP_DEBUG_STREAM_FUNCTION(logger, function, stream_arg) \
+  RCLCPP_LOG_STREAM_FUNCTION(RCUTILS_LOG_SEVERITY_DEBUG, logger, function, stream_args)
+
+/**
+ * \def RCLCPP_DEBUG_STREAM_SKIPFIRST
+ * \copydoc RCLCPP_LOG_STREAM_SKIPFIRST
+ */
+#define RCLCPP_DEBUG_STREAM_SKIPFIRST(logger, stream_arg) \
+  RCLCPP_LOG_STREAM_SKIPFIRST(RCUTILS_LOG_SEVERITY_DEBUG, logger, stream_arg)
+
+/**
+ * \def RCLCPP_DEBUG_STREAM_THROTTLE
+ * \copydoc RCLCPP_LOG_STREAM_THROTTLE
+ */
+#define RCLCPP_DEBUG_STREAM_THROTTLE(logger, clock, duration, stream_arg) \
+  RCLCPP_LOG_STREAM_THROTTLE(RCUTILS_LOG_SEVERITY_DEBUG, logger, clock, duration, stream_arg)
+
+/**
+ * \def RCLCPP_DEBUG_STREAM_SKIPFIRST_THROTTLE
+ * \copydoc RCLCPP_LOG_STREAM_SKIPFIRST_THROTTLE
+ */
+#define RCLCPP_DEBUG_STREAM_SKIPFIRST_THROTTLE(logger, clock, duration, stream_arg) \
+  RCLCPP_LOG_STREAM_SKIPFIRST_THROTTLE(RCUTILS_LOG_SEVERITY_DEBUG, logger, clock, duration, \
+    stream_arg)
+
+#endif
+
+/** @name Logging macros for severity INFO.
+ */
+#if (RCLCPP_LOG_MIN_SEVERITY > RCLCPP_LOG_MIN_SEVERITY_INFO)
+// empty logging macros for severity INFO when being disabled at compile time
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_INFO(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_INFO_ONCE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_INFO_EXPRESSION(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_INFO_FUNCTION(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_INFO_SKIPFIRST(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_INFO_THROTTLE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_INFO_SKIPFIRST_THROTTLE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_INFO_STREAM(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_INFO_STREAM_ONCE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_INFO_STREAM_EXPRESSION(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_INFO_STREAM_FUNCTION(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_INFO_STREAM_SKIPFIRST(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_INFO_STREAM_THROTTLE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_INFO_STREAM_SKIPFIRST_THROTTLE(...)
+
+#else
+/**
+ * \def RCLCPP_INFO
+ * \copydoc RCLCPP_LOG
+ */
+#define RCLCPP_INFO(logger, ...) RCLCPP_LOG(RCUTILS_LOG_SEVERITY_INFO, logger, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_INFO_ONCE
+ * \copydoc RCLCPP_LOG_ONCE
+ */
+#define RCLCPP_INFO_ONCE(logger, ...) \
+  RCLCPP_LOG_ONCE(RCUTILS_LOG_SEVERITY_INFO, logger, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_INFO_EXPRESSION
+ * \copydoc RCLCPP_LOG_EXPRESSION
+ */
+#define RCLCPP_INFO_EXPRESSION(logger, expression, ...) \
+  RCLCPP_LOG_EXPRESSION(RCUTILS_LOG_SEVERITY_INFO, logger, expression, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_INFO_FUNCTION
+ * \copydoc RCLCPP_LOG_FUNCTION
+ */
+#define RCLCPP_INFO_FUNCTION(logger, function, ...) \
+  RCLCPP_LOG_FUNCTION(RCUTILS_LOG_SEVERITY_INFO, logger, function, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_INFO_SKIPFIRST
+ * \copydoc RCLCPP_LOG_SKIPFIRST
+ */
+#define RCLCPP_INFO_SKIPFIRST(logger, ...) \
+  RCLCPP_LOG_SKIPFIRST(RCUTILS_LOG_SEVERITY_INFO, logger, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_INFO_THROTTLE
+ * \copydoc RCLCPP_LOG_THROTTLE
+ */
+#define RCLCPP_INFO_THROTTLE(logger, clock, duration, ...) \
+  RCLCPP_LOG_THROTTLE(RCUTILS_LOG_SEVERITY_INFO, logger, clock, duration, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_INFO_SKIPFIRST_THROTTLE
+ * \copydoc RCLCPP_LOG_SKIPFIRST_THROTTLE
+ */
+#define RCLCPP_INFO_SKIPFIRST_THROTTLE(logger, clock, duration, ...) \
+  RCLCPP_LOG_SKIPFIRST_THROTTLE(RCUTILS_LOG_SEVERITY_INFO, logger, clock, duration, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_INFO_STREAM
+ * \copydoc RCLCPP_LOG_STREAM
+ */
+#define RCLCPP_INFO_STREAM(logger, stream_arg) \
+  RCLCPP_LOG_STREAM(RCUTILS_LOG_SEVERITY_INFO, logger, stream_arg)
+
+/**
+ * \def RCLCPP_INFO_STREAM_ONCE
+ * \copydoc RCLCPP_LOG_STREAM_ONCE
+ */
+#define RCLCPP_INFO_STREAM_ONCE(logger, stream_arg) \
+  RCLCPP_LOG_STREAM_ONCE(RCUTILS_LOG_SEVERITY_INFO, logger, stream_arg)
+
+/**
+ * \def RCLCPP_INFO_STREAM_EXPRESSION
+ * \copydoc RCLCPP_LOG_STREAM_EXPRESSION
+ */
+#define RCLCPP_INFO_STREAM_EXPRESSION(logger, expression, stream_arg) \
+  RCLCPP_LOG_STREAM_EXPRESSION(RCUTILS_LOG_SEVERITY_INFO, logger, expression, stream_arg)
+
+/**
+ * \def RCLCPP_INFO_STREAM_FUNCTION
+ * \copydoc RCLCPP_LOG_STREAM_FUNCTION
+ */
+#define RCLCPP_INFO_STREAM_FUNCTION(logger, function, stream_arg) \
+  RCLCPP_LOG_STREAM_FUNCTION(RCUTILS_LOG_SEVERITY_INFO, logger, function, stream_args)
+
+/**
+ * \def RCLCPP_INFO_STREAM_SKIPFIRST
+ * \copydoc RCLCPP_LOG_STREAM_SKIPFIRST
+ */
+#define RCLCPP_INFO_STREAM_SKIPFIRST(logger, stream_arg) \
+  RCLCPP_LOG_STREAM_SKIPFIRST(RCUTILS_LOG_SEVERITY_INFO, logger, stream_arg)
+
+/**
+ * \def RCLCPP_INFO_STREAM_THROTTLE
+ * \copydoc RCLCPP_LOG_STREAM_THROTTLE
+ */
+#define RCLCPP_INFO_STREAM_THROTTLE(logger, clock, duration, stream_arg) \
+  RCLCPP_LOG_STREAM_THROTTLE(RCUTILS_LOG_SEVERITY_INFO, logger, clock, duration, stream_arg)
+
+/**
+ * \def RCLCPP_INFO_STREAM_SKIPFIRST_THROTTLE
+ * \copydoc RCLCPP_LOG_STREAM_SKIPFIRST_THROTTLE
+ */
+#define RCLCPP_INFO_STREAM_SKIPFIRST_THROTTLE(logger, clock, duration, stream_arg) \
+  RCLCPP_LOG_STREAM_SKIPFIRST_THROTTLE(RCUTILS_LOG_SEVERITY_INFO, logger, clock, duration, \
+    stream_arg)
+
+#endif
+
+/** @name Logging macros for severity WARN.
+ */
+#if (RCLCPP_LOG_MIN_SEVERITY > RCLCPP_LOG_MIN_SEVERITY_WARN)
+// empty logging macros for severity WARN when being disabled at compile time
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_WARN(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_WARN_ONCE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_WARN_EXPRESSION(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_WARN_FUNCTION(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_WARN_SKIPFIRST(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_WARN_THROTTLE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_WARN_SKIPFIRST_THROTTLE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_WARN_STREAM(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_WARN_STREAM_ONCE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_WARN_STREAM_EXPRESSION(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_WARN_STREAM_FUNCTION(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_WARN_STREAM_SKIPFIRST(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_WARN_STREAM_THROTTLE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(...)
+
+#else
+/**
+ * \def RCLCPP_WARN
+ * \copydoc RCLCPP_LOG
+ */
+#define RCLCPP_WARN(logger, ...) RCLCPP_LOG(RCUTILS_LOG_SEVERITY_WARN, logger, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_WARN_ONCE
+ * \copydoc RCLCPP_LOG_ONCE
+ */
+#define RCLCPP_WARN_ONCE(logger, ...) \
+  RCLCPP_LOG_ONCE(RCUTILS_LOG_SEVERITY_WARN, logger, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_WARN_EXPRESSION
+ * \copydoc RCLCPP_LOG_EXPRESSION
+ */
+#define RCLCPP_WARN_EXPRESSION(logger, expression, ...) \
+  RCLCPP_LOG_EXPRESSION(RCUTILS_LOG_SEVERITY_WARN, logger, expression, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_WARN_FUNCTION
+ * \copydoc RCLCPP_LOG_FUNCTION
+ */
+#define RCLCPP_WARN_FUNCTION(logger, function, ...) \
+  RCLCPP_LOG_FUNCTION(RCUTILS_LOG_SEVERITY_WARN, logger, function, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_WARN_SKIPFIRST
+ * \copydoc RCLCPP_LOG_SKIPFIRST
+ */
+#define RCLCPP_WARN_SKIPFIRST(logger, ...) \
+  RCLCPP_LOG_SKIPFIRST(RCUTILS_LOG_SEVERITY_WARN, logger, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_WARN_THROTTLE
+ * \copydoc RCLCPP_LOG_THROTTLE
+ */
+#define RCLCPP_WARN_THROTTLE(logger, clock, duration, ...) \
+  RCLCPP_LOG_THROTTLE(RCUTILS_LOG_SEVERITY_WARN, logger, clock, duration, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_WARN_SKIPFIRST_THROTTLE
+ * \copydoc RCLCPP_LOG_SKIPFIRST_THROTTLE
+ */
+#define RCLCPP_WARN_SKIPFIRST_THROTTLE(logger, clock, duration, ...) \
+  RCLCPP_LOG_SKIPFIRST_THROTTLE(RCUTILS_LOG_SEVERITY_WARN, logger, clock, duration, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_WARN_STREAM
+ * \copydoc RCLCPP_LOG_STREAM
+ */
+#define RCLCPP_WARN_STREAM(logger, stream_arg) \
+  RCLCPP_LOG_STREAM(RCUTILS_LOG_SEVERITY_WARN, logger, stream_arg)
+
+/**
+ * \def RCLCPP_WARN_STREAM_ONCE
+ * \copydoc RCLCPP_LOG_STREAM_ONCE
+ */
+#define RCLCPP_WARN_STREAM_ONCE(logger, stream_arg) \
+  RCLCPP_LOG_STREAM_ONCE(RCUTILS_LOG_SEVERITY_WARN, logger, stream_arg)
+
+/**
+ * \def RCLCPP_WARN_STREAM_EXPRESSION
+ * \copydoc RCLCPP_LOG_STREAM_EXPRESSION
+ */
+#define RCLCPP_WARN_STREAM_EXPRESSION(logger, expression, stream_arg) \
+  RCLCPP_LOG_STREAM_EXPRESSION(RCUTILS_LOG_SEVERITY_WARN, logger, expression, stream_arg)
+
+/**
+ * \def RCLCPP_WARN_STREAM_FUNCTION
+ * \copydoc RCLCPP_LOG_STREAM_FUNCTION
+ */
+#define RCLCPP_WARN_STREAM_FUNCTION(logger, function, stream_arg) \
+  RCLCPP_LOG_STREAM_FUNCTION(RCUTILS_LOG_SEVERITY_WARN, logger, function, stream_args)
+
+/**
+ * \def RCLCPP_WARN_STREAM_SKIPFIRST
+ * \copydoc RCLCPP_LOG_STREAM_SKIPFIRST
+ */
+#define RCLCPP_WARN_STREAM_SKIPFIRST(logger, stream_arg) \
+  RCLCPP_LOG_STREAM_SKIPFIRST(RCUTILS_LOG_SEVERITY_WARN, logger, stream_arg)
+
+/**
+ * \def RCLCPP_WARN_STREAM_THROTTLE
+ * \copydoc RCLCPP_LOG_STREAM_THROTTLE
+ */
+#define RCLCPP_WARN_STREAM_THROTTLE(logger, clock, duration, stream_arg) \
+  RCLCPP_LOG_STREAM_THROTTLE(RCUTILS_LOG_SEVERITY_WARN, logger, clock, duration, stream_arg)
+
+/**
+ * \def RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE
+ * \copydoc RCLCPP_LOG_STREAM_SKIPFIRST_THROTTLE
+ */
+#define RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(logger, clock, duration, stream_arg) \
+  RCLCPP_LOG_STREAM_SKIPFIRST_THROTTLE(RCUTILS_LOG_SEVERITY_WARN, logger, clock, duration, \
+    stream_arg)
+
+#endif
+
+/** @name Logging macros for severity ERROR.
+ */
+#if (RCLCPP_LOG_MIN_SEVERITY > RCLCPP_LOG_MIN_SEVERITY_ERROR)
+// empty logging macros for severity ERROR when being disabled at compile time
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_ERROR(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_ERROR_ONCE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_ERROR_EXPRESSION(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_ERROR_FUNCTION(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_ERROR_SKIPFIRST(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_ERROR_THROTTLE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_ERROR_SKIPFIRST_THROTTLE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_ERROR_STREAM(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_ERROR_STREAM_ONCE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_ERROR_STREAM_EXPRESSION(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_ERROR_STREAM_FUNCTION(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_ERROR_STREAM_SKIPFIRST(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_ERROR_STREAM_THROTTLE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_ERROR_STREAM_SKIPFIRST_THROTTLE(...)
+
+#else
+/**
+ * \def RCLCPP_ERROR
+ * \copydoc RCLCPP_LOG
+ */
+#define RCLCPP_ERROR(logger, ...) RCLCPP_LOG(RCUTILS_LOG_SEVERITY_ERROR, logger, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_ERROR_ONCE
+ * \copydoc RCLCPP_LOG_ONCE
+ */
+#define RCLCPP_ERROR_ONCE(logger, ...) \
+  RCLCPP_LOG_ONCE(RCUTILS_LOG_SEVERITY_ERROR, logger, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_ERROR_EXPRESSION
+ * \copydoc RCLCPP_LOG_EXPRESSION
+ */
+#define RCLCPP_ERROR_EXPRESSION(logger, expression, ...) \
+  RCLCPP_LOG_EXPRESSION(RCUTILS_LOG_SEVERITY_ERROR, logger, expression, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_ERROR_FUNCTION
+ * \copydoc RCLCPP_LOG_FUNCTION
+ */
+#define RCLCPP_ERROR_FUNCTION(logger, function, ...) \
+  RCLCPP_LOG_FUNCTION(RCUTILS_LOG_SEVERITY_ERROR, logger, function, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_ERROR_SKIPFIRST
+ * \copydoc RCLCPP_LOG_SKIPFIRST
+ */
+#define RCLCPP_ERROR_SKIPFIRST(logger, ...) \
+  RCLCPP_LOG_SKIPFIRST(RCUTILS_LOG_SEVERITY_ERROR, logger, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_ERROR_THROTTLE
+ * \copydoc RCLCPP_LOG_THROTTLE
+ */
+#define RCLCPP_ERROR_THROTTLE(logger, clock, duration, ...) \
+  RCLCPP_LOG_THROTTLE(RCUTILS_LOG_SEVERITY_ERROR, logger, clock, duration, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_ERROR_SKIPFIRST_THROTTLE
+ * \copydoc RCLCPP_LOG_SKIPFIRST_THROTTLE
+ */
+#define RCLCPP_ERROR_SKIPFIRST_THROTTLE(logger, clock, duration, ...) \
+  RCLCPP_LOG_SKIPFIRST_THROTTLE(RCUTILS_LOG_SEVERITY_ERROR, logger, clock, duration, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_ERROR_STREAM
+ * \copydoc RCLCPP_LOG_STREAM
+ */
+#define RCLCPP_ERROR_STREAM(logger, stream_arg) \
+  RCLCPP_LOG_STREAM(RCUTILS_LOG_SEVERITY_ERROR, logger, stream_arg)
+
+/**
+ * \def RCLCPP_ERROR_STREAM_ONCE
+ * \copydoc RCLCPP_LOG_STREAM_ONCE
+ */
+#define RCLCPP_ERROR_STREAM_ONCE(logger, stream_arg) \
+  RCLCPP_LOG_STREAM_ONCE(RCUTILS_LOG_SEVERITY_ERROR, logger, stream_arg)
+
+/**
+ * \def RCLCPP_ERROR_STREAM_EXPRESSION
+ * \copydoc RCLCPP_LOG_STREAM_EXPRESSION
+ */
+#define RCLCPP_ERROR_STREAM_EXPRESSION(logger, expression, stream_arg) \
+  RCLCPP_LOG_STREAM_EXPRESSION(RCUTILS_LOG_SEVERITY_ERROR, logger, expression, stream_arg)
+
+/**
+ * \def RCLCPP_ERROR_STREAM_FUNCTION
+ * \copydoc RCLCPP_LOG_STREAM_FUNCTION
+ */
+#define RCLCPP_ERROR_STREAM_FUNCTION(logger, function, stream_arg) \
+  RCLCPP_LOG_STREAM_FUNCTION(RCUTILS_LOG_SEVERITY_ERROR, logger, function, stream_args)
+
+/**
+ * \def RCLCPP_ERROR_STREAM_SKIPFIRST
+ * \copydoc RCLCPP_LOG_STREAM_SKIPFIRST
+ */
+#define RCLCPP_ERROR_STREAM_SKIPFIRST(logger, stream_arg) \
+  RCLCPP_LOG_STREAM_SKIPFIRST(RCUTILS_LOG_SEVERITY_ERROR, logger, stream_arg)
+
+/**
+ * \def RCLCPP_ERROR_STREAM_THROTTLE
+ * \copydoc RCLCPP_LOG_STREAM_THROTTLE
+ */
+#define RCLCPP_ERROR_STREAM_THROTTLE(logger, clock, duration, stream_arg) \
+  RCLCPP_LOG_STREAM_THROTTLE(RCUTILS_LOG_SEVERITY_ERROR, logger, clock, duration, stream_arg)
+
+/**
+ * \def RCLCPP_ERROR_STREAM_SKIPFIRST_THROTTLE
+ * \copydoc RCLCPP_LOG_STREAM_SKIPFIRST_THROTTLE
+ */
+#define RCLCPP_ERROR_STREAM_SKIPFIRST_THROTTLE(logger, clock, duration, stream_arg) \
+  RCLCPP_LOG_STREAM_SKIPFIRST_THROTTLE(RCUTILS_LOG_SEVERITY_ERROR, logger, clock, duration, \
+    stream_arg)
+
+#endif
+
+/** @name Logging macros for severity FATAL.
+ */
+#if (RCLCPP_LOG_MIN_SEVERITY > RCLCPP_LOG_MIN_SEVERITY_FATAL)
+// empty logging macros for severity FATAL when being disabled at compile time
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_FATAL(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_FATAL_ONCE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_FATAL_EXPRESSION(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_FATAL_FUNCTION(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_FATAL_SKIPFIRST(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_FATAL_THROTTLE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_FATAL_SKIPFIRST_THROTTLE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_FATAL_STREAM(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_FATAL_STREAM_ONCE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_FATAL_STREAM_EXPRESSION(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_FATAL_STREAM_FUNCTION(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_FATAL_STREAM_SKIPFIRST(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_FATAL_STREAM_THROTTLE(...)
+/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
+#define RCLCPP_FATAL_STREAM_SKIPFIRST_THROTTLE(...)
+
+#else
+/**
+ * \def RCLCPP_FATAL
+ * \copydoc RCLCPP_LOG
+ */
+#define RCLCPP_FATAL(logger, ...) RCLCPP_LOG(RCUTILS_LOG_SEVERITY_FATAL, logger, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_FATAL_ONCE
+ * \copydoc RCLCPP_LOG_ONCE
+ */
+#define RCLCPP_FATAL_ONCE(logger, ...) \
+  RCLCPP_LOG_ONCE(RCUTILS_LOG_SEVERITY_FATAL, logger, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_FATAL_EXPRESSION
+ * \copydoc RCLCPP_LOG_EXPRESSION
+ */
+#define RCLCPP_FATAL_EXPRESSION(logger, expression, ...) \
+  RCLCPP_LOG_EXPRESSION(RCUTILS_LOG_SEVERITY_FATAL, logger, expression, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_FATAL_FUNCTION
+ * \copydoc RCLCPP_LOG_FUNCTION
+ */
+#define RCLCPP_FATAL_FUNCTION(logger, function, ...) \
+  RCLCPP_LOG_FUNCTION(RCUTILS_LOG_SEVERITY_FATAL, logger, function, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_FATAL_SKIPFIRST
+ * \copydoc RCLCPP_LOG_SKIPFIRST
+ */
+#define RCLCPP_FATAL_SKIPFIRST(logger, ...) \
+  RCLCPP_LOG_SKIPFIRST(RCUTILS_LOG_SEVERITY_FATAL, logger, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_FATAL_THROTTLE
+ * \copydoc RCLCPP_LOG_THROTTLE
+ */
+#define RCLCPP_FATAL_THROTTLE(logger, clock, duration, ...) \
+  RCLCPP_LOG_THROTTLE(RCUTILS_LOG_SEVERITY_FATAL, logger, clock, duration, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_FATAL_SKIPFIRST_THROTTLE
+ * \copydoc RCLCPP_LOG_SKIPFIRST_THROTTLE
+ */
+#define RCLCPP_FATAL_SKIPFIRST_THROTTLE(logger, clock, duration, ...) \
+  RCLCPP_LOG_SKIPFIRST_THROTTLE(RCUTILS_LOG_SEVERITY_FATAL, logger, clock, duration, __VA_ARGS__)
+
+/**
+ * \def RCLCPP_FATAL_STREAM
+ * \copydoc RCLCPP_LOG_STREAM
+ */
+#define RCLCPP_FATAL_STREAM(logger, stream_arg) \
+  RCLCPP_LOG_STREAM(RCUTILS_LOG_SEVERITY_FATAL, logger, stream_arg)
+
+/**
+ * \def RCLCPP_FATAL_STREAM_ONCE
+ * \copydoc RCLCPP_LOG_STREAM_ONCE
+ */
+#define RCLCPP_FATAL_STREAM_ONCE(logger, stream_arg) \
+  RCLCPP_LOG_STREAM_ONCE(RCUTILS_LOG_SEVERITY_FATAL, logger, stream_arg)
+
+/**
+ * \def RCLCPP_FATAL_STREAM_EXPRESSION
+ * \copydoc RCLCPP_LOG_STREAM_EXPRESSION
+ */
+#define RCLCPP_FATAL_STREAM_EXPRESSION(logger, expression, stream_arg) \
+  RCLCPP_LOG_STREAM_EXPRESSION(RCUTILS_LOG_SEVERITY_FATAL, logger, expression, stream_arg)
+
+/**
+ * \def RCLCPP_FATAL_STREAM_FUNCTION
+ * \copydoc RCLCPP_LOG_STREAM_FUNCTION
+ */
+#define RCLCPP_FATAL_STREAM_FUNCTION(logger, function, stream_arg) \
+  RCLCPP_LOG_STREAM_FUNCTION(RCUTILS_LOG_SEVERITY_FATAL, logger, function, stream_args)
+
+/**
+ * \def RCLCPP_FATAL_STREAM_SKIPFIRST
+ * \copydoc RCLCPP_LOG_STREAM_SKIPFIRST
+ */
+#define RCLCPP_FATAL_STREAM_SKIPFIRST(logger, stream_arg) \
+  RCLCPP_LOG_STREAM_SKIPFIRST(RCUTILS_LOG_SEVERITY_FATAL, logger, stream_arg)
+
+/**
+ * \def RCLCPP_FATAL_STREAM_THROTTLE
+ * \copydoc RCLCPP_LOG_STREAM_THROTTLE
+ */
+#define RCLCPP_FATAL_STREAM_THROTTLE(logger, clock, duration, stream_arg) \
+  RCLCPP_LOG_STREAM_THROTTLE(RCUTILS_LOG_SEVERITY_FATAL, logger, clock, duration, stream_arg)
+
+/**
+ * \def RCLCPP_FATAL_STREAM_SKIPFIRST_THROTTLE
+ * \copydoc RCLCPP_LOG_STREAM_SKIPFIRST_THROTTLE
+ */
+#define RCLCPP_FATAL_STREAM_SKIPFIRST_THROTTLE(logger, clock, duration, stream_arg) \
+  RCLCPP_LOG_STREAM_SKIPFIRST_THROTTLE(RCUTILS_LOG_SEVERITY_FATAL, logger, clock, duration, \
+    stream_arg)
+
+#endif
+
+#endif  // RCLCPP__LOGGING_HPP_

rclcpp/include/rclcpp/macros.hpp

@@ -66,6 +66,7 @@
 #define RCLCPP_SMART_PTR_ALIASES_ONLY(...) \
   __RCLCPP_SHARED_PTR_ALIAS(__VA_ARGS__) \
   __RCLCPP_WEAK_PTR_ALIAS(__VA_ARGS__) \
+  __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__) \
   __RCLCPP_MAKE_SHARED_DEFINITION(__VA_ARGS__)
 
 #define __RCLCPP_SHARED_PTR_ALIAS(...) \

rclcpp/include/rclcpp/mapped_ring_buffer.hpp

@@ -1,319 +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.
-
-#ifndef RCLCPP__MAPPED_RING_BUFFER_HPP_
-#define RCLCPP__MAPPED_RING_BUFFER_HPP_
-
-#include <algorithm>
-#include <cstddef>
-#include <cstdint>
-#include <memory>
-#include <mutex>
-#include <stdexcept>
-#include <utility>
-#include <vector>
-
-#include "rclcpp/allocator/allocator_common.hpp"
-#include "rclcpp/macros.hpp"
-#include "rclcpp/visibility_control.hpp"
-
-namespace rclcpp
-{
-namespace mapped_ring_buffer
-{
-
-class RCLCPP_PUBLIC MappedRingBufferBase
-{
-public:
-  RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBufferBase)
-};
-
-/// 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.
- * This class must have a positive, non-zero size.
- * This class cannot be resized nor can it reserve additional space after construction.
- * This class is not CopyConstructable nor CopyAssignable.
- *
- * The key's are not guaranteed to be unique because push_and_replace does not
- * check for colliding keys.
- * It is up to the user to only use unique keys.
- * A side effect of this is that when get_copy_at_key or pop_at_key are called,
- * they return the first encountered instance of the key.
- * But iteration does not begin with the ring buffer's head, and therefore
- * there is no guarantee on which value is returned if a key is used multiple
- * times.
- */
-template<typename T, typename Alloc = std::allocator<void>>
-class MappedRingBuffer : public MappedRingBufferBase
-{
-public:
-  RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBuffer<T, Alloc>)
-  using ElemAllocTraits = allocator::AllocRebind<T, Alloc>;
-  using ElemAlloc = typename ElemAllocTraits::allocator_type;
-  using ElemDeleter = allocator::Deleter<ElemAlloc, T>;
-
-  using ConstElemSharedPtr = std::shared_ptr<const T>;
-  using ElemUniquePtr = std::unique_ptr<T, ElemDeleter>;
-
-  /// Constructor.
-  /**
-   * The constructor will allocate memory while reserving space.
-   *
-   * \param size size of the ring buffer; must be positive and non-zero.
-   * \param allocator optional custom allocator
-   */
-  explicit MappedRingBuffer(size_t size, std::shared_ptr<Alloc> allocator = nullptr)
-  : elements_(size), head_(0)
-  {
-    if (size == 0) {
-      throw std::invalid_argument("size must be a positive, non-zero value");
-    }
-    if (!allocator) {
-      allocator_ = std::make_shared<ElemAlloc>();
-    } else {
-      allocator_ = std::make_shared<ElemAlloc>(*allocator.get());
-    }
-  }
-
-  virtual ~MappedRingBuffer() {}
-
-  /// Return a copy of the value stored in the ring buffer at the given key.
-  /**
-   * The key is matched if an element in the ring buffer has a matching key.
-   * This method will allocate in order to return a copy.
-   *
-   * 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
-  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) {
-      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.");
-      }
-    }
-  }
-
-  /// 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.
-   *
-   * 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
-  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) {
-      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;
-    }
-  }
-
-  /// Insert a key-value pair, displacing an existing pair if necessary.
-  /**
-   * The key's uniqueness is not checked on insertion.
-   * It is up to the user to ensure the key is unique.
-   * This method should not allocate memory.
-   *
-   * 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, ConstElemSharedPtr value)
-  {
-    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.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)
-  {
-    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.
-  bool
-  has_key(uint64_t key)
-  {
-    std::lock_guard<std::mutex> lock(data_mutex_);
-    return elements_.end() != get_iterator_of_key(key);
-  }
-
-private:
-  RCLCPP_DISABLE_COPY(MappedRingBuffer<T, Alloc>)
-
-  struct Element
-  {
-    uint64_t key;
-    ElemUniquePtr unique_value;
-    ConstElemSharedPtr shared_value;
-    bool in_use;
-  };
-
-  using VectorAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<Element>;
-
-  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 {
-        return e.key == key && e.in_use;
-      });
-    return it;
-  }
-
-  std::vector<Element, VectorAlloc> elements_;
-  size_t head_;
-  std::shared_ptr<ElemAlloc> allocator_;
-  std::mutex data_mutex_;
-};
-
-}  // namespace mapped_ring_buffer
-}  // namespace rclcpp
-
-#endif  // RCLCPP__MAPPED_RING_BUFFER_HPP_

rclcpp/include/rclcpp/memory_strategies.hpp

@@ -23,6 +23,10 @@ namespace rclcpp
 namespace memory_strategies
 {
 
+/// Create a MemoryStrategy sharedPtr
+/**
+ * \return a MemoryStrategy sharedPtr
+ */
 RCLCPP_PUBLIC
 memory_strategy::MemoryStrategy::SharedPtr
 create_default_strategy();