29.2.0

Signed-off-by: Chris Lalancette <clalancette@gmail.com>

.github/workflows/mirror-rolling-to-master.yaml

@@ -0,0 +1,13 @@
+name: Mirror rolling to master
+
+on:
+  push:
+    branches: [ rolling ]
+
+jobs:
+  mirror-to-master:
+    runs-on: ubuntu-latest
+    steps:
+    - uses: zofrex/mirror-branch@v1
+      with:
+        target-branch: master

CODEOWNERS

@@ -0,0 +1,2 @@
+# This file was generated by https://github.com/audrow/update-ros2-repos
+* @ivanpauno @hidmic @wjwwood

README.md

@@ -8,10 +8,11 @@ rclcpp provides the standard C++ API for interacting with ROS 2.
 
 `#include "rclcpp/rclcpp.hpp"` allows use of the most common elements of the ROS 2 system.
 
-Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) for a complete list of its main components.
+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://index.ros.org/doc/ros2/Tutorials/Writing-A-Simple-Cpp-Publisher-And-Subscriber/)
-and [Writing a simple service and client](https://index.ros.org/doc/ros2/Tutorials/Writing-A-Simple-Cpp-Service-And-Client/)
+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,41 @@
-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(rosgraph_msgs 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 -Wnon-virtual-dtor -Woverloaded-virtual)
+  # 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()
 
 set(${PROJECT_NAME}_SRCS
@@ -33,22 +45,40 @@ 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/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/executors/static_executor_entities_collector.cpp
   src/rclcpp/executors/static_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
@@ -58,8 +88,8 @@ set(${PROJECT_NAME}_SRCS
   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
@@ -69,16 +99,21 @@ 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_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
@@ -89,11 +124,29 @@ set(${PROJECT_NAME}_SRCS
   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
 )
 
+# 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)
+
+find_package(Python3 REQUIRED COMPONENTS Interpreter)
+
 # "watch" template for changes
 configure_file(
   "resource/logging.hpp.em"
@@ -107,7 +160,7 @@ set(python_code_logging
 string(REPLACE ";" "$<SEMICOLON>" python_code_logging "${python_code_logging}")
 add_custom_command(OUTPUT include/rclcpp/logging.hpp
   COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp"
-  COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_code_logging}"
+  COMMAND Python3::Interpreter ARGS -c "${python_code_logging}"
   DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/logging.hpp.em.watch"
   COMMENT "Expanding logging.hpp.em"
   VERBATIM
@@ -131,7 +184,7 @@ foreach(interface_file ${interface_files})
   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 ${PYTHON_EXECUTABLE} ARGS -c "${python_${interface_name}_traits}"
+    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
@@ -151,7 +204,7 @@ foreach(interface_file ${interface_files})
   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 ${PYTHON_EXECUTABLE} ARGS -c "${python_get_${interface_name}}"
+    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
@@ -160,25 +213,38 @@ foreach(interface_file ${interface_files})
     include/rclcpp/node_interfaces/get_${interface_name}.hpp)
 endforeach()
 
-add_library(${PROJECT_NAME}
-  ${${PROJECT_NAME}_SRCS})
+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>")
-# specific order: dependents before dependencies
-ament_target_dependencies(${PROJECT_NAME}
-  "libstatistics_collector"
-  "rcl"
-  "rcl_yaml_param_parser"
-  "rcpputils"
-  "rcutils"
-  "builtin_interfaces"
-  "rosgraph_msgs"
-  "rosidl_typesupport_cpp"
-  "rosidl_runtime_cpp"
-  "statistics_msgs"
-  "tracetools"
+  "$<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,
@@ -193,466 +259,52 @@ install(
   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})
+
+# Export modern CMake targets
 ament_export_targets(${PROJECT_NAME})
 
-ament_export_dependencies(libstatistics_collector)
-ament_export_dependencies(rcl)
-ament_export_dependencies(rcpputils)
-ament_export_dependencies(rcutils)
-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_runtime_cpp)
-ament_export_dependencies(rcl_yaml_param_parser)
-ament_export_dependencies(statistics_msgs)
-ament_export_dependencies(tracetools)
+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)
-  find_package(rosidl_default_generators REQUIRED)
-
-  find_package(test_msgs REQUIRED)
-
-  include(cmake/rclcpp_add_build_failure_test.cmake)
-
-  add_definitions(-DTEST_RESOURCES_DIRECTORY="${CMAKE_CURRENT_BINARY_DIR}/test/resources")
-
-  rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs
-    test/msg/Header.msg
-    test/msg/MessageWithHeader.msg
-    DEPENDENCIES builtin_interfaces
-    LIBRARY_NAME ${PROJECT_NAME}
-    SKIP_INSTALL
-  )
-
-  ament_add_gtest(test_client test/test_client.cpp)
-  if(TARGET test_client)
-    ament_target_dependencies(test_client
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_client ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_create_timer test/test_create_timer.cpp)
-  if(TARGET test_create_timer)
-    ament_target_dependencies(test_create_timer
-      "rcl_interfaces"
-      "rmw"
-      "rcl"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_create_timer ${PROJECT_NAME})
-    target_include_directories(test_create_timer PRIVATE test/)
-  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_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_function_traits test/test_function_traits.cpp)
-  if(TARGET test_function_traits)
-    target_include_directories(test_function_traits PUBLIC include)
-    ament_target_dependencies(test_function_traits
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-  endif()
-  ament_add_gmock(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_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_intra_process_manager ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_ring_buffer_implementation test/test_ring_buffer_implementation.cpp)
-  if(TARGET test_ring_buffer_implementation)
-    ament_target_dependencies(test_ring_buffer_implementation
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_ring_buffer_implementation ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_intra_process_buffer test/test_intra_process_buffer.cpp)
-  if(TARGET test_intra_process_buffer)
-    ament_target_dependencies(test_intra_process_buffer
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_intra_process_buffer ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_loaned_message test/test_loaned_message.cpp)
-  ament_target_dependencies(test_loaned_message
-    "test_msgs"
-  )
-  target_link_libraries(test_loaned_message ${PROJECT_NAME})
-
-  ament_add_gtest(test_node test/test_node.cpp TIMEOUT 240)
-  if(TARGET test_node)
-    ament_target_dependencies(test_node
-      "rcl_interfaces"
-      "rcpputils"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-      "test_msgs"
-    )
-    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_runtime_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_node_global_args ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_node_options test/test_node_options.cpp)
-  if(TARGET test_node_options)
-    ament_target_dependencies(test_node_options "rcl")
-    target_link_libraries(test_node_options ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_parameter_client test/test_parameter_client.cpp)
-  if(TARGET test_parameter_client)
-    ament_target_dependencies(test_parameter_client
-      "rcl_interfaces"
-    )
-    target_link_libraries(test_parameter_client ${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_runtime_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_runtime_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
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-      "test_msgs"
-    )
-    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_runtime_cpp"
-      "rosidl_typesupport_cpp"
-      "test_msgs"
-    )
-    target_link_libraries(test_publisher_subscription_count_api ${PROJECT_NAME})
-  endif()
-  ament_add_gtest(test_qos test/test_qos.cpp)
-  if(TARGET test_qos)
-    ament_target_dependencies(test_qos
-      "rmw"
-    )
-    target_link_libraries(test_qos
-      ${PROJECT_NAME}
-    )
-  endif()
-  ament_add_gtest(test_qos_event test/test_qos_event.cpp)
-  if(TARGET test_qos_event)
-    ament_target_dependencies(test_qos_event
-      "rmw"
-      "test_msgs"
-    )
-    target_link_libraries(test_qos_event
-      ${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_runtime_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_serialized_message test/test_serialized_message.cpp)
-  if(TARGET test_serialized_message)
-    ament_target_dependencies(test_serialized_message
-      test_msgs
-    )
-    target_link_libraries(test_serialized_message
-      ${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_runtime_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_runtime_cpp"
-      "rosidl_typesupport_cpp"
-      "test_msgs"
-    )
-    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_runtime_cpp"
-      "rosidl_typesupport_cpp"
-      "test_msgs"
-    )
-    target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME})
-  endif()
-  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"
-    )
-    target_link_libraries(test_subscription_traits ${PROJECT_NAME})
-  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()
-
-  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)
-  ament_target_dependencies(test_externally_defined_services
-    "rcl"
-    "test_msgs"
-  )
-  target_link_libraries(test_externally_defined_services ${PROJECT_NAME})
-
-  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_interface_traits test/test_interface_traits.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_interface_traits)
-    ament_target_dependencies(test_interface_traits
-      "rcl")
-    target_link_libraries(test_interface_traits ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_multi_threaded_executor test/executors/test_multi_threaded_executor.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_multi_threaded_executor)
-    ament_target_dependencies(test_multi_threaded_executor
-      "rcl")
-    target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_guard_condition test/test_guard_condition.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_guard_condition)
-    target_link_libraries(test_guard_condition ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_wait_set test/test_wait_set.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_wait_set)
-    ament_target_dependencies(test_wait_set "test_msgs")
-    target_link_libraries(test_wait_set ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_subscription_topic_statistics test/topic_statistics/test_subscription_topic_statistics.cpp
-    APPEND_LIBRARY_DIRS "${append_library_dirs}"
-  )
-  if(TARGET test_subscription_topic_statistics)
-    ament_target_dependencies(test_subscription_topic_statistics
-      "builtin_interfaces"
-      "libstatistics_collector"
-      "rcl_interfaces"
-      "rcutils"
-      "rmw"
-      "rosidl_runtime_cpp"
-      "rosidl_typesupport_cpp"
-      "statistics_msgs"
-      "test_msgs")
-    rosidl_target_interfaces(test_subscription_topic_statistics ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp")
-    target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME})
-  endif()
-
-  ament_add_gtest(test_subscription_options test/test_subscription_options.cpp)
-  if(TARGET test_subscription_options)
-    ament_target_dependencies(test_subscription_options "rcl")
-    target_link_libraries(test_subscription_options ${PROJECT_NAME})
-  endif()
-
-  # Install test resources
-  install(
-    DIRECTORY test/resources
-    DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test)
+  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,22 @@ GENERATE_LATEX         = NO
 ENABLE_PREPROCESSING   = YES
 MACRO_EXPANSION        = YES
 EXPAND_ONLY_PREDEF     = YES
-PREDEFINED             = RCLCPP_PUBLIC=
+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

@@ -1,16 +1,16 @@
 This document is a declaration of software quality for the `rclcpp` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html).
 
-# `rclcpp` Quality Declaration
+# rclcpp Quality Declaration
 
-The package `rclcpp` claims to be in the **Quality Level 4** category.
+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 Quality Categories in REP-2004](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-quality-categories) of the ROS2 developer guide.
+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://www.ros.org/reps/rep-2004.html).
 
 ## 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://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning)
+`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]
 
@@ -39,11 +39,11 @@ Headers under the folder `detail` are not considered part of the public API and
 
 ## Change Control Process [2]
 
-`rclcpp` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process).
+`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://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information.
+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]
 
@@ -51,7 +51,7 @@ This package uses DCO as its confirmation of contributor origin policy. More inf
 
 ### Peer Review Policy [2.iii]
 
-All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information.
+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]
 
@@ -76,25 +76,25 @@ All pull requests must resolve related documentation changes before merging.
 
 ### Public API Documentation [3.ii]
 
-The API is publicly available in the [ROS 2 API documentation](http://docs.ros2.org/latest/api/rclcpp/).
+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/Epr/job/Epr__rclcpp__ubuntu_bionic_amd64/lastBuild/testReport/rclcpp/) can be found a list with the latest results of the various linters being run on the package.
+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/Epr/job/Epr__rclcpp__ubuntu_bionic_amd64/lastBuild/testReport/rclcpp/copyright/).
+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/master/test) directory.
+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:
@@ -111,7 +111,7 @@ The tests aim to cover both typical usage and corner cases, but are quantified b
 
 ### Coverage [4.iii]
 
-`rclcpp` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#code-coverage), and opts to use line coverage instead of branch coverage.
+`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:
 
@@ -121,13 +121,25 @@ This includes:
 
 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]
 
-It is not yet defined if this package requires performance testing and how addresses this topic.
+`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 ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms.
+`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/)
@@ -151,49 +163,49 @@ It also has several test dependencies, which do not affect the resulting quality
 
 The `libstatistics_collector` package provides lightweight aggregation utilities to collect statistics and measure message metrics.
 
-It is **Quality Level 4**, see its [Quality Declaration document](https://github.com/ros-tooling/libstatistics_collector/tree/master/QUALITY_DECLARATION.md).
+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 4**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl/QUALITY_DECLARATION.md).
+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 4**, see its [Quality Declaration document](https://github.com/ros2/rcl/tree/master/rcl_yaml_param_parser/QUALITY_DECLARATION.md).
+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 4**, see its [Quality Declaration document](https://github.com/ros2/rcpputils/blob/master/QUALITY_DECLARATION.md).
+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 4**, see its [Quality Declaration document](https://github.com/ros2/rcutils/blob/master/QUALITY_DECLARATION.md).
+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 4**, see its [Quality Declaration document](https://github.com/ros2/rmw/blob/master/rmw/QUALITY_DECLARATION.md).
+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 4**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/statistics_msgs/QUALITY_DECLARATION.md).
+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 `rcl` so that it may be traced for debugging and performance analysis.
+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 4**, see its [Quality Declaration document](https://gitlab.com/micro-ROS/ros_tracing/ros2_tracing/-/blob/master/tracetools/QUALITY_DECLARATION.md).
+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/README.md

@@ -2,8 +2,8 @@
 
 The ROS client library in C++.
 
-Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) for a complete list of its main components and features.
+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 4** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details.
+This package claims to be in the **Quality Level 1** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details.

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();
   }
@@ -95,7 +95,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

@@ -45,16 +45,11 @@ struct AnyExecutable
   rclcpp::ClientBase::SharedPtr client;
   rclcpp::Waitable::SharedPtr waitable;
   // These are used to keep the scope on the containing items
-  rclcpp::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
-{
-
-using AnyExecutable [[deprecated("use rclcpp::AnyExecutable instead")]] = AnyExecutable;
-
-}  // namespace executor
 }  // namespace rclcpp
 
 #endif  // RCLCPP__ANY_EXECUTABLE_HPP_

rclcpp/include/rclcpp/any_service_callback.hpp

@@ -15,10 +15,12 @@
 #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"
@@ -29,93 +31,210 @@
 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
 {
-private:
-  using SharedPtrCallback = std::function<
-    void (
-      const std::shared_ptr<typename ServiceT::Request>,
-      std::shared_ptr<typename ServiceT::Response>
-    )>;
-  using SharedPtrWithRequestHeaderCallback = std::function<
-    void (
-      const std::shared_ptr<rmw_request_id_t>,
-      const std::shared_ptr<typename ServiceT::Request>,
-      std::shared_ptr<typename ServiceT::Response>
-    )>;
-
-  SharedPtrCallback shared_ptr_callback_;
-  SharedPtrWithRequestHeaderCallback shared_ptr_with_request_header_callback_;
-
 public:
   AnyServiceCallback()
-  : shared_ptr_callback_(nullptr), shared_ptr_with_request_header_callback_(nullptr)
+  : callback_(std::monostate{})
   {}
 
-  AnyServiceCallback(const AnyServiceCallback &) = default;
-
   template<
     typename CallbackT,
-    typename std::enable_if<
+    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
-    >::type * = nullptr
-  >
-  void set(CallbackT callback)
-  {
-    shared_ptr_callback_ = callback;
+      >::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<
+    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
-    >::type * = nullptr
-  >
-  void set(CallbackT callback)
-  {
-    shared_ptr_with_request_header_callback_ = callback;
+      >::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);
+    }
   }
 
-  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)
+  // 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)
   {
-    TRACEPOINT(callback_start, (const void *)this, false);
-    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");
+    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"};
     }
-    TRACEPOINT(callback_end, (const void *)this);
+    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
-    if (shared_ptr_callback_) {
-      TRACEPOINT(
-        rclcpp_callback_register,
-        (const void *)this,
-        get_symbol(shared_ptr_callback_));
-    } else if (shared_ptr_with_request_header_callback_) {
-      TRACEPOINT(
-        rclcpp_callback_register,
-        (const void *)this,
-        get_symbol(shared_ptr_with_request_header_callback_));
-    }
+    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 (
+      std::shared_ptr<typename ServiceT::Request>,
+      std::shared_ptr<typename ServiceT::Response>
+    )>;
+  using SharedPtrWithRequestHeaderCallback = std::function<
+    void (
+      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>
+    )>;
+
+  std::variant<
+    std::monostate,
+    SharedPtrCallback,
+    SharedPtrWithRequestHeaderCallback,
+    SharedPtrDeferResponseCallback,
+    SharedPtrDeferResponseCallbackWithServiceHandle> callback_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/callback_group.hpp

@@ -16,11 +16,14 @@
 #define RCLCPP__CALLBACK_GROUP_HPP_
 
 #include <atomic>
+#include <functional>
+#include <memory>
 #include <mutex>
-#include <string>
 #include <vector>
 
 #include "rclcpp/client.hpp"
+#include "rclcpp/context.hpp"
+#include "rclcpp/guard_condition.hpp"
 #include "rclcpp/publisher_base.hpp"
 #include "rclcpp/service.hpp"
 #include "rclcpp/subscription_base.hpp"
@@ -56,8 +59,50 @@ 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
+  ~CallbackGroup();
 
   template<typename Function>
   rclcpp::SubscriptionBase::SharedPtr
@@ -94,14 +139,90 @@ public:
     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
+  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)
 
@@ -136,12 +257,19 @@ 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>
@@ -159,13 +287,6 @@ private:
   }
 };
 
-namespace callback_group
-{
-
-using CallbackGroupType [[deprecated("use rclcpp::CallbackGroupType instead")]] = CallbackGroupType;
-using CallbackGroup [[deprecated("use rclcpp::CallbackGroup instead")]] = CallbackGroup;
-
-}  // namespace callback_group
 }  // namespace rclcpp
 
 #endif  // RCLCPP__CALLBACK_GROUP_HPP_

rclcpp/include/rclcpp/client.hpp

@@ -16,35 +16,122 @@
 #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;
@@ -61,7 +148,7 @@ 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.
   /**
@@ -150,6 +237,123 @@ 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)
 
@@ -165,10 +369,22 @@ 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};
@@ -178,6 +394,9 @@ 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;
 
@@ -187,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>>;
 
@@ -195,6 +415,55 @@ 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.
@@ -204,22 +473,20 @@ public:
    * \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 subscription.
+   * \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) {
@@ -292,34 +559,89 @@ public:
     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<
@@ -329,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<
@@ -355,28 +682,188 @@ 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
+   */
+  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

@@ -19,6 +19,7 @@
 #include <memory>
 #include <mutex>
 
+#include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/time.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -76,7 +77,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.
@@ -89,6 +193,16 @@ public:
   bool
   ros_time_is_active();
 
+  /**
+   * Cancels an ongoing or future sleep operation of one thread.
+   *
+   * This function can be used by one thread, to wakeup another thread that is
+   * blocked using any of the sleep_ or wait_ methods of this class.
+   */
+  RCLCPP_PUBLIC
+  void
+  cancel_sleep_or_wait();
+
   /// Return the rcl_clock_t clock handle
   RCLCPP_PUBLIC
   rcl_clock_t *
@@ -103,22 +217,22 @@ public:
   std::mutex &
   get_clock_mutex() noexcept;
 
-  // Add a callback to invoke if the jump threshold is exceeded.
+  /// 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
@@ -136,7 +250,7 @@ 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);
 

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"
@@ -47,11 +49,28 @@ public:
 /// 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>
 {
@@ -63,7 +82,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();
@@ -80,8 +99,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.
    *
@@ -112,7 +131,7 @@ public:
   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.
@@ -139,13 +158,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.
   /**
@@ -163,6 +187,11 @@ 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 rclcpp::exceptions::RCLError, if rcl_shutdown fails
@@ -172,7 +201,7 @@ public:
   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.
   /**
@@ -180,7 +209,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.
@@ -198,23 +227,82 @@ 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
@@ -227,7 +315,7 @@ public:
    *
    *   - 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.
@@ -239,71 +327,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.
-   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
-   */
-  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.
-   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
-   * \throws std::runtime_error if a nonexistent wait set is trying to release sigint 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>
@@ -333,7 +359,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();
 
@@ -342,7 +367,7 @@ 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_;
@@ -355,21 +380,44 @@ private:
   // 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::mutex on_shutdown_callbacks_mutex_;
+
+  std::vector<std::shared_ptr<PreShutdownCallback>> pre_shutdown_callbacks_;
+  mutable std::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_;
-
   /// 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

@@ -36,22 +36,6 @@ RCLCPP_PUBLIC
 DefaultContext::SharedPtr
 get_global_default_context();
 
-namespace default_context
-{
-
-using DefaultContext
-[[deprecated("use rclcpp::contexts::DefaultContext instead")]] = DefaultContext;
-
-[[deprecated("use rclcpp::contexts::get_global_default_context() instead")]]
-RCLCPP_PUBLIC
-inline
-DefaultContext::SharedPtr
-get_global_default_context()
-{
-  return rclcpp::contexts::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

@@ -20,13 +20,24 @@
 
 #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.
-/// \internal
+/**
+ * \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(
@@ -34,11 +45,11 @@ create_client(
   std::shared_ptr<node_interfaces::NodeGraphInterface> node_graph,
   std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
   const std::string & service_name,
-  const rmw_qos_profile_t & qos_profile,
-  rclcpp::CallbackGroup::SharedPtr group)
+  const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
+  rclcpp::CallbackGroup::SharedPtr group = nullptr)
 {
   rcl_client_options_t options = rcl_client_get_default_options();
-  options.qos = qos_profile;
+  options.qos = qos.get_rmw_qos_profile();
 
   auto cli = rclcpp::Client<ServiceT>::make_shared(
     node_base.get(),
@@ -50,7 +61,6 @@ create_client(
   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,15 +25,65 @@
 #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>,
+  typename NodeParametersT,
+  typename NodeTopicsT>
+std::shared_ptr<PublisherT>
+create_publisher(
+  NodeParametersT & node_parameters,
+  NodeTopicsT & node_topics,
+  const std::string & topic_name,
+  const rclcpp::QoS & qos,
+  const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options = (
+    rclcpp::PublisherOptionsWithAllocator<AllocatorT>()
+  )
+)
+{
+  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;
+
+  // Create the publisher.
+  auto pub = node_topics_interface->create_publisher(
+    topic_name,
+    rclcpp::create_publisher_factory<MessageT, AllocatorT, PublisherT>(options),
+    actual_qos
+  );
+
+  // 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,
@@ -41,7 +92,7 @@ template<
   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 = (
@@ -49,21 +100,28 @@ create_publisher(
   )
 )
 {
-  // Extract the NodeTopicsInterface from the NodeT.
-  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);
+}
 
-  // Create the publisher.
-  auto pub = node_topics->create_publisher(
-    topic_name,
-    rclcpp::create_publisher_factory<MessageT, AllocatorT, PublisherT>(options),
-    qos
-  );
-
-  // Add the publisher to the node topics interface.
-  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

rclcpp/include/rclcpp/create_subscription.hpp

@@ -18,6 +18,7 @@
 #include <chrono>
 #include <functional>
 #include <memory>
+#include <stdexcept>
 #include <string>
 #include <utility>
 
@@ -28,6 +29,7 @@
 #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"
@@ -39,27 +41,21 @@
 namespace rclcpp
 {
 
-/// 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.
- */
+namespace detail
+{
 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 MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy<
-    CallbackMessageT,
-    AllocatorT
-  >,
-  typename NodeT>
+  typename AllocatorT,
+  typename SubscriptionT,
+  typename MessageMemoryStrategyT,
+  typename NodeParametersT,
+  typename NodeTopicsT
+>
 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,
@@ -72,37 +68,50 @@ create_subscription(
 )
 {
   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);
 
-  std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>>
+  std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics>
   subscription_topic_stats = nullptr;
 
   if (rclcpp::detail::resolve_enable_topic_statistics(
       options,
-      *node_topics->get_node_base_interface()))
+      *node_topics_interface->get_node_base_interface()))
   {
-    std::shared_ptr<Publisher<statistics_msgs::msg::MetricsMessage>> publisher =
-      create_publisher<statistics_msgs::msg::MetricsMessage>(
-      node,
+    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,
-      qos);
+      options.topic_stats_options.qos);
 
-    subscription_topic_stats = std::make_shared<
-      rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>
-      >(node_topics->get_node_base_interface()->get_name(), publisher);
+    subscription_topic_stats =
+      std::make_shared<rclcpp::topic_statistics::SubscriptionTopicStatistics>(
+      node_topics_interface->get_node_base_interface()->get_name(), publisher);
 
-    auto sub_call_back = [subscription_topic_stats]() {
-        subscription_topic_stats->publish_message();
+    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->get_node_timers_interface();
+    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->get_node_base_interface(),
+      node_topics_interface->get_node_base_interface(),
       node_timer_interface
     );
 
@@ -116,11 +125,102 @@ create_subscription(
     subscription_topic_stats
   );
 
-  auto sub = node_topics->create_subscription(topic_name, factory, qos);
-  node_topics->add_subscription(sub, options.callback_group);
+  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);
 
   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

@@ -23,12 +23,63 @@
 
 #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>
@@ -39,16 +90,17 @@ create_timer(
   rclcpp::Clock::SharedPtr clock,
   rclcpp::Duration period,
   CallbackT && callback,
-  rclcpp::CallbackGroup::SharedPtr group = nullptr)
+  rclcpp::CallbackGroup::SharedPtr group = nullptr,
+  bool autostart = true)
 {
-  auto timer = rclcpp::GenericTimer<CallbackT>::make_shared(
+  return create_timer(
     clock,
     period.to_chrono<std::chrono::nanoseconds>(),
     std::forward<CallbackT>(callback),
-    node_base->get_context());
-
-  node_timers->add_timer(timer, group);
-  return timer;
+    group,
+    node_base.get(),
+    node_timers.get(),
+    autostart);
 }
 
 /// Create a timer with a given clock
@@ -59,31 +111,80 @@ create_timer(
   rclcpp::Clock::SharedPtr clock,
   rclcpp::Duration period,
   CallbackT && callback,
-  rclcpp::CallbackGroup::SharedPtr group = nullptr)
+  rclcpp::CallbackGroup::SharedPtr group = nullptr,
+  bool autostart = true)
 {
   return create_timer(
-    rclcpp::node_interfaces::get_node_base_interface(node),
-    rclcpp::node_interfaces::get_node_timers_interface(node),
     clock,
-    period,
+    period.to_chrono<std::chrono::nanoseconds>(),
     std::forward<CallbackT>(callback),
-    group);
+    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 timer with node resources.
+/// Convenience method to create a general timer with node resources.
 /**
  *
  * \tparam DurationRepT
  * \tparam DurationT
  * \tparam CallbackT
- * \param period period to exectute callback
+ * \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
- * \param node_base
- * \param node_timers
- * \return
- * \throws std::invalid argument if either node_base or node_timers
- * are null
+ * \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
@@ -92,7 +193,8 @@ create_wall_timer(
   CallbackT callback,
   rclcpp::CallbackGroup::SharedPtr group,
   node_interfaces::NodeBaseInterface * node_base,
-  node_interfaces::NodeTimersInterface * node_timers)
+  node_interfaces::NodeTimersInterface * node_timers,
+  bool autostart = true)
 {
   if (node_base == nullptr) {
     throw std::invalid_argument{"input node_base cannot be null"};
@@ -102,14 +204,14 @@ create_wall_timer(
     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(
-    std::chrono::duration_cast<std::chrono::nanoseconds>(period),
-    std::move(callback),
-    node_base->get_context());
+    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/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,340 @@
+// 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 {
+    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_intra_process_buffer_type.hpp

@@ -47,6 +47,11 @@ resolve_intra_process_buffer_type(
   return resolved_buffer_type;
 }
 
+RCLCPP_PUBLIC
+rclcpp::IntraProcessBufferType
+resolve_intra_process_buffer_type(
+  const rclcpp::IntraProcessBufferType buffer_type);
+
 }  // namespace detail
 
 }  // namespace rclcpp

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

@@ -30,7 +30,7 @@ namespace detail
 
 std::vector<std::string>
 get_unparsed_ros_arguments(
-  int argc, char const * const argv[],
+  int argc, char const * const * argv,
   rcl_arguments_t * arguments,
   rcl_allocator_t allocator);
 

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"
 
@@ -38,10 +39,7 @@ public:
    */
   Duration(int32_t seconds, uint32_t nanoseconds);
 
-  // This constructor matches any numeric value - ints or floats.
-  explicit Duration(rcl_duration_value_t nanoseconds);
-
-  // This constructor matches std::chrono::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
@@ -72,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;
 
@@ -92,9 +93,13 @@ 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
@@ -106,6 +111,9 @@ 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.
@@ -126,6 +134,13 @@ public:
   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
@@ -140,8 +155,18 @@ public:
 
 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_handler.hpp

@@ -0,0 +1,314 @@
+// 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 <functional>
+#include <memory>
+#include <mutex>
+#include <stdexcept>
+#include <string>
+
+#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();
+
+  /// 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(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(callback_mutex_);
+    if (on_new_event_callback_) {
+      set_on_new_event_callback(nullptr, nullptr);
+      on_new_event_callback_ = nullptr;
+    }
+  }
+
+protected:
+  RCLCPP_PUBLIC
+  void
+  set_on_new_event_callback(rcl_event_callback_t callback, const void * user_data);
+
+  std::recursive_mutex 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);
+      return nullptr;
+    }
+    return std::static_pointer_cast<void>(std::make_shared<EventCallbackInfoT>(callback_info));
+  }
+
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id) override
+  {
+    (void)id;
+    return take_data();
+  }
+
+  /// Execute any entities of the Waitable that are ready.
+  void
+  execute(const std::shared_ptr<void> & data) override
+  {
+    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();
+  }
+
+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_;
+};
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EVENT_HANDLER_HPP_

rclcpp/include/rclcpp/exceptions/exceptions.hpp

@@ -100,6 +100,26 @@ public:
   {}
 };
 
+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
@@ -120,7 +140,7 @@ 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);
+  reset_error_function_t reset_error = rcl_reset_error);
 /* *INDENT-ON* */
 
 class RCLErrorBase
@@ -195,6 +215,14 @@ public:
   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
 {
@@ -211,6 +239,14 @@ public:
   : 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
 {
@@ -245,6 +281,23 @@ public:
   {}
 };
 
+/// 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
 {
@@ -273,6 +326,33 @@ class ParameterModifiedInCallbackException : public std::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
 

rclcpp/include/rclcpp/executor.hpp

@@ -21,6 +21,7 @@
 #include <cstdlib>
 #include <iostream>
 #include <list>
+#include <map>
 #include <memory>
 #include <mutex>
 #include <string>
@@ -28,21 +29,27 @@
 
 #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/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/memory_strategies.hpp"
-#include "rclcpp/memory_strategy.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/wait_set.hpp"
 
 namespace rclcpp
 {
 
 // Forward declaration is used in convenience method signature.
 class Node;
+class ExecutorImplementation;
 
 /// Coordinate the order and timing of available communication tasks.
 /**
@@ -75,13 +82,118 @@ 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
@@ -97,10 +209,19 @@ public:
 
   /// 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
@@ -151,29 +272,80 @@ 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.
+   *
+   * 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));
@@ -188,53 +360,18 @@ public:
    *   code.
    * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
    */
-  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(
-    const 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.
@@ -243,27 +380,71 @@ public:
    * \throws std::runtime_error if there is an issue triggering the guard condition
    */
   RCLCPP_PUBLIC
-  void
+  virtual void
   cancel();
 
-  /// Support dynamic switching of the memory strategy.
+  /// Returns true if the executor is currently spinning.
   /**
-   * 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.
-   * \throws std::runtime_error if memory_strategy is null
+   * This function can be called asynchronously from any thread.
+   * \return True if the executor is currently spinning.
    */
   RCLCPP_PUBLIC
-  void
-  set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy);
+  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,
@@ -274,78 +455,149 @@ protected:
   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_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::CallbackGroup::SharedPtr group);
-
-  RCLCPP_PUBLIC
-  rclcpp::CallbackGroup::SharedPtr
-  get_group_by_timer(rclcpp::TimerBase::SharedPtr timer);
-
+  /// 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_;
 
-  // Mutex to protect the subsequent memory_strategy_.
-  std::mutex memory_strategy_mutex_;
-
-  /// 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_;
 
   RCLCPP_DISABLE_COPY(Executor)
 
-  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
-  std::list<const rcl_guard_condition_t *> guard_conditions_;
+  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
-{
-
-using Executor [[deprecated("use rclcpp::Executor instead")]] = rclcpp::Executor;
-
-}  // namespace executor
 }  // namespace rclcpp
 
 #endif  // RCLCPP__EXECUTOR_HPP_

rclcpp/include/rclcpp/executor_options.hpp

@@ -15,6 +15,8 @@
 #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"
@@ -24,34 +26,32 @@
 namespace rclcpp
 {
 
+class ExecutorOptionsImplementation;
+
 /// Options to be passed to the executor constructor.
 struct ExecutorOptions
 {
-  ExecutorOptions()
-  : memory_strategy(rclcpp::memory_strategies::create_default_strategy()),
-    context(rclcpp::contexts::get_global_default_context()),
-    max_conditions(0)
-  {}
+  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 executor
-{
-
-using ExecutorArgs [[deprecated("use rclcpp::ExecutorOptions instead")]] = ExecutorOptions;
-
-[[deprecated("use rclcpp::ExecutorOptions() instead")]]
-inline
-rclcpp::ExecutorOptions
-create_default_executor_arguments()
-{
-  return rclcpp::ExecutorOptions();
-}
-
-}  // namespace executor
 }  // namespace rclcpp
 
 #endif  // RCLCPP__EXECUTOR_OPTIONS_HPP_

rclcpp/include/rclcpp/executors.hpp

@@ -21,6 +21,7 @@
 #include "rclcpp/executors/multi_threaded_executor.hpp"
 #include "rclcpp/executors/single_threaded_executor.hpp"
 #include "rclcpp/executors/static_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"
@@ -28,6 +29,18 @@
 namespace rclcpp
 {
 
+/// Create a default single-threaded executor and execute all available work exhaustively.
+/** \param[in] node_ptr Shared pointer to the node to spin. */
+RCLCPP_PUBLIC
+void
+spin_all(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  std::chrono::nanoseconds max_duration);
+
+RCLCPP_PUBLIC
+void
+spin_all(rclcpp::Node::SharedPtr node_ptr, std::chrono::nanoseconds max_duration);
+
 /// Create a default single-threaded executor and execute any immediately available work.
 /** \param[in] node_ptr Shared pointer to the node to spin. */
 RCLCPP_PUBLIC
@@ -66,12 +79,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>
+template<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::milli>
 rclcpp::FutureReturnCode
 spin_node_until_future_complete(
   rclcpp::Executor & executor,
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  const 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
@@ -82,13 +95,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::FutureReturnCode
 spin_node_until_future_complete(
   rclcpp::Executor & executor,
   std::shared_ptr<NodeT> node_ptr,
-  const 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(
@@ -104,10 +117,12 @@ template<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::mi
 rclcpp::FutureReturnCode
 spin_until_future_complete(
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  const 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);
 }
 
@@ -116,7 +131,7 @@ template<typename NodeT = rclcpp::Node, typename FutureT, typename TimeRepT = in
 rclcpp::FutureReturnCode
 spin_until_future_complete(
   std::shared_ptr<NodeT> node_ptr,
-  const 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,168 @@
+// 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 "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 = {});
+
+  // 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;
+
+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::WeakPtr,
+    std::owner_less<rclcpp::GuardCondition::WeakPtr>> notify_guard_conditions_;
+};
+
+}  // namespace executors
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -47,11 +47,12 @@ public:
    *
    * \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(
+  explicit MultiThreadedExecutor(
     const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions(),
     size_t number_of_threads = 0,
     bool yield_before_execute = false,
@@ -84,8 +85,6 @@ private:
   size_t number_of_threads_;
   bool yield_before_execute_;
   std::chrono::nanoseconds next_exec_timeout_;
-
-  std::set<TimerBase::SharedPtr> scheduled_timers_;
 };
 
 }  // namespace executors

rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp

@@ -1,238 +0,0 @@
-// 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__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_
-#define RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_
-
-#include <chrono>
-#include <list>
-#include <memory>
-
-#include "rcl/guard_condition.h"
-#include "rcl/wait.h"
-
-#include "rclcpp/experimental/executable_list.hpp"
-#include "rclcpp/macros.hpp"
-#include "rclcpp/memory_strategy.hpp"
-#include "rclcpp/visibility_control.hpp"
-#include "rclcpp/waitable.hpp"
-
-namespace rclcpp
-{
-namespace executors
-{
-
-class StaticExecutorEntitiesCollector final
-  : public rclcpp::Waitable,
-  public std::enable_shared_from_this<StaticExecutorEntitiesCollector>
-{
-public:
-  RCLCPP_SMART_PTR_DEFINITIONS(StaticExecutorEntitiesCollector)
-
-  // Constructor
-  RCLCPP_PUBLIC
-  StaticExecutorEntitiesCollector() = default;
-
-  // Destructor
-  ~StaticExecutorEntitiesCollector();
-
-  /// Initialize StaticExecutorEntitiesCollector
-  /**
-   * \param p_wait_set A reference to the wait set to be used in the executor
-   * \param memory_strategy Shared pointer to the memory strategy to set.
-   * \param executor_guard_condition executor's guard condition
-   * \throws std::runtime_error if memory strategy is null
-   */
-  RCLCPP_PUBLIC
-  void
-  init(
-    rcl_wait_set_t * p_wait_set,
-    rclcpp::memory_strategy::MemoryStrategy::SharedPtr & memory_strategy,
-    rcl_guard_condition_t * executor_guard_condition);
-
-  RCLCPP_PUBLIC
-  void
-  execute() override;
-
-  RCLCPP_PUBLIC
-  void
-  fill_memory_strategy();
-
-  RCLCPP_PUBLIC
-  void
-  fill_executable_list();
-
-  /// Function to reallocate space for entities in the wait set.
-  /**
-   * \throws std::runtime_error if wait set couldn't be cleared or resized.
-   */
-  RCLCPP_PUBLIC
-  void
-  prepare_wait_set();
-
-  /// Function to add_handles_to_wait_set and wait for work and
-  /**
-   * block until the wait set is ready or until the timeout has been exceeded.
-   * \throws std::runtime_error if wait set couldn't be cleared or filled.
-   * \throws any rcl errors from rcl_wait, \sa rclcpp::exceptions::throw_from_rcl_error()
-   */
-  RCLCPP_PUBLIC
-  void
-  refresh_wait_set(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
-
-  /**
-   * \throws std::runtime_error if it couldn't add guard condition to wait set
-   */
-  RCLCPP_PUBLIC
-  bool
-  add_to_wait_set(rcl_wait_set_t * wait_set) override;
-
-  RCLCPP_PUBLIC
-  size_t
-  get_number_of_ready_guard_conditions() override;
-
-  /**
-   * \sa rclcpp::Executor::add_node()
-   * \throw std::runtime_error if node was already added
-   */
-  RCLCPP_PUBLIC
-  void
-  add_node(
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
-
-  /**
-   * \sa rclcpp::Executor::remove_node()
-   * \throw std::runtime_error if no guard condition is associated with node.
-   */
-  RCLCPP_PUBLIC
-  bool
-  remove_node(
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
-
-  /// Complete all available queued work without blocking.
-  /**
-   * This function checks if after the guard condition was triggered
-   * (or a spurious wakeup happened) we are really ready to execute
-   * i.e. re-collect entities
-   */
-  RCLCPP_PUBLIC
-  bool
-  is_ready(rcl_wait_set_t * wait_set) override;
-
-  /// Return number of timers
-  /**
-   * \return number of timers
-   */
-  RCLCPP_PUBLIC
-  size_t
-  get_number_of_timers() {return exec_list_.number_of_timers;}
-
-  /// Return number of subscriptions
-  /**
-   * \return number of subscriptions
-   */
-  RCLCPP_PUBLIC
-  size_t
-  get_number_of_subscriptions() {return exec_list_.number_of_subscriptions;}
-
-  /// Return number of services
-  /**
-   * \return number of services
-   */
-  RCLCPP_PUBLIC
-  size_t
-  get_number_of_services() {return exec_list_.number_of_services;}
-
-  /// Return number of clients
-  /**
-   * \return number of clients
-   */
-  RCLCPP_PUBLIC
-  size_t
-  get_number_of_clients() {return exec_list_.number_of_clients;}
-
-  /// Return number of waitables
-  /**
-   * \return number of waitables
-   */
-  RCLCPP_PUBLIC
-  size_t
-  get_number_of_waitables() {return exec_list_.number_of_waitables;}
-
-  /** Return a SubscritionBase Sharedptr by index.
-   * \param[in] i The index of the SubscritionBase
-   * \return a SubscritionBase shared pointer
-   * \throws std::out_of_range if the argument is higher than the size of the structrue.
-   */
-  RCLCPP_PUBLIC
-  rclcpp::SubscriptionBase::SharedPtr
-  get_subscription(size_t i) {return exec_list_.subscription[i];}
-
-  /** Return a TimerBase Sharedptr by index.
-   * \param[in] i The index of the TimerBase
-   * \return a TimerBase shared pointer
-   * \throws std::out_of_range if the argument is higher than the size.
-   */
-  RCLCPP_PUBLIC
-  rclcpp::TimerBase::SharedPtr
-  get_timer(size_t i) {return exec_list_.timer[i];}
-
-  /** Return a ServiceBase Sharedptr by index.
-   * \param[in] i The index of the ServiceBase
-   * \return a ServiceBase shared pointer
-   * \throws std::out_of_range if the argument is higher than the size.
-   */
-  RCLCPP_PUBLIC
-  rclcpp::ServiceBase::SharedPtr
-  get_service(size_t i) {return exec_list_.service[i];}
-
-  /** Return a ClientBase Sharedptr by index
-   * \param[in] i The index of the ClientBase
-   * \return a ClientBase shared pointer
-   * \throws std::out_of_range if the argument is higher than the size.
-   */
-  RCLCPP_PUBLIC
-  rclcpp::ClientBase::SharedPtr
-  get_client(size_t i) {return exec_list_.client[i];}
-
-  /** Return a Waitable Sharedptr by index
-   * \param[in] i The index of the Waitable
-   * \return a Waitable shared pointer
-   * \throws std::out_of_range if the argument is higher than the size.
-   */
-  RCLCPP_PUBLIC
-  rclcpp::Waitable::SharedPtr
-  get_waitable(size_t i) {return exec_list_.waitable[i];}
-
-private:
-  /// Nodes guard conditions which trigger this waitable
-  std::list<const rcl_guard_condition_t *> guard_conditions_;
-
-  /// Memory strategy: an interface for handling user-defined memory allocation strategies.
-  rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;
-
-  /// List of weak nodes registered in the static executor
-  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
-
-  /// Wait set for managing entities that the rmw layer waits on.
-  rcl_wait_set_t * p_wait_set_ = nullptr;
-
-  /// Executable list: timers, subscribers, clients, services and waitables
-  rclcpp::experimental::ExecutableList exec_list_;
-};
-
-}  // namespace executors
-}  // namespace rclcpp
-
-#endif  // RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_

rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp

@@ -15,23 +15,13 @@
 #ifndef RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_
 #define RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_
 
-#include <cassert>
-#include <cstdlib>
+#include <atomic>
+#include <chrono>
 #include <memory>
-#include <vector>
-#include <string>
-
-#include "rmw/rmw.h"
 
 #include "rclcpp/executor.hpp"
-#include "rclcpp/executors/static_executor_entities_collector.hpp"
-#include "rclcpp/experimental/executable_list.hpp"
-#include "rclcpp/macros.hpp"
-#include "rclcpp/memory_strategies.hpp"
-#include "rclcpp/node.hpp"
-#include "rclcpp/rate.hpp"
-#include "rclcpp/utilities.hpp"
-#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/executors/executor_entities_collection.hpp"
+#include "rclcpp/executors/single_threaded_executor.hpp"
 
 namespace rclcpp
 {
@@ -41,9 +31,15 @@ namespace executors
 /// Static executor implementation
 /**
  * This executor is a static version of the original single threaded executor.
- * It's static because it doesn't reconstruct the executable list for every iteration.
+ * It contains some performance optimization to avoid unnecessary reconstructions of
+ * the executable list for every iteration.
  * All nodes, callbackgroups, timers, subscriptions etc. are created before
  * spin() is called, and modified only when an entity is added/removed to/from a node.
+ * This executor is deprecated because these performance improvements have now been
+ * applied to all other executors.
+ * This executor is also considered unstable due to known bugs.
+ * See the unit-tests that are only applied to `StandardExecutors` for information
+ * on the known limitations.
  *
  * To run this executor instead of SingleThreadedExecutor replace:
  * rclcpp::executors::SingleThreadedExecutor exec;
@@ -54,7 +50,8 @@ namespace executors
  * exec.spin();
  * exec.remove_node(node);
  */
-class StaticSingleThreadedExecutor : public rclcpp::Executor
+class [[deprecated("Use rclcpp::executors::SingleThreadedExecutor")]] StaticSingleThreadedExecutor
+  : public rclcpp::Executor
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(StaticSingleThreadedExecutor)
@@ -64,7 +61,7 @@ public:
   explicit StaticSingleThreadedExecutor(
     const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
 
-  /// Default destrcutor.
+  /// Default destructor.
   RCLCPP_PUBLIC
   virtual ~StaticSingleThreadedExecutor();
 
@@ -78,133 +75,68 @@ public:
   void
   spin() override;
 
-  /// Add a node to the executor.
+  /// Static executor implementation of spin some
   /**
-   * An executor can have zero or more nodes which provide work during `spin` functions.
-   * \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 node was already added or if rcl_trigger_guard_condition
-   * return an error
-   */
-  RCLCPP_PUBLIC
-  void
-  add_node(
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    bool notify = true) override;
-
-  /// Convenience function which takes Node and forwards NodeBaseInterface.
-  /**
-   * \throw std::runtime_error if node was already added or if rcl_trigger_guard_condition
-   * returns an error
-   */
-  RCLCPP_PUBLIC
-  void
-  add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
-
-  /// Remove a node from the 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 rcl_trigger_guard_condition returns an error
-   */
-  RCLCPP_PUBLIC
-  void
-  remove_node(
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    bool notify = true) override;
-
-  /// Convenience function which takes Node and forwards NodeBaseInterface.
-  /**
-   * \throw std::runtime_error if rcl_trigger_guard_condition returns an error
-   */
-  RCLCPP_PUBLIC
-  void
-  remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
-
-  /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
-  /**
-   * \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::execute_ready_executables.
-   *   `-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`.
+   * This non-blocking function will execute entities that
+   * were ready when this API was called, until timeout or no
+   * more work available. Entities that got ready while
+   * executing work, won't be taken into account here.
    *
-   *  Example usage:
-   *  rclcpp::executors::StaticSingleThreadedExecutor exec;
-   *  // ... other part of code like creating node
-   *  // define future
-   *  exec.add_node(node);
-   *  exec.spin_until_future_complete(future);
+   * Example:
+   *   while(condition) {
+   *     spin_some();
+   *     sleep(); // User should have some sync work or
+   *              // sleep to avoid a 100% CPU usage
+   *   }
    */
-  template<typename ResponseT, typename TimeRepT = int64_t, typename TimeT = std::milli>
-  rclcpp::FutureReturnCode
-  spin_until_future_complete(
-    std::shared_future<ResponseT> & future,
-    std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
-  {
-    std::future_status status = future.wait_for(std::chrono::seconds(0));
-    if (status == std::future_status::ready) {
-      return rclcpp::FutureReturnCode::SUCCESS;
-    }
+  RCLCPP_PUBLIC
+  void
+  spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)) override;
 
-    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;
-
-    entities_collector_ = std::make_shared<StaticExecutorEntitiesCollector>();
-    entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_);
-
-    while (rclcpp::ok(this->context_)) {
-      // Do one set of work.
-      entities_collector_->refresh_wait_set(timeout_left);
-      execute_ready_executables();
-      // 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 rclcpp::FutureReturnCode::SUCCESS;
-      }
-      // If the original timeout is < 0, then this is blocking, never TIMEOUT.
-      if (timeout_ns < std::chrono::nanoseconds::zero()) {
-        continue;
-      }
-      // Otherwise check if we still have time to wait, return TIMEOUT if not.
-      auto now = std::chrono::steady_clock::now();
-      if (now >= end_time) {
-        return rclcpp::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 rclcpp::FutureReturnCode::INTERRUPTED;
-  }
+  /// Static executor implementation of spin all
+  /**
+   * This non-blocking function will execute entities until timeout (must be >= 0)
+   * or no more work available.
+   * If timeout is `0`, potentially it blocks forever until no more work is available.
+   * If new entities get ready 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:
-  /// Check which executables in ExecutableList struct are ready from wait_set and execute them.
   /**
-   * \param[in] exec_list Structure that can hold subscriptionbases, timerbases, etc
-   * \param[in] timeout Optional timeout parameter.
+   * @brief Executes ready executables from wait set.
+   * @param collection entities to evaluate for ready executables.
+   * @param wait_result result to check for ready executables.
+   * @param spin_once if true executes only the first ready executable.
+   * @return true if any executable was ready.
    */
-  RCLCPP_PUBLIC
+  bool
+  execute_ready_executables(
+    const rclcpp::executors::ExecutorEntitiesCollection & collection,
+    rclcpp::WaitResult<rclcpp::WaitSet> & wait_result,
+    bool spin_once);
+
   void
-  execute_ready_executables();
+  spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive);
+
+  void
+  spin_once_impl(std::chrono::nanoseconds timeout) override;
+
+  std::optional<rclcpp::WaitResult<rclcpp::WaitSet>>
+  collect_and_wait(std::chrono::nanoseconds timeout);
 
 private:
   RCLCPP_DISABLE_COPY(StaticSingleThreadedExecutor)
-
-  StaticExecutorEntitiesCollector::SharedPtr entities_collector_;
 };
 
 }  // namespace executors

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

@@ -15,6 +15,8 @@
 #ifndef RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_
 #define RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_
 
+#include <vector>
+
 namespace rclcpp
 {
 namespace experimental
@@ -31,8 +33,11 @@ public:
   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

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

@@ -16,13 +16,16 @@
 #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
 {
@@ -42,6 +45,7 @@ public:
 
   virtual bool has_data() const = 0;
   virtual bool use_take_shared_method() const = 0;
+  virtual size_t available_capacity() const = 0;
 };
 
 template<
@@ -63,6 +67,9 @@ public:
 
   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<
@@ -93,6 +100,10 @@ public:
 
     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 {
@@ -122,6 +133,16 @@ public:
     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();
@@ -137,6 +158,11 @@ public:
     return std::is_same<BufferT, MessageSharedPtr>::value;
   }
 
+  size_t available_capacity() const override
+  {
+    return buffer_->available_capacity();
+  }
+
 private:
   std::unique_ptr<BufferImplementationBase<BufferT>> buffer_;
 
@@ -231,6 +257,71 @@ private:
   {
     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

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

@@ -15,9 +15,6 @@
 #ifndef RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_
 #define RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_
 
-#include <algorithm>
-#include <cstddef>
-#include <cstdint>
 #include <memory>
 #include <mutex>
 #include <stdexcept>
@@ -29,6 +26,7 @@
 #include "rclcpp/logging.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "tracetools/tracetools.h"
 
 namespace rclcpp
 {
@@ -37,6 +35,10 @@ 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>
 {
@@ -51,59 +53,245 @@ public:
     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() {}
 
-  void enqueue(BufferT request)
+  /// 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_);
+    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_);
+    if (is_full_()) {
+      read_index_ = next_(read_index_);
     } else {
       size_++;
     }
   }
 
-  BufferT dequeue()
+  /// 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()) {
-      RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Calling dequeue on empty intra-process buffer");
-      throw std::runtime_error("Calling dequeue on empty intra-process buffer");
+    if (!has_data_()) {
+      return BufferT();
     }
 
     auto request = std::move(ring_buffer_[read_index_]);
-    read_index_ = next(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));
+  }
+
+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_;
   }
 
-  inline bool has_data() const
+  /// 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;
   }
 
-  inline bool is_full()
+  /// 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_;
   }
 
-  void clear() {}
+  /// 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_;
+  }
+
+  /// 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) {
+      result_vtr.emplace_back(
+        new typename is_std_unique_ptr<T>::Ptr_type(
+          *(ring_buffer_[(read_index_ + id) % capacity_])));
+    }
+    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 {};
+  }
 
-private:
   size_t capacity_;
 
   std::vector<BufferT> ring_buffer_;
@@ -112,7 +300,7 @@ private:
   size_t read_index_;
   size_t size_;
 
-  std::mutex mutex_;
+  mutable std::mutex mutex_;
 };
 
 }  // namespace buffers

rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp

@@ -16,14 +16,13 @@
 #define RCLCPP__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_
 
 #include <memory>
-#include <type_traits>
+#include <stdexcept>
 #include <utility>
 
-#include "rcl/subscription.h"
-
 #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
 {
@@ -37,13 +36,13 @@ template<
 typename rclcpp::experimental::buffers::IntraProcessBuffer<MessageT, Alloc, Deleter>::UniquePtr
 create_intra_process_buffer(
   IntraProcessBufferType buffer_type,
-  rmw_qos_profile_t qos,
+  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;
+  size_t buffer_size = qos.depth();
 
   using rclcpp::experimental::buffers::IntraProcessBuffer;
   typename IntraProcessBuffer<MessageT, Alloc, Deleter>::UniquePtr buffer;

rclcpp/include/rclcpp/experimental/executable_list.hpp

@@ -15,7 +15,6 @@
 #ifndef RCLCPP__EXPERIMENTAL__EXECUTABLE_LIST_HPP_
 #define RCLCPP__EXPERIMENTAL__EXECUTABLE_LIST_HPP_
 
-#include <memory>
 #include <vector>
 
 #include "rclcpp/client.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] 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.
+   * \param[in] options Options used to configure the executor.
+   */
+  RCLCPP_PUBLIC
+  EventsExecutor(
+    rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue = std::make_unique<
+      rclcpp::experimental::executors::SimpleEventsQueue>(),
+    bool execute_timers_separate_thread = false,
+    const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
+
+  /// 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

@@ -19,24 +19,25 @@
 
 #include <shared_mutex>
 
-#include <algorithm>
-#include <atomic>
-#include <cstdint>
-#include <exception>
-#include <map>
+#include <iterator>
 #include <memory>
-#include <string>
+#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
@@ -112,9 +113,40 @@ public:
    * \param subscription the SubscriptionIntraProcess to register.
    * \return an unsigned 64-bit integer which is the subscription's unique id.
    */
-  RCLCPP_PUBLIC
+  template<
+    typename ROSMessageType,
+    typename Alloc = std::allocator<ROSMessageType>
+  >
   uint64_t
-  add_subscription(rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription);
+  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.
   /**
@@ -131,14 +163,21 @@ public:
    * 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);
+  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.
   /**
@@ -172,16 +211,19 @@ public:
    *
    * \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 Alloc = std::allocator<void>,
-    typename Deleter = std::default_delete<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,
-    std::shared_ptr<typename allocator::AllocRebind<MessageT, Alloc>::allocator_type> allocator)
+    typename allocator::AllocRebind<MessageT, Alloc>::allocator_type & allocator)
   {
     using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
     using MessageAllocatorT = typename MessageAllocTraits::allocator_type;
@@ -202,21 +244,22 @@ public:
       // 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>(msg, sub_ids.take_shared_subscriptions);
+      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 we this case is equivalent to all the buffers requiring 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);
+      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>(
+      this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
         std::move(message),
         concatenated_vector,
         allocator);
@@ -225,24 +268,26 @@ public:
     {
       // 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);
+      auto shared_msg = std::allocate_shared<MessageT, MessageAllocatorT>(allocator, *message);
 
-      this->template add_shared_msg_to_buffers<MessageT>(
+      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>(
+      this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
         std::move(message), sub_ids.take_ownership_subscriptions, allocator);
     }
   }
 
   template<
     typename MessageT,
-    typename Alloc = std::allocator<void>,
-    typename Deleter = std::default_delete<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,
-    std::shared_ptr<typename allocator::AllocRebind<MessageT, Alloc>::allocator_type> allocator)
+    typename allocator::AllocRebind<MessageT, Alloc>::allocator_type & allocator)
   {
     using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
     using MessageAllocatorT = typename MessageAllocTraits::allocator_type;
@@ -263,31 +308,58 @@ public:
       // 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>(
+        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);
+      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>(
+        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>(
+        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
@@ -302,26 +374,12 @@ 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 SubscriptionInfo
-  {
-    SubscriptionInfo() = default;
-
-    rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription;
-    rmw_qos_profile_t qos;
-    const char * topic_name;
-    bool use_take_shared_method;
-  };
-
-  struct PublisherInfo
-  {
-    PublisherInfo() = default;
-
-    rclcpp::PublisherBase::WeakPtr publisher;
-    rmw_qos_profile_t qos;
-    const char * topic_name;
-  };
-
   struct SplittedSubscriptions
   {
     std::vector<uint64_t> take_shared_subscriptions;
@@ -329,10 +387,13 @@ private:
   };
 
   using SubscriptionMap =
-    std::unordered_map<uint64_t, SubscriptionInfo>;
+    std::unordered_map<uint64_t, rclcpp::experimental::SubscriptionIntraProcessBase::WeakPtr>;
 
   using PublisherMap =
-    std::unordered_map<uint64_t, PublisherInfo>;
+    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>;
@@ -348,65 +409,229 @@ private:
 
   RCLCPP_PUBLIC
   bool
-  can_communicate(PublisherInfo pub_info, SubscriptionInfo sub_info) const;
+  can_communicate(
+    rclcpp::PublisherBase::SharedPtr pub,
+    rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr sub) const;
 
-  template<typename MessageT>
-  void
-  add_shared_msg_to_buffers(
-    std::shared_ptr<const MessageT> message,
-    std::vector<uint64_t> subscription_ids)
+  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)
   {
-    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");
+    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);
       }
-      auto subscription_base = subscription_it->second.subscription;
-
-      auto subscription = std::static_pointer_cast<
-        rclcpp::experimental::SubscriptionIntraProcess<MessageT>
-        >(subscription_base);
-
-      subscription->provide_intra_process_message(message);
     }
   }
 
   template<
     typename MessageT,
-    typename Alloc = std::allocator<void>,
-    typename Deleter = std::default_delete<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,
-    std::shared_ptr<typename allocator::AllocRebind<MessageT, Alloc>::allocator_type> allocator)
+    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.subscription;
+      auto subscription_base = subscription_it->second.lock();
+      if (subscription_base == nullptr) {
+        subscriptions_.erase(subscription_it);
+        continue;
+      }
 
-      auto subscription = std::static_pointer_cast<
-        rclcpp::experimental::SubscriptionIntraProcess<MessageT>
+      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);
 
-      if (std::next(it) == subscription_ids.end()) {
-        // If this is the last subscription, give up ownership
-        subscription->provide_intra_process_message(std::move(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 {
-        // Copy the message since we have additional subscriptions to serve
-        MessageUniquePtr copy_message;
-        Deleter deleter = message.get_deleter();
-        auto ptr = MessageAllocTraits::allocate(*allocator.get(), 1);
-        MessageAllocTraits::construct(*allocator.get(), ptr, *message);
-        copy_message = MessageUniquePtr(ptr, deleter);
+        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);
 
-        subscription->provide_intra_process_message(std::move(copy_message));
+            ros_message_subscription->provide_intra_process_message(
+              MessageUniquePtr(ptr, deleter));
+          }
+        }
       }
     }
   }
@@ -414,6 +639,7 @@ private:
   PublisherToSubscriptionIdsMap pub_to_subs_;
   SubscriptionMap subscriptions_;
   PublisherMap publishers_;
+  PublisherBufferMap publisher_buffers_;
 
   mutable std::shared_timed_mutex mutex_;
 };

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

@@ -15,21 +15,22 @@
 #ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_
 #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_
 
-#include <rmw/rmw.h>
+#include <rmw/types.h>
 
-#include <functional>
 #include <memory>
+#include <stdexcept>
 #include <string>
+#include <type_traits>
 #include <utility>
 
-#include "rcl/error_handling.h"
+#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/create_intra_process_buffer.hpp"
-#include "rclcpp/experimental/subscription_intra_process_base.hpp"
+#include "rclcpp/experimental/subscription_intra_process_buffer.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
-#include "rclcpp/waitable.hpp"
 #include "tracetools/tracetools.h"
 
 namespace rclcpp
@@ -39,61 +40,57 @@ namespace experimental
 
 template<
   typename MessageT,
-  typename Alloc = std::allocator<void>,
-  typename Deleter = std::default_delete<MessageT>,
-  typename CallbackMessageT = MessageT>
-class SubscriptionIntraProcess : public SubscriptionIntraProcessBase
+  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 = allocator::AllocRebind<MessageT, Alloc>;
-  using MessageAlloc = typename MessageAllocTraits::allocator_type;
-  using ConstMessageSharedPtr = std::shared_ptr<const MessageT>;
-  using MessageUniquePtr = std::unique_ptr<MessageT, Deleter>;
-
-  using BufferUniquePtr = typename rclcpp::experimental::buffers::IntraProcessBuffer<
-    MessageT,
-    Alloc,
-    Deleter
-    >::UniquePtr;
+  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<CallbackMessageT, Alloc> callback,
+    AnySubscriptionCallback<MessageT, Alloc> callback,
     std::shared_ptr<Alloc> allocator,
     rclcpp::Context::SharedPtr context,
     const std::string & topic_name,
-    rmw_qos_profile_t qos_profile,
+    const rclcpp::QoS & qos_profile,
     rclcpp::IntraProcessBufferType buffer_type)
-  : SubscriptionIntraProcessBase(topic_name, qos_profile),
+  : SubscriptionIntraProcessBuffer<SubscribedType, SubscribedTypeAlloc,
+      SubscribedTypeDeleter, ROSMessageType>(
+      std::make_shared<SubscribedTypeAlloc>(*allocator),
+      context,
+      topic_name,
+      qos_profile,
+      buffer_type),
     any_callback_(callback)
   {
-    if (!std::is_same<MessageT, CallbackMessageT>::value) {
-      throw std::runtime_error("SubscriptionIntraProcess wrong callback type");
-    }
-
-    // Create the intra-process buffer.
-    buffer_ = rclcpp::experimental::create_intra_process_buffer<MessageT, Alloc, Deleter>(
-      buffer_type,
-      qos_profile,
-      allocator);
-
-    // Create the guard condition.
-    rcl_guard_condition_options_t guard_condition_options =
-      rcl_guard_condition_get_default_options();
-
-    gc_ = rcl_get_zero_initialized_guard_condition();
-    rcl_ret_t ret = rcl_guard_condition_init(
-      &gc_, context->get_rcl_context().get(), guard_condition_options);
-
-    if (RCL_RET_OK != ret) {
-      throw std::runtime_error("SubscriptionIntraProcess init error initializing guard condition");
-    }
-
-    TRACEPOINT(
+    TRACETOOLS_TRACEPOINT(
       rclcpp_subscription_callback_added,
-      (const void *)this,
-      (const void *)&any_callback_);
+      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.
@@ -102,72 +99,95 @@ public:
 #endif
   }
 
-  bool
-  is_ready(rcl_wait_set_t * wait_set)
-  {
-    (void)wait_set;
-    return buffer_->has_data();
-  }
-
-  void execute()
-  {
-    execute_impl<CallbackMessageT>();
-  }
+  virtual ~SubscriptionIntraProcess() = default;
 
   void
-  provide_intra_process_message(ConstMessageSharedPtr message)
+  add_to_wait_set(rcl_wait_set_t & wait_set) override
   {
-    buffer_->add_shared(std::move(message));
-    trigger_guard_condition();
+    // 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);
   }
 
-  void
-  provide_intra_process_message(MessageUniquePtr message)
+  std::shared_ptr<void>
+  take_data() override
   {
-    buffer_->add_unique(std::move(message));
-    trigger_guard_condition();
+    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)))
+    );
   }
 
-  bool
-  use_take_shared_method() const
+  void execute(const std::shared_ptr<void> & data) override
   {
-    return buffer_->use_take_shared_method();
-  }
-
-private:
-  void
-  trigger_guard_condition()
-  {
-    rcl_ret_t ret = rcl_trigger_guard_condition(&gc_);
-    (void)ret;
+    execute_impl<SubscribedType>(data);
   }
 
+protected:
   template<typename T>
   typename std::enable_if<std::is_same<T, rcl_serialized_message_t>::value, void>::type
-  execute_impl()
+  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()
+  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 msg = buffer_->consume_shared();
-      any_callback_.dispatch_intra_process(msg, msg_info);
+      ConstMessageSharedPtr shared_msg = shared_ptr->first;
+      any_callback_.dispatch_intra_process(shared_msg, msg_info);
     } else {
-      MessageUniquePtr msg = buffer_->consume_unique();
-      any_callback_.dispatch_intra_process(std::move(msg), msg_info);
+      MessageUniquePtr unique_msg = std::move(shared_ptr->second);
+      any_callback_.dispatch_intra_process(std::move(unique_msg), msg_info);
     }
+    shared_ptr.reset();
   }
 
-  AnySubscriptionCallback<CallbackMessageT, Alloc> any_callback_;
-  BufferUniquePtr buffer_;
+  AnySubscriptionCallback<MessageT, Alloc> any_callback_;
 };
 
 }  // namespace experimental

rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp

@@ -15,17 +15,17 @@
 #ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_
 #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_
 
-#include <rmw/rmw.h>
-
-#include <functional>
+#include <algorithm>
 #include <memory>
 #include <mutex>
 #include <string>
-#include <utility>
 
-#include "rcl/error_handling.h"
+#include "rcl/wait.h"
+#include "rmw/impl/cpp/demangle.hpp"
 
-#include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/guard_condition.hpp"
+#include "rclcpp/logging.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/waitable.hpp"
 
 namespace rclcpp
@@ -38,28 +38,57 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase)
 
+  enum class EntityType : std::size_t
+  {
+    Subscription,
+  };
+
   RCLCPP_PUBLIC
-  SubscriptionIntraProcessBase(const std::string & topic_name, rmw_qos_profile_t qos_profile)
-  : topic_name_(topic_name), qos_profile_(qos_profile)
+  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() {return 1;}
+  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
-  add_to_wait_set(rcl_wait_set_t * wait_set);
+  is_durability_transient_local() const;
 
-  virtual bool
-  is_ready(rcl_wait_set_t * wait_set) = 0;
+  bool
+  is_ready(const rcl_wait_set_t & wait_set) override = 0;
 
-  virtual void
-  execute() = 0;
+  std::shared_ptr<void>
+  take_data() override = 0;
 
-  virtual bool
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id) override
+  {
+    (void)id;
+    return take_data();
+  }
+
+  void
+  execute(const std::shared_ptr<void> & data) override = 0;
+
+  virtual
+  bool
   use_take_shared_method() const = 0;
 
   RCLCPP_PUBLIC
@@ -67,19 +96,113 @@ public:
   get_topic_name() const;
 
   RCLCPP_PUBLIC
-  rmw_qos_profile_t
+  QoS
   get_actual_qos() const;
 
-protected:
-  std::recursive_mutex reentrant_mutex_;
-  rcl_guard_condition_t gc_;
+  /// 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(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(callback_mutex_);
+    on_new_message_callback_ = nullptr;
+  }
+
+protected:
+  std::recursive_mutex callback_mutex_;
+  std::function<void(size_t)> on_new_message_callback_ {nullptr};
+  size_t unread_count_{0};
+  rclcpp::GuardCondition gc_;
 
-private:
   virtual void
   trigger_guard_condition() = 0;
 
+  void
+  invoke_on_new_message()
+  {
+    std::lock_guard<std::recursive_mutex> lock(this->callback_mutex_);
+    if (this->on_new_message_callback_) {
+      this->on_new_message_callback_(1);
+    } else {
+      this->unread_count_++;
+    }
+  }
+
+private:
   std::string topic_name_;
-  rmw_qos_profile_t qos_profile_;
+  QoS qos_profile_;
 };
 
 }  // namespace experimental

rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp

@@ -0,0 +1,202 @@
+// 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(const rcl_wait_set_t & wait_set) override
+  {
+    (void) wait_set;
+    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,10 +80,12 @@ 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
@@ -97,10 +99,12 @@ struct function_traits<
 
 // std::bind for object const 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 ...) 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>>
+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
@@ -114,7 +118,9 @@ 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 ...)>>
@@ -162,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

@@ -42,20 +42,6 @@ RCLCPP_PUBLIC
 std::string
 to_string(const FutureReturnCode & future_return_code);
 
-namespace executor
-{
-
-using FutureReturnCode [[deprecated("use rclcpp::FutureReturnCode instead")]] = FutureReturnCode;
-
-[[deprecated("use rclcpp::to_string(const rclcpp::FutureReturnCode &) instead")]]
-inline
-std::string
-to_string(const rclcpp::FutureReturnCode & future_return_code)
-{
-  return rclcpp::to_string(future_return_code);
-}
-
-}  // namespace executor
 }  // 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,172 @@
+// 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;
+
+  /// 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();
@@ -127,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
@@ -160,14 +166,23 @@ 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::WeakPtr parent_context_;
+  std::weak_ptr<rclcpp::Context> weak_parent_context_;
+  std::shared_ptr<rcl_context_t> rcl_parent_context_;
 
   std::thread listener_thread_;
   bool is_started_;
@@ -178,8 +193,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();
-  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

@@ -40,30 +40,34 @@ public:
    *   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(
-    rclcpp::Context::SharedPtr context =
-    rclcpp::contexts::get_global_default_context());
+    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 context used when creating this guard condition.
+  /// Return the underlying rcl guard condition structure.
   RCLCPP_PUBLIC
-  rclcpp::Context::SharedPtr
-  get_context() const;
+  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;
 
-  /// Notify the wait set waiting on this condition, if any, that the condition had been met.
+  /// 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.
@@ -89,10 +93,44 @@ 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:
-  rclcpp::Context::SharedPtr context_;
   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

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,7 +29,7 @@ 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
   /**
@@ -80,11 +81,30 @@ 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};
 };

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

@@ -31,21 +31,20 @@ 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;
 
-public:
   /// Constructor of the LoanedMessage class.
   /**
    * The constructor of this class allocates memory for a given message type
    * and associates this with a given publisher.
    *
-   * Given the publisher instance, a case differentiation is being performaned
-   * which decides whether the underlying middleware is able to allocate the appropriate
-   * memory for this message type or not.
-   * In the case that the middleware can not loan messages, the passed in allocator instance
-   * is being used to allocate the message within the scope of this class.
-   * Otherwise, the allocator is being ignored and the allocation is solely performaned
+   * 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.
@@ -53,12 +52,12 @@ public:
    * 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 can not allocate messages
+   * \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,
-    std::allocator<MessageT> allocator)
+    MessageAllocator allocator)
   : pub_(pub),
     message_(nullptr),
     message_allocator_(std::move(allocator))
@@ -82,39 +81,14 @@ public:
     }
   }
 
-  /// Constructor of the LoanedMessage class.
-  /**
-   * The constructor of this class allocates memory for a given message type
-   * and associates this with a given publisher.
-   *
-   * Given the publisher instance, a case differentiation is being performaned
-   * which decides whether the underlying middleware is able to allocate the appropriate
-   * memory for this message type or not.
-   * In the case that the middleware can not loan messages, the passed in allocator instance
-   * is being used to allocate the message within the scope of this class.
-   * Otherwise, the allocator is being ignored and the allocation is solely performaned
-   * 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 can not allocate messages
-   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
-   */
-  LoanedMessage(
-    const rclcpp::PublisherBase * pub,
-    std::shared_ptr<std::allocator<MessageT>> allocator)
-  : LoanedMessage(*pub, *allocator)
-  {}
-
   /// 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.
   /**
@@ -183,14 +157,30 @@ public:
   /**
    * 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 Raw pointer to the message instance.
+   * \return std::unique_ptr to the message instance.
    */
-  MessageT * release()
+  std::unique_ptr<MessageT, std::function<void(MessageT *)>>
+  release()
   {
     auto msg = message_;
     message_ = nullptr;
-    return msg;
+
+    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:

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,60 @@ RCLCPP_PUBLIC
 Logger
 get_node_logger(const rcl_node_t * node);
 
+// TODO(ahcorde): Remove deprecated class on the next release (in Rolling after Kilted).
+#if !defined(_WIN32)
+# pragma GCC diagnostic push
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#else  // !defined(_WIN32)
+# pragma warning(push)
+# pragma warning(disable: 4996)
+#endif
+/// 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.
+ */
+[[deprecated("use rclcpp::get_log_directory instead of rclcpp::get_logging_directory")]]
+RCLCPP_PUBLIC
+rcpputils::fs::path
+get_logging_directory();
+
+// remove warning suppression
+#if !defined(_WIN32)
+# pragma GCC diagnostic pop
+#else  // !defined(_WIN32)
+# pragma warning(pop)
+#endif
+
+/// 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 +152,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 +185,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/memory_strategy.hpp

@@ -16,6 +16,7 @@
 #define RCLCPP__MEMORY_STRATEGY_HPP_
 
 #include <list>
+#include <map>
 #include <memory>
 
 #include "rcl/allocator.h"
@@ -42,11 +43,13 @@ class RCLCPP_PUBLIC MemoryStrategy
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy)
-  using WeakNodeList = std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>;
+  using WeakCallbackGroupsToNodesMap = std::map<rclcpp::CallbackGroup::WeakPtr,
+      rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+      std::owner_less<rclcpp::CallbackGroup::WeakPtr>>;
 
   virtual ~MemoryStrategy() = default;
 
-  virtual bool collect_entities(const WeakNodeList & weak_nodes) = 0;
+  virtual bool collect_entities(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0;
 
   virtual size_t number_of_ready_subscriptions() const = 0;
   virtual size_t number_of_ready_services() const = 0;
@@ -61,87 +64,89 @@ public:
   virtual void clear_handles() = 0;
   virtual void remove_null_handles(rcl_wait_set_t * wait_set) = 0;
 
-  virtual void add_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
+  virtual void
+  add_guard_condition(const rclcpp::GuardCondition & guard_condition) = 0;
 
-  virtual void remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
+  virtual void
+  remove_guard_condition(const rclcpp::GuardCondition * guard_condition) = 0;
 
   virtual void
   get_next_subscription(
     rclcpp::AnyExecutable & any_exec,
-    const WeakNodeList & weak_nodes) = 0;
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0;
 
   virtual void
   get_next_service(
     rclcpp::AnyExecutable & any_exec,
-    const WeakNodeList & weak_nodes) = 0;
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0;
 
   virtual void
   get_next_client(
     rclcpp::AnyExecutable & any_exec,
-    const WeakNodeList & weak_nodes) = 0;
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0;
 
   virtual void
   get_next_timer(
     rclcpp::AnyExecutable & any_exec,
-    const WeakNodeList & weak_nodes) = 0;
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0;
 
   virtual void
   get_next_waitable(
     rclcpp::AnyExecutable & any_exec,
-    const WeakNodeList & weak_nodes) = 0;
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0;
 
   virtual rcl_allocator_t
   get_allocator() = 0;
 
   static rclcpp::SubscriptionBase::SharedPtr
   get_subscription_by_handle(
-    std::shared_ptr<const rcl_subscription_t> subscriber_handle,
-    const WeakNodeList & weak_nodes);
+    const std::shared_ptr<const rcl_subscription_t> & subscriber_handle,
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::ServiceBase::SharedPtr
   get_service_by_handle(
-    std::shared_ptr<const rcl_service_t> service_handle,
-    const WeakNodeList & weak_nodes);
+    const std::shared_ptr<const rcl_service_t> & service_handle,
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::ClientBase::SharedPtr
   get_client_by_handle(
-    std::shared_ptr<const rcl_client_t> client_handle,
-    const WeakNodeList & weak_nodes);
+    const std::shared_ptr<const rcl_client_t> & client_handle,
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::TimerBase::SharedPtr
   get_timer_by_handle(
-    std::shared_ptr<const rcl_timer_t> timer_handle,
-    const WeakNodeList & weak_nodes);
+    const std::shared_ptr<const rcl_timer_t> & timer_handle,
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
   get_node_by_group(
-    rclcpp::CallbackGroup::SharedPtr group,
-    const WeakNodeList & weak_nodes);
+    const rclcpp::CallbackGroup::SharedPtr & group,
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::CallbackGroup::SharedPtr
   get_group_by_subscription(
-    rclcpp::SubscriptionBase::SharedPtr subscription,
-    const WeakNodeList & weak_nodes);
+    const rclcpp::SubscriptionBase::SharedPtr & subscription,
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::CallbackGroup::SharedPtr
   get_group_by_service(
-    rclcpp::ServiceBase::SharedPtr service,
-    const WeakNodeList & weak_nodes);
+    const rclcpp::ServiceBase::SharedPtr & service,
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::CallbackGroup::SharedPtr
   get_group_by_client(
-    rclcpp::ClientBase::SharedPtr client,
-    const WeakNodeList & weak_nodes);
+    const rclcpp::ClientBase::SharedPtr & client,
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::CallbackGroup::SharedPtr
   get_group_by_timer(
-    rclcpp::TimerBase::SharedPtr timer,
-    const WeakNodeList & weak_nodes);
+    const rclcpp::TimerBase::SharedPtr & timer,
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::CallbackGroup::SharedPtr
   get_group_by_waitable(
-    rclcpp::Waitable::SharedPtr waitable,
-    const WeakNodeList & weak_nodes);
+    const rclcpp::Waitable::SharedPtr & waitable,
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 };
 
 }  // namespace memory_strategy

rclcpp/include/rclcpp/network_flow_endpoint.hpp

@@ -0,0 +1,115 @@
+// Copyright 2020 Ericsson AB
+//
+// 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__NETWORK_FLOW_ENDPOINT_HPP_
+#define RCLCPP__NETWORK_FLOW_ENDPOINT_HPP_
+
+#include <cstdint>
+#include <string>
+#include <iostream>
+
+#include "rcl/network_flow_endpoints.h"
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Forward declaration
+class NetworkFlowEndpoint;
+
+/// Check if two NetworkFlowEndpoint instances are equal
+RCLCPP_PUBLIC
+bool operator==(const NetworkFlowEndpoint & left, const NetworkFlowEndpoint & right);
+
+/// Check if two NetworkFlowEndpoint instances are not equal
+RCLCPP_PUBLIC
+bool operator!=(const NetworkFlowEndpoint & left, const NetworkFlowEndpoint & right);
+
+/// Streaming helper for NetworkFlowEndpoint
+RCLCPP_PUBLIC
+std::ostream & operator<<(std::ostream & os, const NetworkFlowEndpoint & network_flow_endpoint);
+
+/**
+ * Class describes a network flow endpoint based on the counterpart definition
+ * in the RMW layer.
+ */
+class NetworkFlowEndpoint
+{
+public:
+  /// Construct from rcl_network_flow_endpoint_t
+  RCLCPP_PUBLIC
+  explicit NetworkFlowEndpoint(const rcl_network_flow_endpoint_t & network_flow_endpoint)
+  : transport_protocol_(
+      rcl_network_flow_endpoint_get_transport_protocol_string(network_flow_endpoint.
+      transport_protocol)),
+    internet_protocol_(
+      rcl_network_flow_endpoint_get_internet_protocol_string(
+        network_flow_endpoint.internet_protocol)),
+    transport_port_(network_flow_endpoint.transport_port),
+    flow_label_(network_flow_endpoint.flow_label),
+    dscp_(network_flow_endpoint.dscp),
+    internet_address_(network_flow_endpoint.internet_address)
+  {
+  }
+
+  /// Get transport protocol
+  RCLCPP_PUBLIC
+  const std::string & transport_protocol() const;
+
+  /// Get internet protocol
+  RCLCPP_PUBLIC
+  const std::string & internet_protocol() const;
+
+  /// Get transport port
+  RCLCPP_PUBLIC
+  uint16_t transport_port() const;
+
+  /// Get flow label
+  RCLCPP_PUBLIC
+  uint32_t flow_label() const;
+
+  /// Get DSCP
+  RCLCPP_PUBLIC
+  uint8_t dscp() const;
+
+  /// Get internet address
+  RCLCPP_PUBLIC
+  const std::string & internet_address() const;
+
+  /// Compare two NetworkFlowEndpoint instances
+  friend bool rclcpp::operator==(
+    const NetworkFlowEndpoint & left,
+    const NetworkFlowEndpoint & right);
+  friend bool rclcpp::operator!=(
+    const NetworkFlowEndpoint & left,
+    const NetworkFlowEndpoint & right);
+
+  /// Streaming helper
+  friend std::ostream & rclcpp::operator<<(
+    std::ostream & os,
+    const NetworkFlowEndpoint & network_flow_endpoint);
+
+private:
+  std::string transport_protocol_;
+  std::string internet_protocol_;
+  uint16_t transport_port_;
+  uint32_t flow_label_;
+  uint8_t dscp_;
+  std::string internet_address_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NETWORK_FLOW_ENDPOINT_HPP_

rclcpp/include/rclcpp/node.hpp

@@ -17,6 +17,7 @@
 
 #include <atomic>
 #include <condition_variable>
+#include <functional>
 #include <list>
 #include <map>
 #include <memory>
@@ -26,6 +27,8 @@
 #include <utility>
 #include <vector>
 
+#include "rcutils/macros.h"
+
 #include "rcl/error_handling.h"
 #include "rcl/node.h"
 
@@ -39,6 +42,10 @@
 #include "rclcpp/clock.hpp"
 #include "rclcpp/context.hpp"
 #include "rclcpp/event.hpp"
+#include "rclcpp/generic_client.hpp"
+#include "rclcpp/generic_publisher.hpp"
+#include "rclcpp/generic_service.hpp"
+#include "rclcpp/generic_subscription.hpp"
 #include "rclcpp/logger.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/message_memory_strategy.hpp"
@@ -51,6 +58,7 @@
 #include "rclcpp/node_interfaces/node_time_source_interface.hpp"
 #include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/node_interfaces/node_type_descriptions_interface.hpp"
 #include "rclcpp/node_interfaces/node_waitables_interface.hpp"
 #include "rclcpp/node_options.hpp"
 #include "rclcpp/parameter.hpp"
@@ -139,12 +147,20 @@ public:
   /// Create and return a callback group.
   RCLCPP_PUBLIC
   rclcpp::CallbackGroup::SharedPtr
-  create_callback_group(rclcpp::CallbackGroupType group_type);
+  create_callback_group(
+    rclcpp::CallbackGroupType group_type,
+    bool automatically_add_to_executor_with_node = true);
 
-  /// Return the list of callback groups in the node.
+  /// Iterate over the callback groups in the node, calling the given function on each valid one.
+  /**
+   * This method is called in a thread-safe way, and also makes sure to only call the given
+   * function on those items that are still valid.
+   *
+   * \param[in] func The callback function to call on each valid callback group.
+   */
   RCLCPP_PUBLIC
-  const std::vector<rclcpp::CallbackGroup::WeakPtr> &
-  get_callback_groups() const;
+  void
+  for_each_callback_group(const node_interfaces::NodeBaseInterface::CallbackGroupFunction & func);
 
   /// Create and return a Publisher.
   /**
@@ -159,7 +175,7 @@ public:
    * pub = node->create_publisher<MsgT>("chatter", QoS(10));  // implicitly KeepLast
    * pub = node->create_publisher<MsgT>("chatter", QoS(KeepLast(10)));
    * pub = node->create_publisher<MsgT>("chatter", QoS(KeepAll()));
-   * pub = node->create_publisher<MsgT>("chatter", QoS(1).best_effort().volatile());
+   * pub = node->create_publisher<MsgT>("chatter", QoS(1).best_effort().durability_volatile());
    * {
    *   rclcpp::QoS custom_qos(KeepLast(10), rmw_qos_profile_sensor_data);
    *   pub = node->create_publisher<MsgT>("chatter", custom_qos);
@@ -199,13 +215,8 @@ public:
     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 MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy<
-      CallbackMessageT,
-      AllocatorT
-    >
+    typename SubscriptionT = rclcpp::Subscription<MessageT, AllocatorT>,
+    typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType
   >
   std::shared_ptr<SubscriptionT>
   create_subscription(
@@ -219,38 +230,53 @@ public:
     )
   );
 
-  /// Create a timer.
+  /// Create a wall timer that uses the wall clock to drive the callback.
   /**
    * \param[in] period Time interval between triggers of the callback.
    * \param[in] callback User-defined callback function.
    * \param[in] group Callback group to execute this timer's callback in.
+   * \param[in] autostart The state of the clock on initialization.
    */
   template<typename DurationRepT = int64_t, typename DurationT = std::milli, typename CallbackT>
   typename rclcpp::WallTimer<CallbackT>::SharedPtr
   create_wall_timer(
+    std::chrono::duration<DurationRepT, DurationT> period,
+    CallbackT callback,
+    rclcpp::CallbackGroup::SharedPtr group = nullptr,
+    bool autostart = true);
+
+  /// Create a timer that uses the node clock to drive the callback.
+  /**
+   * \param[in] period Time interval between triggers of the callback.
+   * \param[in] callback User-defined callback function.
+   * \param[in] group Callback group to execute this timer's callback in.
+   */
+  template<typename DurationRepT = int64_t, typename DurationT = std::milli, typename CallbackT>
+  typename rclcpp::GenericTimer<CallbackT>::SharedPtr
+  create_timer(
     std::chrono::duration<DurationRepT, DurationT> period,
     CallbackT callback,
     rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
   /// Create and return a Client.
   /**
-   * \param[in] service_name The topic to service on.
-   * \param[in] rmw_qos_profile_t Quality of service profile for client.
-   * \param[in] group Callback group to call the service.
+   * \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(
     const std::string & service_name,
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default,
+    const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
     rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
   /// Create and return a Service.
   /**
    * \param[in] service_name The topic to service on.
    * \param[in] callback User-defined callback function.
-   * \param[in] rmw_qos_profile_t Quality of service profile for client.
+   * \param[in] qos Quality of service profile for the service.
    * \param[in] group Callback group to call the service.
    * \return Shared pointer to the created service.
    */
@@ -259,9 +285,94 @@ public:
   create_service(
     const std::string & service_name,
     CallbackT && callback,
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default,
+    const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
     rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
+  /// Create and return a GenericClient.
+  /**
+   * \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] 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 GenericClient.
+   */
+  RCLCPP_PUBLIC
+  rclcpp::GenericClient::SharedPtr
+  create_generic_client(
+    const std::string & service_name,
+    const std::string & service_type,
+    const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
+    rclcpp::CallbackGroup::SharedPtr group = nullptr);
+
+  /// Create and return a GenericService.
+  /**
+   * \param[in] service_name The topic to service on.
+   * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool"
+   * \param[in] callback User-defined callback function.
+   * \param[in] qos Quality of service profile for the service.
+   * \param[in] group Callback group to call the service.
+   * \return Shared pointer to the created service.
+   */
+  template<typename CallbackT>
+  typename rclcpp::GenericService::SharedPtr
+  create_generic_service(
+    const std::string & service_name,
+    const std::string & service_type,
+    CallbackT && callback,
+    const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
+    rclcpp::CallbackGroup::SharedPtr group = nullptr);
+
+  /// 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[in] topic_name Topic name
+   * \param[in] topic_type Topic type
+   * \param[in] 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`.
+   * \return Shared pointer to the created generic publisher.
+   */
+  template<typename AllocatorT = std::allocator<void>>
+  std::shared_ptr<rclcpp::GenericPublisher> create_generic_publisher(
+    const std::string & topic_name,
+    const std::string & topic_type,
+    const rclcpp::QoS & qos,
+    const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options = (
+      rclcpp::PublisherOptionsWithAllocator<AllocatorT>()
+    )
+  );
+
+  /// 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[in] topic_name Topic name
+   * \param[in] topic_type Topic type
+   * \param[in] qos %QoS settings
+   * \param[in] callback Callback for new messages of serialized form
+   * \param[in] 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`.
+   * \return Shared pointer to the created generic subscription.
+   */
+  template<
+    typename CallbackT,
+    typename AllocatorT = std::allocator<void>>
+  std::shared_ptr<rclcpp::GenericSubscription> create_generic_subscription(
+    const std::string & topic_name,
+    const std::string & topic_type,
+    const rclcpp::QoS & qos,
+    CallbackT && callback,
+    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
+      rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
+    )
+  );
+
   /// Declare and initialize a parameter, return the effective value.
   /**
    * This method is used to declare that a parameter exists on this node.
@@ -279,11 +390,21 @@ public:
    *
    * If `ignore_override` is `true`, the parameter override will be ignored.
    *
-   * This method, if successful, will result in any callback registered with
-   * add_on_set_parameters_callback to be called.
+   * This method will result in any callback registered with
+   * `add_on_set_parameters_callback` and `add_post_set_parameters_callback`
+   * to be called for the parameter being set.
+   *
+   * If a callback was registered previously with `add_on_set_parameters_callback`,
+   * it will be called prior to setting the parameter for the node.
    * If that callback prevents the initial value for the parameter from being
    * set then rclcpp::exceptions::InvalidParameterValueException is thrown.
    *
+   * If a callback was registered previously with `add_post_set_parameters_callback`,
+   * it will be called after setting the parameter successfully for the node.
+   *
+   * This method will _not_ result in any callbacks registered with
+   * `add_pre_set_parameters_callback` to be called.
+   *
    * The returned reference will remain valid until the parameter is
    * undeclared.
    *
@@ -301,16 +422,43 @@ public:
    *   name is invalid.
    * \throws rclcpp::exceptions::InvalidParameterValueException if initial
    *   value fails to be set.
+   * \throws rclcpp::exceptions::InvalidParameterTypeException
+   *   if the type of the default value or override is wrong.
    */
   RCLCPP_PUBLIC
   const rclcpp::ParameterValue &
   declare_parameter(
     const std::string & name,
-    const rclcpp::ParameterValue & default_value = rclcpp::ParameterValue(),
+    const rclcpp::ParameterValue & default_value,
     const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
     rcl_interfaces::msg::ParameterDescriptor(),
     bool ignore_override = false);
 
+  /// Declare and initialize a parameter, return the effective value.
+  /**
+   * Same as the previous one, but a default value is not provided and the user
+   * must provide a parameter override of the correct type.
+   *
+   * \param[in] name The name of the parameter.
+   * \param[in] type Desired type of the parameter, which will enforced at runtime.
+   * \param[in] parameter_descriptor An optional, custom description for
+   *   the parameter.
+   * \param[in] ignore_override When `true`, the parameter override is ignored.
+   *    Default to `false`.
+   * \return A const reference to the value of the parameter.
+   * \throws Same as the previous overload taking a default value.
+   * \throws rclcpp::exceptions::InvalidParameterTypeException
+   *   if an override is not provided or the provided override is of the wrong type.
+   */
+  RCLCPP_PUBLIC
+  const rclcpp::ParameterValue &
+  declare_parameter(
+    const std::string & name,
+    rclcpp::ParameterType type,
+    const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
+    rcl_interfaces::msg::ParameterDescriptor{},
+    bool ignore_override = false);
+
   /// Declare and initialize a parameter with a type.
   /**
    * See the non-templated declare_parameter() on this class for details.
@@ -341,6 +489,18 @@ public:
     rcl_interfaces::msg::ParameterDescriptor(),
     bool ignore_override = false);
 
+  /// Declare and initialize a parameter with a type.
+  /**
+   * See the non-templated declare_parameter() on this class for details.
+   */
+  template<typename ParameterT>
+  auto
+  declare_parameter(
+    const std::string & name,
+    const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
+    rcl_interfaces::msg::ParameterDescriptor(),
+    bool ignore_override = false);
+
   /// Declare and initialize several parameters with the same namespace and type.
   /**
    * For each key in the map, a parameter with a name of "namespace.key"
@@ -361,11 +521,22 @@ public:
    * If `ignore_overrides` is `true`, all the overrides of the parameters declared
    * by the function call will be ignored.
    *
+   * This method will result in any callback registered with
+   * `add_on_set_parameters_callback` and `add_post_set_parameters_callback`
+   * to be called once for each parameter.
+   *
    * This method, if successful, will result in any callback registered with
-   * add_on_set_parameters_callback to be called, once for each parameter.
+   * `add_on_set_parameters_callback` to be called, once for each parameter.
    * If that callback prevents the initial value for any parameter from being
    * set then rclcpp::exceptions::InvalidParameterValueException is thrown.
    *
+   * If a callback was registered previously with `add_post_set_parameters_callback`,
+   * it will be called after setting the parameters successfully for the node,
+   * once for each parameter.
+   *
+   * This method will _not_ result in any callbacks registered with
+   * `add_pre_set_parameters_callback` to be called.
+   *
    * \param[in] namespace_ The namespace in which to declare the parameters.
    * \param[in] parameters The parameters to set in the given namespace.
    * \param[in] ignore_overrides When `true`, the parameters overrides are ignored.
@@ -403,8 +574,9 @@ public:
 
   /// Undeclare a previously declared parameter.
   /**
-   * This method will not cause a callback registered with
-   * add_on_set_parameters_callback to be called.
+   * This method will _not_ cause a callback registered with any of the
+   * `add_pre_set_parameters_callback`, `add_on_set_parameters_callback` and
+   * `add_post_set_parameters_callback` to be called.
    *
    * \param[in] name The name of the parameter to be undeclared.
    * \throws rclcpp::exceptions::ParameterNotDeclaredException if the parameter
@@ -438,11 +610,24 @@ public:
    * Parameter overrides are ignored by set_parameter.
    *
    * This method will result in any callback registered with
-   * add_on_set_parameters_callback to be called.
+   * `add_pre_set_parameters_callback`, add_on_set_parameters_callback` and
+   * `add_post_set_parameters_callback` to be called once for the parameter
+   * being set.
+   *
+   * This method will result in any callback registered with
+   * `add_on_set_parameters_callback` to be called.
    * If the callback prevents the parameter from being set, then it will be
    * reflected in the SetParametersResult that is returned, but no exception
    * will be thrown.
    *
+   * If a callback was registered previously with `add_pre_set_parameters_callback`,
+   * it will be called once prior to the validation of the parameter for the node.
+   * If this callback makes modified parameter list empty, then it will be reflected
+   * in the returned result; no exceptions will be raised in this case.
+   *
+   * If a callback was registered previously with `add_post_set_parameters_callback`,
+   * it will be called once after setting the parameter successfully for the node.
+   *
    * If the value type of the parameter is rclcpp::PARAMETER_NOT_SET, and the
    * existing parameter type is something else, then the parameter will be
    * implicitly undeclared.
@@ -479,11 +664,25 @@ public:
    * corresponding SetParametersResult in the vector returned by this function.
    *
    * This method will result in any callback registered with
-   * add_on_set_parameters_callback to be called, once for each parameter.
+   * `add_pre_set_parameters_callback`, `add_on_set_parameters_callback` and
+   * `add_post_set_parameters_callback` to be called once for each parameter.
+
+   * If a callback was registered previously with `add_pre_set_parameters_callback`,
+   * it will be called prior to the validation of parameters for the node,
+   * once for each parameter.
+   * If this callback makes modified parameter list empty, then it will be reflected
+   * in the returned result; no exceptions will be raised in this case.
+   *
+   * This method will result in any callback registered with
+   * `add_on_set_parameters_callback` to be called, once for each parameter.
    * If the callback prevents the parameter from being set, then, as mentioned
    * before, it will be reflected in the corresponding SetParametersResult
    * that is returned, but no exception will be thrown.
    *
+   * If a callback was registered previously with `add_post_set_parameters_callback`,
+   * it will be called after setting the parameters successfully for the node,
+   * once for each parameter.
+   *
    * Like set_parameter() this method will implicitly undeclare parameters
    * with the type rclcpp::PARAMETER_NOT_SET.
    *
@@ -510,11 +709,25 @@ public:
    * If the exception is thrown then none of the parameters will have been set.
    *
    * This method will result in any callback registered with
-   * add_on_set_parameters_callback to be called, just one time.
+   * `add_pre_set_parameters_callback`, `add_on_set_parameters_callback` and
+   * `add_post_set_parameters_callback` to be called only 'once' for all parameters.
+   *
+   * If a callback was registered previously with `add_pre_set_parameters_callback`,
+   * it will be called prior to the validation of node parameters, just one time
+   * for all parameters.
+   * If this callback makes modified parameter list empty, then it will be reflected
+   * in the returned result; no exceptions will be raised in this case.
+   *
+   * This method will result in any callback registered with
+   * 'add_on_set_parameters_callback' to be called, just one time.
    * If the callback prevents the parameters from being set, then it will be
    * reflected in the SetParametersResult which is returned, but no exception
    * will be thrown.
    *
+   * If a callback was registered previously with `add_post_set_parameters_callback`,
+   * it will be called after setting the node parameters successfully, just one time
+   * for all parameters.
+   *
    * If you pass multiple rclcpp::Parameter instances with the same name, then
    * only the last one in the vector (forward iteration) will be set.
    *
@@ -534,17 +747,21 @@ public:
   /**
    * If the parameter has not been declared, then this method may throw the
    * rclcpp::exceptions::ParameterNotDeclaredException exception.
+   * If the parameter has not been initialized, then this method may throw the
+   * rclcpp::exceptions::ParameterUninitializedException exception.
    *
    * If undeclared parameters are allowed, see the node option
    * rclcpp::NodeOptions::allow_undeclared_parameters, then this method will
-   * not throw an exception, and instead return a default initialized
-   * rclcpp::Parameter, which has a type of
+   * not throw the rclcpp::exceptions::ParameterNotDeclaredException exception,
+   * and instead return a default initialized rclcpp::Parameter, which has a type of
    * rclcpp::ParameterType::PARAMETER_NOT_SET.
    *
    * \param[in] name The name of the parameter to get.
    * \return The requested parameter inside of a rclcpp parameter object.
    * \throws rclcpp::exceptions::ParameterNotDeclaredException if the parameter
    *   has not been declared and undeclared parameters are not allowed.
+   * \throws rclcpp::exceptions::ParameterUninitializedException if the parameter
+   *   has not been initialized.
    */
   RCLCPP_PUBLIC
   rclcpp::Parameter
@@ -608,14 +825,32 @@ public:
     ParameterT & parameter,
     const ParameterT & alternative_value) const;
 
+  /// Return the parameter value, or the "alternative_value" if not set.
+  /**
+   * If the parameter was not set, then the "alternative_value" argument is returned.
+   *
+   * This method will not throw the rclcpp::exceptions::ParameterNotDeclaredException exception.
+   *
+   * In all cases, the parameter is never set or declared within the node.
+   *
+   * \param[in] name The name of the parameter to get.
+   * \param[in] alternative_value Value to be stored in output if the parameter was not set.
+   * \returns The value of the parameter.
+   */
+  template<typename ParameterT>
+  ParameterT
+  get_parameter_or(
+    const std::string & name,
+    const ParameterT & alternative_value) const;
+
   /// Return the parameters by the given parameter names.
   /**
-   * Like get_parameters(), this method may throw the
+   * Like get_parameter(const std::string &), this method may throw the
    * rclcpp::exceptions::ParameterNotDeclaredException exception if the
    * requested parameter has not been declared and undeclared parameters are
-   * not allowed.
+   * not allowed, and may throw the rclcpp::exceptions::ParameterUninitializedException exception.
    *
-   * Also like get_parameters(), if undeclared parameters are allowed and the
+   * Also like get_parameter(const std::string &), if undeclared parameters are allowed and the
    * parameter has not been declared, then the corresponding rclcpp::Parameter
    * will be default initialized and therefore have the type
    * rclcpp::ParameterType::PARAMETER_NOT_SET.
@@ -625,6 +860,8 @@ public:
    * \throws rclcpp::exceptions::ParameterNotDeclaredException if any of the
    *   parameters have not been declared and undeclared parameters are not
    *   allowed.
+   * \throws rclcpp::exceptions::ParameterUninitializedException if any of the
+   *   parameters have not been initialized.
    */
   RCLCPP_PUBLIC
   std::vector<rclcpp::Parameter>
@@ -739,18 +976,98 @@ public:
 
   /// Return a list of parameters with any of the given prefixes, up to the given depth.
   /**
-   * \todo: properly document and test this method.
+   * Parameters are separated into a hierarchy using the "." (dot) character.
+   * The "prefixes" argument is a way to select only particular parts of the hierarchy.
+   *
+   * \param[in] prefixes The list of prefixes that should be searched for within the
+   * current parameters. If this vector of prefixes is empty, then list_parameters
+   * will return all parameters.
+   * \param[in] depth An unsigned integer that represents the recursive depth to search.
+   * If this depth = 0, then all parameters that fit the prefixes will be returned.
+   * \returns A ListParametersResult message which contains both an array of unique prefixes
+   * and an array of names that were matched to the prefixes given.
    */
   RCLCPP_PUBLIC
   rcl_interfaces::msg::ListParametersResult
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const;
 
+  using PreSetParametersCallbackHandle =
+    rclcpp::node_interfaces::PreSetParametersCallbackHandle;
+  using PreSetParametersCallbackType =
+    rclcpp::node_interfaces::NodeParametersInterface::PreSetParametersCallbackType;
+
   using OnSetParametersCallbackHandle =
     rclcpp::node_interfaces::OnSetParametersCallbackHandle;
-  using OnParametersSetCallbackType =
-    rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType;
+  using OnSetParametersCallbackType =
+    rclcpp::node_interfaces::NodeParametersInterface::OnSetParametersCallbackType;
 
-  /// Add a callback for when parameters are being set.
+  using PostSetParametersCallbackHandle =
+    rclcpp::node_interfaces::PostSetParametersCallbackHandle;
+  using PostSetParametersCallbackType =
+    rclcpp::node_interfaces::NodeParametersInterface::PostSetParametersCallbackType;
+
+  /// Add a callback that gets triggered before parameters are validated.
+  /**
+   * This callback can be used to modify the original list of parameters being
+   * set by the user.
+   *
+   * The modified list of parameters is then forwarded to the "on set parameter"
+   * callback for validation.
+   *
+   * The callback is called whenever any of the `set_parameter*` methods are called
+   * or when a set parameter service request is received.
+   *
+   * The callback takes a reference to the vector of parameters to be set.
+   *
+   * The vector of parameters may be modified by the callback.
+   *
+   * One of the use case of "pre set callback" can be updating additional parameters
+   * conditioned on changes to a parameter.
+   *
+   * Users should retain a copy of the returned shared pointer, as the callback
+   * is valid only as long as the smart pointer is alive.
+   *
+   * For an example callback:
+   *
+   *```cpp
+   * void
+   * preSetParameterCallback(std::vector<rclcpp::Parameter> & parameters)
+   * {
+   *  for (auto & param : parameters) {
+   *    if (param.get_name() == "param1") {
+   *      parameters.push_back(rclcpp::Parameter("param2", 4.0));
+   *    }
+   *  }
+   * }
+   * ```
+   * The above callback appends 'param2' to the list of parameters to be set if
+   * 'param1' is being set by the user.
+   *
+   * All parameters in the vector will be set atomically.
+   *
+   * Note that the callback is only called while setting parameters with `set_parameter`,
+   * `set_parameters`, `set_parameters_atomically`, or externally with a parameters service.
+   *
+   * The callback is not called when parameters are declared with `declare_parameter`
+   * or `declare_parameters`.
+   *
+   * The callback is not called when parameters are undeclared with `undeclare_parameter`.
+   *
+   * An empty modified parameter list from the callback will result in "set_parameter*"
+   * returning an unsuccessful result.
+   *
+   * The `remove_pre_set_parameters_callback` can be used to deregister the callback.
+   *
+   * \param callback The callback to register.
+   * \returns A shared pointer. The callback is valid as long as the smart pointer is alive.
+   * \throws std::bad_alloc if the allocation of the PreSetParametersCallbackHandle fails.
+   */
+  RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
+  PreSetParametersCallbackHandle::SharedPtr
+  add_pre_set_parameters_callback(PreSetParametersCallbackType callback);
+
+  /// Add a callback to validate parameters before they are set.
   /**
    * The callback signature is designed to allow handling of any of the above
    * `set_parameter*` or `declare_parameter*` methods, and so it takes a const
@@ -758,6 +1075,9 @@ public:
    * rcl_interfaces::msg::SetParametersResult to indicate whether or not the
    * parameter should be set or not, and if not why.
    *
+   * Users should retain a copy of the returned shared pointer, as the callback
+   * is valid only as long as the smart pointer is alive.
+   *
    * For an example callback:
    *
    * ```cpp
@@ -781,11 +1101,16 @@ public:
    *
    * This allows the node developer to control which parameters may be changed.
    *
+   * It is considered bad practice to reject changes for "unknown" parameters as this prevents
+   * other parts of the node (that may be aware of these parameters) from handling them.
+   *
    * Note that the callback is called when declare_parameter() and its variants
    * are called, and so you cannot assume the parameter has been set before
    * this callback, so when checking a new value against the existing one, you
    * must account for the case where the parameter is not yet set.
    *
+   * The callback is not called when parameters are undeclared with `undeclare_parameter`.
+   *
    * Some constraints like read_only are enforced before the callback is called.
    *
    * The callback may introspect other already set parameters (by calling any
@@ -812,8 +1137,82 @@ public:
    * \throws std::bad_alloc if the allocation of the OnSetParametersCallbackHandle fails.
    */
   RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
   OnSetParametersCallbackHandle::SharedPtr
-  add_on_set_parameters_callback(OnParametersSetCallbackType callback);
+  add_on_set_parameters_callback(OnSetParametersCallbackType callback);
+
+  /// Add a callback that gets triggered after parameters are set successfully.
+  /**
+   * The callback is called when any of the `set_parameter*` or `declare_parameter*`
+   * methods are successful.
+   *
+   * Users should retain a copy of the returned shared pointer, as the callback
+   * is valid only as long as the smart pointer is alive.
+   *
+   * The callback takes a reference to a const vector of parameters that have been
+   * set successfully.
+   *
+   * The post callback can be valuable as a place to cause side-effects based on
+   * parameter changes.
+   * For instance updating internally tracked class attributes once parameters
+   * have been changed successfully.
+   *
+   * For an example callback:
+   *
+   * ```cpp
+   * void
+   * postSetParameterCallback(const std::vector<rclcpp::Parameter> & parameters)
+   * {
+   *  for(const auto & param:parameters) {
+   *   // the internal class member can be changed after
+   *   // successful change to param1 or param2
+   *    if(param.get_name() == "param1") {
+   *      internal_tracked_class_parameter_1_ = param.get_value<double>();
+   *    }
+   *    else if(param.get_name() == "param2") {
+   *      internal_tracked_class_parameter_2_ = param.get_value<double>();
+   *    }
+   *  }
+   * }
+   * ```
+   *
+   * The above callback takes a const reference to list of parameters that have been
+   * set successfully and as a result of this updates the internally tracked class attributes
+   * `internal_tracked_class_parameter_1_` and `internal_tracked_class_parameter_2_`
+   * respectively.
+   *
+   * This callback should not modify parameters.
+   *
+   * The callback is called when parameters are declared with `declare_parameter`
+   * or `declare_parameters`. See `declare_parameter` or `declare_parameters` above.
+   *
+   * The callback is not called when parameters are undeclared with `undeclare_parameter`.
+   *
+   * If you want to make changes to parameters based on changes to another, use
+   * `add_pre_set_parameters_callback`.
+   *
+   * The `remove_post_set_parameters_callback` can be used to deregister the callback.
+   *
+   * \param callback The callback to register.
+   * \returns A shared pointer. The callback is valid as long as the smart pointer is alive.
+   * \throws std::bad_alloc if the allocation of the OnSetParametersCallbackHandle fails.
+   */
+  RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
+  PostSetParametersCallbackHandle::SharedPtr
+  add_post_set_parameters_callback(PostSetParametersCallbackType callback);
+
+  /// Remove a callback registered with `add_pre_set_parameters_callback`.
+  /**
+   * Delete a handler returned by `add_pre_set_parameters_callback`.
+   *
+   * \param handler The callback handler to remove.
+   * \throws std::runtime_error if the handler was not created with `add_pre_set_parameters_callback`,
+   *   or if it has been removed before.
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_pre_set_parameters_callback(const PreSetParametersCallbackHandle * const handler);
 
   /// Remove a callback registered with `add_on_set_parameters_callback`.
   /**
@@ -842,25 +1241,17 @@ public:
   void
   remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler);
 
-  /// Register a callback to be called anytime a parameter is about to be changed.
+  /// Remove a callback registered with `add_post_set_parameters_callback`.
   /**
-   * \deprecated Use add_on_set_parameters_callback instead.
-   * With this method, only one callback can be set at a time. The callback that was previously
-   * set by this method is returned or `nullptr` if no callback was previously set.
+   * Delete a handler returned by `add_post_set_parameters_callback`.
    *
-   * The callbacks added with `add_on_set_parameters_callback` are stored in a different place.
-   * `remove_on_set_parameters_callback` can't be used with the callbacks registered with this
-   * method. For removing it, use `set_on_parameters_set_callback(nullptr)`.
-   *
-   * \param[in] callback The callback to be called when the value for a
-   *   parameter is about to be set.
-   * \return The previous callback that was registered, if there was one,
-   *   otherwise nullptr.
+   * \param handler The callback handler to remove.
+   * \throws std::runtime_error if the handler was not created with `add_post_set_parameters_callback`,
+   *   or if it has been removed before.
    */
-  [[deprecated("use add_on_set_parameters_callback(OnParametersSetCallbackType callback) instead")]]
   RCLCPP_PUBLIC
-  OnParametersSetCallbackType
-  set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback);
+  void
+  remove_post_set_parameters_callback(const PostSetParametersCallbackHandle * const handler);
 
   /// Get the fully-qualified names of all available nodes.
   /**
@@ -889,11 +1280,15 @@ public:
   std::map<std::string, std::vector<std::string>>
   get_service_names_and_types() const;
 
-  /// Return the number of publishers that are advertised on a given topic.
+  /// Return a map of existing service names to list of service types for a specific node.
   /**
-   * \param[in] topic_name the topic_name on which to count the publishers.
-   * \return number of publishers that are advertised on a given topic.
-   * \throws std::runtime_error if publishers could not be counted
+   * This function only considers services - not clients.
+   * The returned names are the actual names used and do not have remap rules applied.
+   *
+   * \param[in] node_name name of the node.
+   * \param[in] namespace_ namespace of the node.
+   * \return a map of existing service names to list of service types.
+   * \throws std::runtime_error anything that rcl_error can throw.
    */
   RCLCPP_PUBLIC
   std::map<std::string, std::vector<std::string>>
@@ -901,20 +1296,46 @@ public:
     const std::string & node_name,
     const std::string & namespace_) const;
 
-  RCLCPP_PUBLIC
-  size_t
-  count_publishers(const std::string & topic_name) const;
-
-  /// Return the number of subscribers who have created a subscription for a given topic.
+  /// Return the number of publishers created for a given topic.
   /**
-   * \param[in] topic_name the topic_name on which to count the subscribers.
-   * \return number of subscribers who have created a subscription for a given topic.
+   * \param[in] topic_name the actual topic name used; it will not be automatically remapped.
+   * \return number of publishers that have been created for the given topic.
    * \throws std::runtime_error if publishers could not be counted
    */
   RCLCPP_PUBLIC
   size_t
+  count_publishers(const std::string & topic_name) const;
+
+  /// Return the number of subscribers created for a given topic.
+  /**
+   * \param[in] topic_name the actual topic name used; it will not be automatically remapped.
+   * \return number of subscribers that have been created for the given topic.
+   * \throws std::runtime_error if subscribers could not be counted
+   */
+  RCLCPP_PUBLIC
+  size_t
   count_subscribers(const std::string & topic_name) const;
 
+  /// Return the number of clients created for a given service.
+  /**
+   * \param[in] service_name the actual service name used; it will not be automatically remapped.
+   * \return number of clients that have been created for the given service.
+   * \throws std::runtime_error if clients could not be counted
+   */
+  RCLCPP_PUBLIC
+  size_t
+  count_clients(const std::string & service_name) const;
+
+  /// Return the number of services created for a given service.
+  /**
+   * \param[in] service_name the actual service name used; it will not be automatically remapped.
+   * \return number of services that have been created for the given service.
+   * \throws std::runtime_error if services could not be counted
+   */
+  RCLCPP_PUBLIC
+  size_t
+  count_services(const std::string & service_name) const;
+
   /// Return the topic endpoint information about publishers on a given topic.
   /**
    * The returned parameter is a list of topic endpoint information, where each item will contain
@@ -930,7 +1351,7 @@ public:
    * A relative or private topic will be expanded using this node's namespace and name.
    * The queried `topic_name` is not remapped.
    *
-   * \param[in] topic_name the topic_name on which to find the publishers.
+   * \param[in] topic_name the actual topic name used; it will not be automatically remapped.
    * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name,
    *   otherwise it should be a valid ROS topic name. Defaults to `false`.
    * \return a list of TopicEndpointInfo representing all the publishers on this topic.
@@ -956,7 +1377,7 @@ public:
    * A relative or private topic will be expanded using this node's namespace and name.
    * The queried `topic_name` is not remapped.
    *
-   * \param[in] topic_name the topic_name on which to find the subscriptions.
+   * \param[in] topic_name the actual topic name used; it will not be automatically remapped.
    * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name,
    *   otherwise it should be a valid ROS topic name. Defaults to `false`.
    * \return a list of TopicEndpointInfo representing all the subscriptions on this topic.
@@ -1062,11 +1483,16 @@ public:
   rclcpp::node_interfaces::NodeParametersInterface::SharedPtr
   get_node_parameters_interface();
 
-  /// Return the Node's internal NodeParametersInterface implementation.
+  /// Return the Node's internal NodeTimeSourceInterface implementation.
   RCLCPP_PUBLIC
   rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr
   get_node_time_source_interface();
 
+  /// Return the Node's internal NodeTypeDescriptionsInterface implementation.
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr
+  get_node_type_descriptions_interface();
+
   /// Return the sub-namespace, if this is a sub-node, otherwise an empty string.
   /**
    * The returned sub-namespace is either the accumulated sub-namespaces which
@@ -1190,10 +1616,6 @@ protected:
 private:
   RCLCPP_DISABLE_COPY(Node)
 
-  RCLCPP_PUBLIC
-  bool
-  group_in_node(CallbackGroup::SharedPtr group);
-
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
   rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
@@ -1203,11 +1625,18 @@ private:
   rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
   rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
   rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_;
+  rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr node_type_descriptions_;
   rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_;
 
   const rclcpp::NodeOptions node_options_;
   const std::string sub_namespace_;
   const std::string effective_namespace_;
+
+  class NodeImpl;
+  // This member is meant to be a place to backport features into stable distributions,
+  // and new features targeting Rolling should not use this.
+  // See the comment in node.cpp for more information.
+  std::shared_ptr<NodeImpl> hidden_impl_{nullptr};
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/node_impl.hpp

@@ -36,10 +36,13 @@
 
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/create_client.hpp"
+#include "rclcpp/create_generic_publisher.hpp"
+#include "rclcpp/create_generic_subscription.hpp"
 #include "rclcpp/create_publisher.hpp"
 #include "rclcpp/create_service.hpp"
-#include "rclcpp/create_timer.hpp"
+#include "rclcpp/create_generic_service.hpp"
 #include "rclcpp/create_subscription.hpp"
+#include "rclcpp/create_timer.hpp"
 #include "rclcpp/detail/resolve_enable_topic_statistics.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/qos.hpp"
@@ -84,7 +87,6 @@ template<
   typename MessageT,
   typename CallbackT,
   typename AllocatorT,
-  typename CallbackMessageT,
   typename SubscriptionT,
   typename MessageMemoryStrategyT>
 std::shared_ptr<SubscriptionT>
@@ -109,9 +111,27 @@ typename rclcpp::WallTimer<CallbackT>::SharedPtr
 Node::create_wall_timer(
   std::chrono::duration<DurationRepT, DurationT> period,
   CallbackT callback,
-  rclcpp::CallbackGroup::SharedPtr group)
+  rclcpp::CallbackGroup::SharedPtr group,
+  bool autostart)
 {
   return rclcpp::create_wall_timer(
+    period,
+    std::move(callback),
+    group,
+    this->node_base_.get(),
+    this->node_timers_.get(),
+    autostart);
+}
+
+template<typename DurationRepT, typename DurationT, typename CallbackT>
+typename rclcpp::GenericTimer<CallbackT>::SharedPtr
+Node::create_timer(
+  std::chrono::duration<DurationRepT, DurationT> period,
+  CallbackT callback,
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  return rclcpp::create_timer(
+    this->get_clock(),
     period,
     std::move(callback),
     group,
@@ -123,7 +143,7 @@ template<typename ServiceT>
 typename Client<ServiceT>::SharedPtr
 Node::create_client(
   const std::string & service_name,
-  const rmw_qos_profile_t & qos_profile,
+  const rclcpp::QoS & qos,
   rclcpp::CallbackGroup::SharedPtr group)
 {
   return rclcpp::create_client<ServiceT>(
@@ -131,7 +151,7 @@ Node::create_client(
     node_graph_,
     node_services_,
     extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
-    qos_profile,
+    qos,
     group);
 }
 
@@ -140,7 +160,7 @@ typename rclcpp::Service<ServiceT>::SharedPtr
 Node::create_service(
   const std::string & service_name,
   CallbackT && callback,
-  const rmw_qos_profile_t & qos_profile,
+  const rclcpp::QoS & qos,
   rclcpp::CallbackGroup::SharedPtr group)
 {
   return rclcpp::create_service<ServiceT, CallbackT>(
@@ -148,10 +168,66 @@ Node::create_service(
     node_services_,
     extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
     std::forward<CallbackT>(callback),
-    qos_profile,
+    qos,
     group);
 }
 
+template<typename CallbackT>
+typename rclcpp::GenericService::SharedPtr
+Node::create_generic_service(
+  const std::string & service_name,
+  const std::string & service_type,
+  CallbackT && callback,
+  const rclcpp::QoS & qos,
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  return rclcpp::create_generic_service<CallbackT>(
+    node_base_,
+    node_services_,
+    extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
+    service_type,
+    std::forward<CallbackT>(callback),
+    qos,
+    group);
+}
+
+template<typename AllocatorT>
+std::shared_ptr<rclcpp::GenericPublisher>
+Node::create_generic_publisher(
+  const std::string & topic_name,
+  const std::string & topic_type,
+  const rclcpp::QoS & qos,
+  const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options)
+{
+  return rclcpp::create_generic_publisher(
+    node_topics_,
+    extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()),
+    topic_type,
+    qos,
+    options
+  );
+}
+
+template<typename CallbackT, typename AllocatorT>
+std::shared_ptr<rclcpp::GenericSubscription>
+Node::create_generic_subscription(
+  const std::string & topic_name,
+  const std::string & topic_type,
+  const rclcpp::QoS & qos,
+  CallbackT && callback,
+  const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options)
+{
+  return rclcpp::create_generic_subscription(
+    node_topics_,
+    extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()),
+    topic_type,
+    qos,
+    std::forward<CallbackT>(callback),
+    options
+  );
+}
+
+
 template<typename ParameterT>
 auto
 Node::declare_parameter(
@@ -172,6 +248,28 @@ Node::declare_parameter(
   }
 }
 
+template<typename ParameterT>
+auto
+Node::declare_parameter(
+  const std::string & name,
+  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor,
+  bool ignore_override)
+{
+  // get advantage of parameter value template magic to get
+  // the correct rclcpp::ParameterType from ParameterT
+  rclcpp::ParameterValue value{ParameterT{}};
+  try {
+    return this->declare_parameter(
+      name,
+      value.get_type(),
+      parameter_descriptor,
+      ignore_override
+    ).get<ParameterT>();
+  } catch (const ParameterTypeException &) {
+    throw exceptions::UninitializedStaticallyTypedParameterException(name);
+  }
+}
+
 template<typename ParameterT>
 std::vector<ParameterT>
 Node::declare_parameters(
@@ -253,6 +351,17 @@ Node::get_parameter_or(
   return got_parameter;
 }
 
+template<typename ParameterT>
+ParameterT
+Node::get_parameter_or(
+  const std::string & name,
+  const ParameterT & alternative_value) const
+{
+  ParameterT parameter;
+  get_parameter_or(name, parameter, alternative_value);
+  return parameter;
+}
+
 // this is a partially-specialized version of get_parameter above,
 // where our concrete type for ParameterT is std::map, but the to-be-determined
 // type is the value in the map.

rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp

@@ -0,0 +1,210 @@
+// 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__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_
+#define RCLCPP__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_
+
+#include <memory>
+#include <tuple>
+#include <type_traits>
+#include <utility>
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+namespace detail
+{
+
+// Support and Helper template classes for the NodeInterfaces class.
+
+template<typename NodeT, typename ... Ts>
+std::tuple<std::shared_ptr<Ts>...>
+init_tuple(NodeT & n);
+
+/// Stores the interfaces in a tuple, provides constructors, and getters.
+template<typename ... InterfaceTs>
+struct NodeInterfacesStorage
+{
+  template<typename NodeT>
+  NodeInterfacesStorage(NodeT & node)  // NOLINT(runtime/explicit)
+  : interfaces_(init_tuple<decltype(node), InterfaceTs ...>(node))
+  {}
+
+  NodeInterfacesStorage()
+  : interfaces_()
+  {}
+
+  explicit NodeInterfacesStorage(std::shared_ptr<InterfaceTs>... args)
+  : interfaces_(args ...)
+  {}
+
+  /// Individual Node Interface non-const getter.
+  template<typename NodeInterfaceT>
+  std::shared_ptr<NodeInterfaceT>
+  get()
+  {
+    static_assert(
+      (std::is_same_v<NodeInterfaceT, InterfaceTs>|| ...),
+      "NodeInterfaces class does not contain given NodeInterfaceT");
+    return std::get<std::shared_ptr<NodeInterfaceT>>(interfaces_);
+  }
+
+  /// Individual Node Interface const getter.
+  template<typename NodeInterfaceT>
+  std::shared_ptr<const NodeInterfaceT>
+  get() const
+  {
+    static_assert(
+      (std::is_same_v<NodeInterfaceT, InterfaceTs>|| ...),
+      "NodeInterfaces class does not contain given NodeInterfaceT");
+    return std::get<std::shared_ptr<NodeInterfaceT>>(interfaces_);
+  }
+
+protected:
+  std::tuple<std::shared_ptr<InterfaceTs>...> interfaces_;
+};
+
+/// Prototype of NodeInterfacesSupports.
+/**
+ * Should read NodeInterfacesSupports<..., T, ...> as "NodeInterfaces supports T", and
+ * if NodeInterfacesSupport is specialized for T, the is_supported should be
+ * set to std::true_type, but by default it is std::false_type, which will
+ * lead to a compiler error when trying to use T with NodeInterfaces.
+ */
+template<typename StorageClassT, typename ... Ts>
+struct NodeInterfacesSupports;
+
+/// Prototype of NodeInterfacesSupportCheck template meta-function.
+/**
+ * This meta-function checks that all the types given are supported,
+ * throwing a more human-readable error if an unsupported type is used.
+ */
+template<typename StorageClassT, typename ... InterfaceTs>
+struct NodeInterfacesSupportCheck;
+
+/// Iterating specialization that ensures classes are supported and inherited.
+template<typename StorageClassT, typename NextInterfaceT, typename ... RemainingInterfaceTs>
+struct NodeInterfacesSupportCheck<StorageClassT, NextInterfaceT, RemainingInterfaceTs ...>
+  : public NodeInterfacesSupportCheck<StorageClassT, RemainingInterfaceTs ...>
+{
+  static_assert(
+    NodeInterfacesSupports<StorageClassT, NextInterfaceT>::is_supported::value,
+    "given NodeInterfaceT is not supported by rclcpp::node_interfaces::NodeInterfaces");
+};
+
+/// Terminating case when there are no more "RemainingInterfaceTs".
+template<typename StorageClassT>
+struct NodeInterfacesSupportCheck<StorageClassT>
+{};
+
+/// Default specialization, needs to be specialized for each supported interface.
+template<typename StorageClassT, typename ... RemainingInterfaceTs>
+struct NodeInterfacesSupports
+{
+  // Specializations need to set this to std::true_type in addition to other interfaces.
+  using is_supported = std::false_type;
+};
+
+/// Terminating specialization of NodeInterfacesSupports.
+template<typename StorageClassT>
+struct NodeInterfacesSupports<StorageClassT>
+  : public StorageClassT
+{
+  /// Perfect forwarding constructor to get arguments down to StorageClassT.
+  template<typename ... ArgsT>
+  explicit NodeInterfacesSupports(ArgsT && ... args)
+  : StorageClassT(std::forward<ArgsT>(args) ...)
+  {}
+};
+
+// Helper functions to initialize the tuple in NodeInterfaces.
+
+template<typename StorageClassT, typename ElementT, typename TupleT, typename NodeT>
+void
+init_element(TupleT & t, NodeT & n)
+{
+  std::get<std::shared_ptr<ElementT>>(t) =
+    NodeInterfacesSupports<StorageClassT, ElementT>::get_from_node_like(n);
+}
+
+template<typename NodeT, typename ... Ts>
+std::tuple<std::shared_ptr<Ts>...>
+init_tuple(NodeT & n)
+{
+  using StorageClassT = NodeInterfacesStorage<Ts ...>;
+  std::tuple<std::shared_ptr<Ts>...> t;
+  (init_element<StorageClassT, Ts>(t, n), ...);
+  return t;
+}
+
+/// Macro for creating specializations with less boilerplate.
+/**
+ * You can use this macro to add support for your interface class if:
+ *
+ * - The standard getter is get_node_{NodeInterfaceName}_interface(), and
+ * - the getter returns a non-const shared_ptr<{NodeInterfaceType}>
+ *
+ * Examples of using this can be seen in the standard node interface headers
+ * in rclcpp, e.g. rclcpp/node_interfaces/node_base_interface.hpp has:
+ *
+ *   RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)
+ *
+ * If your interface has a non-standard getter, or you want to instrument it or
+ * something like that, then you'll need to create your own specialization of
+ * the NodeInterfacesSupports struct without this macro.
+ */
+// *INDENT-OFF*
+#define RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(NodeInterfaceType, NodeInterfaceName) \
+  namespace rclcpp::node_interfaces::detail { \
+  template<typename StorageClassT, typename ... RemainingInterfaceTs> \
+  struct NodeInterfacesSupports< \
+    StorageClassT, \
+    NodeInterfaceType, \
+    RemainingInterfaceTs ...> \
+    : public NodeInterfacesSupports<StorageClassT, RemainingInterfaceTs ...> \
+  { \
+    using is_supported = std::true_type; \
+ \
+    template<typename NodeT> \
+    static \
+    std::shared_ptr<NodeInterfaceType> \
+    get_from_node_like(NodeT & node_like) \
+    { \
+      return node_like.get_node_ ## NodeInterfaceName ## _interface(); \
+    } \
+ \
+    /* Perfect forwarding constructor to get arguments down to StorageClassT (eventually). */ \
+    template<typename ... ArgsT> \
+    explicit NodeInterfacesSupports(ArgsT && ... args) \
+    : NodeInterfacesSupports<StorageClassT, RemainingInterfaceTs ...>( \
+        std::forward<ArgsT>(args) ...) \
+    {} \
+ \
+    std::shared_ptr<NodeInterfaceType> \
+    get_node_ ## NodeInterfaceName ## _interface() \
+    { \
+      return StorageClassT::template get<NodeInterfaceType>(); \
+    } \
+  }; \
+  }  // namespace rclcpp::node_interfaces::detail
+// *INDENT-ON*
+
+}  // namespace detail
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -15,11 +15,14 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_
 
+#include <atomic>
 #include <memory>
+#include <mutex>
 #include <string>
 #include <vector>
 
 #include "rcl/node.h"
+#include "rclcpp/callback_group.hpp"
 #include "rclcpp/context.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
@@ -31,11 +34,18 @@ namespace node_interfaces
 {
 
 /// Implementation of the NodeBase part of the Node API.
-class NodeBase : public NodeBaseInterface
+class NodeBase : public NodeBaseInterface, public std::enable_shared_from_this<NodeBase>
 {
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBase)
 
+  /// Constructor.
+  /**
+   * If nullptr (default) is given for the default_callback_group, one will
+   * be created by the constructor using the create_callback_group() method,
+   * but virtual dispatch will not occur so overrides of that method will not
+   * be used.
+   */
   RCLCPP_PUBLIC
   NodeBase(
     const std::string & node_name,
@@ -43,7 +53,8 @@ public:
     rclcpp::Context::SharedPtr context,
     const rcl_node_options_t & rcl_node_options,
     bool use_intra_process_default,
-    bool enable_topic_statistics_default);
+    bool enable_topic_statistics_default,
+    rclcpp::CallbackGroup::SharedPtr default_callback_group = nullptr);
 
   RCLCPP_PUBLIC
   virtual
@@ -83,7 +94,9 @@ public:
 
   RCLCPP_PUBLIC
   rclcpp::CallbackGroup::SharedPtr
-  create_callback_group(rclcpp::CallbackGroupType group_type) override;
+  create_callback_group(
+    rclcpp::CallbackGroupType group_type,
+    bool automatically_add_to_executor_with_node = true) override;
 
   RCLCPP_PUBLIC
   rclcpp::CallbackGroup::SharedPtr
@@ -93,21 +106,33 @@ public:
   bool
   callback_group_in_node(rclcpp::CallbackGroup::SharedPtr group) override;
 
+  /// Iterate over the stored callback groups, calling the given function on each valid one.
+  /**
+   * This method is called in a thread-safe way, and also makes sure to only call the given
+   * function on those items that are still valid.
+   *
+   * \param[in] func The callback function to call on each valid callback group.
+   */
   RCLCPP_PUBLIC
-  const std::vector<rclcpp::CallbackGroup::WeakPtr> &
-  get_callback_groups() const override;
+  void
+  for_each_callback_group(const CallbackGroupFunction & func) override;
 
   RCLCPP_PUBLIC
   std::atomic_bool &
   get_associated_with_executor_atomic() override;
 
+  [[deprecated("Use get_shared_notify_guard_condition or trigger_notify_guard_condition instead")]]
   RCLCPP_PUBLIC
-  rcl_guard_condition_t *
+  rclcpp::GuardCondition &
   get_notify_guard_condition() override;
 
   RCLCPP_PUBLIC
-  std::unique_lock<std::recursive_mutex>
-  acquire_notify_guard_condition_lock() const override;
+  rclcpp::GuardCondition::SharedPtr
+  get_shared_notify_guard_condition() override;
+
+  RCLCPP_PUBLIC
+  void
+  trigger_notify_guard_condition() override;
 
   RCLCPP_PUBLIC
   bool
@@ -116,6 +141,10 @@ public:
   bool
   get_enable_topic_statistics_default() const override;
 
+  std::string
+  resolve_topic_or_service_name(
+    const std::string & name, bool is_service, bool only_expand = false) const override;
+
 private:
   RCLCPP_DISABLE_COPY(NodeBase)
 
@@ -126,13 +155,14 @@ private:
   std::shared_ptr<rcl_node_t> node_handle_;
 
   rclcpp::CallbackGroup::SharedPtr default_callback_group_;
+  std::mutex callback_groups_mutex_;
   std::vector<rclcpp::CallbackGroup::WeakPtr> callback_groups_;
 
   std::atomic_bool associated_with_executor_;
 
   /// Guard condition for notifying the Executor of changes to this node.
   mutable std::recursive_mutex notify_guard_condition_mutex_;
-  rcl_guard_condition_t notify_guard_condition_ = rcl_get_zero_initialized_guard_condition();
+  std::shared_ptr<rclcpp::GuardCondition> notify_guard_condition_;
   bool notify_guard_condition_is_valid_;
 };
 

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -15,16 +15,18 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_
 
+#include <atomic>
+#include <functional>
 #include <memory>
-#include <mutex>
 #include <string>
-#include <vector>
 
 #include "rcl/node.h"
 
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/context.hpp"
+#include "rclcpp/guard_condition.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -106,7 +108,9 @@ public:
   RCLCPP_PUBLIC
   virtual
   rclcpp::CallbackGroup::SharedPtr
-  create_callback_group(rclcpp::CallbackGroupType group_type) = 0;
+  create_callback_group(
+    rclcpp::CallbackGroupType group_type,
+    bool automatically_add_to_executor_with_node = true) = 0;
 
   /// Return the default callback group.
   RCLCPP_PUBLIC
@@ -120,11 +124,19 @@ public:
   bool
   callback_group_in_node(rclcpp::CallbackGroup::SharedPtr group) = 0;
 
-  /// Return list of callback groups associated with this node.
+  using CallbackGroupFunction = std::function<void (rclcpp::CallbackGroup::SharedPtr)>;
+
+  /// Iterate over the stored callback groups, calling the given function on each valid one.
+  /**
+   * This method is called in a thread-safe way, and also makes sure to only call the given
+   * function on those items that are still valid.
+   *
+   * \param[in] func The callback function to call on each valid callback group.
+   */
   RCLCPP_PUBLIC
   virtual
-  const std::vector<rclcpp::CallbackGroup::WeakPtr> &
-  get_callback_groups() const = 0;
+  void
+  for_each_callback_group(const CallbackGroupFunction & func) = 0;
 
   /// Return the atomic bool which is used to ensure only one executor is used.
   RCLCPP_PUBLIC
@@ -132,23 +144,36 @@ public:
   std::atomic_bool &
   get_associated_with_executor_atomic() = 0;
 
-  /// Return guard condition that should be notified when the internal node state changes.
+  /// Return a guard condition that should be notified when the internal node state changes.
   /**
    * For example, this should be notified when a publisher is added or removed.
    *
-   * \return the rcl_guard_condition_t if it is valid, else nullptr
+   * \return the GuardCondition if it is valid, else throw runtime error
    */
   RCLCPP_PUBLIC
   virtual
-  rcl_guard_condition_t *
+  rclcpp::GuardCondition &
   get_notify_guard_condition() = 0;
 
-  /// Acquire and return a scoped lock that protects the notify guard condition.
-  /** This should be used when triggering the notify guard condition. */
+  /// Return a guard condition that should be notified when the internal node state changes.
+  /**
+   * For example, this should be notified when a publisher is added or removed.
+   *
+   * \return the GuardCondition if it is valid, else nullptr
+   */
   RCLCPP_PUBLIC
   virtual
-  std::unique_lock<std::recursive_mutex>
-  acquire_notify_guard_condition_lock() const = 0;
+  rclcpp::GuardCondition::SharedPtr
+  get_shared_notify_guard_condition() = 0;
+
+  /// Trigger the guard condition that notifies of internal node state changes.
+  /**
+   * For example, this should be notified when a publisher is added or removed.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  trigger_notify_guard_condition() = 0;
 
   /// Return the default preference for using intra process communication.
   RCLCPP_PUBLIC
@@ -161,9 +186,18 @@ public:
   virtual
   bool
   get_enable_topic_statistics_default() const = 0;
+
+  /// Expand and remap a given topic or service name.
+  RCLCPP_PUBLIC
+  virtual
+  std::string
+  resolve_topic_or_service_name(
+    const std::string & name, bool is_service, bool only_expand = false) const = 0;
 };
 
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_clock.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_
 
+#include "rcl/time.h"
 #include "rclcpp/clock.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
@@ -42,7 +43,8 @@ public:
     rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
     rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
     rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
-    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging);
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    rcl_clock_type_t clock_type);
 
   RCLCPP_PUBLIC
   virtual
@@ -67,7 +69,7 @@ private:
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
   rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
 
-  rclcpp::Clock::SharedPtr ros_clock_;
+  rclcpp::Clock::SharedPtr clock_;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp

@@ -17,6 +17,7 @@
 
 #include "rclcpp/clock.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -50,4 +51,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeClockInterface, clock)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

@@ -15,11 +15,14 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_
 
+#include <atomic>
 #include <chrono>
+#include <condition_variable>
 #include <map>
 #include <memory>
 #include <mutex>
 #include <string>
+#include <tuple>
 #include <utility>
 #include <vector>
 
@@ -70,10 +73,34 @@ public:
     const std::string & node_name,
     const std::string & namespace_) const override;
 
+  RCLCPP_PUBLIC
+  std::map<std::string, std::vector<std::string>>
+  get_client_names_and_types_by_node(
+    const std::string & node_name,
+    const std::string & namespace_) const override;
+
+  RCLCPP_PUBLIC
+  std::map<std::string, std::vector<std::string>>
+  get_publisher_names_and_types_by_node(
+    const std::string & node_name,
+    const std::string & namespace_,
+    bool no_demangle = false) const override;
+
+  RCLCPP_PUBLIC
+  std::map<std::string, std::vector<std::string>>
+  get_subscriber_names_and_types_by_node(
+    const std::string & node_name,
+    const std::string & namespace_,
+    bool no_demangle = false) const override;
+
   RCLCPP_PUBLIC
   std::vector<std::string>
   get_node_names() const override;
 
+  RCLCPP_PUBLIC
+  std::vector<std::tuple<std::string, std::string, std::string>>
+  get_node_names_with_enclaves() const override;
+
   RCLCPP_PUBLIC
   std::vector<std::pair<std::string, std::string>>
   get_node_names_and_namespaces() const override;
@@ -86,6 +113,14 @@ public:
   size_t
   count_subscribers(const std::string & topic_name) const override;
 
+  RCLCPP_PUBLIC
+  size_t
+  count_clients(const std::string & service_name) const override;
+
+  RCLCPP_PUBLIC
+  size_t
+  count_services(const std::string & service_name) const override;
+
   RCLCPP_PUBLIC
   const rcl_guard_condition_t *
   get_graph_guard_condition() const override;
@@ -110,7 +145,7 @@ public:
 
   RCLCPP_PUBLIC
   size_t
-  count_graph_users() override;
+  count_graph_users() const override;
 
   RCLCPP_PUBLIC
   std::vector<rclcpp::TopicEndpointInfo>

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

@@ -20,6 +20,7 @@
 #include <chrono>
 #include <map>
 #include <string>
+#include <tuple>
 #include <utility>
 #include <vector>
 
@@ -28,6 +29,7 @@
 
 #include "rclcpp/event.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -55,7 +57,8 @@ public:
     node_namespace_(info.node_namespace),
     topic_type_(info.topic_type),
     endpoint_type_(static_cast<rclcpp::EndpointType>(info.endpoint_type)),
-    qos_profile_({info.qos_profile.history, info.qos_profile.depth}, info.qos_profile)
+    qos_profile_({info.qos_profile.history, info.qos_profile.depth}, info.qos_profile),
+    topic_type_hash_(info.topic_type_hash)
   {
     std::copy(info.endpoint_gid, info.endpoint_gid + RMW_GID_STORAGE_SIZE, endpoint_gid_.begin());
   }
@@ -120,6 +123,16 @@ public:
   const rclcpp::QoS &
   qos_profile() const;
 
+  /// Get a mutable reference to the type hash of the topic endpoint.
+  RCLCPP_PUBLIC
+  rosidl_type_hash_t &
+  topic_type_hash();
+
+  /// Get a const reference to the type hash of the topic endpoint.
+  RCLCPP_PUBLIC
+  const rosidl_type_hash_t &
+  topic_type_hash() const;
+
 private:
   std::string node_name_;
   std::string node_namespace_;
@@ -127,6 +140,7 @@ private:
   rclcpp::EndpointType endpoint_type_;
   std::array<uint8_t, RMW_GID_STORAGE_SIZE> endpoint_gid_;
   rclcpp::QoS qos_profile_;
+  rosidl_type_hash_t topic_type_hash_;
 };
 
 namespace node_interfaces
@@ -146,6 +160,9 @@ public:
   /**
    * A topic is considered to exist when at least one publisher or subscriber
    * exists for it, whether they be local or remote to this process.
+   * The returned names are the actual names of the topics, either announced by another nodes or by this one.
+   * Attempting to create publishers or subscribers using names returned by this function may not
+   * result in the desired topic name being used depending on the remap rules in use.
    *
    * \param[in] no_demangle if true, topic names and types are not demangled
    */
@@ -159,6 +176,9 @@ public:
    * A service is considered to exist when at least one service server or
    * service client exists for it, whether they be local or remote to this
    * process.
+   * The returned names are the actual names of the services, either announced by another nodes or by this one.
+   * Attempting to create clients or services using names returned by this function may not result in
+   * the desired service name being used depending on the remap rules in use.
    */
   RCLCPP_PUBLIC
   virtual
@@ -168,6 +188,9 @@ public:
   /// Return a map of existing service names to list of service types for a specific node.
   /**
    * This function only considers services - not clients.
+   * The returned names are the actual names after remap rules applied.
+   * Attempting to create service clients using names returned by this function may not
+   * result in the desired service name being used depending on the remap rules in use.
    *
    * \param[in] node_name name of the node
    * \param[in] namespace_ namespace of the node
@@ -179,30 +202,127 @@ public:
     const std::string & node_name,
     const std::string & namespace_) const = 0;
 
+  /// Return a map of existing service names and types with a specific node.
+  /**
+   * This function only considers clients - not service servers.
+   * The returned names are the actual names after remap rules applied.
+   * Attempting to create service servers using names returned by this function may not
+   * result in the desired service name being used depending on the remap rules in use.
+   *
+   * \param[in] node_name name of the node
+   * \param[in] namespace_ namespace of the node
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::map<std::string, std::vector<std::string>>
+  get_client_names_and_types_by_node(
+    const std::string & node_name,
+    const std::string & namespace_) const = 0;
+
+  /// Return a map of existing topic names to list of topic types for a specific node.
+  /**
+   * This function only considers publishers - not subscribers.
+   * The returned names are the actual names after remap rules applied.
+   * Attempting to create publishers or subscribers using names returned by this function may not
+   * result in the desired topic name being used depending on the remap rules in use.
+   *
+   * \param[in] node_name name of the node
+   * \param[in] namespace_ namespace of the node
+   * \param[in] no_demangle if true, topic names and types are not demangled
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::map<std::string, std::vector<std::string>>
+  get_publisher_names_and_types_by_node(
+    const std::string & node_name,
+    const std::string & namespace_,
+    bool no_demangle = false) const = 0;
+
+  /// Return a map of existing topic names to list of topic types for a specific node.
+  /**
+   * This function only considers subscribers - not publishers.
+   * The returned names are the actual names after remap rules applied.
+   * Attempting to create publishers or subscribers using names returned by this function may not
+   * result in the desired topic name being used depending on the remap rules in use.
+   *
+   * \param[in] node_name name of the node
+   * \param[in] namespace_ namespace of the node
+   * \param[in] no_demangle if true, topic names and types are not demangled
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::map<std::string, std::vector<std::string>>
+  get_subscriber_names_and_types_by_node(
+    const std::string & node_name,
+    const std::string & namespace_,
+    bool no_demangle = false) const = 0;
+
   /// Return a vector of existing node names (string).
+  /*
+   * The returned names are the actual names after remap rules applied.
+   */
   RCLCPP_PUBLIC
   virtual
   std::vector<std::string>
   get_node_names() const = 0;
 
+  /// Return a vector of existing node names, namespaces and enclaves (tuple of string).
+  /*
+   * The returned names are the actual names after remap rules applied.
+   * The enclaves contain the runtime security artifacts, those can be
+   * used to establish secured network.
+   * See https://design.ros2.org/articles/ros2_security_enclaves.html
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<std::tuple<std::string, std::string, std::string>>
+  get_node_names_with_enclaves() const = 0;
+
   /// Return a vector of existing node names and namespaces (pair of string).
+  /*
+   * The returned names are the actual names after remap rules applied.
+   */
   RCLCPP_PUBLIC
   virtual
   std::vector<std::pair<std::string, std::string>>
   get_node_names_and_namespaces() const = 0;
 
   /// Return the number of publishers that are advertised on a given topic.
+  /*
+   * \param[in] topic_name the actual topic name used; it will not be automatically remapped.
+   */
   RCLCPP_PUBLIC
   virtual
   size_t
   count_publishers(const std::string & topic_name) const = 0;
 
   /// Return the number of subscribers who have created a subscription for a given topic.
+  /*
+   * \param[in] topic_name the actual topic name used; it will not be automatically remapped.
+   */
   RCLCPP_PUBLIC
   virtual
   size_t
   count_subscribers(const std::string & topic_name) const = 0;
 
+  /// Return the number of clients created for a given service.
+  /*
+   * \param[in] service_name the actual service name used; it will not be automatically remapped.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  count_clients(const std::string & service_name) const = 0;
+
+  /// Return the number of services created for a given service.
+  /*
+   * \param[in] service_name the actual service name used; it will not be automatically remapped.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  count_services(const std::string & service_name) const = 0;
+
   /// Return the rcl guard condition which is triggered when the ROS graph changes.
   RCLCPP_PUBLIC
   virtual
@@ -263,10 +383,13 @@ public:
   RCLCPP_PUBLIC
   virtual
   size_t
-  count_graph_users() = 0;
+  count_graph_users() const = 0;
 
   /// Return the topic endpoint information about publishers on a given topic.
   /**
+   * \param[in] topic_name the actual topic name used; it will not be automatically remapped.
+   * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name,
+   *   otherwise it should be a valid ROS topic name.
    * \sa rclcpp::Node::get_publishers_info_by_topic
    */
   RCLCPP_PUBLIC
@@ -276,6 +399,9 @@ public:
 
   /// Return the topic endpoint information about subscriptions on a given topic.
   /**
+   * \param[in] topic_name the actual topic name used; it will not be automatically remapped.
+   * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name,
+   *   otherwise it should be a valid ROS topic name.
    * \sa rclcpp::Node::get_subscriptions_info_by_topic
    */
   RCLCPP_PUBLIC
@@ -287,4 +413,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeGraphInterface, graph)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp

@@ -0,0 +1,168 @@
+// 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__NODE_INTERFACES__NODE_INTERFACES_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_INTERFACES_HPP_
+
+#include <memory>
+
+#include "rclcpp/detail/template_unique.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
+
+#define ALL_RCLCPP_NODE_INTERFACES \
+  rclcpp::node_interfaces::NodeBaseInterface, \
+  rclcpp::node_interfaces::NodeClockInterface, \
+  rclcpp::node_interfaces::NodeGraphInterface, \
+  rclcpp::node_interfaces::NodeLoggingInterface, \
+  rclcpp::node_interfaces::NodeParametersInterface, \
+  rclcpp::node_interfaces::NodeServicesInterface, \
+  rclcpp::node_interfaces::NodeTimeSourceInterface, \
+  rclcpp::node_interfaces::NodeTimersInterface, \
+  rclcpp::node_interfaces::NodeTopicsInterface, \
+  rclcpp::node_interfaces::NodeTypeDescriptionsInterface, \
+  rclcpp::node_interfaces::NodeWaitablesInterface
+
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+
+/// A helper class for aggregating node interfaces.
+template<typename ... InterfaceTs>
+class NodeInterfaces
+  : public detail::NodeInterfacesSupportCheck<
+    detail::NodeInterfacesStorage<InterfaceTs ...>,
+    InterfaceTs ...
+  >,
+  public detail::NodeInterfacesSupports<
+    detail::NodeInterfacesStorage<InterfaceTs ...>,
+    InterfaceTs ...
+  >
+{
+  static_assert(
+    0 != sizeof ...(InterfaceTs),
+    "must provide at least one interface as a template argument");
+  static_assert(
+    rclcpp::detail::template_unique_v<InterfaceTs ...>,
+    "must provide unique template parameters");
+
+  using NodeInterfacesSupportsT = detail::NodeInterfacesSupports<
+    detail::NodeInterfacesStorage<InterfaceTs ...>,
+    InterfaceTs ...
+  >;
+
+public:
+  /// Create a new NodeInterfaces object using the given node-like object's interfaces.
+  /**
+   * Specify which interfaces you need by passing them as template parameters.
+   *
+   * This allows you to aggregate interfaces from different sources together to pass as a single
+   * aggregate object to any functions that take node interfaces or node-likes, without needing to
+   * templatize that function.
+   *
+   * You may also use this constructor to create a NodeInterfaces that contains a subset of
+   * another NodeInterfaces' interfaces.
+   *
+   * Finally, this class supports implicit conversion from node-like objects, allowing you to
+   * directly pass a node-like to a function that takes a NodeInterfaces object.
+   *
+   * Usage examples:
+   *   ```cpp
+   *   // Suppose we have some function:
+   *   void fn(NodeInterfaces<NodeBaseInterface, NodeClockInterface> interfaces);
+   *
+   *   // Then we can, explicitly:
+   *   rclcpp::Node node("some_node");
+   *   auto ni = NodeInterfaces<NodeBaseInterface, NodeClockInterface>(node);
+   *   fn(ni);
+   *
+   *   // But also:
+   *   fn(node);
+   *
+   *   // Subsetting a NodeInterfaces object also works!
+   *   auto ni_base = NodeInterfaces<NodeBaseInterface>(ni);
+   *
+   *   // Or aggregate them (you could aggregate interfaces from disparate node-likes)
+   *   auto ni_aggregated = NodeInterfaces<NodeBaseInterface, NodeClockInterface>(
+   *     node->get_node_base_interface(),
+   *     node->get_node_clock_interface()
+   *   )
+   *
+   *   // And then to access the interfaces:
+   *   // Get with get<>
+   *   auto base = ni.get<NodeBaseInterface>();
+   *
+   *   // Or the appropriate getter
+   *   auto clock = ni.get_clock_interface();
+   *   ```
+   *
+   * You may use any of the standard node interfaces that come with rclcpp:
+   *   - rclcpp::node_interfaces::NodeBaseInterface
+   *   - rclcpp::node_interfaces::NodeClockInterface
+   *   - rclcpp::node_interfaces::NodeGraphInterface
+   *   - rclcpp::node_interfaces::NodeLoggingInterface
+   *   - rclcpp::node_interfaces::NodeParametersInterface
+   *   - rclcpp::node_interfaces::NodeServicesInterface
+   *   - rclcpp::node_interfaces::NodeTimeSourceInterface
+   *   - rclcpp::node_interfaces::NodeTimersInterface
+   *   - rclcpp::node_interfaces::NodeTopicsInterface
+   *   - rclcpp::node_interfaces::NodeTypeDescriptionsInterface
+   *   - rclcpp::node_interfaces::NodeWaitablesInterface
+   *
+   * Or you use custom interfaces as long as you make a template specialization
+   * of the rclcpp::node_interfaces::detail::NodeInterfacesSupport struct using
+   * the RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT macro.
+   *
+   * Usage example:
+   *   ```cpp
+   *   RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)
+   *   ```
+   *
+   * If you choose not to use the helper macro, then you can specialize the
+   * template yourself, but you must:
+   *
+   *   - Provide a template specialization of the get_from_node_like method that gets the interface
+   *     from any node-like that stores the interface, using the node-like's getter
+   *   - Designate the is_supported type as std::true_type using a using directive
+   *   - Provide any number of getter methods to be used to obtain the interface with the
+   *     NodeInterface object, noting that the getters of the storage class will apply to all
+   *     supported interfaces.
+   *     - The getter method names should not clash in name with any other interface getter
+   *       specializations if those other interfaces are meant to be aggregated in the same
+   *       NodeInterfaces object.
+   *
+   * \param[in] node Node-like object from which to get the node interfaces
+   */
+  template<typename NodeT>
+  NodeInterfaces(NodeT & node)  // NOLINT(runtime/explicit)
+  : NodeInterfacesSupportsT(node)
+  {}
+
+  // Create a NodeInterfaces object with no bound interfaces
+  NodeInterfaces()
+  : NodeInterfacesSupportsT()
+  {}
+
+  explicit NodeInterfaces(std::shared_ptr<InterfaceTs>... args)
+  : NodeInterfacesSupportsT(args ...)
+  {}
+};
+
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_INTERFACES_HPP_

rclcpp/include/rclcpp/node_interfaces/node_logging.hpp

@@ -23,6 +23,9 @@
 #include "rclcpp/node_interfaces/node_logging_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
 
+#include "rcl_interfaces/srv/get_logger_levels.hpp"
+#include "rcl_interfaces/srv/set_logger_levels.hpp"
+
 namespace rclcpp
 {
 namespace node_interfaces
@@ -35,7 +38,7 @@ public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeLoggingInterface)
 
   RCLCPP_PUBLIC
-  explicit NodeLogging(rclcpp::node_interfaces::NodeBaseInterface * node_base);
+  explicit NodeLogging(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base);
 
   RCLCPP_PUBLIC
   virtual
@@ -49,13 +52,21 @@ public:
   const char *
   get_logger_name() const override;
 
+  RCLCPP_PUBLIC
+  void
+  create_logger_services(
+    node_interfaces::NodeServicesInterface::SharedPtr node_services) override;
+
 private:
   RCLCPP_DISABLE_COPY(NodeLogging)
 
   /// Handle to the NodeBaseInterface given in the constructor.
-  rclcpp::node_interfaces::NodeBaseInterface * node_base_;
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
 
   rclcpp::Logger logger_;
+
+  rclcpp::Service<rcl_interfaces::srv::GetLoggerLevels>::SharedPtr get_loggers_service_;
+  rclcpp::Service<rcl_interfaces::srv::SetLoggerLevels>::SharedPtr set_loggers_service_;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp

@@ -19,6 +19,8 @@
 
 #include "rclcpp/logger.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -37,21 +39,34 @@ public:
   ~NodeLoggingInterface() = default;
 
   /// Return the logger of the node.
-  /** \return The logger of the node. */
+  /**
+   * \return The logger of the node.
+   */
   RCLCPP_PUBLIC
   virtual
   rclcpp::Logger
   get_logger() const = 0;
 
   /// Return the logger name associated with the node.
-  /** \return The logger name associated with the node. */
+  /**
+   * \return The logger name associated with the node.
+   */
   RCLCPP_PUBLIC
   virtual
   const char *
   get_logger_name() const = 0;
+
+  /// create logger services
+  RCLCPP_PUBLIC
+  virtual
+  void
+  create_logger_services(
+    node_interfaces::NodeServicesInterface::SharedPtr node_services) = 0;
 };
 
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeLoggingInterface, logging)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_LOGGING_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -15,12 +15,15 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_
 
+#include <list>
 #include <map>
 #include <memory>
-#include <list>
+#include <mutex>
 #include <string>
 #include <vector>
 
+#include "rcutils/macros.h"
+
 #include "rcl_interfaces/msg/list_parameters_result.hpp"
 #include "rcl_interfaces/msg/parameter_descriptor.hpp"
 #include "rcl_interfaces/msg/parameter_event.hpp"
@@ -82,6 +85,16 @@ class NodeParameters : public NodeParametersInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeParameters)
 
+  /// Constructor.
+  /**
+   * If using automatically_declare_parameters_from_overrides, overrides of
+   * get_parameter_overrides(), has_parameter(), declare_parameter() will not
+   * be respected.
+   * If this is an issue, pass false for
+   * automatically_declare_parameters_from_overrides and invoke
+   * perform_automatically_declare_parameters_from_overrides() manually after
+   * construction.
+   */
   RCLCPP_PUBLIC
   NodeParameters(
     const node_interfaces::NodeBaseInterface::SharedPtr node_base,
@@ -106,8 +119,18 @@ public:
   declare_parameter(
     const std::string & name,
     const rclcpp::ParameterValue & default_value,
-    const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor,
-    bool ignore_override) override;
+    const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
+    rcl_interfaces::msg::ParameterDescriptor{},
+    bool ignore_override = false) override;
+
+  RCLCPP_PUBLIC
+  const rclcpp::ParameterValue &
+  declare_parameter(
+    const std::string & name,
+    rclcpp::ParameterType type,
+    const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
+    rcl_interfaces::msg::ParameterDescriptor(),
+    bool ignore_override = false) override;
 
   RCLCPP_PUBLIC
   void
@@ -160,23 +183,47 @@ public:
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const override;
 
   RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
+  PreSetParametersCallbackHandle::SharedPtr
+  add_pre_set_parameters_callback(PreSetParametersCallbackType callback) override;
+
+  RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
   OnSetParametersCallbackHandle::SharedPtr
-  add_on_set_parameters_callback(OnParametersSetCallbackType callback) override;
+  add_on_set_parameters_callback(OnSetParametersCallbackType callback) override;
+
+  RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
+  PostSetParametersCallbackHandle::SharedPtr
+  add_post_set_parameters_callback(PostSetParametersCallbackType callback) override;
 
   RCLCPP_PUBLIC
   void
   remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) override;
 
-  [[deprecated("use add_on_set_parameters_callback(OnParametersSetCallbackType callback) instead")]]
   RCLCPP_PUBLIC
-  OnParametersSetCallbackType
-  set_on_parameters_set_callback(OnParametersSetCallbackType callback) override;
+  void
+  remove_post_set_parameters_callback(const PostSetParametersCallbackHandle * const handler)
+  override;
+
+  RCLCPP_PUBLIC
+  void
+  remove_pre_set_parameters_callback(const PreSetParametersCallbackHandle * const handler) override;
 
   RCLCPP_PUBLIC
   const std::map<std::string, rclcpp::ParameterValue> &
   get_parameter_overrides() const override;
 
-  using CallbacksContainerType = std::list<OnSetParametersCallbackHandle::WeakPtr>;
+  using PreSetCallbacksHandleContainer = std::list<PreSetParametersCallbackHandle::WeakPtr>;
+  using OnSetCallbacksHandleContainer = std::list<OnSetParametersCallbackHandle::WeakPtr>;
+  using PostSetCallbacksHandleContainer = std::list<PostSetParametersCallbackHandle::WeakPtr>;
+  using CallbacksContainerType [[deprecated("use OnSetCallbacksHandleContainer instead")]] =
+    OnSetCallbacksHandleContainer;
+
+protected:
+  RCLCPP_PUBLIC
+  void
+  perform_automatically_declare_parameters_from_overrides();
 
 private:
   RCLCPP_DISABLE_COPY(NodeParameters)
@@ -188,9 +235,11 @@ private:
   // declare_parameter, etc).  In those cases, this will be set to false.
   bool parameter_modification_enabled_{true};
 
-  OnParametersSetCallbackType on_parameters_set_callback_ = nullptr;
+  PreSetCallbacksHandleContainer pre_set_parameters_callback_container_;
 
-  CallbacksContainerType on_parameters_set_callback_container_;
+  OnSetCallbacksHandleContainer on_set_parameters_callback_container_;
+
+  PostSetCallbacksHandleContainer post_set_parameters_callback_container_;
 
   std::map<std::string, ParameterInfo> parameters_;
 

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -15,8 +15,8 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
 
+#include <functional>
 #include <map>
-#include <memory>
 #include <string>
 #include <vector>
 
@@ -25,6 +25,7 @@
 #include "rcl_interfaces/msg/set_parameters_result.hpp"
 
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -33,16 +34,36 @@ namespace rclcpp
 namespace node_interfaces
 {
 
+struct PreSetParametersCallbackHandle
+{
+  RCLCPP_SMART_PTR_DEFINITIONS(PreSetParametersCallbackHandle)
+
+  using PreSetParametersCallbackType =
+    std::function<void (std::vector<rclcpp::Parameter> &)>;
+
+  PreSetParametersCallbackType callback;
+};
+
 struct OnSetParametersCallbackHandle
 {
   RCLCPP_SMART_PTR_DEFINITIONS(OnSetParametersCallbackHandle)
 
-  using OnParametersSetCallbackType =
+  using OnSetParametersCallbackType =
     std::function<
     rcl_interfaces::msg::SetParametersResult(
       const std::vector<rclcpp::Parameter> &)>;
 
-  OnParametersSetCallbackType callback;
+  OnSetParametersCallbackType callback;
+};
+
+struct PostSetParametersCallbackHandle
+{
+  RCLCPP_SMART_PTR_DEFINITIONS(PostSetParametersCallbackHandle)
+
+  using PostSetParametersCallbackType =
+    std::function<void (const std::vector<rclcpp::Parameter> &)>;
+
+  PostSetParametersCallbackType callback;
 };
 
 /// Pure virtual interface class for the NodeParameters part of the Node API.
@@ -64,7 +85,21 @@ public:
   const rclcpp::ParameterValue &
   declare_parameter(
     const std::string & name,
-    const rclcpp::ParameterValue & default_value = rclcpp::ParameterValue(),
+    const rclcpp::ParameterValue & default_value,
+    const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
+    rcl_interfaces::msg::ParameterDescriptor(),
+    bool ignore_override = false) = 0;
+
+  /// Declare a parameter.
+  /**
+   * \sa rclcpp::Node::declare_parameter
+   */
+  RCLCPP_PUBLIC
+  virtual
+  const rclcpp::ParameterValue &
+  declare_parameter(
+    const std::string & name,
+    rclcpp::ParameterType type,
     const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
     rcl_interfaces::msg::ParameterDescriptor(),
     bool ignore_override = false) = 0;
@@ -96,7 +131,7 @@ public:
   std::vector<rcl_interfaces::msg::SetParametersResult>
   set_parameters(const std::vector<rclcpp::Parameter> & parameters) = 0;
 
-  /// Set and initialize a parameter, all at once.
+  /// Set one or more parameters, all at once.
   /**
    * \sa rclcpp::Node::set_parameters_atomically
    */
@@ -171,16 +206,46 @@ public:
   rcl_interfaces::msg::ListParametersResult
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const = 0;
 
-  using OnParametersSetCallbackType = OnSetParametersCallbackHandle::OnParametersSetCallbackType;
+  using OnSetParametersCallbackType = OnSetParametersCallbackHandle::OnSetParametersCallbackType;
+  using PostSetParametersCallbackType =
+    PostSetParametersCallbackHandle::PostSetParametersCallbackType;
+  using PreSetParametersCallbackType = PreSetParametersCallbackHandle::PreSetParametersCallbackType;
 
-  /// Add a callback for when parameters are being set.
+  /// Add a callback that gets triggered before parameters are validated.
+  /**
+   * \sa rclcpp::Node::add_pre_set_parameters_callback
+   */
+  RCLCPP_PUBLIC
+  virtual
+  PreSetParametersCallbackHandle::SharedPtr
+  add_pre_set_parameters_callback(PreSetParametersCallbackType callback) = 0;
+
+  /// Add a callback to validate parameters before they are set.
   /**
    * \sa rclcpp::Node::add_on_set_parameters_callback
    */
   RCLCPP_PUBLIC
   virtual
   OnSetParametersCallbackHandle::SharedPtr
-  add_on_set_parameters_callback(OnParametersSetCallbackType callback) = 0;
+  add_on_set_parameters_callback(OnSetParametersCallbackType callback) = 0;
+
+  /// Add a callback that gets triggered after parameters are set successfully.
+  /**
+   * \sa rclcpp::Node::add_post_set_parameters_callback
+   */
+  RCLCPP_PUBLIC
+  virtual
+  PostSetParametersCallbackHandle::SharedPtr
+  add_post_set_parameters_callback(PostSetParametersCallbackType callback) = 0;
+
+  /// Remove a callback registered with `add_pre_set_parameters_callback`.
+  /**
+   * \sa rclcpp::Node::remove_pre_set_parameters_callback
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  remove_pre_set_parameters_callback(const PreSetParametersCallbackHandle * const handler) = 0;
 
   /// Remove a callback registered with `add_on_set_parameters_callback`.
   /**
@@ -191,16 +256,14 @@ public:
   void
   remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) = 0;
 
-  /// Register a callback for when parameters are being set, return an existing one.
+  /// Remove a callback registered with `add_post_set_parameters_callback`.
   /**
-   * \deprecated Use add_on_set_parameters_callback instead.
-   * \sa rclcpp::Node::set_on_parameters_set_callback
+   * \sa rclcpp::Node::remove_post_set_parameters_callback
    */
-  [[deprecated("use add_on_set_parameters_callback(OnParametersSetCallbackType callback) instead")]]
   RCLCPP_PUBLIC
   virtual
-  OnParametersSetCallbackType
-  set_on_parameters_set_callback(OnParametersSetCallbackType callback) = 0;
+  void
+  remove_post_set_parameters_callback(const PostSetParametersCallbackHandle * const handler) = 0;
 
   /// Return the initial parameter values used by the NodeParameters to override default values.
   RCLCPP_PUBLIC
@@ -212,4 +275,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeParametersInterface, parameters)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_