28.1.0

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

README.md

@@ -8,7 +8,7 @@ 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.
 
-The link to the latest API documentation can be found on the rclcpp package info page, at the [ROS Index](https://index.ros.org/p/rclcpp/).
+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

rclcpp/CHANGELOG.rst

@@ -2,6 +2,331 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+28.1.0 (2024-04-16)
+-------------------
+* Remove references to index.ros.org. (`#2504 <https://github.com/ros2/rclcpp/issues/2504>`_)
+* Reduce overhead for inheriting from rclcpp::Executor when base functionality is not reused (`#2506 <https://github.com/ros2/rclcpp/issues/2506>`_)
+* Contributors: Chris Lalancette, William Woodall, jmachowinski
+
+28.0.1 (2024-04-16)
+-------------------
+* [wjwwood] Updated "Data race fixes" (`#2500 <https://github.com/ros2/rclcpp/issues/2500>`_)
+  * Fix callback group logic in executor
+  * fix: Fixed unnecessary copy of wait_set
+  * fix(executor): Fixed race conditions with rebuild of wait_sets
+  Before this change, the rebuild of wait set would be triggered
+  after the wait set was waken up. With bad timing, this could
+  lead to the rebuild not happening with multi threaded executor.
+  * fix(Executor): Fixed lost of entities rebuild request
+  * chore: Added assert for not set callback_group in execute_any_executable
+  * Add test for cbg getting reset
+  Co-authored-by: Janosch Machowinski <j.machowinski@nospam.org>
+  * chore: renamed test cases to snake_case
+  * style
+  * fixup test to avoid polling and short timeouts
+  * fix: Use correct notify_waitable\_ instance
+  * fix(StaticSingleThreadedExecutor): Added missing special case handling for current_notify_waitable\_
+  * fix(TestCallbackGroup): Fixed test after change to timers
+  ---------
+  Co-authored-by: Janosch Machowinski <j.machowinski@cellumation.com>
+  Co-authored-by: Michael Carroll <mjcarroll@intrinsic.ai>
+  Co-authored-by: Janosch Machowinski <j.machowinski@nospam.org>
+* fixup var names to snake case (`#2501 <https://github.com/ros2/rclcpp/issues/2501>`_)
+* Added optional TimerInfo to timer callback (`#2343 <https://github.com/ros2/rclcpp/issues/2343>`_)
+  Co-authored-by: Alexis Tsogias <a.tsogias@cellumation.com>
+  Co-authored-by: Janosch Machowinski <J.Machowinski@cellumation.com>
+* Fix uninitialized memory in test (`#2498 <https://github.com/ros2/rclcpp/issues/2498>`_)
+  When I added in the tests for large messages, I made a mistake and reserved space in the strings, but didn't actually expand it.  Thus, we were writing into uninitialized memory.  Fix this by just using the correct constructor for string, which will allocate and initialize the memory properly.
+* Ensure waitables handle guard condition retriggering (`#2483 <https://github.com/ros2/rclcpp/issues/2483>`_)
+  Co-authored-by: Michael Carroll <mjcarroll@intrinsic.ai>
+* fix: init concatenated_vector with begin() & end() (`#2492 <https://github.com/ros2/rclcpp/issues/2492>`_)
+  * this commit will fix the warning [-Wstringop-overflow=] `#2461 <https://github.com/ros2/rclcpp/issues/2461>`_
+* Use the same context for the specified node in rclcpp::spin functions (`#2433 <https://github.com/ros2/rclcpp/issues/2433>`_)
+  * Use the same conext for the specified node in rclcpp::spin_xx functions
+  * Add test for spinning with non-default-context
+  * Format code
+  ---------
+* Disable compare-function-pointers in test_utilities (`#2489 <https://github.com/ros2/rclcpp/issues/2489>`_)
+* address ambiguous auto variable. (`#2481 <https://github.com/ros2/rclcpp/issues/2481>`_)
+* Increase the cppcheck timeout to 1200 seconds (`#2484 <https://github.com/ros2/rclcpp/issues/2484>`_)
+* Removed test_timers_manager clang warning (`#2479 <https://github.com/ros2/rclcpp/issues/2479>`_)
+* Flaky timer test fix (`#2469 <https://github.com/ros2/rclcpp/issues/2469>`_)
+  * fix(time_source): Fixed possible race condition
+  * fix(test_executors_time_cancel_behaviour): Fixed multiple race conditions
+  ---------
+  Co-authored-by: Janosch Machowinski <j.machowinski@nospam.org>
+* Add tracepoint for generic publisher/subscriber (`#2448 <https://github.com/ros2/rclcpp/issues/2448>`_)
+* update rclcpp::Waitable API to use references and const (`#2467 <https://github.com/ros2/rclcpp/issues/2467>`_)
+* Utilize rclcpp::WaitSet as part of the executors (`#2142 <https://github.com/ros2/rclcpp/issues/2142>`_)
+  * Deprecate callback_group call taking context
+  * Add base executor objects that can be used by implementors
+  * Template common operations
+  * Address reviewer feedback:
+  * Add callback to EntitiesCollector constructor
+  * Make function to check automatically added callback groups take a list
+  * Lint
+  * Address reviewer feedback and fix templates
+  * Lint and docs
+  * Make executor own the notify waitable
+  * Add pending queue to collector, remove from waitable
+  Also change node's get_guard_condition to return shared_ptr
+  * Change interrupt guard condition to shared_ptr
+  Check if guard condition is valid before adding it to the waitable
+  * Lint and docs
+  * Utilize rclcpp::WaitSet as part of the executors
+  * Don't exchange atomic twice
+  * Fix add_node and add more tests
+  * Make get_notify_guard_condition follow API tick-tock
+  * Improve callback group tick-tocking
+  * Don't lock twice
+  * Address reviewer feedback
+  * Add thread safety annotations and make locks consistent
+  * @wip
+  * Reset callback groups for multithreaded executor
+  * Avoid many small function calls when building executables
+  * Re-trigger guard condition if buffer has data
+  * Address reviewer feedback
+  * Trace points
+  * Remove tracepoints
+  * Reducing diff
+  * Reduce diff
+  * Uncrustify
+  * Restore tests
+  * Back to weak_ptr and reduce test time
+  * reduce diff and lint
+  * Restore static single threaded tests that weren't working before
+  * Restore more tests
+  * Fix multithreaded test
+  * Fix assert
+  * Fix constructor test
+  * Change ready_executables signature back
+  * Don't enforce removing callback groups before nodes
+  * Remove the "add_valid_node" API
+  * Only notify if the trigger condition is valid
+  * Only trigger if valid and needed
+  * Fix spin_some/spin_all implementation
+  * Restore single threaded executor
+  * Picking ABI-incompatible executor changes
+  * Add PIMPL
+  * Additional waitset prune
+  * Fix bad merge
+  * Expand test timeout
+  * Introduce method to clear expired entities from a collection
+  * Make sure to call remove_expired_entities().
+  * Prune queued work when callback group is removed
+  * Prune subscriptions from dynamic storage
+  * Styles fixes.
+  * Re-trigger guard conditions
+  * Condense to just use watiable.take_data
+  * Lint
+  * Address reviewer comments (nits)
+  * Lock mutex when copying
+  * Refactors to static single threaded based on reviewers
+  * More small refactoring
+  * Lint
+  * Lint
+  * Add ready executable accessors to WaitResult
+  * Make use of accessors from wait_set
+  * Fix tests
+  * Fix more tests
+  * Tidy up single threaded executor implementation
+  * Don't null out timer, rely on call
+  * change how timers are checked from wait result in executors
+  * peak -> peek
+  * fix bug in next_waitable logic
+  * fix bug in StaticSTE that broke the add callback groups to executor tests
+  * style
+  ---------
+  Co-authored-by: Chris Lalancette <clalancette@gmail.com>
+  Co-authored-by: William Woodall <william@osrfoundation.org>
+* fix flakiness in TestTimersManager unit-test (`#2468 <https://github.com/ros2/rclcpp/issues/2468>`_)
+  the previous version of the test was relying on the assumption that a timer with 1ms period gets called at least 6 times if the main thread waits 15ms. this is true most of the times, but it's not guaranteed, especially when running the test on windows CI servers. the new version of the test makes no assumptions on how much time it takes for the timers manager to invoke the timers, but rather focuses on ensuring that they are called the right amount of times, which is what's important for the purpose of the test
+* Contributors: Alberto Soragna, Alejandro Hernández Cordero, Chris Lalancette, Homalozoa X, Kotaro Yoshimoto, Michael Carroll, Tomoya Fujita, William Woodall, h-suzuki-isp, jmachowinski
+
+28.0.0 (2024-03-28)
+-------------------
+* fix spin_some_max_duration unit-test for events-executor (`#2465 <https://github.com/ros2/rclcpp/issues/2465>`_)
+* refactor and improve the parameterized spin_some tests for executors (`#2460 <https://github.com/ros2/rclcpp/issues/2460>`_)
+  * refactor and improve the spin_some parameterized tests for executors
+  * disable spin_some_max_duration for the StaticSingleThreadedExecutor and EventsExecutor
+  * fixup and clarify the docstring for Executor::spin_some()
+  * style
+  * review comments
+  ---------
+* enable simulation clock for timer canceling test. (`#2458 <https://github.com/ros2/rclcpp/issues/2458>`_)
+  * enable simulation clock for timer canceling test.
+  * move MainExecutorTypes to test_executors_timer_cancel_behavior.cpp.
+  ---------
+* Revert "relax the test simulation rate for timer canceling tests. (`#2453 <https://github.com/ros2/rclcpp/issues/2453>`_)" (`#2456 <https://github.com/ros2/rclcpp/issues/2456>`_)
+  This reverts commit 1c350d0d7fb9c7158e0a39057112486ddbd38e9a.
+* relax the test simulation rate for timer canceling tests. (`#2453 <https://github.com/ros2/rclcpp/issues/2453>`_)
+* Fix TypeAdapted publishing with large messages. (`#2443 <https://github.com/ros2/rclcpp/issues/2443>`_)
+  Mostly by ensuring we aren't attempting to store
+  large messages on the stack.  Also add in tests.
+  I verified that before these changes, the tests failed,
+  while after them they succeed.
+* Implement generic client (`#2358 <https://github.com/ros2/rclcpp/issues/2358>`_)
+  * Implement generic client
+  * Fix the incorrect parameter declaration
+  * Deleted copy constructor and assignment for FutureAndRequestId
+  * Update codes after rebase
+  * Address review comments
+  * Address review comments from iuhilnehc-ynos
+  * Correct an error in a description
+  * Fix window build errors
+  * Address review comments from William
+  * Add doc strings to create_generic_client
+  ---------
+* Rule of five: implement move operators (`#2425 <https://github.com/ros2/rclcpp/issues/2425>`_)
+* Various cleanups to deal with uncrustify 0.78. (`#2439 <https://github.com/ros2/rclcpp/issues/2439>`_)
+  These should also work with uncrustify 0.72.
+* Remove the set_deprecated signatures in any_subscription_callback. (`#2431 <https://github.com/ros2/rclcpp/issues/2431>`_)
+  These have been deprecated since April 2021, so it is safe
+  to remove them now.
+* fix doxygen syntax for NodeInterfaces (`#2428 <https://github.com/ros2/rclcpp/issues/2428>`_)
+* Set hints to find the python version we actually want. (`#2426 <https://github.com/ros2/rclcpp/issues/2426>`_)
+  The comment in the commit explains the reasoning behind it.
+* Update quality declaration documents (`#2427 <https://github.com/ros2/rclcpp/issues/2427>`_)
+* feat: add/minus for msg::Time and rclcpp::Duration (`#2419 <https://github.com/ros2/rclcpp/issues/2419>`_)
+  * feat: add/minus for msg::Time and rclcpp::Duration
+* Contributors: Alberto Soragna, Barry Xu, Chris Lalancette, Christophe Bedard, HuaTsai, Jonas Otto, Tim Clephas, Tomoya Fujita, William Woodall
+
+27.0.0 (2024-02-07)
+-------------------
+* Split test_executors up into smaller chunks. (`#2421 <https://github.com/ros2/rclcpp/issues/2421>`_)
+* [events executor] - Fix Behavior with Timer Cancel (`#2375 <https://github.com/ros2/rclcpp/issues/2375>`_)
+* Removed deprecated header (`#2413 <https://github.com/ros2/rclcpp/issues/2413>`_)
+* Make sure to mark RingBuffer methods as 'override'. (`#2410 <https://github.com/ros2/rclcpp/issues/2410>`_)
+* Contributors: Alejandro Hernández Cordero, Chris Lalancette, Matt Condino
+
+26.0.0 (2024-01-24)
+-------------------
+* Increase the cppcheck timeout to 600 seconds. (`#2409 <https://github.com/ros2/rclcpp/issues/2409>`_)
+* Add transient local durability support to publisher and subscriptions when using intra-process communication (`#2303 <https://github.com/ros2/rclcpp/issues/2303>`_)
+* Stop storing the context in the guard condition. (`#2400 <https://github.com/ros2/rclcpp/issues/2400>`_)
+* Contributors: Chris Lalancette, Jeffery Hsu
+
+25.0.0 (2023-12-26)
+-------------------
+* Updated GenericSubscription to AnySubscriptionCallback (`#1928 <https://github.com/ros2/rclcpp/issues/1928>`_)
+* make type support helper supported for service (`#2209 <https://github.com/ros2/rclcpp/issues/2209>`_)
+* Adding QoS to subscription options (`#2323 <https://github.com/ros2/rclcpp/issues/2323>`_)
+* Switch to target_link_libraries. (`#2374 <https://github.com/ros2/rclcpp/issues/2374>`_)
+* aligh with rcl that a rosout publisher of a node might not exist (`#2357 <https://github.com/ros2/rclcpp/issues/2357>`_)
+* Fix data race in EventHandlerBase (`#2349 <https://github.com/ros2/rclcpp/issues/2349>`_)
+* Support users holding onto shared pointers in the message memory pool (`#2336 <https://github.com/ros2/rclcpp/issues/2336>`_)
+* Contributors: Chen Lihui, Chris Lalancette, DensoADAS, Lucas Wendland, mauropasse
+
+24.0.0 (2023-11-06)
+-------------------
+* fix (signal_handler.hpp): spelling (`#2356 <https://github.com/ros2/rclcpp/issues/2356>`_)
+* Updates to not use std::move in some places. (`#2353 <https://github.com/ros2/rclcpp/issues/2353>`_)
+* rclcpp::Time::max() clock type support. (`#2352 <https://github.com/ros2/rclcpp/issues/2352>`_)
+* Serialized Messages with Topic Statistics (`#2274 <https://github.com/ros2/rclcpp/issues/2274>`_)
+* Add a custom deleter when constructing rcl_service_t (`#2351 <https://github.com/ros2/rclcpp/issues/2351>`_)
+* Disable the loaned messages inside the executor. (`#2335 <https://github.com/ros2/rclcpp/issues/2335>`_)
+* Use message_info in SubscriptionTopicStatistics instead of typed message (`#2337 <https://github.com/ros2/rclcpp/issues/2337>`_)
+* Add missing 'enable_rosout' comments (`#2345 <https://github.com/ros2/rclcpp/issues/2345>`_)
+* Adjust rclcpp usage of type description service (`#2344 <https://github.com/ros2/rclcpp/issues/2344>`_)
+* address rate related flaky tests. (`#2329 <https://github.com/ros2/rclcpp/issues/2329>`_)
+* Fixes pointed out by the clang analyzer. (`#2339 <https://github.com/ros2/rclcpp/issues/2339>`_)
+* Remove useless ROSRate class (`#2326 <https://github.com/ros2/rclcpp/issues/2326>`_)
+* Contributors: Alexey Merzlyakov, Chris Lalancette, Jiaqi Li, Lucas Wendland, Michael Carroll, Michael Orlov, Tomoya Fujita, Zard-C
+
+23.2.0 (2023-10-09)
+-------------------
+* add clients & services count (`#2072 <https://github.com/ros2/rclcpp/issues/2072>`_)
+* remove invalid sized allocation test for SerializedMessage. (`#2330 <https://github.com/ros2/rclcpp/issues/2330>`_)
+* Adding API to copy all parameters from one node to another (`#2304 <https://github.com/ros2/rclcpp/issues/2304>`_)
+* Contributors: Minju, Lee, Steve Macenski, Tomoya Fujita
+
+23.1.0 (2023-10-04)
+-------------------
+* Add locking to protect the TimeSource::NodeState::node_base\_ (`#2320 <https://github.com/ros2/rclcpp/issues/2320>`_)
+* Update SignalHandler get_global_signal_handler to avoid complex types in static memory (`#2316 <https://github.com/ros2/rclcpp/issues/2316>`_)
+* Removing Old Connext Tests (`#2313 <https://github.com/ros2/rclcpp/issues/2313>`_)
+* Documentation for list_parameters  (`#2315 <https://github.com/ros2/rclcpp/issues/2315>`_)
+* Decouple rosout publisher init from node init. (`#2174 <https://github.com/ros2/rclcpp/issues/2174>`_)
+* fix the depth to relative in list_parameters (`#2300 <https://github.com/ros2/rclcpp/issues/2300>`_)
+* Contributors: Chris Lalancette, Lucas Wendland, Minju, Lee, Tomoya Fujita, Tully Foote
+
+23.0.0 (2023-09-08)
+-------------------
+* Fix the return type of Rate::period. (`#2301 <https://github.com/ros2/rclcpp/issues/2301>`_)
+* Update API docs links in package READMEs (`#2302 <https://github.com/ros2/rclcpp/issues/2302>`_)
+* Cleanup flaky timers_manager tests. (`#2299 <https://github.com/ros2/rclcpp/issues/2299>`_)
+* Contributors: Chris Lalancette, Christophe Bedard
+
+22.2.0 (2023-09-07)
+-------------------
+* Topic correct typeadapter deduction (`#2294 <https://github.com/ros2/rclcpp/issues/2294>`_)
+* Fix C++20 allocator construct deprecation (`#2292 <https://github.com/ros2/rclcpp/issues/2292>`_)
+* Make Rate to select the clock to work with (`#2123 <https://github.com/ros2/rclcpp/issues/2123>`_)
+* Correct the position of a comment. (`#2290 <https://github.com/ros2/rclcpp/issues/2290>`_)
+* Remove unnecessary lambda captures in the tests. (`#2289 <https://github.com/ros2/rclcpp/issues/2289>`_)
+* Add rcl_logging_interface as an explicit dependency. (`#2284 <https://github.com/ros2/rclcpp/issues/2284>`_)
+* Revamp list_parameters to be more efficient and easier to read. (`#2282 <https://github.com/ros2/rclcpp/issues/2282>`_)
+* Contributors: AiVerisimilitude, Alexey Merzlyakov, Chen Lihui, Chris Lalancette, Jiaqi Li
+
+22.1.0 (2023-08-21)
+-------------------
+* Do not crash Executor when send_response fails due to client failure. (`#2276 <https://github.com/ros2/rclcpp/issues/2276>`_)
+* Adding Custom Unknown Type Error (`#2272 <https://github.com/ros2/rclcpp/issues/2272>`_)
+* Add a pimpl inside rclcpp::Node for future distro backports (`#2228 <https://github.com/ros2/rclcpp/issues/2228>`_)
+* Remove an unused variable from the events executor tests. (`#2270 <https://github.com/ros2/rclcpp/issues/2270>`_)
+* Add spin_all shortcut (`#2246 <https://github.com/ros2/rclcpp/issues/2246>`_)
+* Adding Missing Group Exceptions (`#2256 <https://github.com/ros2/rclcpp/issues/2256>`_)
+* Change associated clocks storage to unordered_set (`#2257 <https://github.com/ros2/rclcpp/issues/2257>`_)
+* associated clocks should be protected by mutex. (`#2255 <https://github.com/ros2/rclcpp/issues/2255>`_)
+* Instrument loaned message publication code path (`#2240 <https://github.com/ros2/rclcpp/issues/2240>`_)
+* Contributors: Chris Lalancette, Christophe Bedard, Emerson Knapp, Luca Della Vedova, Lucas Wendland, Tomoya Fujita, Tony Najjar
+
+22.0.0 (2023-07-11)
+-------------------
+* Implement get_node_type_descriptions_interface for lifecyclenode and add smoke test for it (`#2237 <https://github.com/ros2/rclcpp/issues/2237>`_)
+* Add new node interface TypeDescriptionsInterface to provide GetTypeDescription service (`#2224 <https://github.com/ros2/rclcpp/issues/2224>`_)
+* Move always_false_v to detail namespace (`#2232 <https://github.com/ros2/rclcpp/issues/2232>`_)
+* Revamp the test_subscription.cpp tests. (`#2227 <https://github.com/ros2/rclcpp/issues/2227>`_)
+* warning: comparison of integer expressions of different signedness (`#2219 <https://github.com/ros2/rclcpp/issues/2219>`_)
+* Modifies timers API to select autostart state (`#2005 <https://github.com/ros2/rclcpp/issues/2005>`_)
+* Enable callback group tests for connextdds (`#2182 <https://github.com/ros2/rclcpp/issues/2182>`_)
+* Contributors: Chris Lalancette, Christopher Wecht, Eloy Briceno, Emerson Knapp, Nathan Wiebe Neufeldt, Tomoya Fujita
+
+21.3.0 (2023-06-12)
+-------------------
+* Fix up misspellings of "receive". (`#2208 <https://github.com/ros2/rclcpp/issues/2208>`_)
+* Remove flaky stressAddRemoveNode test (`#2206 <https://github.com/ros2/rclcpp/issues/2206>`_)
+* Use TRACETOOLS\_ prefix for tracepoint-related macros (`#2162 <https://github.com/ros2/rclcpp/issues/2162>`_)
+* Contributors: Chris Lalancette, Christophe Bedard, Michael Carroll
+
+21.2.0 (2023-06-07)
+-------------------
+* remove nolint since ament_cpplint updated for the c++17 header (`#2198 <https://github.com/ros2/rclcpp/issues/2198>`_)
+* Feature/available capacity of ipm (`#2173 <https://github.com/ros2/rclcpp/issues/2173>`_)
+* add mutex to protect events_executor current entity collection (`#2187 <https://github.com/ros2/rclcpp/issues/2187>`_)
+* Declare rclcpp callbacks before the rcl entities (`#2024 <https://github.com/ros2/rclcpp/issues/2024>`_)
+* Contributors: Alberto Soragna, Chen Lihui, DensoADAS, mauropasse
+
+21.1.1 (2023-05-11)
+-------------------
+* Fix race condition in events-executor (`#2177 <https://github.com/ros2/rclcpp/issues/2177>`_)
+* Add missing stdexcept include (`#2186 <https://github.com/ros2/rclcpp/issues/2186>`_)
+* Fix a format-security warning when building with clang (`#2171 <https://github.com/ros2/rclcpp/issues/2171>`_)
+* Fix delivered message kind (`#2175 <https://github.com/ros2/rclcpp/issues/2175>`_)
+* Contributors: Alberto Soragna, Chris Lalancette, methylDragon, Øystein Sture
+
+21.1.0 (2023-04-27)
+-------------------
+
+21.0.0 (2023-04-18)
+-------------------
+* Add support for logging service. (`#2122 <https://github.com/ros2/rclcpp/issues/2122>`_)
+* Picking ABI-incompatible executor changes (`#2170 <https://github.com/ros2/rclcpp/issues/2170>`_)
+* add events-executor and timers-manager in rclcpp (`#2155 <https://github.com/ros2/rclcpp/issues/2155>`_)
+* Create common structures for executors to use (`#2143 <https://github.com/ros2/rclcpp/issues/2143>`_)
+* Implement deliver message kind (`#2168 <https://github.com/ros2/rclcpp/issues/2168>`_)
+* Contributors: Alberto Soragna, Lei Liu, Michael Carroll, methylDragon
+
 20.0.0 (2023-04-13)
 -------------------
 * applied tracepoints for ring_buffer (`#2091 <https://github.com/ros2/rclcpp/issues/2091>`_)

rclcpp/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.12)
+cmake_minimum_required(VERSION 3.20)
 
 project(rclcpp)
 
@@ -10,11 +10,14 @@ 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)
@@ -42,7 +45,9 @@ 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
@@ -69,6 +74,7 @@ set(${PROJECT_NAME}_SRCS
   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_subscription.cpp
   src/rclcpp/graph_listener.cpp
@@ -91,6 +97,7 @@ 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
@@ -104,6 +111,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/qos.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
@@ -120,6 +128,21 @@ set(${PROJECT_NAME}_SRCS
   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
@@ -198,22 +221,28 @@ target_include_directories(${PROJECT_NAME} PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
-target_link_libraries(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
-# specific order: dependents before dependencies
-ament_target_dependencies(${PROJECT_NAME}
-  "ament_index_cpp"
-  "libstatistics_collector"
-  "rcl"
-  "rcl_interfaces"
-  "rcl_yaml_param_parser"
-  "rcpputils"
-  "rcutils"
-  "builtin_interfaces"
-  "rosgraph_msgs"
-  "rosidl_typesupport_cpp"
-  "rosidl_runtime_cpp"
-  "statistics_msgs"
-  "tracetools"
+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,
@@ -235,20 +264,23 @@ ament_export_libraries(${PROJECT_NAME})
 # Export modern CMake targets
 ament_export_targets(${PROJECT_NAME})
 
-# specific order: dependents before dependencies
-ament_export_dependencies(ament_index_cpp)
-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_lint_auto REQUIRED)
@@ -266,7 +298,7 @@ install(
 
 if(TEST cppcheck)
   # must set the property after ament_package()
-  set_tests_properties(cppcheck PROPERTIES TIMEOUT 500)
+  set_tests_properties(cppcheck PROPERTIES TIMEOUT 1200)
 endif()
 
 if(TEST cpplint)

rclcpp/QUALITY_DECLARATION.md

@@ -4,7 +4,7 @@ This document is a declaration of software quality for the `rclcpp` package, bas
 
 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://www.ros.org/reps/rep-2004.html) 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]
 
@@ -94,7 +94,7 @@ There is an automated test which runs a linter that ensures each file has at lea
 
 ### 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:
@@ -129,7 +129,7 @@ Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly
 
 `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/master/rclcpp/test/benchmark).
+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/)
@@ -139,7 +139,7 @@ Changes that introduce regressions in performance must be adequately justified i
 
 ### 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://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.
+`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/)
@@ -163,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 1**, 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 1**, 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 1**, 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 1**, 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 1**, 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 1**, 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 1**, 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 `rclcpp` so that it may be traced for debugging and performance analysis.
 
-It is **Quality Level 1**, see its [Quality Declaration document](https://gitlab.com/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,7 +2,7 @@
 
 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
 

rclcpp/include/rclcpp/any_executable.hpp

@@ -45,9 +45,9 @@ 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;
-  std::shared_ptr<void> data;
+  rclcpp::CallbackGroup::SharedPtr callback_group {nullptr};
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base {nullptr};
+  std::shared_ptr<void> data {nullptr};
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/any_service_callback.hpp

@@ -156,7 +156,7 @@ public:
     const std::shared_ptr<rmw_request_id_t> & request_header,
     std::shared_ptr<typename ServiceT::Request> request)
   {
-    TRACEPOINT(callback_start, static_cast<const void *>(this), false);
+    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.
@@ -182,7 +182,7 @@ public:
       const auto & cb = std::get<SharedPtrWithRequestHeaderCallback>(callback_);
       cb(request_header, std::move(request), response);
     }
-    TRACEPOINT(callback_end, static_cast<const void *>(this));
+    TRACETOOLS_TRACEPOINT(callback_end, static_cast<const void *>(this));
     return response;
   }
 
@@ -191,9 +191,9 @@ public:
 #ifndef TRACETOOLS_DISABLED
     std::visit(
       [this](auto && arg) {
-        if (TRACEPOINT_ENABLED(rclcpp_callback_register)) {
+        if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) {
           char * symbol = tracetools::get_symbol(arg);
-          DO_TRACEPOINT(
+          TRACETOOLS_DO_TRACEPOINT(
             rclcpp_callback_register,
             static_cast<const void *>(this),
             symbol);

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -20,7 +20,7 @@
 #include <stdexcept>
 #include <type_traits>
 #include <utility>
-#include <variant>  // NOLINT[build/include_order]
+#include <variant>
 
 #include "rosidl_runtime_cpp/traits.hpp"
 #include "tracetools/tracetools.h"
@@ -30,19 +30,19 @@
 #include "rclcpp/detail/subscription_callback_type_helper.hpp"
 #include "rclcpp/function_traits.hpp"
 #include "rclcpp/message_info.hpp"
+#include "rclcpp/serialization.hpp"
 #include "rclcpp/serialized_message.hpp"
 #include "rclcpp/type_adapter.hpp"
 
-
-template<class>
-inline constexpr bool always_false_v = false;
-
 namespace rclcpp
 {
 
 namespace detail
 {
 
+template<class>
+inline constexpr bool always_false_v = false;
+
 template<typename MessageT, typename AllocatorT>
 struct MessageDeleterHelper
 {
@@ -158,13 +158,14 @@ struct AnySubscriptionCallbackPossibleTypes
 template<
   typename MessageT,
   typename AllocatorT,
-  bool is_adapted_type = rclcpp::TypeAdapter<MessageT>::is_specialized::value
+  bool is_adapted_type = rclcpp::TypeAdapter<MessageT>::is_specialized::value,
+  bool is_serialized_type = serialization_traits::is_serialized_message_class<MessageT>::value
 >
 struct AnySubscriptionCallbackHelper;
 
 /// Specialization for when MessageT is not a TypeAdapter.
 template<typename MessageT, typename AllocatorT>
-struct AnySubscriptionCallbackHelper<MessageT, AllocatorT, false>
+struct AnySubscriptionCallbackHelper<MessageT, AllocatorT, false, false>
 {
   using CallbackTypes = AnySubscriptionCallbackPossibleTypes<MessageT, AllocatorT>;
 
@@ -194,7 +195,7 @@ struct AnySubscriptionCallbackHelper<MessageT, AllocatorT, false>
 
 /// Specialization for when MessageT is a TypeAdapter.
 template<typename MessageT, typename AllocatorT>
-struct AnySubscriptionCallbackHelper<MessageT, AllocatorT, true>
+struct AnySubscriptionCallbackHelper<MessageT, AllocatorT, true, false>
 {
   using CallbackTypes = AnySubscriptionCallbackPossibleTypes<MessageT, AllocatorT>;
 
@@ -232,6 +233,26 @@ struct AnySubscriptionCallbackHelper<MessageT, AllocatorT, true>
   >;
 };
 
+/// Specialization for when MessageT is a SerializedMessage to exclude duplicated declarations.
+template<typename MessageT, typename AllocatorT>
+struct AnySubscriptionCallbackHelper<MessageT, AllocatorT, false, true>
+{
+  using CallbackTypes = AnySubscriptionCallbackPossibleTypes<MessageT, AllocatorT>;
+
+  using variant_type = std::variant<
+    typename CallbackTypes::ConstRefSerializedMessageCallback,
+    typename CallbackTypes::ConstRefSerializedMessageWithInfoCallback,
+    typename CallbackTypes::UniquePtrSerializedMessageCallback,
+    typename CallbackTypes::UniquePtrSerializedMessageWithInfoCallback,
+    typename CallbackTypes::SharedConstPtrSerializedMessageCallback,
+    typename CallbackTypes::SharedConstPtrSerializedMessageWithInfoCallback,
+    typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageCallback,
+    typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageWithInfoCallback,
+    typename CallbackTypes::SharedPtrSerializedMessageCallback,
+    typename CallbackTypes::SharedPtrSerializedMessageWithInfoCallback
+  >;
+};
+
 }  // namespace detail
 
 template<
@@ -372,58 +393,12 @@ public:
     // converted to one another, e.g. shared_ptr and unique_ptr.
     using scbth = detail::SubscriptionCallbackTypeHelper<MessageT, CallbackT>;
 
-    // Determine if the given CallbackT is a deprecated signature or not.
-    constexpr auto is_deprecated =
-      rclcpp::function_traits::same_arguments<
-      typename scbth::callback_type,
-      std::function<void(std::shared_ptr<MessageT>)>
-      >::value
-      ||
-      rclcpp::function_traits::same_arguments<
-      typename scbth::callback_type,
-      std::function<void(std::shared_ptr<MessageT>, const rclcpp::MessageInfo &)>
-      >::value;
-
-    // Use the discovered type to force the type of callback when assigning
-    // into the variant.
-    if constexpr (is_deprecated) {
-      // If deprecated, call sub-routine that is deprecated.
-      set_deprecated(static_cast<typename scbth::callback_type>(callback));
-    } else {
-      // Otherwise just assign it.
-      callback_variant_ = static_cast<typename scbth::callback_type>(callback);
-    }
+    callback_variant_ = static_cast<typename scbth::callback_type>(callback);
 
     // Return copy of self for easier testing, normally will be compiled out.
     return *this;
   }
 
-  /// Function for shared_ptr to non-const MessageT, which is deprecated.
-  template<typename SetT>
-#if !defined(RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS)
-  // suppress deprecation warnings in `test_any_subscription_callback.cpp`
-  [[deprecated("use 'void(std::shared_ptr<const MessageT>)' instead")]]
-#endif
-  void
-  set_deprecated(std::function<void(std::shared_ptr<SetT>)> callback)
-  {
-    callback_variant_ = callback;
-  }
-
-  /// Function for shared_ptr to non-const MessageT with MessageInfo, which is deprecated.
-  template<typename SetT>
-#if !defined(RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS)
-  // suppress deprecation warnings in `test_any_subscription_callback.cpp`
-  [[deprecated(
-          "use 'void(std::shared_ptr<const MessageT>, const rclcpp::MessageInfo &)' instead"
-  )]]
-#endif
-  void
-  set_deprecated(std::function<void(std::shared_ptr<SetT>, const rclcpp::MessageInfo &)> callback)
-  {
-    callback_variant_ = callback;
-  }
-
   std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>
   create_ros_unique_ptr_from_ros_shared_ptr_message(
     const std::shared_ptr<const ROSMessageType> & message)
@@ -487,12 +462,14 @@ public:
   }
 
   // Dispatch when input is a ros message and the output could be anything.
-  void
+  template<typename TMsg = ROSMessageType>
+  typename std::enable_if<!serialization_traits::is_serialized_message_class<TMsg>::value,
+    void>::type
   dispatch(
     std::shared_ptr<ROSMessageType> message,
     const rclcpp::MessageInfo & message_info)
   {
-    TRACEPOINT(callback_start, static_cast<const void *>(this), false);
+    TRACETOOLS_TRACEPOINT(callback_start, static_cast<const void *>(this), false);
     // Check if the variant is "unset", throw if it is.
     if (callback_variant_.index() == 0) {
       if (std::get<0>(callback_variant_) == nullptr) {
@@ -580,19 +557,19 @@ public:
         }
         // condition to catch unhandled callback types
         else {  // NOLINT[readability/braces]
-          static_assert(always_false_v<T>, "unhandled callback type");
+          static_assert(detail::always_false_v<T>, "unhandled callback type");
         }
       }, callback_variant_);
-    TRACEPOINT(callback_end, static_cast<const void *>(this));
+    TRACETOOLS_TRACEPOINT(callback_end, static_cast<const void *>(this));
   }
 
   // Dispatch when input is a serialized message and the output could be anything.
   void
   dispatch(
-    std::shared_ptr<rclcpp::SerializedMessage> serialized_message,
+    std::shared_ptr<const rclcpp::SerializedMessage> serialized_message,
     const rclcpp::MessageInfo & message_info)
   {
-    TRACEPOINT(callback_start, static_cast<const void *>(this), false);
+    TRACETOOLS_TRACEPOINT(callback_start, static_cast<const void *>(this), false);
     // Check if the variant is "unset", throw if it is.
     if (callback_variant_.index() == 0) {
       if (std::get<0>(callback_variant_) == nullptr) {
@@ -660,10 +637,10 @@ public:
         }
         // condition to catch unhandled callback types
         else {  // NOLINT[readability/braces]
-          static_assert(always_false_v<T>, "unhandled callback type");
+          static_assert(detail::always_false_v<T>, "unhandled callback type");
         }
       }, callback_variant_);
-    TRACEPOINT(callback_end, static_cast<const void *>(this));
+    TRACETOOLS_TRACEPOINT(callback_end, static_cast<const void *>(this));
   }
 
   void
@@ -671,7 +648,7 @@ public:
     std::shared_ptr<const SubscribedType> message,
     const rclcpp::MessageInfo & message_info)
   {
-    TRACEPOINT(callback_start, static_cast<const void *>(this), true);
+    TRACETOOLS_TRACEPOINT(callback_start, static_cast<const void *>(this), true);
     // Check if the variant is "unset", throw if it is.
     if (callback_variant_.index() == 0) {
       if (std::get<0>(callback_variant_) == nullptr) {
@@ -790,10 +767,10 @@ public:
         }
         // condition to catch unhandled callback types
         else {  // NOLINT[readability/braces]
-          static_assert(always_false_v<T>, "unhandled callback type");
+          static_assert(detail::always_false_v<T>, "unhandled callback type");
         }
       }, callback_variant_);
-    TRACEPOINT(callback_end, static_cast<const void *>(this));
+    TRACETOOLS_TRACEPOINT(callback_end, static_cast<const void *>(this));
   }
 
   void
@@ -801,7 +778,7 @@ public:
     std::unique_ptr<SubscribedType, SubscribedTypeDeleter> message,
     const rclcpp::MessageInfo & message_info)
   {
-    TRACEPOINT(callback_start, static_cast<const void *>(this), true);
+    TRACETOOLS_TRACEPOINT(callback_start, static_cast<const void *>(this), true);
     // Check if the variant is "unset", throw if it is.
     if (callback_variant_.index() == 0) {
       if (std::get<0>(callback_variant_) == nullptr) {
@@ -924,10 +901,10 @@ public:
         }
         // condition to catch unhandled callback types
         else {  // NOLINT[readability/braces]
-          static_assert(always_false_v<T>, "unhandled callback type");
+          static_assert(detail::always_false_v<T>, "unhandled callback type");
         }
       }, callback_variant_);
-    TRACEPOINT(callback_end, static_cast<const void *>(this));
+    TRACETOOLS_TRACEPOINT(callback_end, static_cast<const void *>(this));
   }
 
   constexpr
@@ -965,9 +942,9 @@ public:
 #ifndef TRACETOOLS_DISABLED
     std::visit(
       [this](auto && callback) {
-        if (TRACEPOINT_ENABLED(rclcpp_callback_register)) {
+        if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) {
           char * symbol = tracetools::get_symbol(callback);
-          DO_TRACEPOINT(
+          TRACETOOLS_DO_TRACEPOINT(
             rclcpp_callback_register,
             static_cast<const void *>(this),
             symbol);

rclcpp/include/rclcpp/callback_group.hpp

@@ -129,8 +129,7 @@ public:
    * added to the executor in either case.
    *
    * \param[in] group_type The type of the callback group.
-   * \param[in] get_node_context Lambda to retrieve the node context when
-   *   checking that the creating node is valid and using the guard condition.
+   * \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.
@@ -138,7 +137,7 @@ public:
   RCLCPP_PUBLIC
   explicit CallbackGroup(
     CallbackGroupType group_type,
-    std::function<rclcpp::Context::SharedPtr(void)> get_node_context,
+    rclcpp::Context::WeakPtr context,
     bool automatically_add_to_executor_with_node = true);
 
   /// Default destructor.
@@ -185,7 +184,8 @@ public:
    * \return the number of entities in the callback group.
    */
   RCLCPP_PUBLIC
-  size_t size() const;
+  size_t
+  size() const;
 
   /// Return a reference to the 'can be taken' atomic boolean.
   /**
@@ -217,7 +217,8 @@ public:
    * \param[in] waitable_fuinc Function to execute for each waitable
    */
   RCLCPP_PUBLIC
-  void collect_all_ptrs(
+  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,
@@ -249,16 +250,6 @@ public:
   bool
   automatically_add_to_executor_with_node() const;
 
-  /// Retrieve the guard condition used to signal changes to this callback group.
-  /**
-   * \param[in] context_ptr context to use when creating the guard condition
-   * \return guard condition if it is valid, otherwise nullptr.
-   */
-  [[deprecated("Use get_notify_guard_condition() without arguments")]]
-  RCLCPP_PUBLIC
-  rclcpp::GuardCondition::SharedPtr
-  get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr);
-
   /// Retrieve the guard condition used to signal changes to this callback group.
   /**
    * \return guard condition if it is valid, otherwise nullptr.
@@ -318,7 +309,7 @@ protected:
   std::shared_ptr<rclcpp::GuardCondition> notify_guard_condition_ = nullptr;
   std::recursive_mutex notify_guard_condition_mutex_;
 
-  std::function<rclcpp::Context::SharedPtr(void)> get_context_;
+  rclcpp::Context::WeakPtr context_;
 
 private:
   template<typename TypeT, typename Function>

rclcpp/include/rclcpp/client.hpp

@@ -20,13 +20,13 @@
 #include <future>
 #include <memory>
 #include <mutex>
-#include <optional>  // NOLINT, cpplint doesn't think this is a cpp std header
+#include <optional>
 #include <sstream>
 #include <string>
 #include <tuple>
 #include <unordered_map>
 #include <utility>
-#include <variant>  // NOLINT
+#include <variant>
 #include <vector>
 
 #include "rcl/client.h"
@@ -115,6 +115,29 @@ struct FutureAndRequestId
   /// 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
@@ -363,12 +386,16 @@ protected:
   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};
-
-  std::recursive_mutex callback_mutex_;
-  std::function<void(size_t)> on_new_response_callback_{nullptr};
 };
 
 template<typename ServiceT>
@@ -767,19 +794,11 @@ public:
     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();
+    return detail::prune_requests_older_than_impl(
+      pending_requests_,
+      pending_requests_mutex_,
+      time_point,
+      pruned_requests);
   }
 
   /// Configure client introspection.
@@ -845,7 +864,7 @@ protected:
         "Received invalid sequence number. Ignoring...");
       return std::nullopt;
     }
-    auto value = std::move(it->second.second);
+    std::optional<CallbackInfoVariant> value = std::move(it->second.second);
     this->pending_requests_.erase(request_number);
     return value;
   }

rclcpp/include/rclcpp/clock.hpp

@@ -193,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 *

rclcpp/include/rclcpp/context.hpp

@@ -26,6 +26,7 @@
 #include <unordered_set>
 #include <utility>
 #include <vector>
+#include <stdexcept>
 
 #include "rcl/context.h"
 #include "rcl/guard_condition.h"

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_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_subscription.hpp

@@ -45,13 +45,15 @@ namespace rclcpp
  * 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>>
+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,
-  std::function<void(std::shared_ptr<rclcpp::SerializedMessage>)> callback,
+  CallbackT && callback,
   const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
     rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
   )
@@ -60,13 +62,20 @@ std::shared_ptr<GenericSubscription> create_generic_subscription(
   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,
-    callback,
+    any_subscription_callback,
     options);
 
   topics_interface->add_subscription(subscription, options.callback_group);

rclcpp/include/rclcpp/create_subscription.hpp

@@ -50,8 +50,8 @@ template<
   typename SubscriptionT,
   typename MessageMemoryStrategyT,
   typename NodeParametersT,
-  typename NodeTopicsT,
-  typename ROSMessageType = typename SubscriptionT::ROSMessageType>
+  typename NodeTopicsT
+>
 typename std::shared_ptr<SubscriptionT>
 create_subscription(
   NodeParametersT & node_parameters,
@@ -70,7 +70,7 @@ create_subscription(
   using rclcpp::node_interfaces::get_node_topics_interface;
   auto node_topics_interface = get_node_topics_interface(node_topics);
 
-  std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<ROSMessageType>>
+  std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics>
   subscription_topic_stats = nullptr;
 
   if (rclcpp::detail::resolve_enable_topic_statistics(
@@ -80,8 +80,7 @@ create_subscription(
     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::to_string(options.topic_stats_options.publish_period.count()) + " ms");
     }
 
     std::shared_ptr<Publisher<statistics_msgs::msg::MetricsMessage>>
@@ -89,14 +88,14 @@ create_subscription(
       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<ROSMessageType>
-      >(node_topics_interface->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);
 
     std::weak_ptr<
-      rclcpp::topic_statistics::SubscriptionTopicStatistics<ROSMessageType>
+      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();

rclcpp/include/rclcpp/create_timer.hpp

@@ -90,7 +90,8 @@ 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(
     clock,
@@ -98,7 +99,8 @@ create_timer(
     std::forward<CallbackT>(callback),
     group,
     node_base.get(),
-    node_timers.get());
+    node_timers.get(),
+    autostart);
 }
 
 /// Create a timer with a given clock
@@ -109,7 +111,8 @@ 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(
     clock,
@@ -117,7 +120,8 @@ create_timer(
     std::forward<CallbackT>(callback),
     group,
     rclcpp::node_interfaces::get_node_base_interface(node).get(),
-    rclcpp::node_interfaces::get_node_timers_interface(node).get());
+    rclcpp::node_interfaces::get_node_timers_interface(node).get(),
+    autostart);
 }
 
 /// Convenience method to create a general timer with node resources.
@@ -132,6 +136,7 @@ create_timer(
  * \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
@@ -144,7 +149,8 @@ create_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 (clock == nullptr) {
     throw std::invalid_argument{"clock cannot be null"};
@@ -160,7 +166,7 @@ create_timer(
 
   // Add a new generic timer.
   auto timer = rclcpp::GenericTimer<CallbackT>::make_shared(
-    std::move(clock), period_ns, std::move(callback), node_base->get_context());
+    std::move(clock), period_ns, std::move(callback), node_base->get_context(), autostart);
   node_timers->add_timer(timer, group);
   return timer;
 }
@@ -187,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"};
@@ -201,7 +208,7 @@ create_wall_timer(
 
   // Add a new wall timer.
   auto timer = rclcpp::WallTimer<CallbackT>::make_shared(
-    period_ns, 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;
 }

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/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"
 
@@ -158,6 +159,14 @@ private:
   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/event_handler.hpp

@@ -117,12 +117,12 @@ public:
   /// Add the Waitable to a wait set.
   RCLCPP_PUBLIC
   void
-  add_to_wait_set(rcl_wait_set_t * wait_set) override;
+  add_to_wait_set(rcl_wait_set_t & wait_set) override;
 
   /// Check if the Waitable is ready.
   RCLCPP_PUBLIC
   bool
-  is_ready(rcl_wait_set_t * wait_set) override;
+  is_ready(const rcl_wait_set_t & wait_set) override;
 
   /// Set a callback to be called when each new event instance occurs.
   /**
@@ -260,6 +260,16 @@ public:
     }
   }
 
+  ~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
@@ -284,7 +294,7 @@ public:
 
   /// Execute any entities of the Waitable that are ready.
   void
-  execute(std::shared_ptr<void> & data) override
+  execute(const std::shared_ptr<void> & data) override
   {
     if (!data) {
       throw std::runtime_error("'data' is empty");

rclcpp/include/rclcpp/exceptions/exceptions.hpp

@@ -206,6 +206,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
 {
@@ -222,6 +230,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
 {

rclcpp/include/rclcpp/executor.hpp

@@ -19,7 +19,6 @@
 #include <cassert>
 #include <chrono>
 #include <cstdlib>
-#include <deque>
 #include <iostream>
 #include <list>
 #include <map>
@@ -273,29 +272,51 @@ 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);
 
-  /// Collect work once and execute all available work, optionally within a duration.
+  /// 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
@@ -345,52 +366,12 @@ public:
     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;
-
-    if (spinning.exchange(true)) {
-      throw std::runtime_error("spin_until_future_complete() called while already spinning");
-    }
-    RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
-    while (rclcpp::ok(this->context_) && spinning.load()) {
-      // Do one item of work.
-      spin_once_impl(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.
@@ -399,7 +380,7 @@ public:
    * \throws std::runtime_error if there is an issue triggering the guard condition
    */
   RCLCPP_PUBLIC
-  void
+  virtual void
   cancel();
 
   /// Returns true if the executor is currently spinning.
@@ -412,6 +393,14 @@ public:
   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
@@ -426,6 +415,23 @@ protected:
     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.
@@ -466,7 +472,7 @@ protected:
    */
   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.
   /**
@@ -529,6 +535,15 @@ protected:
     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
+   */
+  void
+  trigger_entity_recollect(bool notify);
+
   /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins.
   std::atomic_bool spinning;
 
@@ -554,7 +569,7 @@ protected:
    * 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.
+   * waitable will awake the executor and rebuild the collections.
    */
   std::shared_ptr<rclcpp::executors::ExecutorNotifyWaitable> notify_waitable_;
 
@@ -563,8 +578,9 @@ protected:
   /// Collector used to associate executable entities from nodes and guard conditions
   rclcpp::executors::ExecutorEntitiesCollector collector_;
 
-  /// Waitset to be waited on.
+  /// 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(
@@ -574,9 +590,6 @@ protected:
   std::shared_ptr<rclcpp::executors::ExecutorNotifyWaitable> current_notify_waitable_
   RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
-  /// Hold the list of executables currently available to be executed.
-  std::deque<rclcpp::AnyExecutable> ready_executables_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
-
   /// shutdown callback handle registered to Context
   rclcpp::OnShutdownCallbackHandle shutdown_callback_handle_;
 

rclcpp/include/rclcpp/executors.hpp

@@ -29,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
@@ -108,7 +120,9 @@ spin_until_future_complete(
   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);
 }
 

rclcpp/include/rclcpp/executors/executor_entities_collection.hpp

@@ -178,6 +178,12 @@ struct ExecutorEntitiesCollection
 
   /// 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

rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp

@@ -48,11 +48,10 @@ public:
   ~ExecutorNotifyWaitable() override = default;
 
   RCLCPP_PUBLIC
-  ExecutorNotifyWaitable(const ExecutorNotifyWaitable & other);
-
+  ExecutorNotifyWaitable(ExecutorNotifyWaitable & other);
 
   RCLCPP_PUBLIC
-  ExecutorNotifyWaitable & operator=(const ExecutorNotifyWaitable & other);
+  ExecutorNotifyWaitable & operator=(ExecutorNotifyWaitable & other);
 
   /// Add conditions to the wait set
   /**
@@ -60,7 +59,7 @@ public:
    */
   RCLCPP_PUBLIC
   void
-  add_to_wait_set(rcl_wait_set_t * wait_set) override;
+  add_to_wait_set(rcl_wait_set_t & wait_set) override;
 
   /// Check conditions against the wait set
   /**
@@ -69,7 +68,7 @@ public:
    */
   RCLCPP_PUBLIC
   bool
-  is_ready(rcl_wait_set_t * wait_set) override;
+  is_ready(const rcl_wait_set_t & wait_set) override;
 
   /// Perform work associated with the waitable.
   /**
@@ -78,7 +77,7 @@ public:
    */
   RCLCPP_PUBLIC
   void
-  execute(std::shared_ptr<void> & data) override;
+  execute(const std::shared_ptr<void> & data) override;
 
   /// Retrieve data to be used in the next execute call.
   /**

rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp

@@ -113,21 +113,21 @@ protected:
    * @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);
 
-  RCLCPP_PUBLIC
   void
   spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive);
 
-  RCLCPP_PUBLIC
   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)
 };

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

@@ -19,6 +19,7 @@
 #include <stdexcept>
 #include <type_traits>
 #include <utility>
+#include <vector>
 
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/allocator/allocator_deleter.hpp"
@@ -44,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<
@@ -65,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<
@@ -95,7 +100,7 @@ public:
 
     buffer_ = std::move(buffer_impl);
 
-    TRACEPOINT(
+    TRACETOOLS_TRACEPOINT(
       rclcpp_buffer_to_ipb,
       static_cast<const void *>(buffer_.get()),
       static_cast<const void *>(this));
@@ -128,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();
@@ -143,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_;
 
@@ -237,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,6 +15,7 @@
 #ifndef RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_
 #define RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_
 
+#include <memory>
 #include <mutex>
 #include <stdexcept>
 #include <utility>
@@ -52,7 +53,10 @@ public:
     if (capacity == 0) {
       throw std::invalid_argument("capacity must be a positive, non-zero value");
     }
-    TRACEPOINT(rclcpp_construct_ring_buffer, static_cast<const void *>(this), capacity_);
+    TRACETOOLS_TRACEPOINT(
+      rclcpp_construct_ring_buffer,
+      static_cast<const void *>(this),
+      capacity_);
   }
 
   virtual ~RingBufferImplementation() {}
@@ -63,13 +67,13 @@ public:
    *
    * \param request the element to be stored in the ring buffer
    */
-  void enqueue(BufferT request)
+  void enqueue(BufferT request) override
   {
     std::lock_guard<std::mutex> lock(mutex_);
 
     write_index_ = next_(write_index_);
     ring_buffer_[write_index_] = std::move(request);
-    TRACEPOINT(
+    TRACETOOLS_TRACEPOINT(
       rclcpp_ring_buffer_enqueue,
       static_cast<const void *>(this),
       write_index_,
@@ -89,7 +93,7 @@ public:
    *
    * \return the element that is being removed from the ring buffer
    */
-  BufferT dequeue()
+  BufferT dequeue() override
   {
     std::lock_guard<std::mutex> lock(mutex_);
 
@@ -98,7 +102,7 @@ public:
     }
 
     auto request = std::move(ring_buffer_[read_index_]);
-    TRACEPOINT(
+    TRACETOOLS_TRACEPOINT(
       rclcpp_ring_buffer_dequeue,
       static_cast<const void *>(this),
       read_index_,
@@ -110,6 +114,17 @@ public:
     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.
@@ -129,7 +144,7 @@ public:
    *
    * \return `true` if there is data and `false` otherwise
    */
-  inline bool has_data() const
+  inline bool has_data() const override
   {
     std::lock_guard<std::mutex> lock(mutex_);
     return has_data_();
@@ -148,9 +163,21 @@ public:
     return is_full_();
   }
 
-  void clear()
+  /// 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
   {
-    TRACEPOINT(rclcpp_ring_buffer_clear, static_cast<const void *>(this));
+    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:
@@ -189,6 +216,82 @@ private:
     return size_ == capacity_;
   }
 
+  /// Get the remaining capacity to store messages
+  /**
+   * This member function is not thread-safe.
+   *
+   * \return the number of free capacity for new messages
+   */
+  inline size_t available_capacity_() const
+  {
+    return capacity_ - size_;
+  }
+
+  /// 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 {};
+  }
+
   size_t capacity_;
 
   std::vector<BufferT> ring_buffer_;

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

@@ -243,6 +243,11 @@ private:
   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
@@ -269,9 +274,12 @@ private:
   rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue_;
 
   std::shared_ptr<rclcpp::executors::ExecutorEntitiesCollector> entities_collector_;
-  std::shared_ptr<rclcpp::executors::ExecutorEntitiesCollection> current_entities_collection_;
   std::shared_ptr<rclcpp::executors::ExecutorNotifyWaitable> notify_waitable_;
 
+  /// Mutex to protect the current_entities_collection_
+  std::recursive_mutex collection_mutex_;
+  std::shared_ptr<rclcpp::executors::ExecutorEntitiesCollection> current_entities_collection_;
+
   /// Flag used to reduce the number of unnecessary waitable events
   std::atomic<bool> notify_waitable_event_pushed_ {false};
 

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

@@ -15,6 +15,8 @@
 #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
@@ -34,6 +36,7 @@ enum ExecutorEventType
 struct ExecutorEvent
 {
   const void * entity_key;
+  std::shared_ptr<void> data;
   int waitable_data;
   ExecutorEventType type;
   size_t num_events;

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

@@ -28,6 +28,7 @@
 #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"
@@ -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.
   /**
@@ -214,7 +253,8 @@ public:
       // 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(),
@@ -292,6 +332,34 @@ public:
     }
   }
 
+  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
@@ -306,6 +374,11 @@ public:
   rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr
   get_subscription_intra_process(uint64_t intra_process_subscription_id);
 
+  /// Return the lowest available capacity for all subscription buffers for a publisher id.
+  RCLCPP_PUBLIC
+  size_t
+  lowest_available_capacity(const uint64_t intra_process_publisher_id) const;
+
 private:
   struct SplittedSubscriptions
   {
@@ -319,6 +392,9 @@ private:
   using PublisherMap =
     std::unordered_map<uint64_t, rclcpp::PublisherBase::WeakPtr>;
 
+  using PublisherBufferMap =
+    std::unordered_map<uint64_t, rclcpp::experimental::buffers::IntraProcessBufferBase::WeakPtr>;
+
   using PublisherToSubscriptionIdsMap =
     std::unordered_map<uint64_t, SplittedSubscriptions>;
 
@@ -337,6 +413,54 @@ private:
     rclcpp::PublisherBase::SharedPtr pub,
     rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr sub) const;
 
+  template<
+    typename ROSMessageType,
+    typename Alloc = std::allocator<ROSMessageType>
+  >
+  void do_transient_local_publish(
+    const uint64_t pub_id, const uint64_t sub_id,
+    const bool use_take_shared_method)
+  {
+    using ROSMessageTypeAllocatorTraits = allocator::AllocRebind<ROSMessageType, Alloc>;
+    using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
+    using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>;
+
+    auto publisher_buffer = publisher_buffers_[pub_id].lock();
+    if (!publisher_buffer) {
+      throw std::runtime_error("publisher buffer has unexpectedly gone out of scope");
+    }
+    auto buffer = std::dynamic_pointer_cast<
+      rclcpp::experimental::buffers::IntraProcessBuffer<
+        ROSMessageType,
+        ROSMessageTypeAllocator,
+        ROSMessageTypeDeleter
+      >
+      >(publisher_buffer);
+    if (!buffer) {
+      throw std::runtime_error(
+              "failed to dynamic cast publisher's IntraProcessBufferBase to "
+              "IntraProcessBuffer<ROSMessageType,ROSMessageTypeAllocator,"
+              "ROSMessageTypeDeleter> which can happen when the publisher and "
+              "subscription use different allocator types, which is not supported");
+    }
+    if (use_take_shared_method) {
+      auto data_vec = buffer->get_all_data_shared();
+      for (auto shared_data : data_vec) {
+        this->template add_shared_msg_to_buffer<
+          ROSMessageType, ROSMessageTypeAllocator, ROSMessageTypeDeleter, ROSMessageType>(
+          shared_data, sub_id);
+      }
+    } else {
+      auto data_vec = buffer->get_all_data_unique();
+      for (auto & owned_data : data_vec) {
+        auto allocator = ROSMessageTypeAllocator();
+        this->template add_owned_msg_to_buffer<
+          ROSMessageType, ROSMessageTypeAllocator, ROSMessageTypeDeleter, ROSMessageType>(
+          std::move(owned_data), sub_id, allocator);
+      }
+    }
+  }
+
   template<
     typename MessageT,
     typename Alloc,
@@ -462,7 +586,7 @@ private:
           auto ptr = MessageAllocTraits::allocate(allocator, 1);
           MessageAllocTraits::construct(allocator, ptr, *message);
 
-          subscription->provide_intra_process_data(std::move(MessageUniquePtr(ptr, deleter)));
+          subscription->provide_intra_process_data(MessageUniquePtr(ptr, deleter));
         }
 
         continue;
@@ -481,13 +605,13 @@ private:
                 "subscription use different allocator types, which is not supported");
       }
 
-      if constexpr (rclcpp::TypeAdapter<MessageT>::is_specialized::value) {
+      if constexpr (rclcpp::TypeAdapter<MessageT, ROSMessageType>::is_specialized::value) {
         ROSMessageTypeAllocator ros_message_alloc(allocator);
-        auto ptr = ros_message_alloc.allocate(1);
-        ros_message_alloc.construct(ptr);
+        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>::convert_to_ros_message(*message, *ptr);
+        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 {
@@ -505,7 +629,7 @@ private:
             MessageAllocTraits::construct(allocator, ptr, *message);
 
             ros_message_subscription->provide_intra_process_message(
-              std::move(MessageUniquePtr(ptr, deleter)));
+              MessageUniquePtr(ptr, deleter));
           }
         }
       }
@@ -515,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/subscription_intra_process.hpp

@@ -87,7 +87,7 @@ public:
       buffer_type),
     any_callback_(callback)
   {
-    TRACEPOINT(
+    TRACETOOLS_TRACEPOINT(
       rclcpp_subscription_callback_added,
       static_cast<const void *>(this),
       static_cast<const void *>(&any_callback_));
@@ -101,6 +101,23 @@ public:
 
   virtual ~SubscriptionIntraProcess() = default;
 
+  void
+  add_to_wait_set(rcl_wait_set_t & wait_set) override
+  {
+    // This block is necessary when the guard condition wakes the wait set, but
+    // the intra process waitable was not handled before the wait set is waited
+    // on again.
+    // Basically we're keeping the guard condition triggered so long as there is
+    // data in the buffer.
+    if (this->buffer_->has_data()) {
+      // If there is data still to be processed, indicate to the
+      // executor or waitset by triggering the guard condition.
+      this->trigger_guard_condition();
+    }
+    // Let the parent classes handle the rest of the work:
+    return SubscriptionIntraProcessBufferT::add_to_wait_set(wait_set);
+  }
+
   std::shared_ptr<void>
   take_data() override
   {
@@ -132,7 +149,7 @@ public:
     );
   }
 
-  void execute(std::shared_ptr<void> & data) override
+  void execute(const std::shared_ptr<void> & data) override
   {
     execute_impl<SubscribedType>(data);
   }
@@ -140,17 +157,16 @@ public:
 protected:
   template<typename T>
   typename std::enable_if<std::is_same<T, rcl_serialized_message_t>::value, void>::type
-  execute_impl(std::shared_ptr<void> & data)
+  execute_impl(const std::shared_ptr<void> &)
   {
-    (void)data;
     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(std::shared_ptr<void> & data)
+  execute_impl(const std::shared_ptr<void> & data)
   {
-    if (!data) {
+    if (nullptr == data) {
       return;
     }
 

rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp

@@ -60,10 +60,19 @@ public:
 
   RCLCPP_PUBLIC
   void
-  add_to_wait_set(rcl_wait_set_t * wait_set) override;
+  add_to_wait_set(rcl_wait_set_t & wait_set) override;
+
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  available_capacity() const = 0;
+
+  RCLCPP_PUBLIC
+  bool
+  is_durability_transient_local() const;
 
   bool
-  is_ready(rcl_wait_set_t * wait_set) override = 0;
+  is_ready(const rcl_wait_set_t & wait_set) override = 0;
 
   std::shared_ptr<void>
   take_data() override = 0;
@@ -76,7 +85,7 @@ public:
   }
 
   void
-  execute(std::shared_ptr<void> & data) override = 0;
+  execute(const std::shared_ptr<void> & data) override = 0;
 
   virtual
   bool

rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp

@@ -30,6 +30,7 @@
 #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"
 
@@ -93,14 +94,23 @@ public:
       buffer_type,
       qos_profile,
       std::make_shared<Alloc>(subscribed_type_allocator_));
-    TRACEPOINT(
+    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(rcl_wait_set_t * wait_set) override
+  is_ready(const rcl_wait_set_t & wait_set) override
   {
     (void) wait_set;
     return buffer_->has_data();
@@ -169,6 +179,11 @@ public:
     return buffer_->use_take_shared_method();
   }
 
+  size_t available_capacity() const override
+  {
+    return buffer_->available_capacity();
+  }
+
 protected:
   void
   trigger_guard_condition() override

rclcpp/include/rclcpp/experimental/timers_manager.hpp

@@ -22,10 +22,10 @@
 #include <functional>
 #include <memory>
 #include <mutex>
+#include <optional>
 #include <thread>
 #include <utility>
 #include <vector>
-
 #include "rclcpp/context.hpp"
 #include "rclcpp/timer.hpp"
 
@@ -86,7 +86,8 @@ public:
   RCLCPP_PUBLIC
   TimersManager(
     std::shared_ptr<rclcpp::Context> context,
-    std::function<void(const rclcpp::TimerBase *)> on_ready_callback = nullptr);
+    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.
@@ -164,21 +165,23 @@ public:
    * 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);
+  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::chrono::nanoseconds to wait,
+   * @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::chrono::nanoseconds get_head_timeout();
+  std::optional<std::chrono::nanoseconds> get_head_timeout();
 
 private:
   RCLCPP_DISABLE_COPY(TimersManager)
@@ -512,12 +515,13 @@ private:
    * @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::chrono::nanoseconds to wait,
+   * @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::chrono::nanoseconds get_head_timeout_unsafe();
+  std::optional<std::chrono::nanoseconds> get_head_timeout_unsafe();
 
   /**
    * @brief Executes all the timers currently ready when the function is invoked
@@ -527,7 +531,8 @@ private:
   void execute_ready_timers_unsafe();
 
   // Callback to be called when timer is ready
-  std::function<void(const rclcpp::TimerBase *)> on_ready_callback_ = nullptr;
+  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_;

rclcpp/include/rclcpp/generic_client.hpp

@@ -0,0 +1,207 @@
+// 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 <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 *;   // Serialized data pointer of request message
+  using Response = void *;  // Serialized 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>;
+
+  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;
+  };
+
+  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 `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.
+   */
+  RCLCPP_PUBLIC
+  FutureAndRequestId
+  async_send_request(const Request request);
+
+  /// 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);
+  }
+
+  RCLCPP_PUBLIC
+  size_t
+  prune_pending_requests();
+
+  RCLCPP_PUBLIC
+  bool
+  remove_pending_request(
+    int64_t request_id);
+
+  /// 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 CallbackInfoVariant = std::variant<
+    std::promise<SharedResponse>>;  // Use variant for extension
+
+  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

@@ -77,7 +77,7 @@ public:
   : rclcpp::PublisherBase(
       node_base,
       topic_name,
-      *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib),
+      *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,

rclcpp/include/rclcpp/generic_subscription.hpp

@@ -74,20 +74,32 @@ public:
     const std::string & topic_name,
     const std::string & topic_type,
     const rclcpp::QoS & qos,
-    // TODO(nnmm): Add variant for callback with message info. See issue #1604.
-    std::function<void(std::shared_ptr<rclcpp::SerializedMessage>)> callback,
+    AnySubscriptionCallback<rclcpp::SerializedMessage, AllocatorT> callback,
     const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options)
   : SubscriptionBase(
       node_base,
-      *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib),
+      *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),
-    callback_(callback),
+    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;
@@ -150,8 +162,7 @@ public:
 
 private:
   RCLCPP_DISABLE_COPY(GenericSubscription)
-
-  std::function<void(std::shared_ptr<rclcpp::SerializedMessage>)> callback_;
+  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_;
 };

rclcpp/include/rclcpp/guard_condition.hpp

@@ -48,7 +48,7 @@ public:
    */
   RCLCPP_PUBLIC
   explicit GuardCondition(
-    rclcpp::Context::SharedPtr 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());
@@ -57,11 +57,6 @@ public:
   virtual
   ~GuardCondition();
 
-  /// Return the context used when creating this guard condition.
-  RCLCPP_PUBLIC
-  rclcpp::Context::SharedPtr
-  get_context() const;
-
   /// Return the underlying rcl guard condition structure.
   RCLCPP_PUBLIC
   rcl_guard_condition_t &
@@ -105,7 +100,7 @@ public:
    */
   RCLCPP_PUBLIC
   void
-  add_to_wait_set(rcl_wait_set_t * wait_set);
+  add_to_wait_set(rcl_wait_set_t & wait_set);
 
   /// Set a callback to be called whenever the guard condition is triggered.
   /**
@@ -128,13 +123,14 @@ public:
   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};
-  rcl_wait_set_t * wait_set_{nullptr};
+  // 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/logger.hpp

@@ -126,9 +126,6 @@ private:
   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

rclcpp/include/rclcpp/node.hpp

@@ -42,6 +42,7 @@
 #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_subscription.hpp"
 #include "rclcpp/logger.hpp"
@@ -56,6 +57,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"
@@ -232,13 +234,15 @@ public:
    * \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);
+    rclcpp::CallbackGroup::SharedPtr group = nullptr,
+    bool autostart = true);
 
   /// Create a timer that uses the node clock to drive the callback.
   /**
@@ -317,6 +321,22 @@ public:
     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 GenericPublisher.
   /**
    * The returned pointer will never be empty, but this function can throw various exceptions, for
@@ -355,12 +375,14 @@ public:
  * `%callback_group`.
    * \return Shared pointer to the created generic subscription.
    */
-  template<typename AllocatorT = std::allocator<void>>
+  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,
-    std::function<void(std::shared_ptr<rclcpp::SerializedMessage>)> callback,
+    CallbackT && callback,
     const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
       rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
     )
@@ -969,7 +991,16 @@ 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
@@ -1302,6 +1333,26 @@ 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
@@ -1454,6 +1505,11 @@ 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
@@ -1586,11 +1642,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

@@ -110,14 +110,16 @@ 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());
+    this->node_timers_.get(),
+    autostart);
 }
 
 template<typename DurationRepT, typename DurationT, typename CallbackT>
@@ -219,13 +221,13 @@ Node::create_generic_publisher(
   );
 }
 
-template<typename AllocatorT>
+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,
-  std::function<void(std::shared_ptr<rclcpp::SerializedMessage>)> callback,
+  CallbackT && callback,
   const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options)
 {
   return rclcpp::create_generic_subscription(
@@ -233,7 +235,7 @@ Node::create_generic_subscription(
     extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()),
     topic_type,
     qos,
-    std::move(callback),
+    std::forward<CallbackT>(callback),
     options
   );
 }

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

@@ -167,6 +167,7 @@ init_tuple(NodeT & n)
  * 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> \
@@ -189,7 +190,7 @@ init_tuple(NodeT & n)
     /* Perfect forwarding constructor to get arguments down to StorageClassT (eventually). */ \
     template<typename ... ArgsT> \
     explicit NodeInterfacesSupports(ArgsT && ... args) \
-      : NodeInterfacesSupports<StorageClassT, RemainingInterfaceTs ...>( \
+    : NodeInterfacesSupports<StorageClassT, RemainingInterfaceTs ...>( \
         std::forward<ArgsT>(args) ...) \
     {} \
  \
@@ -200,6 +201,7 @@ init_tuple(NodeT & n)
     } \
   }; \
   }  // namespace rclcpp::node_interfaces::detail
+// *INDENT-ON*
 
 }  // namespace detail
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

@@ -113,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;

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

@@ -305,6 +305,24 @@ public:
   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

rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp

@@ -30,6 +30,7 @@
   rclcpp::node_interfaces::NodeTimeSourceInterface, \
   rclcpp::node_interfaces::NodeTimersInterface, \
   rclcpp::node_interfaces::NodeTopicsInterface, \
+  rclcpp::node_interfaces::NodeTypeDescriptionsInterface, \
   rclcpp::node_interfaces::NodeWaitablesInterface
 
 
@@ -118,6 +119,7 @@ public:
    *   - 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
@@ -125,7 +127,9 @@ public:
    * the RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT macro.
    *
    * Usage example:
-   *   ```RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)```
+   *   ```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:

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,7 @@
 
 #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"
 
@@ -54,6 +55,13 @@ 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

rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp

@@ -0,0 +1,63 @@
+// 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__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_HPP_
+
+#include <memory>
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_logging_interface.hpp"
+#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/node_interfaces/node_type_descriptions_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeTypeDescriptions part of the Node API.
+class NodeTypeDescriptions : public NodeTypeDescriptionsInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTypeDescriptions)
+
+  RCLCPP_PUBLIC
+  explicit NodeTypeDescriptions(
+    NodeBaseInterface::SharedPtr node_base,
+    NodeLoggingInterface::SharedPtr node_logging,
+    NodeParametersInterface::SharedPtr node_parameters,
+    NodeServicesInterface::SharedPtr node_services);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTypeDescriptions();
+
+private:
+  RCLCPP_DISABLE_COPY(NodeTypeDescriptions)
+
+  // Pimpl hides helper types and functions used for wrapping a C service, which would be
+  // awkward to expose in this header.
+  class NodeTypeDescriptionsImpl;
+  std::unique_ptr<NodeTypeDescriptionsImpl> impl_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_HPP_

rclcpp/include/rclcpp/node_interfaces/node_type_descriptions_interface.hpp

@@ -0,0 +1,44 @@
+// 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__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_INTERFACE_HPP_
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeTypeDescriptions part of the Node API.
+class NodeTypeDescriptionsInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTypeDescriptionsInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTypeDescriptionsInterface() = default;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(
+  rclcpp::node_interfaces::NodeTypeDescriptionsInterface, type_descriptions)
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_options.hpp

@@ -43,6 +43,7 @@ public:
    *   - arguments = {}
    *   - parameter_overrides = {}
    *   - use_global_arguments = true
+   *   - enable_rosout = true
    *   - use_intra_process_comms = false
    *   - enable_topic_statistics = false
    *   - start_parameter_services = true
@@ -50,6 +51,7 @@ public:
    *   - clock_type = RCL_ROS_TIME
    *   - clock_qos = rclcpp::ClockQoS()
    *   - use_clock_thread = true
+   *   - enable_logger_service = false
    *   - rosout_qos = rclcpp::RosoutQoS()
    *   - parameter_event_qos = rclcpp::ParameterEventQoS
    *     - with history setting and depth from rmw_qos_profile_parameter_events
@@ -232,6 +234,24 @@ public:
   NodeOptions &
   start_parameter_services(bool start_parameter_services);
 
+  /// Return the enable_logger_service flag.
+  RCLCPP_PUBLIC
+  bool
+  enable_logger_service() const;
+
+  /// Set the enable_logger_service flag, return this for logger idiom.
+  /**
+   * If true, ROS services are created to allow external nodes to get
+   * and set logger levels of this node.
+   *
+   * If false, loggers will still be configured and set logger levels locally,
+   * but logger levels cannot be changed remotely .
+   *
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  enable_logger_service(bool enable_log_service);
+
   /// Return the start_parameter_event_publisher flag.
   RCLCPP_PUBLIC
   bool
@@ -421,6 +441,8 @@ private:
 
   bool use_clock_thread_ {true};
 
+  bool enable_logger_service_ {false};
+
   rclcpp::QoS parameter_event_qos_ = rclcpp::ParameterEventsQoS(
     rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)
   );

rclcpp/include/rclcpp/parameter_events_filter.hpp

@@ -48,7 +48,7 @@ public:
    *
    * Example Usage:
    *
-   * If you have recieved a parameter event and are only interested in parameters foo and
+   * If you have received a parameter event and are only interested in parameters foo and
    * bar being added or changed but don't care about deletion.
    *
    * ```cpp

rclcpp/include/rclcpp/parameter_value.hpp

@@ -24,6 +24,7 @@
 
 #include "rcl_interfaces/msg/parameter_type.hpp"
 #include "rcl_interfaces/msg/parameter_value.hpp"
+#include "rclcpp/exceptions/exceptions.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp

rclcpp/include/rclcpp/publisher.hpp

@@ -32,6 +32,9 @@
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/allocator/allocator_deleter.hpp"
 #include "rclcpp/detail/resolve_use_intra_process.hpp"
+#include "rclcpp/detail/resolve_intra_process_buffer_type.hpp"
+#include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
+#include "rclcpp/experimental/create_intra_process_buffer.hpp"
 #include "rclcpp/experimental/intra_process_manager.hpp"
 #include "rclcpp/get_message_type_support_handle.hpp"
 #include "rclcpp/is_ros_compatible_type.hpp"
@@ -109,6 +112,12 @@ public:
   [[deprecated("use std::shared_ptr<const PublishedType>")]] =
     std::shared_ptr<const PublishedType>;
 
+  using BufferSharedPtr = typename rclcpp::experimental::buffers::IntraProcessBuffer<
+    ROSMessageType,
+    ROSMessageTypeAllocator,
+    ROSMessageTypeDeleter
+    >::SharedPtr;
+
   RCLCPP_SMART_PTR_DEFINITIONS(Publisher<MessageT, AllocatorT>)
 
   /// Default constructor.
@@ -171,11 +180,14 @@ public:
         throw std::invalid_argument(
                 "intraprocess communication is not allowed with a zero qos history depth value");
       }
-      if (qos.durability() != rclcpp::DurabilityPolicy::Volatile) {
-        throw std::invalid_argument(
-                "intraprocess communication allowed only with volatile durability");
+      if (qos.durability() == rclcpp::DurabilityPolicy::TransientLocal) {
+        buffer_ = rclcpp::experimental::create_intra_process_buffer<
+          ROSMessageType, ROSMessageTypeAllocator, ROSMessageTypeDeleter>(
+          rclcpp::detail::resolve_intra_process_buffer_type(options_.intra_process_buffer_type),
+          qos,
+          std::make_shared<ROSMessageTypeAllocator>(ros_message_type_allocator_));
       }
-      uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this());
+      uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this(), buffer_);
       this->setup_intra_process(
         intra_process_publisher_id,
         ipm);
@@ -242,9 +254,18 @@ public:
     if (inter_process_publish_needed) {
       auto shared_msg =
         this->do_intra_process_ros_message_publish_and_return_shared(std::move(msg));
+      if (buffer_) {
+        buffer_->add_shared(shared_msg);
+      }
       this->do_inter_process_publish(*shared_msg);
     } else {
-      this->do_intra_process_ros_message_publish(std::move(msg));
+      if (buffer_) {
+        auto shared_msg =
+          this->do_intra_process_ros_message_publish_and_return_shared(std::move(msg));
+        buffer_->add_shared(shared_msg);
+      } else {
+        this->do_intra_process_ros_message_publish(std::move(msg));
+      }
     }
   }
 
@@ -269,8 +290,8 @@ public:
   {
     // Avoid allocating when not using intra process.
     if (!intra_process_is_enabled_) {
-      // In this case we're not using intra process.
-      return this->do_inter_process_publish(msg);
+      this->do_inter_process_publish(msg);
+      return;
     }
     // Otherwise we have to allocate memory in a unique_ptr and pass it along.
     // As the message is not const, a copy should be made.
@@ -297,26 +318,31 @@ public:
   >
   publish(std::unique_ptr<T, PublishedTypeDeleter> msg)
   {
-    // Avoid allocating when not using intra process.
     if (!intra_process_is_enabled_) {
       // In this case we're not using intra process.
-      ROSMessageType ros_msg;
-      rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, ros_msg);
-      return this->do_inter_process_publish(ros_msg);
+      auto ros_msg_ptr = std::make_unique<ROSMessageType>();
+      rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, *ros_msg_ptr);
+      this->do_inter_process_publish(*ros_msg_ptr);
+      return;
     }
 
     bool inter_process_publish_needed =
       get_subscription_count() > get_intra_process_subscription_count();
 
     if (inter_process_publish_needed) {
-      ROSMessageType ros_msg;
-      // TODO(clalancette): This is unnecessarily doing an additional conversion
-      // that may have already been done in do_intra_process_publish_and_return_shared().
-      // We should just reuse that effort.
-      rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, ros_msg);
+      auto ros_msg_ptr = std::make_shared<ROSMessageType>();
+      rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, *ros_msg_ptr);
       this->do_intra_process_publish(std::move(msg));
-      this->do_inter_process_publish(ros_msg);
+      this->do_inter_process_publish(*ros_msg_ptr);
+      if (buffer_) {
+        buffer_->add_shared(ros_msg_ptr);
+      }
     } else {
+      if (buffer_) {
+        auto ros_msg_ptr = std::make_shared<ROSMessageType>();
+        rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, *ros_msg_ptr);
+        buffer_->add_shared(ros_msg_ptr);
+      }
       this->do_intra_process_publish(std::move(msg));
     }
   }
@@ -339,13 +365,12 @@ public:
   >
   publish(const T & msg)
   {
-    // Avoid double allocating when not using intra process.
     if (!intra_process_is_enabled_) {
       // Convert to the ROS message equivalent and publish it.
-      ROSMessageType ros_msg;
-      rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(msg, ros_msg);
-      // In this case we're not using intra process.
-      return this->do_inter_process_publish(ros_msg);
+      auto ros_msg_ptr = std::make_unique<ROSMessageType>();
+      rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(msg, *ros_msg_ptr);
+      this->do_inter_process_publish(*ros_msg_ptr);
+      return;
     }
 
     // Otherwise we have to allocate memory in a unique_ptr and pass it along.
@@ -390,7 +415,7 @@ public:
     if (this->can_loan_messages()) {
       // we release the ownership from the rclpp::LoanedMessage instance
       // and let the middleware clean up the memory.
-      this->do_loaned_message_publish(std::move(loaned_msg.release()));
+      this->do_loaned_message_publish(loaned_msg.release());
     } else {
       // we don't release the ownership, let the middleware copy the ros message
       // and thus the destructor of rclcpp::LoanedMessage cleans up the memory.
@@ -421,7 +446,7 @@ protected:
   void
   do_inter_process_publish(const ROSMessageType & msg)
   {
-    TRACEPOINT(rclcpp_publish, nullptr, static_cast<const void *>(&msg));
+    TRACETOOLS_TRACEPOINT(rclcpp_publish, nullptr, static_cast<const void *>(&msg));
     auto status = rcl_publish(publisher_handle_.get(), &msg, nullptr);
 
     if (RCL_RET_PUBLISHER_INVALID == status) {
@@ -456,6 +481,7 @@ protected:
   do_loaned_message_publish(
     std::unique_ptr<ROSMessageType, std::function<void(ROSMessageType *)>> msg)
   {
+    TRACETOOLS_TRACEPOINT(rclcpp_publish, nullptr, static_cast<const void *>(msg.get()));
     auto status = rcl_publish_loaned_message(publisher_handle_.get(), msg.get(), nullptr);
 
     if (RCL_RET_PUBLISHER_INVALID == status) {
@@ -484,7 +510,7 @@ protected:
     if (!msg) {
       throw std::runtime_error("cannot publish msg which is a null pointer");
     }
-    TRACEPOINT(
+    TRACETOOLS_TRACEPOINT(
       rclcpp_intra_publish,
       static_cast<const void *>(publisher_handle_.get()),
       msg.get());
@@ -506,7 +532,7 @@ protected:
     if (!msg) {
       throw std::runtime_error("cannot publish msg which is a null pointer");
     }
-    TRACEPOINT(
+    TRACETOOLS_TRACEPOINT(
       rclcpp_intra_publish,
       static_cast<const void *>(publisher_handle_.get()),
       msg.get());
@@ -529,7 +555,7 @@ protected:
     if (!msg) {
       throw std::runtime_error("cannot publish msg which is a null pointer");
     }
-    TRACEPOINT(
+    TRACETOOLS_TRACEPOINT(
       rclcpp_intra_publish,
       static_cast<const void *>(publisher_handle_.get()),
       msg.get());
@@ -580,6 +606,8 @@ protected:
   PublishedTypeDeleter published_type_deleter_;
   ROSMessageTypeAllocator ros_message_type_allocator_;
   ROSMessageTypeDeleter ros_message_type_deleter_;
+
+  BufferSharedPtr buffer_{nullptr};
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/publisher_base.hpp

@@ -139,6 +139,12 @@ public:
   size_t
   get_intra_process_subscription_count() const;
 
+  /// Get if durability is transient local
+  /** \return If durability is transient local*/
+  RCLCPP_PUBLIC
+  bool
+  is_durability_transient_local() const;
+
   /// Manually assert that this Publisher is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC).
   /**
    * If the rmw Liveliness policy is set to RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, the creator
@@ -215,6 +221,17 @@ public:
   std::vector<rclcpp::NetworkFlowEndpoint>
   get_network_flow_endpoints() const;
 
+  /// Return the lowest available capacity for all subscription buffers.
+  /**
+   * For intraprocess communication return the lowest buffer capacity for all subscriptions.
+   * If intraprocess is disabled or no intraprocess subscriptions present, return maximum of size_t.
+   * On failure return 0.
+   * \return lowest buffer capacity for all subscriptions
+   */
+  RCLCPP_PUBLIC
+  size_t
+  lowest_available_ipm_capacity() const;
+
   /// Wait until all published messages are acknowledged or until the specified timeout elapses.
   /**
    * This method waits until all published messages are acknowledged by all matching

rclcpp/include/rclcpp/publisher_options.hpp

@@ -24,6 +24,7 @@
 
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/detail/rmw_implementation_specific_publisher_payload.hpp"
+#include "rclcpp/intra_process_buffer_type.hpp"
 #include "rclcpp/intra_process_setting.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/event_handler.hpp"
@@ -40,6 +41,9 @@ struct PublisherOptionsBase
   /// Setting to explicitly set intraprocess communications.
   IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault;
 
+  /// Setting the data-type stored in the intraprocess buffer
+  IntraProcessBufferType intra_process_buffer_type = IntraProcessBufferType::SharedPtr;
+
   /// Callbacks for various events related to publishers.
   PublisherEventCallbacks event_callbacks;
 

rclcpp/include/rclcpp/qos_event.hpp

@@ -1,22 +0,0 @@
-// Copyright 2019 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef RCLCPP__QOS_EVENT_HPP_
-#define RCLCPP__QOS_EVENT_HPP_
-
-#warning This header is obsolete, please include rclcpp/event_handler.hpp instead
-
-#include "rclcpp/event_handler.hpp"
-
-#endif  // RCLCPP__QOS_EVENT_HPP_

rclcpp/include/rclcpp/rate.hpp

@@ -19,6 +19,8 @@
 #include <memory>
 #include <thread>
 
+#include "rclcpp/clock.hpp"
+#include "rclcpp/duration.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -31,9 +33,20 @@ class RateBase
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(RateBase)
 
+  RCLCPP_PUBLIC
   virtual ~RateBase() {}
+
+  RCLCPP_PUBLIC
   virtual bool sleep() = 0;
+
+  [[deprecated("use get_type() instead")]]
+  RCLCPP_PUBLIC
   virtual bool is_steady() const = 0;
+
+  RCLCPP_PUBLIC
+  virtual rcl_clock_type_t get_type() const = 0;
+
+  RCLCPP_PUBLIC
   virtual void reset() = 0;
 };
 
@@ -42,14 +55,13 @@ using std::chrono::duration_cast;
 using std::chrono::nanoseconds;
 
 template<class Clock = std::chrono::high_resolution_clock>
-class GenericRate : public RateBase
+class [[deprecated("use rclcpp::Rate class instead of GenericRate")]] GenericRate : public RateBase
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(GenericRate)
 
   explicit GenericRate(double rate)
-  : GenericRate<Clock>(
-      duration_cast<nanoseconds>(duration<double>(1.0 / rate)))
+  : period_(duration_cast<nanoseconds>(duration<double>(1.0 / rate))), last_interval_(Clock::now())
   {}
   explicit GenericRate(std::chrono::nanoseconds period)
   : period_(period), last_interval_(Clock::now())
@@ -87,12 +99,18 @@ public:
     return true;
   }
 
+  [[deprecated("use get_type() instead")]]
   virtual bool
   is_steady() const
   {
     return Clock::is_steady;
   }
 
+  virtual rcl_clock_type_t get_type() const
+  {
+    return Clock::is_steady ? RCL_STEADY_TIME : RCL_SYSTEM_TIME;
+  }
+
   virtual void
   reset()
   {
@@ -112,8 +130,59 @@ private:
   std::chrono::time_point<Clock, ClockDurationNano> last_interval_;
 };
 
-using Rate = GenericRate<std::chrono::system_clock>;
-using WallRate = GenericRate<std::chrono::steady_clock>;
+class Rate : public RateBase
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(Rate)
+
+  RCLCPP_PUBLIC
+  explicit Rate(
+    const double rate,
+    Clock::SharedPtr clock = std::make_shared<Clock>(RCL_SYSTEM_TIME));
+
+  RCLCPP_PUBLIC
+  explicit Rate(
+    const Duration & period,
+    Clock::SharedPtr clock = std::make_shared<Clock>(RCL_SYSTEM_TIME));
+
+  RCLCPP_PUBLIC
+  virtual bool
+  sleep();
+
+  [[deprecated("use get_type() instead")]]
+  RCLCPP_PUBLIC
+  virtual bool
+  is_steady() const;
+
+  RCLCPP_PUBLIC
+  virtual rcl_clock_type_t
+  get_type() const;
+
+  RCLCPP_PUBLIC
+  virtual void
+  reset();
+
+  RCLCPP_PUBLIC
+  std::chrono::nanoseconds
+  period() const;
+
+private:
+  RCLCPP_DISABLE_COPY(Rate)
+
+  Clock::SharedPtr clock_;
+  Duration period_;
+  Time last_interval_;
+};
+
+class WallRate : public Rate
+{
+public:
+  RCLCPP_PUBLIC
+  explicit WallRate(const double rate);
+
+  RCLCPP_PUBLIC
+  explicit WallRate(const Duration & period);
+};
 
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/rclcpp.hpp

@@ -54,6 +54,7 @@
  *   - rclcpp::ParameterValue
  *   - rclcpp::AsyncParametersClient
  *   - rclcpp::SyncParametersClient
+ *   - rclcpp::copy_all_parameter_values()
  *   - rclcpp/parameter.hpp
  *   - rclcpp/parameter_value.hpp
  *   - rclcpp/parameter_client.hpp
@@ -95,6 +96,9 @@
  * - Get the number of publishers or subscribers on a topic:
  *   - rclcpp::Node::count_publishers()
  *   - rclcpp::Node::count_subscribers()
+ * - Get the number of clients or servers on a service:
+ *   - rclcpp::Node::count_clients()
+ *   - rclcpp::Node::count_services()
  *
  * And components related to logging:
  *
@@ -164,6 +168,7 @@
 #include <csignal>
 #include <memory>
 
+#include "rclcpp/copy_all_parameter_values.hpp"
 #include "rclcpp/executors.hpp"
 #include "rclcpp/guard_condition.hpp"
 #include "rclcpp/logging.hpp"

rclcpp/include/rclcpp/service.hpp

@@ -265,15 +265,19 @@ protected:
 
   std::shared_ptr<rcl_node_t> node_handle_;
 
+  std::recursive_mutex callback_mutex_;
+  // It is important to declare on_new_request_callback_ before
+  // service_handle_, so on destruction the service is
+  // destroyed first. Otherwise, the rmw service callback
+  // would point briefly to a destroyed function.
+  std::function<void(size_t)> on_new_request_callback_{nullptr};
+  // Declare service_handle_ after callback
   std::shared_ptr<rcl_service_t> service_handle_;
   bool owns_rcl_handle_ = true;
 
   rclcpp::Logger node_logger_;
 
   std::atomic<bool> in_use_by_wait_set_{false};
-
-  std::recursive_mutex callback_mutex_;
-  std::function<void(size_t)> on_new_request_callback_{nullptr};
 };
 
 template<typename ServiceT>
@@ -348,7 +352,7 @@ public:
 
       rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service");
     }
-    TRACEPOINT(
+    TRACETOOLS_TRACEPOINT(
       rclcpp_service_callback_added,
       static_cast<const void *>(get_service_handle().get()),
       static_cast<const void *>(&any_callback_));
@@ -383,7 +387,7 @@ public:
     }
 
     service_handle_ = service_handle;
-    TRACEPOINT(
+    TRACETOOLS_TRACEPOINT(
       rclcpp_service_callback_added,
       static_cast<const void *>(get_service_handle().get()),
       static_cast<const void *>(&any_callback_));
@@ -420,7 +424,7 @@ public:
     // In this case, rcl owns the service handle memory
     service_handle_ = std::shared_ptr<rcl_service_t>(new rcl_service_t);
     service_handle_->impl = service_handle->impl;
-    TRACEPOINT(
+    TRACETOOLS_TRACEPOINT(
       rclcpp_service_callback_added,
       static_cast<const void *>(get_service_handle().get()),
       static_cast<const void *>(&any_callback_));
@@ -482,6 +486,14 @@ public:
   {
     rcl_ret_t ret = rcl_send_response(get_service_handle().get(), &req_id, &response);
 
+    if (ret == RCL_RET_TIMEOUT) {
+      RCLCPP_WARN(
+        node_logger_.get_child("rclcpp"),
+        "failed to send response to %s (timeout): %s",
+        this->get_service_name(), rcl_get_error_string().str);
+      rcl_reset_error();
+      return;
+    }
     if (ret != RCL_RET_OK) {
       rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send response");
     }

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -17,7 +17,6 @@
 
 #include <memory>
 #include <vector>
-#include <utility>
 
 #include "rcl/allocator.h"
 
@@ -121,8 +120,8 @@ public:
       }
     }
     for (size_t i = 0; i < waitable_handles_.size(); ++i) {
-      if (waitable_handles_[i]->is_ready(wait_set)) {
-        waitable_triggered_handles_.emplace_back(std::move(waitable_handles_[i]));
+      if (!waitable_handles_[i]->is_ready(*wait_set)) {
+        waitable_handles_[i].reset();
       }
     }
 
@@ -146,7 +145,10 @@ public:
       timer_handles_.end()
     );
 
-    waitable_handles_.clear();
+    waitable_handles_.erase(
+      std::remove(waitable_handles_.begin(), waitable_handles_.end(), nullptr),
+      waitable_handles_.end()
+    );
   }
 
   bool collect_entities(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
@@ -235,7 +237,7 @@ public:
     }
 
     for (const std::shared_ptr<Waitable> & waitable : waitable_handles_) {
-      waitable->add_to_wait_set(wait_set);
+      waitable->add_to_wait_set(*wait_set);
     }
     return true;
   }
@@ -368,7 +370,8 @@ public:
           ++it;
           continue;
         }
-        if (!timer->call()) {
+        auto data = timer->call();
+        if (!data) {
           // timer was cancelled, skip it.
           ++it;
           continue;
@@ -377,6 +380,7 @@ public:
         any_exec.timer = timer;
         any_exec.callback_group = group;
         any_exec.node_base = get_node_by_group(group, weak_groups_to_nodes);
+        any_exec.data = data;
         timer_handles_.erase(it);
         return;
       }
@@ -390,9 +394,8 @@ public:
     rclcpp::AnyExecutable & any_exec,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
   {
-    auto & waitable_handles = waitable_triggered_handles_;
-    auto it = waitable_handles.begin();
-    while (it != waitable_handles.end()) {
+    auto it = waitable_handles_.begin();
+    while (it != waitable_handles_.end()) {
       std::shared_ptr<Waitable> & waitable = *it;
       if (waitable) {
         // Find the group for this handle and see if it can be serviced
@@ -400,7 +403,7 @@ public:
         if (!group) {
           // Group was not found, meaning the waitable is not valid...
           // Remove it from the ready list and continue looking
-          it = waitable_handles.erase(it);
+          it = waitable_handles_.erase(it);
           continue;
         }
         if (!group->can_be_taken_from().load()) {
@@ -413,11 +416,11 @@ public:
         any_exec.waitable = waitable;
         any_exec.callback_group = group;
         any_exec.node_base = get_node_by_group(group, weak_groups_to_nodes);
-        waitable_handles.erase(it);
+        waitable_handles_.erase(it);
         return;
       }
       // Else, the waitable is no longer valid, remove it and continue
-      it = waitable_handles.erase(it);
+      it = waitable_handles_.erase(it);
     }
   }
 
@@ -498,8 +501,6 @@ private:
   VectorRebind<std::shared_ptr<const rcl_timer_t>> timer_handles_;
   VectorRebind<std::shared_ptr<Waitable>> waitable_handles_;
 
-  VectorRebind<std::shared_ptr<Waitable>> waitable_triggered_handles_;
-
   std::shared_ptr<VoidAlloc> allocator_;
 };
 

rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp

@@ -15,10 +15,17 @@
 #ifndef RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_
 #define RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_
 
+#include <array>
+#include <cstring>
 #include <memory>
+#include <mutex>
+#include <stdexcept>
+#include <type_traits>
 
 #include "rosidl_runtime_cpp/traits.hpp"
 
+#include "rclcpp/logger.hpp"
+#include "rclcpp/logging.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/message_memory_strategy.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -50,13 +57,24 @@ class MessagePoolMemoryStrategy
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(MessagePoolMemoryStrategy)
 
-  /// Default constructor
   MessagePoolMemoryStrategy()
-  : next_array_index_(0)
   {
+    pool_mutex_ = std::make_shared<std::mutex>();
+
+    pool_ = std::shared_ptr<std::array<MessageT *, Size>>(
+      new std::array<MessageT *, Size>,
+      [](std::array<MessageT *, Size> * arr) {
+        for (size_t i = 0; i < Size; ++i) {
+          free((*arr)[i]);
+        }
+        delete arr;
+      });
+
+    free_list_ = std::make_shared<CircularArray<Size>>();
+
     for (size_t i = 0; i < Size; ++i) {
-      pool_[i].msg_ptr_ = std::make_shared<MessageT>();
-      pool_[i].used = false;
+      (*pool_)[i] = static_cast<MessageT *>(malloc(sizeof(MessageT)));
+      free_list_->push_back(i);
     }
   }
 
@@ -68,43 +86,85 @@ public:
    */
   std::shared_ptr<MessageT> borrow_message()
   {
-    size_t current_index = next_array_index_;
-    next_array_index_ = (next_array_index_ + 1) % Size;
-    if (pool_[current_index].used) {
-      throw std::runtime_error("Tried to access message that was still in use! Abort.");
+    std::lock_guard<std::mutex> lock(*pool_mutex_);
+    if (free_list_->size() == 0) {
+      throw std::runtime_error("No more free slots in the pool");
     }
-    pool_[current_index].msg_ptr_->~MessageT();
-    new (pool_[current_index].msg_ptr_.get())MessageT;
 
-    pool_[current_index].used = true;
-    return pool_[current_index].msg_ptr_;
+    size_t current_index = free_list_->pop_front();
+
+    return std::shared_ptr<MessageT>(
+      new((*pool_)[current_index]) MessageT(),
+      [pool = this->pool_, pool_mutex = this->pool_mutex_,
+      free_list = this->free_list_](MessageT * p) {
+        std::lock_guard<std::mutex> lock(*pool_mutex);
+        for (size_t i = 0; i < Size; ++i) {
+          if ((*pool)[i] == p) {
+            p->~MessageT();
+            free_list->push_back(i);
+            break;
+          }
+        }
+      });
   }
 
   /// Return a message to the message pool.
   /**
-   * Manage metadata in the message pool ring buffer to release the message.
+   * This does nothing since the message isn't returned to the pool until the user has dropped
+   * all references.
    * \param[in] msg Shared pointer to the message to return.
    */
   void return_message(std::shared_ptr<MessageT> & msg)
   {
-    for (size_t i = 0; i < Size; ++i) {
-      if (pool_[i].msg_ptr_ == msg) {
-        pool_[i].used = false;
-        return;
-      }
-    }
-    throw std::runtime_error("Unrecognized message ptr in return_message.");
+    (void)msg;
   }
 
 protected:
-  struct PoolMember
+  template<size_t N>
+  class CircularArray
   {
-    std::shared_ptr<MessageT> msg_ptr_;
-    bool used;
+public:
+    void push_back(const size_t v)
+    {
+      if (size_ + 1 > N) {
+        throw std::runtime_error("Tried to push too many items into the array");
+      }
+      array_[(front_ + size_) % N] = v;
+      ++size_;
+    }
+
+    size_t pop_front()
+    {
+      if (size_ < 1) {
+        throw std::runtime_error("Tried to pop item from empty array");
+      }
+
+      size_t val = array_[front_];
+
+      front_ = (front_ + 1) % N;
+      --size_;
+
+      return val;
+    }
+
+    size_t size() const
+    {
+      return size_;
+    }
+
+private:
+    size_t front_ = 0;
+    size_t size_ = 0;
+    std::array<size_t, N> array_;
   };
 
-  std::array<PoolMember, Size> pool_;
-  size_t next_array_index_;
+  // It's very important that these are shared_ptrs, since users of this class might hold a
+  // reference to a pool item longer than the lifetime of the class.  In that scenario, the
+  // shared_ptr ensures that the lifetime of these variables outlives this class, and hence ensures
+  // the custom destructor for each pool item can successfully run.
+  std::shared_ptr<std::mutex> pool_mutex_;
+  std::shared_ptr<std::array<MessageT *, Size>> pool_;
+  std::shared_ptr<CircularArray<Size>> free_list_;
 };
 
 }  // namespace message_pool_memory_strategy

rclcpp/include/rclcpp/subscription.hpp

@@ -104,7 +104,7 @@ public:
 
 private:
   using SubscriptionTopicStatisticsSharedPtr =
-    std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<ROSMessageType>>;
+    std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics>;
 
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(Subscription)
@@ -127,6 +127,7 @@ public:
    *   of the following conditions are true: qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_ALL,
    *   qos_profile.depth == 0 or qos_profile.durability != RMW_QOS_POLICY_DURABILITY_VOLATILE).
    */
+  // *INDENT-OFF*
   Subscription(
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
     const rosidl_message_type_support_t & type_support_handle,
@@ -148,6 +149,7 @@ public:
     any_callback_(callback),
     options_(options),
     message_memory_strategy_(message_memory_strategy)
+  // *INDENT-ON*
   {
     // Setup intra process publishing if requested.
     if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) {
@@ -163,10 +165,6 @@ public:
         throw std::invalid_argument(
                 "intraprocess communication is not allowed with 0 depth qos policy");
       }
-      if (qos_profile.durability() != rclcpp::DurabilityPolicy::Volatile) {
-        throw std::invalid_argument(
-                "intraprocess communication allowed only with volatile durability");
-      }
 
       using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess<
         MessageT,
@@ -185,7 +183,7 @@ public:
         this->get_topic_name(),  // important to get like this, as it has the fully-qualified name
         qos_profile,
         resolve_intra_process_buffer_type(options_.intra_process_buffer_type, callback));
-      TRACEPOINT(
+      TRACETOOLS_TRACEPOINT(
         rclcpp_subscription_init,
         static_cast<const void *>(get_subscription_handle().get()),
         static_cast<const void *>(subscription_intra_process_.get()));
@@ -193,7 +191,8 @@ public:
       // Add it to the intra process manager.
       using rclcpp::experimental::IntraProcessManager;
       auto ipm = context->get_sub_context<IntraProcessManager>();
-      uint64_t intra_process_subscription_id = ipm->add_subscription(subscription_intra_process_);
+      uint64_t intra_process_subscription_id = ipm->template add_subscription<
+        ROSMessageType, ROSMessageTypeAllocator>(subscription_intra_process_);
       this->setup_intra_process(intra_process_subscription_id, ipm);
     }
 
@@ -201,11 +200,11 @@ public:
       this->subscription_topic_statistics_ = std::move(subscription_topic_statistics);
     }
 
-    TRACEPOINT(
+    TRACETOOLS_TRACEPOINT(
       rclcpp_subscription_init,
       static_cast<const void *>(get_subscription_handle().get()),
       static_cast<const void *>(this));
-    TRACEPOINT(
+    TRACETOOLS_TRACEPOINT(
       rclcpp_subscription_callback_added,
       static_cast<const void *>(this),
       static_cast<const void *>(&any_callback_));
@@ -316,7 +315,7 @@ public:
     if (subscription_topic_statistics_) {
       const auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(now);
       const auto time = rclcpp::Time(nanos.time_since_epoch().count());
-      subscription_topic_statistics_->handle_message(*typed_message, time);
+      subscription_topic_statistics_->handle_message(message_info.get_rmw_message_info(), time);
     }
   }
 
@@ -325,8 +324,20 @@ public:
     const std::shared_ptr<rclcpp::SerializedMessage> & serialized_message,
     const rclcpp::MessageInfo & message_info) override
   {
-    // TODO(wjwwood): enable topic statistics for serialized messages
+    std::chrono::time_point<std::chrono::system_clock> now;
+    if (subscription_topic_statistics_) {
+      // get current time before executing callback to
+      // exclude callback duration from topic statistics result.
+      now = std::chrono::system_clock::now();
+    }
+
     any_callback_.dispatch(serialized_message, message_info);
+
+    if (subscription_topic_statistics_) {
+      const auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(now);
+      const auto time = rclcpp::Time(nanos.time_since_epoch().count());
+      subscription_topic_statistics_->handle_message(message_info.get_rmw_message_info(), time);
+    }
   }
 
   void
@@ -357,7 +368,7 @@ public:
     if (subscription_topic_statistics_) {
       const auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(now);
       const auto time = rclcpp::Time(nanos.time_since_epoch().count());
-      subscription_topic_statistics_->handle_message(*typed_message, time);
+      subscription_topic_statistics_->handle_message(message_info.get_rmw_message_info(), time);
     }
   }
 

rclcpp/include/rclcpp/subscription_base.hpp

@@ -260,13 +260,13 @@ public:
   bool
   is_serialized() const;
 
-  /// Return the type of the subscription.
+  /// Return the delivered message kind.
   /**
    * \return `DeliveredMessageKind`, which adjusts how messages are received and delivered.
    */
   RCLCPP_PUBLIC
   DeliveredMessageKind
-  get_subscription_type() const;
+  get_delivered_message_kind() const;
 
   /// Get matching publisher count.
   /** \return The number of publishers on this topic. */
@@ -645,6 +645,14 @@ protected:
   rclcpp::node_interfaces::NodeBaseInterface * const node_base_;
 
   std::shared_ptr<rcl_node_t> node_handle_;
+
+  std::recursive_mutex callback_mutex_;
+  // It is important to declare on_new_message_callback_ before
+  // subscription_handle_, so on destruction the subscription is
+  // destroyed first. Otherwise, the rmw subscription callback
+  // would point briefly to a destroyed function.
+  std::function<void(size_t)> on_new_message_callback_{nullptr};
+  // Declare subscription_handle_ after callback
   std::shared_ptr<rcl_subscription_t> subscription_handle_;
   std::shared_ptr<rcl_subscription_t> intra_process_subscription_handle_;
   rclcpp::Logger node_logger_;
@@ -663,15 +671,12 @@ private:
   RCLCPP_DISABLE_COPY(SubscriptionBase)
 
   rosidl_message_type_support_t type_support_;
-  DeliveredMessageKind delivered_message_type_;
+  DeliveredMessageKind delivered_message_kind_;
 
   std::atomic<bool> subscription_in_use_by_wait_set_{false};
   std::atomic<bool> intra_process_subscription_waitable_in_use_by_wait_set_{false};
   std::unordered_map<rclcpp::EventHandlerBase *,
     std::atomic<bool>> qos_events_in_use_by_wait_set_;
-
-  std::recursive_mutex callback_mutex_;
-  std::function<void(size_t)> on_new_message_callback_{nullptr};
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/subscription_factory.hpp

@@ -75,15 +75,14 @@ template<
   typename CallbackT,
   typename AllocatorT,
   typename SubscriptionT = rclcpp::Subscription<MessageT, AllocatorT>,
-  typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType,
-  typename ROSMessageType = typename SubscriptionT::ROSMessageType
+  typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType
 >
 SubscriptionFactory
 create_subscription_factory(
   CallbackT && callback,
   const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
   typename MessageMemoryStrategyT::SharedPtr msg_mem_strat,
-  std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<ROSMessageType>>
+  std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics>
   subscription_topic_stats = nullptr
 )
 {

rclcpp/include/rclcpp/subscription_options.hpp

@@ -77,6 +77,10 @@ struct SubscriptionOptionsBase
     // Topic statistics publication period in ms. Defaults to one second.
     // Only values greater than zero are allowed.
     std::chrono::milliseconds publish_period{std::chrono::seconds(1)};
+
+    // An optional QoS which can provide topic_statistics with a stable QoS separate from
+    // the subscription's current QoS settings which could be unstable.
+    rclcpp::QoS qos = SystemDefaultsQoS();
   };
 
   TopicStatisticsOptions topic_stats_options;

rclcpp/include/rclcpp/time.hpp

@@ -57,6 +57,10 @@ public:
   RCLCPP_PUBLIC
   Time(const Time & rhs);
 
+  /// Move constructor
+  RCLCPP_PUBLIC
+  Time(Time && rhs) noexcept;
+
   /// Time constructor
   /**
    * \param time_msg builtin_interfaces time message to copy
@@ -84,6 +88,7 @@ public:
   operator builtin_interfaces::msg::Time() const;
 
   /**
+   * Copy assignment operator
    * \throws std::runtime_error if seconds are negative
    */
   RCLCPP_PUBLIC
@@ -100,6 +105,13 @@ public:
   Time &
   operator=(const builtin_interfaces::msg::Time & time_msg);
 
+  /**
+   * Move assignment operator
+   */
+  RCLCPP_PUBLIC
+  Time &
+  operator=(Time && rhs) noexcept;
+
   /**
    * \throws std::runtime_error if the time sources are different
    */
@@ -189,7 +201,7 @@ public:
    */
   RCLCPP_PUBLIC
   static Time
-  max();
+  max(rcl_clock_type_t clock_type = RCL_SYSTEM_TIME);  // NOLINT
 
   /// Get the seconds since epoch
   /**
@@ -222,6 +234,15 @@ RCLCPP_PUBLIC
 Time
 operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs);
 
+/// Convert rcl_time_point_value_t to builtin_interfaces::msg::Time
+/**
+ * \param[in] time_point is a rcl_time_point_value_t
+ * \return the builtin_interfaces::msg::Time from the time_point
+ */
+RCLCPP_PUBLIC
+builtin_interfaces::msg::Time
+convert_rcl_time_to_sec_nanos(const rcl_time_point_value_t & time_point);
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__TIME_HPP_

rclcpp/include/rclcpp/timer.hpp

@@ -18,6 +18,7 @@
 #include <atomic>
 #include <chrono>
 #include <functional>
+#include <optional>
 #include <memory>
 #include <sstream>
 #include <thread>
@@ -43,6 +44,12 @@
 namespace rclcpp
 {
 
+struct TimerInfo
+{
+  Time expected_call_time;
+  Time actual_call_time;
+};
+
 class TimerBase
 {
 public:
@@ -53,12 +60,17 @@ public:
    * \param clock A clock to use for time and sleeping
    * \param period The interval at which the timer fires
    * \param context node context
+   * \param autostart timer state on initialization
+   *
+   * In order to activate a timer that is not started on initialization,
+   * user should call the reset() method.
    */
   RCLCPP_PUBLIC
   explicit TimerBase(
     Clock::SharedPtr clock,
     std::chrono::nanoseconds period,
-    rclcpp::Context::SharedPtr context);
+    rclcpp::Context::SharedPtr context,
+    bool autostart = true);
 
   /// TimerBase destructor
   RCLCPP_PUBLIC
@@ -96,16 +108,20 @@ public:
    * The multithreaded executor takes advantage of this to avoid scheduling
    * the callback multiple times.
    *
-   * \return `true` if the callback should be executed, `false` if the timer was canceled.
+   * \return a valid shared_ptr if the callback should be executed,
+   *         an invalid shared_ptr (nullptr) if the timer was canceled.
    */
   RCLCPP_PUBLIC
-  virtual bool
+  virtual std::shared_ptr<void>
   call() = 0;
 
   /// Call the callback function when the timer signal is emitted.
+  /**
+   * \param[in] data the pointer returned by the call function
+   */
   RCLCPP_PUBLIC
   virtual void
-  execute_callback() = 0;
+  execute_callback(const std::shared_ptr<void> & data) = 0;
 
   RCLCPP_PUBLIC
   std::shared_ptr<const rcl_timer_t>
@@ -193,16 +209,17 @@ protected:
   set_on_reset_callback(rcl_event_callback_t callback, const void * user_data);
 };
 
-
 using VoidCallbackType = std::function<void ()>;
 using TimerCallbackType = std::function<void (TimerBase &)>;
+using TimerInfoCallbackType = std::function<void (const TimerInfo &)>;
 
 /// Generic timer. Periodically executes a user-specified callback.
 template<
   typename FunctorT,
   typename std::enable_if<
     rclcpp::function_traits::same_arguments<FunctorT, VoidCallbackType>::value ||
-    rclcpp::function_traits::same_arguments<FunctorT, TimerCallbackType>::value
+    rclcpp::function_traits::same_arguments<FunctorT, TimerCallbackType>::value ||
+    rclcpp::function_traits::same_arguments<FunctorT, TimerInfoCallbackType>::value
   >::type * = nullptr
 >
 class GenericTimer : public TimerBase
@@ -216,21 +233,22 @@ public:
    * \param[in] period The interval at which the timer fires.
    * \param[in] callback User-specified callback function.
    * \param[in] context custom context to be used.
+   * \param autostart timer state on initialization
    */
   explicit GenericTimer(
     Clock::SharedPtr clock, std::chrono::nanoseconds period, FunctorT && callback,
-    rclcpp::Context::SharedPtr context
+    rclcpp::Context::SharedPtr context, bool autostart = true
   )
-  : TimerBase(clock, period, context), callback_(std::forward<FunctorT>(callback))
+  : TimerBase(clock, period, context, autostart), callback_(std::forward<FunctorT>(callback))
   {
-    TRACEPOINT(
+    TRACETOOLS_TRACEPOINT(
       rclcpp_timer_callback_added,
       static_cast<const void *>(get_timer_handle().get()),
       reinterpret_cast<const void *>(&callback_));
 #ifndef TRACETOOLS_DISABLED
-    if (TRACEPOINT_ENABLED(rclcpp_callback_register)) {
+    if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) {
       char * symbol = tracetools::get_symbol(callback_);
-      DO_TRACEPOINT(
+      TRACETOOLS_DO_TRACEPOINT(
         rclcpp_callback_register,
         reinterpret_cast<const void *>(&callback_),
         symbol);
@@ -250,28 +268,29 @@ public:
    * \sa rclcpp::TimerBase::call
    * \throws std::runtime_error if it failed to notify timer that callback will occurr
    */
-  bool
+  std::shared_ptr<void>
   call() override
   {
-    rcl_ret_t ret = rcl_timer_call(timer_handle_.get());
+    auto timer_call_info_ = std::make_shared<rcl_timer_call_info_t>();
+    rcl_ret_t ret = rcl_timer_call_with_info(timer_handle_.get(), timer_call_info_.get());
     if (ret == RCL_RET_TIMER_CANCELED) {
-      return false;
+      return nullptr;
     }
     if (ret != RCL_RET_OK) {
       throw std::runtime_error("Failed to notify timer that callback occurred");
     }
-    return true;
+    return timer_call_info_;
   }
 
   /**
    * \sa rclcpp::TimerBase::execute_callback
    */
   void
-  execute_callback() override
+  execute_callback(const std::shared_ptr<void> & data) override
   {
-    TRACEPOINT(callback_start, reinterpret_cast<const void *>(&callback_), false);
-    execute_callback_delegate<>();
-    TRACEPOINT(callback_end, reinterpret_cast<const void *>(&callback_));
+    TRACETOOLS_TRACEPOINT(callback_start, reinterpret_cast<const void *>(&callback_), false);
+    execute_callback_delegate<>(*static_cast<rcl_timer_call_info_t *>(data.get()));
+    TRACETOOLS_TRACEPOINT(callback_end, reinterpret_cast<const void *>(&callback_));
   }
 
   // void specialization
@@ -282,7 +301,7 @@ public:
     >::type * = nullptr
   >
   void
-  execute_callback_delegate()
+  execute_callback_delegate(const rcl_timer_call_info_t &)
   {
     callback_();
   }
@@ -294,11 +313,26 @@ public:
     >::type * = nullptr
   >
   void
-  execute_callback_delegate()
+  execute_callback_delegate(const rcl_timer_call_info_t &)
   {
     callback_(*this);
   }
 
+
+  template<
+    typename CallbackT = FunctorT,
+    typename std::enable_if<
+      rclcpp::function_traits::same_arguments<CallbackT, TimerInfoCallbackType>::value
+    >::type * = nullptr
+  >
+  void
+  execute_callback_delegate(const rcl_timer_call_info_t & timer_call_info)
+  {
+    const TimerInfo info{Time{timer_call_info.expected_call_time, clock_->get_clock_type()},
+      Time{timer_call_info.actual_call_time, clock_->get_clock_type()}};
+    callback_(info);
+  }
+
   /// Is the clock steady (i.e. is the time between ticks constant?)
   /** \return True if the clock used by this timer is steady. */
   bool
@@ -317,7 +351,8 @@ template<
   typename FunctorT,
   typename std::enable_if<
     rclcpp::function_traits::same_arguments<FunctorT, VoidCallbackType>::value ||
-    rclcpp::function_traits::same_arguments<FunctorT, TimerCallbackType>::value
+    rclcpp::function_traits::same_arguments<FunctorT, TimerCallbackType>::value ||
+    rclcpp::function_traits::same_arguments<FunctorT, TimerInfoCallbackType>::value
   >::type * = nullptr
 >
 class WallTimer : public GenericTimer<FunctorT>
@@ -330,13 +365,15 @@ public:
    * \param period The interval at which the timer fires
    * \param callback The callback function to execute every interval
    * \param context node context
+   * \param autostart timer state on initialization
    */
   WallTimer(
     std::chrono::nanoseconds period,
     FunctorT && callback,
-    rclcpp::Context::SharedPtr context)
+    rclcpp::Context::SharedPtr context,
+    bool autostart = true)
   : GenericTimer<FunctorT>(
-      std::make_shared<Clock>(RCL_STEADY_TIME), period, std::move(callback), context)
+      std::make_shared<Clock>(RCL_STEADY_TIME), period, std::move(callback), context, autostart)
   {}
 
 protected:

rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp

@@ -48,21 +48,12 @@ using libstatistics_collector::moving_average_statistics::StatisticData;
 /**
  * Class used to collect, measure, and publish topic statistics data. Current statistics
  * supported for subscribers are received message age and received message period.
- *
- * \tparam CallbackMessageT the subscribed message type
- */
-template<typename CallbackMessageT>
+  */
 class SubscriptionTopicStatistics
 {
-  using TopicStatsCollector =
-    libstatistics_collector::topic_statistics_collector::TopicStatisticsCollector<
-    CallbackMessageT>;
-  using ReceivedMessageAge =
-    libstatistics_collector::topic_statistics_collector::ReceivedMessageAgeCollector<
-    CallbackMessageT>;
-  using ReceivedMessagePeriod =
-    libstatistics_collector::topic_statistics_collector::ReceivedMessagePeriodCollector<
-    CallbackMessageT>;
+  using TopicStatsCollector = libstatistics_collector::TopicStatisticsCollector;
+  using ReceivedMessageAge = libstatistics_collector::ReceivedMessageAgeCollector;
+  using ReceivedMessagePeriod = libstatistics_collector::ReceivedMessagePeriodCollector;
 
 public:
   /// Construct a SubscriptionTopicStatistics object.
@@ -101,16 +92,16 @@ public:
   /**
    * This method acquires a lock to prevent race conditions to collectors list.
    *
-   * \param received_message the message received by the subscription
+   * \param message_info the message info corresponding to the received message
    * \param now_nanoseconds current time in nanoseconds
    */
   virtual void handle_message(
-    const CallbackMessageT & received_message,
+    const rmw_message_info_t & message_info,
     const rclcpp::Time now_nanoseconds) const
   {
     std::lock_guard<std::mutex> lock(mutex_);
     for (const auto & collector : subscriber_statistics_collectors_) {
-      collector->OnMessageReceived(received_message, now_nanoseconds.nanoseconds());
+      collector->OnMessageReceived(message_info, now_nanoseconds.nanoseconds());
     }
   }
 

rclcpp/include/rclcpp/typesupport_helpers.hpp

@@ -22,6 +22,7 @@
 
 #include "rcpputils/shared_library.hpp"
 #include "rosidl_runtime_cpp/message_type_support_decl.hpp"
+#include "rosidl_runtime_cpp/service_type_support_decl.hpp"
 
 #include "rclcpp/visibility_control.hpp"
 
@@ -40,11 +41,15 @@ get_typesupport_library(const std::string & type, const std::string & typesuppor
 /// Extract the type support handle from the library.
 /**
  * The library needs to match the topic type. The shared library must stay loaded for the lifetime of the result.
+ *
+ * \deprecated Use get_message_typesupport_handle() instead
+ *
  * \param[in] type The topic type, e.g. "std_msgs/msg/String"
  * \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp"
  * \param[in] library The shared type support library
  * \return A type support handle
  */
+[[deprecated("Use `get_message_typesupport_handle` instead")]]
 RCLCPP_PUBLIC
 const rosidl_message_type_support_t *
 get_typesupport_handle(
@@ -52,6 +57,40 @@ get_typesupport_handle(
   const std::string & typesupport_identifier,
   rcpputils::SharedLibrary & library);
 
+/// Extract the message type support handle from the library.
+/**
+ * The library needs to match the topic type. The shared library must stay loaded for the lifetime of the result.
+ *
+ * \param[in] type The topic type, e.g. "std_msgs/msg/String"
+ * \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp"
+ * \param[in] library The shared type support library
+ * \throws std::runtime_error if the symbol of type not found in the library.
+ * \return A message type support handle
+ */
+RCLCPP_PUBLIC
+const rosidl_message_type_support_t *
+get_message_typesupport_handle(
+  const std::string & type,
+  const std::string & typesupport_identifier,
+  rcpputils::SharedLibrary & library);
+
+/// Extract the service type support handle from the library.
+/**
+ * The library needs to match the topic type. The shared library must stay loaded for the lifetime of the result.
+ *
+ * \param[in] type The service type, e.g. "std_srvs/srv/Empty"
+ * \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp"
+ * \param[in] library The shared type support library
+ * \throws std::runtime_error if the symbol of type not found in the library.
+ * \return A service type support handle
+ */
+RCLCPP_PUBLIC
+const rosidl_service_type_support_t *
+get_service_typesupport_handle(
+  const std::string & type,
+  const std::string & typesupport_identifier,
+  rcpputils::SharedLibrary & library);
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__TYPESUPPORT_HELPERS_HPP_

rclcpp/include/rclcpp/wait_result.hpp

@@ -17,13 +17,21 @@
 
 #include <cassert>
 #include <functional>
+#include <iostream>
+#include <memory>
 #include <stdexcept>
+#include <utility>
 
 #include "rcl/wait.h"
 
 #include "rclcpp/macros.hpp"
 #include "rclcpp/wait_result_kind.hpp"
 
+#include "rclcpp/client.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/timer.hpp"
+
 namespace rclcpp
 {
 
@@ -134,6 +142,151 @@ public:
     }
   }
 
+  /// Get the next ready timer and its index in the wait result, but do not clear it.
+  /**
+   * The returned timer is not cleared automatically, as it the case with the
+   * other next_ready_*()-like functions.
+   * Instead, this function returns the timer and the index that identifies it
+   * in the wait result, so that it can be cleared (marked as taken or used)
+   * in a separate step with clear_timer_with_index().
+   * This is necessary in some multi-threaded executor implementations.
+   *
+   * If the timer is not cleared using the index, subsequent calls to this
+   * function will return the same timer.
+   *
+   * If there is no ready timer, then nullptr will be returned and the index
+   * will be invalid and should not be used.
+   *
+   * \param[in] start_index index at which to start searching for the next ready
+   *   timer in the wait result. If the start_index is out of bounds for the
+   *   list of timers in the wait result, then {nullptr, start_index} will be
+   *   returned. Defaults to 0.
+   * \return next ready timer pointer and its index in the wait result, or
+   *   {nullptr, start_index} if none was found.
+   */
+  std::pair<std::shared_ptr<rclcpp::TimerBase>, size_t>
+  peek_next_ready_timer(size_t start_index = 0)
+  {
+    check_wait_result_dirty();
+    auto ret = std::shared_ptr<rclcpp::TimerBase>{nullptr};
+    size_t ii = start_index;
+    if (this->kind() == WaitResultKind::Ready) {
+      auto & wait_set = this->get_wait_set();
+      auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set();
+      for (; ii < wait_set.size_of_timers(); ++ii) {
+        if (rcl_wait_set.timers[ii] != nullptr) {
+          ret = wait_set.timers(ii);
+          break;
+        }
+      }
+    }
+    return {ret, ii};
+  }
+
+  /// Clear the timer at the given index.
+  /**
+   * Clearing a timer from the wait result prevents it from being returned by
+   * the peek_next_ready_timer() on subsequent calls.
+   *
+   * The index should come from the peek_next_ready_timer() function, and
+   * should only be used with this function if the timer pointer was valid.
+   *
+   * \throws std::out_of_range if the given index is out of range
+   */
+  void
+  clear_timer_with_index(size_t index)
+  {
+    auto & wait_set = this->get_wait_set();
+    auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set();
+    if (index >= wait_set.size_of_timers()) {
+      throw std::out_of_range("given timer index is out of range");
+    }
+    rcl_wait_set.timers[index] = nullptr;
+  }
+
+  /// Get the next ready subscription, clearing it from the wait result.
+  std::shared_ptr<rclcpp::SubscriptionBase>
+  next_ready_subscription()
+  {
+    check_wait_result_dirty();
+    auto ret = std::shared_ptr<rclcpp::SubscriptionBase>{nullptr};
+    if (this->kind() == WaitResultKind::Ready) {
+      auto & wait_set = this->get_wait_set();
+      auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set();
+      for (size_t ii = 0; ii < wait_set.size_of_subscriptions(); ++ii) {
+        if (rcl_wait_set.subscriptions[ii] != nullptr) {
+          ret = wait_set.subscriptions(ii);
+          rcl_wait_set.subscriptions[ii] = nullptr;
+          break;
+        }
+      }
+    }
+    return ret;
+  }
+
+  /// Get the next ready service, clearing it from the wait result.
+  std::shared_ptr<rclcpp::ServiceBase>
+  next_ready_service()
+  {
+    check_wait_result_dirty();
+    auto ret = std::shared_ptr<rclcpp::ServiceBase>{nullptr};
+    if (this->kind() == WaitResultKind::Ready) {
+      auto & wait_set = this->get_wait_set();
+      auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set();
+      for (size_t ii = 0; ii < wait_set.size_of_services(); ++ii) {
+        if (rcl_wait_set.services[ii] != nullptr) {
+          ret = wait_set.services(ii);
+          rcl_wait_set.services[ii] = nullptr;
+          break;
+        }
+      }
+    }
+    return ret;
+  }
+
+  /// Get the next ready client, clearing it from the wait result.
+  std::shared_ptr<rclcpp::ClientBase>
+  next_ready_client()
+  {
+    check_wait_result_dirty();
+    auto ret = std::shared_ptr<rclcpp::ClientBase>{nullptr};
+    if (this->kind() == WaitResultKind::Ready) {
+      auto & wait_set = this->get_wait_set();
+      auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set();
+      for (size_t ii = 0; ii < wait_set.size_of_clients(); ++ii) {
+        if (rcl_wait_set.clients[ii] != nullptr) {
+          ret = wait_set.clients(ii);
+          rcl_wait_set.clients[ii] = nullptr;
+          break;
+        }
+      }
+    }
+    return ret;
+  }
+
+  /// Get the next ready waitable, clearing it from the wait result.
+  std::shared_ptr<rclcpp::Waitable>
+  next_ready_waitable()
+  {
+    check_wait_result_dirty();
+    auto waitable = std::shared_ptr<rclcpp::Waitable>{nullptr};
+    auto data = std::shared_ptr<void>{nullptr};
+
+    if (this->kind() == WaitResultKind::Ready) {
+      auto & wait_set = this->get_wait_set();
+      auto & rcl_wait_set = wait_set.get_rcl_wait_set();
+      while (next_waitable_index_ < wait_set.size_of_waitables()) {
+        auto cur_waitable = wait_set.waitables(next_waitable_index_++);
+        if (cur_waitable != nullptr && cur_waitable->is_ready(rcl_wait_set)) {
+          waitable = cur_waitable;
+          break;
+        }
+      }
+    }
+
+    return waitable;
+  }
+
 private:
   RCLCPP_DISABLE_COPY(WaitResult)
 
@@ -151,12 +304,25 @@ private:
     // Should be enforced by the static factory methods on this class.
     assert(WaitResultKind::Ready == wait_result_kind);
     // Secure thread-safety (if provided) and shared ownership (if needed).
-    wait_set_pointer_->wait_result_acquire();
+    this->get_wait_set().wait_result_acquire();
   }
 
-  const WaitResultKind wait_result_kind_;
+  /// Check if the wait result is invalid because the wait set was modified.
+  void
+  check_wait_result_dirty()
+  {
+    // In the case that the wait set was modified while the result was out,
+    // we must mark the wait result as no longer valid
+    if (wait_set_pointer_ && this->get_wait_set().wait_result_dirty_) {
+      this->wait_result_kind_ = WaitResultKind::Invalid;
+    }
+  }
+
+  WaitResultKind wait_result_kind_;
 
   WaitSetT * wait_set_pointer_ = nullptr;
+
+  size_t next_waitable_index_ = 0;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/wait_result_kind.hpp

@@ -26,6 +26,7 @@ enum RCLCPP_PUBLIC WaitResultKind
   Ready,  //<! Kind used when something in the wait set was ready.
   Timeout,  //<! Kind used when the wait resulted in a timeout.
   Empty,  //<! Kind used when trying to wait on an empty wait set.
+  Invalid,  //<! Kind used when wait result has been invalidated.
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp

@@ -203,13 +203,13 @@ protected:
           needs_pruning_ = true;
           continue;
         }
-        const auto & waitable = waitable_entry.waitable;
-        subscriptions_from_waitables += waitable->get_number_of_ready_subscriptions();
-        guard_conditions_from_waitables += waitable->get_number_of_ready_guard_conditions();
-        timers_from_waitables += waitable->get_number_of_ready_timers();
-        clients_from_waitables += waitable->get_number_of_ready_clients();
-        services_from_waitables += waitable->get_number_of_ready_services();
-        events_from_waitables += waitable->get_number_of_ready_events();
+        auto & waitable = *waitable_entry.waitable;
+        subscriptions_from_waitables += waitable.get_number_of_ready_subscriptions();
+        guard_conditions_from_waitables += waitable.get_number_of_ready_guard_conditions();
+        timers_from_waitables += waitable.get_number_of_ready_timers();
+        clients_from_waitables += waitable.get_number_of_ready_clients();
+        services_from_waitables += waitable.get_number_of_ready_services();
+        events_from_waitables += waitable.get_number_of_ready_events();
       }
       rcl_ret_t ret = rcl_wait_set_resize(
         &rcl_wait_set_,
@@ -340,7 +340,6 @@ protected:
       }
     }
 
-
     // Add services.
     for (const auto & service : services) {
       if (!service) {
@@ -376,7 +375,7 @@ protected:
         needs_pruning_ = true;
         continue;
       }
-      waitable_entry.waitable->add_to_wait_set(&rcl_wait_set_);
+      waitable_entry.waitable->add_to_wait_set(rcl_wait_set_);
     }
   }
 
@@ -398,6 +397,32 @@ protected:
     needs_resize_ = true;
   }
 
+  size_t size_of_subscriptions() const {return 0;}
+  size_t size_of_timers() const {return 0;}
+  size_t size_of_clients() const {return 0;}
+  size_t size_of_services() const {return 0;}
+  size_t size_of_waitables() const {return 0;}
+
+  template<class SubscriptionsIterable>
+  typename SubscriptionsIterable::value_type
+  subscriptions(size_t) const {return nullptr;}
+
+  template<class TimersIterable>
+  typename TimersIterable::value_type
+  timers(size_t) const {return nullptr;}
+
+  template<class ClientsIterable>
+  typename ClientsIterable::value_type
+  clients(size_t) const {return nullptr;}
+
+  template<class ServicesIterable>
+  typename ServicesIterable::value_type
+  services(size_t) const {return nullptr;}
+
+  template<class WaitablesIterable>
+  typename WaitablesIterable::value_type
+  waitables(size_t) const {return nullptr;}
+
   rcl_wait_set_t rcl_wait_set_;
   rclcpp::Context::SharedPtr context_;
 

rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp

@@ -386,6 +386,8 @@ public:
         return weak_ptr.expired();
       };
     // remove guard conditions which have been deleted
+    subscriptions_.erase(
+      std::remove_if(subscriptions_.begin(), subscriptions_.end(), p), subscriptions_.end());
     guard_conditions_.erase(
       std::remove_if(guard_conditions_.begin(), guard_conditions_.end(), p),
       guard_conditions_.end());
@@ -451,6 +453,61 @@ public:
     reset_all(shared_waitables_);
   }
 
+  size_t size_of_subscriptions() const
+  {
+    return shared_subscriptions_.size();
+  }
+
+  size_t size_of_timers() const
+  {
+    return shared_timers_.size();
+  }
+
+  size_t size_of_clients() const
+  {
+    return shared_clients_.size();
+  }
+
+  size_t size_of_services() const
+  {
+    return shared_services_.size();
+  }
+
+  size_t size_of_waitables() const
+  {
+    return shared_waitables_.size();
+  }
+
+  std::shared_ptr<rclcpp::SubscriptionBase>
+  subscriptions(size_t ii) const
+  {
+    return shared_subscriptions_[ii].subscription;
+  }
+
+  std::shared_ptr<rclcpp::TimerBase>
+  timers(size_t ii) const
+  {
+    return shared_timers_[ii];
+  }
+
+  std::shared_ptr<rclcpp::ClientBase>
+  clients(size_t ii) const
+  {
+    return shared_clients_[ii];
+  }
+
+  std::shared_ptr<rclcpp::ServiceBase>
+  services(size_t ii) const
+  {
+    return shared_services_[ii];
+  }
+
+  std::shared_ptr<rclcpp::Waitable>
+  waitables(size_t ii) const
+  {
+    return shared_waitables_[ii].waitable;
+  }
+
   size_t ownership_reference_counter_ = 0;
 
   SequenceOfWeakSubscriptions subscriptions_;

rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp

@@ -188,6 +188,61 @@ public:
     // Explicitly do nothing.
   }
 
+  size_t size_of_subscriptions() const
+  {
+    return subscriptions_.size();
+  }
+
+  size_t size_of_timers() const
+  {
+    return timers_.size();
+  }
+
+  size_t size_of_clients() const
+  {
+    return clients_.size();
+  }
+
+  size_t size_of_services() const
+  {
+    return services_.size();
+  }
+
+  size_t size_of_waitables() const
+  {
+    return waitables_.size();
+  }
+
+  typename ArrayOfSubscriptions::value_type
+  subscriptions(size_t ii) const
+  {
+    return subscriptions_[ii];
+  }
+
+  typename ArrayOfTimers::value_type
+  timers(size_t ii) const
+  {
+    return timers_[ii];
+  }
+
+  typename ArrayOfClients::value_type
+  clients(size_t ii) const
+  {
+    return clients_[ii];
+  }
+
+  typename ArrayOfServices::value_type
+  services(size_t ii) const
+  {
+    return services_[ii];
+  }
+
+  typename ArrayOfWaitables::value_type
+  waitables(size_t ii) const
+  {
+    return waitables_[ii];
+  }
+
   const ArrayOfSubscriptions subscriptions_;
   const ArrayOfGuardConditions guard_conditions_;
   const ArrayOfTimers timers_;

rclcpp/include/rclcpp/wait_set_template.hpp

@@ -153,6 +153,7 @@ public:
             throw std::runtime_error("subscription already associated with a wait set");
           }
           this->storage_add_subscription(std::move(local_subscription));
+          if (this->wait_result_holding_) {this->wait_result_dirty_ = true;}
         }
         if (mask.include_events) {
           for (auto key_event_pair : inner_subscription->get_event_handlers()) {
@@ -164,6 +165,7 @@ public:
               throw std::runtime_error("subscription event already associated with a wait set");
             }
             this->storage_add_waitable(std::move(event), std::move(local_subscription));
+            if (this->wait_result_holding_) {this->wait_result_dirty_ = true;}
           }
         }
         if (mask.include_intra_process_waitable) {
@@ -180,6 +182,7 @@ public:
             this->storage_add_waitable(
               std::move(inner_subscription->get_intra_process_waitable()),
               std::move(local_subscription));
+            if (this->wait_result_holding_) {this->wait_result_dirty_ = true;}
           }
         }
       });
@@ -224,6 +227,7 @@ public:
           auto local_subscription = inner_subscription;
           local_subscription->exchange_in_use_by_wait_set_state(local_subscription.get(), false);
           this->storage_remove_subscription(std::move(local_subscription));
+          if (this->wait_result_holding_) {this->wait_result_dirty_ = true;}
         }
         if (mask.include_events) {
           for (auto key_event_pair : inner_subscription->get_event_handlers()) {
@@ -231,6 +235,7 @@ public:
             auto local_subscription = inner_subscription;
             local_subscription->exchange_in_use_by_wait_set_state(event.get(), false);
             this->storage_remove_waitable(std::move(event));
+            if (this->wait_result_holding_) {this->wait_result_dirty_ = true;}
           }
         }
         if (mask.include_intra_process_waitable) {
@@ -239,6 +244,7 @@ public:
             // This is the case when intra process is enabled for the subscription.
             inner_subscription->exchange_in_use_by_wait_set_state(local_waitable.get(), false);
             this->storage_remove_waitable(std::move(local_waitable));
+            if (this->wait_result_holding_) {this->wait_result_dirty_ = true;}
           }
         }
       });
@@ -289,6 +295,7 @@ public:
         // fixed sized storage policies.
         // It will throw if the guard condition has already been added.
         this->storage_add_guard_condition(std::move(inner_guard_condition));
+        if (this->wait_result_holding_) {this->wait_result_dirty_ = true;}
       });
   }
 
@@ -326,6 +333,7 @@ public:
         // fixed sized storage policies.
         // It will throw if the guard condition is not in the wait set.
         this->storage_remove_guard_condition(std::move(inner_guard_condition));
+        if (this->wait_result_holding_) {this->wait_result_dirty_ = true;}
       });
   }
 
@@ -357,6 +365,7 @@ public:
         // fixed sized storage policies.
         // It will throw if the timer has already been added.
         this->storage_add_timer(std::move(inner_timer));
+        if (this->wait_result_holding_) {this->wait_result_dirty_ = true;}
       });
   }
 
@@ -384,6 +393,7 @@ public:
         // fixed sized storage policies.
         // It will throw if the timer is not in the wait set.
         this->storage_remove_timer(std::move(inner_timer));
+        if (this->wait_result_holding_) {this->wait_result_dirty_ = true;}
       });
   }
 
@@ -415,6 +425,7 @@ public:
         // fixed sized storage policies.
         // It will throw if the client has already been added.
         this->storage_add_client(std::move(inner_client));
+        if (this->wait_result_holding_) {this->wait_result_dirty_ = true;}
       });
   }
 
@@ -442,6 +453,7 @@ public:
         // fixed sized storage policies.
         // It will throw if the client is not in the wait set.
         this->storage_remove_client(std::move(inner_client));
+        if (this->wait_result_holding_) {this->wait_result_dirty_ = true;}
       });
   }
 
@@ -473,6 +485,7 @@ public:
         // fixed sized storage policies.
         // It will throw if the service has already been added.
         this->storage_add_service(std::move(inner_service));
+        if (this->wait_result_holding_) {this->wait_result_dirty_ = true;}
       });
   }
 
@@ -500,6 +513,7 @@ public:
         // fixed sized storage policies.
         // It will throw if the service is not in the wait set.
         this->storage_remove_service(std::move(inner_service));
+        if (this->wait_result_holding_) {this->wait_result_dirty_ = true;}
       });
   }
 
@@ -551,6 +565,7 @@ public:
         // fixed sized storage policies.
         // It will throw if the waitable has already been added.
         this->storage_add_waitable(std::move(inner_waitable), std::move(associated_entity));
+        if (this->wait_result_holding_) {this->wait_result_dirty_ = true;}
       });
   }
 
@@ -578,6 +593,7 @@ public:
         // fixed sized storage policies.
         // It will throw if the waitable is not in the wait set.
         this->storage_remove_waitable(std::move(inner_waitable));
+        if (this->wait_result_holding_) {this->wait_result_dirty_ = true;}
       });
   }
 
@@ -715,6 +731,7 @@ private:
       throw std::runtime_error("wait_result_acquire() called while already holding");
     }
     wait_result_holding_ = true;
+    wait_result_dirty_ = false;
     // this method comes from the SynchronizationPolicy
     this->sync_wait_result_acquire();
     // this method comes from the StoragePolicy
@@ -734,6 +751,7 @@ private:
       throw std::runtime_error("wait_result_release() called while not holding");
     }
     wait_result_holding_ = false;
+    wait_result_dirty_ = false;
     // this method comes from the StoragePolicy
     this->storage_release_ownerships();
     // this method comes from the SynchronizationPolicy
@@ -741,6 +759,7 @@ private:
   }
 
   bool wait_result_holding_ = false;
+  bool wait_result_dirty_ = false;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/waitable.hpp

@@ -101,6 +101,23 @@ public:
   size_t
   get_number_of_ready_guard_conditions();
 
+#ifdef __GNUC__
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Woverloaded-virtual"
+#endif
+  /// Deprecated.
+  /**
+   * \sa add_to_wait_set(rcl_wait_set_t &)
+   */
+  [[deprecated("Use add_to_wait_set(rcl_wait_set_t & wait_set) signature")]]
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_to_wait_set(rcl_wait_set_t * wait_set);
+#ifdef __GNUC__
+#pragma GCC diagnostic pop
+#endif
+
   /// Add the Waitable to a wait set.
   /**
    * \param[in] wait_set A handle to the wait set to add the Waitable to.
@@ -109,7 +126,24 @@ public:
   RCLCPP_PUBLIC
   virtual
   void
-  add_to_wait_set(rcl_wait_set_t * wait_set) = 0;
+  add_to_wait_set(rcl_wait_set_t & wait_set);
+
+#ifdef __GNUC__
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Woverloaded-virtual"
+#endif
+  /// Deprecated.
+  /**
+   * \sa is_ready(const rcl_wait_set_t &)
+   */
+  [[deprecated("Use is_ready(const rcl_wait_set_t & wait_set) signature")]]
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  is_ready(rcl_wait_set_t * wait_set);
+#ifdef __GNUC__
+#pragma GCC diagnostic pop
+#endif
 
   /// Check if the Waitable is ready.
   /**
@@ -124,7 +158,7 @@ public:
   RCLCPP_PUBLIC
   virtual
   bool
-  is_ready(rcl_wait_set_t * wait_set) = 0;
+  is_ready(const rcl_wait_set_t & wait_set);
 
   /// Take the data so that it can be consumed with `execute`.
   /**
@@ -178,6 +212,23 @@ public:
   std::shared_ptr<void>
   take_data_by_entity_id(size_t id);
 
+#ifdef __GNUC__
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Woverloaded-virtual"
+#endif
+  /// Deprecated.
+  /**
+   * \sa execute(const std::shared_ptr<void> &)
+   */
+  [[deprecated("Use execute(const std::shared_ptr<void> & data) signature")]]
+  RCLCPP_PUBLIC
+  virtual
+  void
+  execute(std::shared_ptr<void> & data);
+#ifdef __GNUC__
+#pragma GCC diagnostic pop
+#endif
+
   /// Execute data that is passed in.
   /**
    * Before calling this method, the Waitable should be added to a wait set,
@@ -203,7 +254,7 @@ public:
   RCLCPP_PUBLIC
   virtual
   void
-  execute(std::shared_ptr<void> & data) = 0;
+  execute(const std::shared_ptr<void> & data);
 
   /// Exchange the "in use by wait set" state for this timer.
   /**

rclcpp/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp</name>
-  <version>20.0.0</version>
+  <version>28.1.0</version>
   <description>The ROS client library in C++.</description>
 
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
@@ -22,6 +22,7 @@
   <build_depend>builtin_interfaces</build_depend>
   <build_depend>rcl_interfaces</build_depend>
   <build_depend>rosgraph_msgs</build_depend>
+  <build_depend>rosidl_runtime_c</build_depend>
   <build_depend>rosidl_runtime_cpp</build_depend>
   <build_depend>rosidl_typesupport_c</build_depend>
   <build_depend>rosidl_typesupport_cpp</build_depend>
@@ -29,12 +30,14 @@
   <build_export_depend>builtin_interfaces</build_export_depend>
   <build_export_depend>rcl_interfaces</build_export_depend>
   <build_export_depend>rosgraph_msgs</build_export_depend>
+  <build_export_depend>rosidl_runtime_c</build_export_depend>
   <build_export_depend>rosidl_runtime_cpp</build_export_depend>
   <build_export_depend>rosidl_typesupport_c</build_export_depend>
   <build_export_depend>rosidl_typesupport_cpp</build_export_depend>
 
   <depend>libstatistics_collector</depend>
   <depend>rcl</depend>
+  <depend>rcl_logging_interface</depend>
   <depend>rcl_yaml_param_parser</depend>
   <depend>rcpputils</depend>
   <depend>rcutils</depend>

rclcpp/src/rclcpp/any_executable.cpp

@@ -16,7 +16,6 @@
 
 using rclcpp::AnyExecutable;
 
-RCLCPP_PUBLIC
 AnyExecutable::AnyExecutable()
 : subscription(nullptr),
   timer(nullptr),
@@ -24,8 +23,7 @@ AnyExecutable::AnyExecutable()
   client(nullptr),
   waitable(nullptr),
   callback_group(nullptr),
-  node_base(nullptr),
-  data(nullptr)
+  node_base(nullptr)
 {}
 
 AnyExecutable::~AnyExecutable()

rclcpp/src/rclcpp/callback_group.cpp

@@ -31,12 +31,12 @@ using rclcpp::CallbackGroupType;
 
 CallbackGroup::CallbackGroup(
   CallbackGroupType group_type,
-  std::function<rclcpp::Context::SharedPtr(void)> get_context,
+  rclcpp::Context::WeakPtr context,
   bool automatically_add_to_executor_with_node)
 : type_(group_type), associated_with_executor_(false),
   can_be_taken_from_(true),
   automatically_add_to_executor_with_node_(automatically_add_to_executor_with_node),
-  get_context_(get_context)
+  context_(context)
 {}
 
 CallbackGroup::~CallbackGroup()
@@ -124,40 +124,12 @@ CallbackGroup::automatically_add_to_executor_with_node() const
   return automatically_add_to_executor_with_node_;
 }
 
-// \TODO(mjcarroll) Deprecated, remove on tock
-rclcpp::GuardCondition::SharedPtr
-CallbackGroup::get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr)
-{
-  std::lock_guard<std::recursive_mutex> lock(notify_guard_condition_mutex_);
-  if (notify_guard_condition_ && context_ptr != notify_guard_condition_->get_context()) {
-    if (associated_with_executor_) {
-      trigger_notify_guard_condition();
-    }
-    notify_guard_condition_ = nullptr;
-  }
-
-  if (!notify_guard_condition_) {
-    notify_guard_condition_ = std::make_shared<rclcpp::GuardCondition>(context_ptr);
-  }
-
-  return notify_guard_condition_;
-}
-
 rclcpp::GuardCondition::SharedPtr
 CallbackGroup::get_notify_guard_condition()
 {
   std::lock_guard<std::recursive_mutex> lock(notify_guard_condition_mutex_);
-  if (!this->get_context_) {
-    throw std::runtime_error("Callback group was created without context and not passed context");
-  }
-  auto context_ptr = this->get_context_();
+  rclcpp::Context::SharedPtr context_ptr = context_.lock();
   if (context_ptr && context_ptr->is_valid()) {
-    if (notify_guard_condition_ && context_ptr != notify_guard_condition_->get_context()) {
-      if (associated_with_executor_) {
-        trigger_notify_guard_condition();
-      }
-      notify_guard_condition_ = nullptr;
-    }
     if (!notify_guard_condition_) {
       notify_guard_condition_ = std::make_shared<rclcpp::GuardCondition>(context_ptr);
     }

rclcpp/src/rclcpp/client.cpp

@@ -125,7 +125,6 @@ bool
 ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
 {
   auto start = std::chrono::steady_clock::now();
-  // make an event to reuse, rather than create a new one each time
   auto node_ptr = node_graph_.lock();
   if (!node_ptr) {
     throw InvalidNodeError();
@@ -138,6 +137,7 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
     // check was non-blocking, return immediately
     return false;
   }
+  // make an event to reuse, rather than create a new one each time
   auto event = node_ptr->get_graph_event();
   // update the time even on the first loop to account for time spent in the first call
   // to this->server_is_ready()

rclcpp/src/rclcpp/clock.cpp

@@ -49,6 +49,10 @@ public:
 
   rcl_clock_t rcl_clock_;
   rcl_allocator_t allocator_;
+  bool stop_sleeping_ = false;
+  bool shutdown_ = false;
+  std::condition_variable cv_;
+  std::mutex wait_mutex_;
   std::mutex clock_mutex_;
 };
 
@@ -79,8 +83,20 @@ Clock::now() const
   return now;
 }
 
+void
+Clock::cancel_sleep_or_wait()
+{
+  {
+    std::unique_lock lock(impl_->wait_mutex_);
+    impl_->stop_sleeping_ = true;
+  }
+  impl_->cv_.notify_one();
+}
+
 bool
-Clock::sleep_until(Time until, Context::SharedPtr context)
+Clock::sleep_until(
+  Time until,
+  Context::SharedPtr context)
 {
   if (!context || !context->is_valid()) {
     throw std::runtime_error("context cannot be slept with because it's invalid");
@@ -91,12 +107,14 @@ Clock::sleep_until(Time until, Context::SharedPtr context)
   }
   bool time_source_changed = false;
 
-  std::condition_variable cv;
-
   // Wake this thread if the context is shutdown
   rclcpp::OnShutdownCallbackHandle shutdown_cb_handle = context->add_on_shutdown_callback(
-    [&cv]() {
-      cv.notify_one();
+    [this]() {
+      {
+        std::unique_lock lock(impl_->wait_mutex_);
+        impl_->shutdown_ = true;
+      }
+      impl_->cv_.notify_one();
     });
   // No longer need the shutdown callback when this function exits
   auto callback_remover = rcpputils::scope_exit(
@@ -112,22 +130,24 @@ Clock::sleep_until(Time until, Context::SharedPtr context)
     const std::chrono::steady_clock::time_point chrono_until =
       chrono_entry + std::chrono::nanoseconds(delta_t.nanoseconds());
 
-    // loop over spurious wakeups but notice shutdown
-    std::unique_lock lock(impl_->clock_mutex_);
-    while (now() < until && context->is_valid()) {
-      cv.wait_until(lock, chrono_until);
+    // loop over spurious wakeups but notice shutdown or stop of sleep
+    std::unique_lock lock(impl_->wait_mutex_);
+    while (now() < until && !impl_->stop_sleeping_ && !impl_->shutdown_ && context->is_valid()) {
+      impl_->cv_.wait_until(lock, chrono_until);
     }
+    impl_->stop_sleeping_ = false;
   } else if (this_clock_type == RCL_SYSTEM_TIME) {
     auto system_time = std::chrono::system_clock::time_point(
       // Cast because system clock resolution is too big for nanoseconds on some systems
       std::chrono::duration_cast<std::chrono::system_clock::duration>(
         std::chrono::nanoseconds(until.nanoseconds())));
 
-    // loop over spurious wakeups but notice shutdown
-    std::unique_lock lock(impl_->clock_mutex_);
-    while (now() < until && context->is_valid()) {
-      cv.wait_until(lock, system_time);
+    // loop over spurious wakeups but notice shutdown or stop of sleep
+    std::unique_lock lock(impl_->wait_mutex_);
+    while (now() < until && !impl_->stop_sleeping_ && !impl_->shutdown_ && context->is_valid()) {
+      impl_->cv_.wait_until(lock, system_time);
     }
+    impl_->stop_sleeping_ = false;
   } else if (this_clock_type == RCL_ROS_TIME) {
     // Install jump handler for any amount of time change, for two purposes:
     // - if ROS time is active, check if time reached on each new clock sample
@@ -139,11 +159,12 @@ Clock::sleep_until(Time until, Context::SharedPtr context)
     threshold.min_forward.nanoseconds = 1;
     auto clock_handler = create_jump_callback(
       nullptr,
-      [&cv, &time_source_changed](const rcl_time_jump_t & jump) {
+      [this, &time_source_changed](const rcl_time_jump_t & jump) {
         if (jump.clock_change != RCL_ROS_TIME_NO_CHANGE) {
+          std::lock_guard<std::mutex> lk(impl_->wait_mutex_);
           time_source_changed = true;
         }
-        cv.notify_one();
+        impl_->cv_.notify_one();
       },
       threshold);
 
@@ -153,19 +174,25 @@ Clock::sleep_until(Time until, Context::SharedPtr context)
         std::chrono::duration_cast<std::chrono::system_clock::duration>(
           std::chrono::nanoseconds(until.nanoseconds())));
 
-      // loop over spurious wakeups but notice shutdown or time source change
-      std::unique_lock lock(impl_->clock_mutex_);
-      while (now() < until && context->is_valid() && !time_source_changed) {
-        cv.wait_until(lock, system_time);
+      // loop over spurious wakeups but notice shutdown, stop of sleep or time source change
+      std::unique_lock lock(impl_->wait_mutex_);
+      while (now() < until && !impl_->stop_sleeping_ && !impl_->shutdown_ && context->is_valid() &&
+        !time_source_changed)
+      {
+        impl_->cv_.wait_until(lock, system_time);
       }
+      impl_->stop_sleeping_ = false;
     } else {
       // RCL_ROS_TIME with ros_time_is_active.
       // Just wait without "until" because installed
       // jump callbacks wake the cv on every new sample.
-      std::unique_lock lock(impl_->clock_mutex_);
-      while (now() < until && context->is_valid() && !time_source_changed) {
-        cv.wait(lock);
+      std::unique_lock lock(impl_->wait_mutex_);
+      while (now() < until && !impl_->stop_sleeping_ && !impl_->shutdown_ && context->is_valid() &&
+        !time_source_changed)
+      {
+        impl_->cv_.wait(lock);
       }
+      impl_->stop_sleeping_ = false;
     }
   }
 

rclcpp/src/rclcpp/context.cpp

@@ -462,7 +462,7 @@ template<Context::ShutdownType shutdown_type>
 std::vector<rclcpp::Context::ShutdownCallback>
 Context::get_shutdown_callback() const
 {
-  const auto get_callback_vector = [this](auto & mutex, auto & callback_set) {
+  const auto get_callback_vector = [](auto & mutex, auto & callback_set) {
       const std::lock_guard<std::mutex> lock(mutex);
       std::vector<rclcpp::Context::ShutdownCallback> callbacks;
       for (auto & callback : callback_set) {
@@ -496,7 +496,7 @@ Context::sleep_for(const std::chrono::nanoseconds & nanoseconds)
       std::unique_lock<std::mutex> lock(interrupt_mutex_);
       auto start = std::chrono::steady_clock::now();
       // this will release the lock while waiting
-      interrupt_condition_variable_.wait_for(lock, nanoseconds);
+      interrupt_condition_variable_.wait_for(lock, time_left);
       time_left -= std::chrono::steady_clock::now() - start;
     }
   } while (time_left > std::chrono::nanoseconds::zero() && this->is_valid());

rclcpp/src/rclcpp/create_generic_client.cpp

@@ -0,0 +1,44 @@
+// 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.
+
+#include "rclcpp/create_generic_client.hpp"
+#include "rclcpp/generic_client.hpp"
+
+namespace rclcpp
+{
+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::CallbackGroup::SharedPtr group)
+{
+  rcl_client_options_t options = rcl_client_get_default_options();
+  options.qos = qos.get_rmw_qos_profile();
+
+  auto cli = rclcpp::GenericClient::make_shared(
+    node_base.get(),
+    node_graph,
+    service_name,
+    service_type,
+    options);
+
+  auto cli_base_ptr = std::dynamic_pointer_cast<rclcpp::ClientBase>(cli);
+  node_services->add_client(cli_base_ptr, group);
+  return cli;
+}
+}  // namespace rclcpp

rclcpp/src/rclcpp/detail/resolve_intra_process_buffer_type.cpp

@@ -0,0 +1,37 @@
+// 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.
+
+#include <rclcpp/detail/resolve_intra_process_buffer_type.hpp>
+
+namespace rclcpp
+{
+
+namespace detail
+{
+rclcpp::IntraProcessBufferType
+resolve_intra_process_buffer_type(
+  const rclcpp::IntraProcessBufferType buffer_type)
+{
+  if (buffer_type == IntraProcessBufferType::CallbackDefault) {
+    throw std::invalid_argument(
+            "IntraProcessBufferType::CallbackDefault is not allowed "
+            "when there is no callback function");
+  }
+
+  return buffer_type;
+}
+
+}  // namespace detail
+
+}  // namespace rclcpp

rclcpp/src/rclcpp/duration.cpp

@@ -28,6 +28,8 @@
 
 #include "rcutils/logging_macros.h"
 
+#include "rclcpp/utilities.hpp"
+
 namespace rclcpp
 {
 
@@ -316,4 +318,50 @@ Duration::from_nanoseconds(rcl_duration_value_t nanoseconds)
   return ret;
 }
 
+builtin_interfaces::msg::Time
+operator+(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs)
+{
+  if (lhs.sec < 0) {
+    throw std::runtime_error("message time is negative");
+  }
+
+  rcl_time_point_value_t rcl_time;
+  rcl_time = RCL_S_TO_NS(static_cast<int64_t>(lhs.sec));
+  rcl_time += lhs.nanosec;
+
+  if (rclcpp::add_will_overflow(rcl_time, rhs.nanoseconds())) {
+    throw std::overflow_error("addition leads to int64_t overflow");
+  }
+  if (rclcpp::add_will_underflow(rcl_time, rhs.nanoseconds())) {
+    throw std::underflow_error("addition leads to int64_t underflow");
+  }
+
+  rcl_time += rhs.nanoseconds();
+
+  return convert_rcl_time_to_sec_nanos(rcl_time);
+}
+
+builtin_interfaces::msg::Time
+operator-(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs)
+{
+  if (lhs.sec < 0) {
+    throw std::runtime_error("message time is negative");
+  }
+
+  rcl_time_point_value_t rcl_time;
+  rcl_time = RCL_S_TO_NS(static_cast<int64_t>(lhs.sec));
+  rcl_time += lhs.nanosec;
+
+  if (rclcpp::sub_will_overflow(rcl_time, rhs.nanoseconds())) {
+    throw std::overflow_error("addition leads to int64_t overflow");
+  }
+  if (rclcpp::sub_will_underflow(rcl_time, rhs.nanoseconds())) {
+    throw std::underflow_error("addition leads to int64_t underflow");
+  }
+
+  rcl_time -= rhs.nanoseconds();
+
+  return convert_rcl_time_to_sec_nanos(rcl_time);
+}
+
 }  // namespace rclcpp

rclcpp/src/rclcpp/event_handler.cpp

@@ -15,6 +15,7 @@
 #include <stdexcept>
 #include <string>
 
+#include "rcl/error_handling.h"
 #include "rcl/event.h"
 
 #include "rclcpp/event_handler.hpp"
@@ -39,13 +40,6 @@ UnsupportedEventTypeException::UnsupportedEventTypeException(
 
 EventHandlerBase::~EventHandlerBase()
 {
-  // Since the rmw event listener holds a reference to
-  // this 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();
-
   if (rcl_event_fini(&event_handle_) != RCL_RET_OK) {
     RCUTILS_LOG_ERROR_NAMED(
       "rclcpp",
@@ -63,9 +57,9 @@ EventHandlerBase::get_number_of_ready_events()
 
 /// Add the Waitable to a wait set.
 void
-EventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set)
+EventHandlerBase::add_to_wait_set(rcl_wait_set_t & wait_set)
 {
-  rcl_ret_t ret = rcl_wait_set_add_event(wait_set, &event_handle_, &wait_set_event_index_);
+  rcl_ret_t ret = rcl_wait_set_add_event(&wait_set, &event_handle_, &wait_set_event_index_);
   if (RCL_RET_OK != ret) {
     exceptions::throw_from_rcl_error(ret, "Couldn't add event to wait set");
   }
@@ -73,9 +67,9 @@ EventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set)
 
 /// Check if the Waitable is ready.
 bool
-EventHandlerBase::is_ready(rcl_wait_set_t * wait_set)
+EventHandlerBase::is_ready(const rcl_wait_set_t & wait_set)
 {
-  return wait_set->events[wait_set_event_index_] == &event_handle_;
+  return wait_set.events[wait_set_event_index_] == &event_handle_;
 }
 
 void

rclcpp/src/rclcpp/executor.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <algorithm>
+#include <cassert>
 #include <chrono>
 #include <iterator>
 #include <memory>
@@ -42,28 +43,36 @@
 using namespace std::chrono_literals;
 
 using rclcpp::Executor;
-class rclcpp::ExecutorImplementation {};
 
 /// Mask to indicate to the waitset to only add the subscription.
 /// The events and intraprocess waitable are already added via the callback group.
 static constexpr rclcpp::SubscriptionWaitSetMask kDefaultSubscriptionMask = {true, false, false};
 
+class rclcpp::ExecutorImplementation {};
+
+Executor::Executor(const std::shared_ptr<rclcpp::Context> & context)
+: spinning(false),
+  entities_need_rebuild_(true),
+  collector_(nullptr),
+  wait_set_({}, {}, {}, {}, {}, {}, context)
+{
+}
+
 Executor::Executor(const rclcpp::ExecutorOptions & options)
 : spinning(false),
   interrupt_guard_condition_(std::make_shared<rclcpp::GuardCondition>(options.context)),
   shutdown_guard_condition_(std::make_shared<rclcpp::GuardCondition>(options.context)),
+  context_(options.context),
   notify_waitable_(std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>(
       [this]() {
         this->entities_need_rebuild_.store(true);
       })),
+  entities_need_rebuild_(true),
   collector_(notify_waitable_),
   wait_set_({}, {}, {}, {}, {}, {}, options.context),
   current_notify_waitable_(notify_waitable_),
   impl_(std::make_unique<rclcpp::ExecutorImplementation>())
 {
-  // Store the context for later use.
-  context_ = options.context;
-
   shutdown_callback_handle_ = context_->add_on_shutdown_callback(
     [weak_gc = std::weak_ptr<rclcpp::GuardCondition>{shutdown_guard_condition_}]() {
       auto strong_gc = weak_gc.lock();
@@ -74,6 +83,8 @@ Executor::Executor(const rclcpp::ExecutorOptions & options)
 
   notify_waitable_->add_guard_condition(interrupt_guard_condition_);
   notify_waitable_->add_guard_condition(shutdown_guard_condition_);
+
+  wait_set_.add_waitable(notify_waitable_);
 }
 
 Executor::~Executor()
@@ -82,46 +93,31 @@ Executor::~Executor()
 
   notify_waitable_->remove_guard_condition(interrupt_guard_condition_);
   notify_waitable_->remove_guard_condition(shutdown_guard_condition_);
-
-  std::cout << "Executor::~Executor" << std::endl;
-
   current_collection_.timers.update(
     {}, {},
-    [this](auto timer) {
-      std::cout << "remove_timer(" << timer.get() << ")" << std::endl;
-      wait_set_.remove_timer(timer);
-    });
+    [this](auto timer) {wait_set_.remove_timer(timer);});
 
   current_collection_.subscriptions.update(
     {}, {},
     [this](auto subscription) {
-      std::cout << "remove_subscription(" << subscription.get() << ")" << std::endl;
       wait_set_.remove_subscription(subscription, kDefaultSubscriptionMask);
     });
 
   current_collection_.clients.update(
     {}, {},
-    [this](auto client) {
-      std::cout << "remove_client(" << client.get() << ")" << std::endl;
-      wait_set_.remove_client(client);});
+    [this](auto client) {wait_set_.remove_client(client);});
 
   current_collection_.services.update(
     {}, {},
-    [this](auto service) {
-      std::cout << "remove_service(" << service.get() << ")" << std::endl;
-      wait_set_.remove_service(service);});
+    [this](auto service) {wait_set_.remove_service(service);});
 
   current_collection_.guard_conditions.update(
     {}, {},
-    [this](auto guard_condition) {
-      std::cout << "remove_guard_condition(" << guard_condition.get() << ")" << std::endl;
-        wait_set_.remove_guard_condition(guard_condition);});
+    [this](auto guard_condition) {wait_set_.remove_guard_condition(guard_condition);});
 
   current_collection_.waitables.update(
     {}, {},
-    [this](auto waitable) {
-      std::cout << "remove_waitable(" << waitable.get() << ")" << std::endl;
-      wait_set_.remove_waitable(waitable);});
+    [this](auto waitable) {wait_set_.remove_waitable(waitable);});
 
   // Remove shutdown callback handle registered to Context
   if (!context_->remove_on_shutdown_callback(shutdown_callback_handle_)) {
@@ -132,6 +128,21 @@ Executor::~Executor()
   }
 }
 
+void
+Executor::trigger_entity_recollect(bool notify)
+{
+  this->entities_need_rebuild_.store(true);
+
+  if (!spinning.load() && entities_need_rebuild_.exchange(false)) {
+    std::lock_guard<std::mutex> guard(mutex_);
+    this->collect_entities();
+  }
+
+  if (notify) {
+    interrupt_guard_condition_->trigger();
+  }
+}
+
 std::vector<rclcpp::CallbackGroup::WeakPtr>
 Executor::get_all_callback_groups()
 {
@@ -162,19 +173,12 @@ Executor::add_callback_group(
   (void) node_ptr;
   this->collector_.add_callback_group(group_ptr);
 
-  if (!spinning.load()) {
-    std::lock_guard<std::mutex> guard(mutex_);
-    this->collect_entities();
-  }
-
-  if (notify) {
-    try {
-      interrupt_guard_condition_->trigger();
-    } catch (const rclcpp::exceptions::RCLError & ex) {
-      throw std::runtime_error(
-              std::string(
-                "Failed to trigger guard condition on callback group add: ") + ex.what());
-    }
+  try {
+    this->trigger_entity_recollect(notify);
+  } catch (const rclcpp::exceptions::RCLError & ex) {
+    throw std::runtime_error(
+            std::string(
+              "Failed to trigger guard condition on callback group add: ") + ex.what());
   }
 }
 
@@ -183,19 +187,12 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt
 {
   this->collector_.add_node(node_ptr);
 
-  if (!spinning.load()) {
-    std::lock_guard<std::mutex> guard(mutex_);
-    this->collect_entities();
-  }
-
-  if (notify) {
-    try {
-      interrupt_guard_condition_->trigger();
-    } catch (const rclcpp::exceptions::RCLError & ex) {
-      throw std::runtime_error(
-              std::string(
-                "Failed to trigger guard condition on node add: ") + ex.what());
-    }
+  try {
+    this->trigger_entity_recollect(notify);
+  } catch (const rclcpp::exceptions::RCLError & ex) {
+    throw std::runtime_error(
+            std::string(
+              "Failed to trigger guard condition on node add: ") + ex.what());
   }
 }
 
@@ -206,18 +203,12 @@ Executor::remove_callback_group(
 {
   this->collector_.remove_callback_group(group_ptr);
 
-  if (!spinning.load()) {
-    std::lock_guard<std::mutex> guard(mutex_);
-    this->collect_entities();
-  }
-  if (notify) {
-    try {
-      interrupt_guard_condition_->trigger();
-    } catch (const rclcpp::exceptions::RCLError & ex) {
-      throw std::runtime_error(
-              std::string(
-                "Failed to trigger guard condition on callback group remove: ") + ex.what());
-    }
+  try {
+    this->trigger_entity_recollect(notify);
+  } catch (const rclcpp::exceptions::RCLError & ex) {
+    throw std::runtime_error(
+            std::string(
+              "Failed to trigger guard condition on callback group remove: ") + ex.what());
   }
 }
 
@@ -232,19 +223,12 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node
 {
   this->collector_.remove_node(node_ptr);
 
-  if (!spinning.load()) {
-    std::lock_guard<std::mutex> guard(mutex_);
-    this->collect_entities();
-  }
-
-  if (notify) {
-    try {
-      interrupt_guard_condition_->trigger();
-    } catch (const rclcpp::exceptions::RCLError & ex) {
-      throw std::runtime_error(
-              std::string(
-                "Failed to trigger guard condition on node remove: ") + ex.what());
-    }
+  try {
+    this->trigger_entity_recollect(notify);
+  } catch (const rclcpp::exceptions::RCLError & ex) {
+    throw std::runtime_error(
+            std::string(
+              "Failed to trigger guard condition on node remove: ") + ex.what());
   }
 }
 
@@ -265,6 +249,59 @@ Executor::spin_node_once_nanoseconds(
   this->remove_node(node, false);
 }
 
+rclcpp::FutureReturnCode
+Executor::spin_until_future_complete_impl(
+  std::chrono::nanoseconds timeout,
+  const std::function<std::future_status(std::chrono::nanoseconds wait_time)> & wait_for_future)
+{
+  // 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 = wait_for_future(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;
+
+  if (spinning.exchange(true)) {
+    throw std::runtime_error("spin_until_future_complete() called while already spinning");
+  }
+  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+  while (rclcpp::ok(this->context_) && spinning.load()) {
+    // Do one item of work.
+    spin_once_impl(timeout_left);
+
+    // Check if the future is set, return SUCCESS if it is.
+    status = wait_for_future(std::chrono::seconds(0));
+    if (status == std::future_status::ready) {
+      return 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 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;
+}
+
 void
 Executor::spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node)
 {
@@ -284,6 +321,22 @@ void Executor::spin_some(std::chrono::nanoseconds max_duration)
   return this->spin_some_impl(max_duration, false);
 }
 
+void
+Executor::spin_node_all(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
+  std::chrono::nanoseconds max_duration)
+{
+  this->add_node(node, false);
+  spin_all(max_duration);
+  this->remove_node(node, false);
+}
+
+void
+Executor::spin_node_all(std::shared_ptr<rclcpp::Node> node, std::chrono::nanoseconds max_duration)
+{
+  this->spin_node_all(node->get_node_base_interface(), max_duration);
+}
+
 void Executor::spin_all(std::chrono::nanoseconds max_duration)
 {
   if (max_duration < 0ns) {
@@ -313,43 +366,25 @@ Executor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive)
   }
   RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
 
-  size_t work_in_queue = 0;
-  bool has_waited = false;
-
-  {
-    std::lock_guard<std::mutex> lock(mutex_);
-    work_in_queue = ready_executables_.size();
-  }
-  // The logic below is to guarantee that we:
-  // a) run all of the work in the queue before we spin the first time
-  // b) spin at least once
-  // c) run all of the work in the queue after we spin
-
   while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) {
-    AnyExecutable any_exec;
-    if (work_in_queue > 0) {
-      // If there is work in the queue, then execute it
-      // This covers the case that there are things left in the queue from a
-      // previous spin.
-      if (get_next_ready_executable(any_exec)) {
-        execute_any_executable(any_exec);
-      }
-    } else if (!has_waited && !work_in_queue) {
-      // Once the ready queue is empty, then we need to wait at least once.
+    if (!wait_result_.has_value()) {
       wait_for_work(std::chrono::milliseconds(0));
-      has_waited = true;
-    } else if (has_waited && !work_in_queue) {
-      // Once we have emptied the ready queue, but have already waited:
-      if (!exhaustive) {
-        // In the case of spin some, then we can exit
-        break;
-      } else {
-        // In the case of spin all, then we will allow ourselves to wait again.
-        has_waited = false;
-      }
     }
-    std::lock_guard<std::mutex> lock(mutex_);
-    work_in_queue = ready_executables_.size();
+
+    AnyExecutable any_exec;
+    if (get_next_ready_executable(any_exec)) {
+      execute_any_executable(any_exec);
+    } else {
+      // If nothing is ready, reset the result to signal we are
+      // ready to wait again
+      wait_result_.reset();
+    }
+
+    if (!wait_result_.has_value() && !exhaustive) {
+      // In the case of spin some, then we can exit
+      // In the case of spin all, then we will allow ourselves to wait again.
+      break;
+    }
   }
 }
 
@@ -391,14 +426,18 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
     return;
   }
 
+  assert(
+    (void("cannot execute an AnyExecutable without a valid callback group"),
+    any_exec.callback_group));
+
   if (any_exec.timer) {
-    TRACEPOINT(
+    TRACETOOLS_TRACEPOINT(
       rclcpp_executor_execute,
       static_cast<const void *>(any_exec.timer->get_timer_handle().get()));
-    execute_timer(any_exec.timer);
+    execute_timer(any_exec.timer, any_exec.data);
   }
   if (any_exec.subscription) {
-    TRACEPOINT(
+    TRACETOOLS_TRACEPOINT(
       rclcpp_executor_execute,
       static_cast<const void *>(any_exec.subscription->get_subscription_handle().get()));
     execute_subscription(any_exec.subscription);
@@ -410,13 +449,12 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
     execute_client(any_exec.client);
   }
   if (any_exec.waitable) {
-    any_exec.waitable->execute(any_exec.data);
+    const std::shared_ptr<void> & const_data = any_exec.data;
+    any_exec.waitable->execute(const_data);
   }
 
   // Reset the callback_group, regardless of type
-  if (any_exec.callback_group) {
-    any_exec.callback_group->can_be_taken_from().store(true);
-  }
+  any_exec.callback_group->can_be_taken_from().store(true);
 }
 
 template<typename Taker, typename Handler>
@@ -464,7 +502,7 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription)
   rclcpp::MessageInfo message_info;
   message_info.get_rmw_message_info().from_intra_process = false;
 
-  switch (subscription->get_subscription_type()) {
+  switch (subscription->get_delivered_message_kind()) {
     // Deliver ROS message
     case rclcpp::DeliveredMessageKind::ROS_MESSAGE:
       {
@@ -514,6 +552,11 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription)
             subscription->get_topic_name(),
             [&]() {return subscription->take_type_erased(message.get(), message_info);},
             [&]() {subscription->handle_message(message, message_info);});
+          // TODO(clalancette): In the case that the user is using the MessageMemoryPool,
+          // and they take a shared_ptr reference to the message in the callback, this can
+          // inadvertently return the message to the pool when the user is still using it.
+          // This is a bug that needs to be fixed in the pool, and we should probably have
+          // a custom deleter for the message that actually does the return_message().
           subscription->return_message(message);
         }
         break;
@@ -550,13 +593,12 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription)
         throw std::runtime_error("Delivered message kind is not supported");
       }
   }
-  return;
 }
 
 void
-Executor::execute_timer(rclcpp::TimerBase::SharedPtr timer)
+Executor::execute_timer(rclcpp::TimerBase::SharedPtr timer, const std::shared_ptr<void> & data_ptr)
 {
-  timer->execute_callback();
+  timer->execute_callback(data_ptr);
 }
 
 void
@@ -572,8 +614,7 @@ Executor::execute_service(rclcpp::ServiceBase::SharedPtr service)
 }
 
 void
-Executor::execute_client(
-  rclcpp::ClientBase::SharedPtr client)
+Executor::execute_client(rclcpp::ClientBase::SharedPtr client)
 {
   auto request_header = client->create_request_header();
   std::shared_ptr<void> response = client->create_response();
@@ -587,6 +628,9 @@ Executor::execute_client(
 void
 Executor::collect_entities()
 {
+  // Updating the entity collection and waitset expires any active result
+  this->wait_result_.reset();
+
   // Get the current list of available waitables from the collector.
   rclcpp::executors::ExecutorEntitiesCollection collection;
   this->collector_.update_collections();
@@ -604,29 +648,23 @@ Executor::collect_entities()
     collection.waitables.insert({notify_waitable.get(), {notify_waitable, {}}});
   }
 
-  std::cout << "current_collection.timers: " << current_collection_.timers.size() << std::endl;
-  std::cout << "next_collection.timers: " << collection.timers.size() << std::endl;
+  // We must remove expired entities here, so that we don't continue to use older entities.
+  // See https://github.com/ros2/rclcpp/issues/2180 for more information.
+  current_collection_.remove_expired_entities();
+
   // Update each of the groups of entities in the current collection, adding or removing
   // from the wait set as necessary.
   current_collection_.timers.update(
     collection.timers,
-    [this](auto timer) {
-        std::cout << "add_timer(" << timer << ")" << std::endl;
-        wait_set_.add_timer(timer);},
-    [this](auto timer) {
-        std::cout << "remove_timer(" << timer << ")" << std::endl;
-        wait_set_.remove_timer(timer);});
+    [this](auto timer) {wait_set_.add_timer(timer);},
+    [this](auto timer) {wait_set_.remove_timer(timer);});
 
-  std::cout << "current_collection.subscriptions: " << current_collection_.subscriptions.size() << std::endl;
-  std::cout << "next_collection.subscriptions: " << collection.subscriptions.size() << std::endl;
   current_collection_.subscriptions.update(
     collection.subscriptions,
     [this](auto subscription) {
-      std::cout << "add_subscription(" << subscription << ")" << std::endl;
       wait_set_.add_subscription(subscription, kDefaultSubscriptionMask);
     },
     [this](auto subscription) {
-      std::cout << "remove_subscription(" << subscription << ")" << std::endl;
       wait_set_.remove_subscription(subscription, kDefaultSubscriptionMask);
     });
 
@@ -647,54 +685,164 @@ Executor::collect_entities()
 
   current_collection_.waitables.update(
     collection.waitables,
-    [this](auto waitable) {
-      wait_set_.add_waitable(waitable);
-    },
+    [this](auto waitable) {wait_set_.add_waitable(waitable);},
     [this](auto waitable) {wait_set_.remove_waitable(waitable);});
 
-  this->entities_need_rebuild_.store(false);
+  // In the case that an entity already has an expired weak pointer
+  // before being removed from the waitset, additionally prune the waitset.
+  this->wait_set_.prune_deleted_entities();
 }
 
 void
 Executor::wait_for_work(std::chrono::nanoseconds timeout)
 {
-  TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count());
+  TRACETOOLS_TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count());
 
-  std::lock_guard<std::mutex> guard(mutex_);
-  if (current_collection_.empty() || this->entities_need_rebuild_.load()) {
-    this->collect_entities();
+  // Clear any previous wait result
+  this->wait_result_.reset();
+
+  {
+    std::lock_guard<std::mutex> guard(mutex_);
+    if (this->entities_need_rebuild_.exchange(false) || current_collection_.empty()) {
+      this->collect_entities();
+    }
   }
-
-  auto wait_result = wait_set_.wait(timeout);
-  if (wait_result.kind() == WaitResultKind::Empty) {
+  this->wait_result_.emplace(wait_set_.wait(timeout));
+  if (!this->wait_result_ || this->wait_result_->kind() == WaitResultKind::Empty) {
     RCUTILS_LOG_WARN_NAMED(
       "rclcpp",
       "empty wait set received in wait(). This should never happen.");
+  } else {
+    if (this->wait_result_->kind() == WaitResultKind::Ready && current_notify_waitable_) {
+      auto & rcl_wait_set = this->wait_result_->get_wait_set().get_rcl_wait_set();
+      if (current_notify_waitable_->is_ready(rcl_wait_set)) {
+        current_notify_waitable_->execute(current_notify_waitable_->take_data());
+      }
+    }
   }
-  rclcpp::executors::ready_executables(current_collection_, wait_result, ready_executables_);
 }
 
 bool
 Executor::get_next_ready_executable(AnyExecutable & any_executable)
 {
-  TRACEPOINT(rclcpp_executor_get_next_ready);
+  TRACETOOLS_TRACEPOINT(rclcpp_executor_get_next_ready);
 
-  std::lock_guard<std::mutex> guard(mutex_);
-  if (ready_executables_.size() == 0) {
+  bool valid_executable = false;
+
+  if (!wait_result_.has_value() || wait_result_->kind() != rclcpp::WaitResultKind::Ready) {
     return false;
   }
 
-  any_executable = ready_executables_.front();
-  ready_executables_.pop_front();
-
-  if (any_executable.callback_group &&
-    any_executable.callback_group->type() == CallbackGroupType::MutuallyExclusive)
-  {
-    assert(any_executable.callback_group->can_be_taken_from().load());
-    any_executable.callback_group->can_be_taken_from().store(false);
+  if (!valid_executable) {
+    size_t current_timer_index = 0;
+    while (true) {
+      auto [timer, timer_index] = wait_result_->peek_next_ready_timer(current_timer_index);
+      if (nullptr == timer) {
+        break;
+      }
+      current_timer_index = timer_index;
+      auto entity_iter = current_collection_.timers.find(timer->get_timer_handle().get());
+      if (entity_iter != current_collection_.timers.end()) {
+        auto callback_group = entity_iter->second.callback_group.lock();
+        if (!callback_group || !callback_group->can_be_taken_from()) {
+          current_timer_index++;
+          continue;
+        }
+        // At this point the timer is either ready for execution or was perhaps
+        // it was canceled, based on the result of call(), but either way it
+        // should not be checked again from peek_next_ready_timer(), so clear
+        // it from the wait result.
+        wait_result_->clear_timer_with_index(current_timer_index);
+        // Check that the timer should be called still, i.e. it wasn't canceled.
+        any_executable.data = timer->call();
+        if (!any_executable.data) {
+          current_timer_index++;
+          continue;
+        }
+        any_executable.timer = timer;
+        any_executable.callback_group = callback_group;
+        valid_executable = true;
+        break;
+      }
+      current_timer_index++;
+    }
   }
 
-  return true;
+  if (!valid_executable) {
+    while (auto subscription = wait_result_->next_ready_subscription()) {
+      auto entity_iter = current_collection_.subscriptions.find(
+        subscription->get_subscription_handle().get());
+      if (entity_iter != current_collection_.subscriptions.end()) {
+        auto callback_group = entity_iter->second.callback_group.lock();
+        if (!callback_group || !callback_group->can_be_taken_from()) {
+          continue;
+        }
+        any_executable.subscription = subscription;
+        any_executable.callback_group = callback_group;
+        valid_executable = true;
+        break;
+      }
+    }
+  }
+
+  if (!valid_executable) {
+    while (auto service = wait_result_->next_ready_service()) {
+      auto entity_iter = current_collection_.services.find(service->get_service_handle().get());
+      if (entity_iter != current_collection_.services.end()) {
+        auto callback_group = entity_iter->second.callback_group.lock();
+        if (!callback_group || !callback_group->can_be_taken_from()) {
+          continue;
+        }
+        any_executable.service = service;
+        any_executable.callback_group = callback_group;
+        valid_executable = true;
+        break;
+      }
+    }
+  }
+
+  if (!valid_executable) {
+    while (auto client = wait_result_->next_ready_client()) {
+      auto entity_iter = current_collection_.clients.find(client->get_client_handle().get());
+      if (entity_iter != current_collection_.clients.end()) {
+        auto callback_group = entity_iter->second.callback_group.lock();
+        if (!callback_group || !callback_group->can_be_taken_from()) {
+          continue;
+        }
+        any_executable.client = client;
+        any_executable.callback_group = callback_group;
+        valid_executable = true;
+        break;
+      }
+    }
+  }
+
+  if (!valid_executable) {
+    while (auto waitable = wait_result_->next_ready_waitable()) {
+      auto entity_iter = current_collection_.waitables.find(waitable.get());
+      if (entity_iter != current_collection_.waitables.end()) {
+        auto callback_group = entity_iter->second.callback_group.lock();
+        if (!callback_group || !callback_group->can_be_taken_from()) {
+          continue;
+        }
+        any_executable.waitable = waitable;
+        any_executable.callback_group = callback_group;
+        any_executable.data = waitable->take_data();
+        valid_executable = true;
+        break;
+      }
+    }
+  }
+
+  if (any_executable.callback_group) {
+    if (any_executable.callback_group->type() == CallbackGroupType::MutuallyExclusive) {
+      assert(any_executable.callback_group->can_be_taken_from().load());
+      any_executable.callback_group->can_be_taken_from().store(false);
+    }
+  }
+
+
+  return valid_executable;
 }
 
 bool

rclcpp/src/rclcpp/executors.cpp

@@ -14,10 +14,29 @@
 
 #include "rclcpp/executors.hpp"
 
+void
+rclcpp::spin_all(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  std::chrono::nanoseconds max_duration)
+{
+  rclcpp::ExecutorOptions options;
+  options.context = node_ptr->get_context();
+  rclcpp::executors::SingleThreadedExecutor exec(options);
+  exec.spin_node_all(node_ptr, max_duration);
+}
+
+void
+rclcpp::spin_all(rclcpp::Node::SharedPtr node_ptr, std::chrono::nanoseconds max_duration)
+{
+  rclcpp::spin_all(node_ptr->get_node_base_interface(), max_duration);
+}
+
 void
 rclcpp::spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
 {
-  rclcpp::executors::SingleThreadedExecutor exec;
+  rclcpp::ExecutorOptions options;
+  options.context = node_ptr->get_context();
+  rclcpp::executors::SingleThreadedExecutor exec(options);
   exec.spin_node_some(node_ptr);
 }
 
@@ -30,7 +49,9 @@ rclcpp::spin_some(rclcpp::Node::SharedPtr node_ptr)
 void
 rclcpp::spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
 {
-  rclcpp::executors::SingleThreadedExecutor exec;
+  rclcpp::ExecutorOptions options;
+  options.context = node_ptr->get_context();
+  rclcpp::executors::SingleThreadedExecutor exec(options);
   exec.add_node(node_ptr);
   exec.spin();
   exec.remove_node(node_ptr);

rclcpp/src/rclcpp/executors/executor_entities_collection.cpp

@@ -39,6 +39,30 @@ void ExecutorEntitiesCollection::clear()
   waitables.clear();
 }
 
+size_t ExecutorEntitiesCollection::remove_expired_entities()
+{
+  auto remove_entities = [](auto & collection) -> size_t {
+      size_t removed = 0;
+      for (auto it = collection.begin(); it != collection.end(); ) {
+        if (it->second.entity.expired()) {
+          ++removed;
+          it = collection.erase(it);
+        } else {
+          ++it;
+        }
+      }
+      return removed;
+    };
+
+  return
+    remove_entities(subscriptions) +
+    remove_entities(timers) +
+    remove_entities(guard_conditions) +
+    remove_entities(clients) +
+    remove_entities(services) +
+    remove_entities(waitables);
+}
+
 void
 build_entities_collection(
   const std::vector<rclcpp::CallbackGroup::WeakPtr> & callback_groups,
@@ -129,7 +153,7 @@ ready_executables(
         continue;
       }
       auto group_info = group_cache(entity_iter->second.callback_group);
-      if (group_info && !group_info->can_be_taken_from().load()) {
+      if (!group_info || !group_info->can_be_taken_from().load()) {
         continue;
       }
       if (!entity->call()) {
@@ -152,7 +176,7 @@ ready_executables(
         continue;
       }
       auto group_info = group_cache(entity_iter->second.callback_group);
-      if (group_info && !group_info->can_be_taken_from().load()) {
+      if (!group_info || !group_info->can_be_taken_from().load()) {
         continue;
       }
       rclcpp::AnyExecutable exec;
@@ -172,7 +196,7 @@ ready_executables(
         continue;
       }
       auto group_info = group_cache(entity_iter->second.callback_group);
-      if (group_info && !group_info->can_be_taken_from().load()) {
+      if (!group_info || !group_info->can_be_taken_from().load()) {
         continue;
       }
       rclcpp::AnyExecutable exec;
@@ -192,7 +216,7 @@ ready_executables(
         continue;
       }
       auto group_info = group_cache(entity_iter->second.callback_group);
-      if (group_info && !group_info->can_be_taken_from().load()) {
+      if (!group_info || !group_info->can_be_taken_from().load()) {
         continue;
       }
       rclcpp::AnyExecutable exec;
@@ -203,22 +227,21 @@ ready_executables(
     }
   }
 
-  for (auto & [handle, entry] : collection.waitables) {
+  for (const auto & [handle, entry] : collection.waitables) {
     auto waitable = entry.entity.lock();
     if (!waitable) {
       continue;
     }
-    if (!waitable->is_ready(&rcl_wait_set)) {
+    if (!waitable->is_ready(rcl_wait_set)) {
       continue;
     }
     auto group_info = group_cache(entry.callback_group);
-    if (group_info && !group_info->can_be_taken_from().load()) {
+    if (!group_info || !group_info->can_be_taken_from().load()) {
       continue;
     }
     rclcpp::AnyExecutable exec;
     exec.waitable = waitable;
     exec.callback_group = group_info;
-    exec.data = waitable->take_data();
     executables.push_back(exec);
     added++;
   }

rclcpp/src/rclcpp/executors/executor_entities_collector.cpp

@@ -61,6 +61,12 @@ ExecutorEntitiesCollector::~ExecutorEntitiesCollector()
     if (group_ptr) {
       group_ptr->get_associated_with_executor_atomic().store(false);
     }
+    // Disassociate the guard condition from the executor notify waitable
+    auto guard_condition_it = weak_groups_to_guard_conditions_.find(weak_group_ptr);
+    if (guard_condition_it != weak_groups_to_guard_conditions_.end()) {
+      this->notify_waitable_->remove_guard_condition(guard_condition_it->second);
+      weak_groups_to_guard_conditions_.erase(guard_condition_it);
+    }
   }
   pending_manually_added_groups_.clear();
   pending_manually_removed_groups_.clear();
@@ -144,6 +150,11 @@ ExecutorEntitiesCollector::add_callback_group(rclcpp::CallbackGroup::SharedPtr g
   }
 
   this->pending_manually_added_groups_.insert(group_ptr);
+
+  // Store callback group notify guard condition in map and add it to the notify waitable
+  auto group_guard_condition = group_ptr->get_notify_guard_condition();
+  weak_groups_to_guard_conditions_.insert({group_ptr, group_guard_condition});
+  this->notify_waitable_->add_guard_condition(group_guard_condition);
 }
 
 void
@@ -341,6 +352,13 @@ ExecutorEntitiesCollector::process_queues()
     auto group_ptr = weak_group_ptr.lock();
     if (group_ptr) {
       this->add_callback_group_to_collection(group_ptr, manually_added_groups_);
+    } else {
+      // Disassociate the guard condition from the executor notify waitable
+      auto guard_condition_it = weak_groups_to_guard_conditions_.find(weak_group_ptr);
+      if (guard_condition_it != weak_groups_to_guard_conditions_.end()) {
+        this->notify_waitable_->remove_guard_condition(guard_condition_it->second);
+        weak_groups_to_guard_conditions_.erase(guard_condition_it);
+      }
     }
   }
   pending_manually_added_groups_.clear();

rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp

@@ -27,15 +27,17 @@ ExecutorNotifyWaitable::ExecutorNotifyWaitable(std::function<void(void)> on_exec
 {
 }
 
-ExecutorNotifyWaitable::ExecutorNotifyWaitable(const ExecutorNotifyWaitable & other)
-: ExecutorNotifyWaitable(other.execute_callback_)
+ExecutorNotifyWaitable::ExecutorNotifyWaitable(ExecutorNotifyWaitable & other)
 {
+  std::lock_guard<std::mutex> lock(other.guard_condition_mutex_);
+  this->execute_callback_ = other.execute_callback_;
   this->notify_guard_conditions_ = other.notify_guard_conditions_;
 }
 
-ExecutorNotifyWaitable & ExecutorNotifyWaitable::operator=(const ExecutorNotifyWaitable & other)
+ExecutorNotifyWaitable & ExecutorNotifyWaitable::operator=(ExecutorNotifyWaitable & other)
 {
   if (this != &other) {
+    std::lock_guard<std::mutex> lock(other.guard_condition_mutex_);
     this->execute_callback_ = other.execute_callback_;
     this->notify_guard_conditions_ = other.notify_guard_conditions_;
   }
@@ -43,17 +45,20 @@ ExecutorNotifyWaitable & ExecutorNotifyWaitable::operator=(const ExecutorNotifyW
 }
 
 void
-ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set)
+ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t & wait_set)
 {
   std::lock_guard<std::mutex> lock(guard_condition_mutex_);
+
+  // Note: no guard conditions need to be re-triggered, since the guard
+  // conditions in this class are not tracking a stateful condition, but instead
+  // only serve to interrupt the wait set when new information is available to
+  // consider.
   for (auto weak_guard_condition : this->notify_guard_conditions_) {
     auto guard_condition = weak_guard_condition.lock();
     if (!guard_condition) {continue;}
 
     rcl_guard_condition_t * cond = &guard_condition->get_rcl_guard_condition();
-
-    rcl_ret_t ret = rcl_wait_set_add_guard_condition(
-      wait_set, cond, NULL);
+    rcl_ret_t ret = rcl_wait_set_add_guard_condition(&wait_set, cond, NULL);
 
     if (RCL_RET_OK != ret) {
       rclcpp::exceptions::throw_from_rcl_error(
@@ -63,21 +68,22 @@ ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set)
 }
 
 bool
-ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set)
+ExecutorNotifyWaitable::is_ready(const rcl_wait_set_t & wait_set)
 {
   std::lock_guard<std::mutex> lock(guard_condition_mutex_);
 
   bool any_ready = false;
-  for (size_t ii = 0; ii < wait_set->size_of_guard_conditions; ++ii) {
-    auto rcl_guard_condition = wait_set->guard_conditions[ii];
+  for (size_t ii = 0; ii < wait_set.size_of_guard_conditions; ++ii) {
+    const auto * rcl_guard_condition = wait_set.guard_conditions[ii];
 
     if (nullptr == rcl_guard_condition) {
       continue;
     }
-    for (auto weak_guard_condition : this->notify_guard_conditions_) {
+    for (const auto & weak_guard_condition : this->notify_guard_conditions_) {
       auto guard_condition = weak_guard_condition.lock();
       if (guard_condition && &guard_condition->get_rcl_guard_condition() == rcl_guard_condition) {
         any_ready = true;
+        break;
       }
     }
   }
@@ -85,7 +91,7 @@ ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set)
 }
 
 void
-ExecutorNotifyWaitable::execute(std::shared_ptr<void> & data)
+ExecutorNotifyWaitable::execute(const std::shared_ptr<void> & data)
 {
   (void) data;
   this->execute_callback_();

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -55,7 +55,7 @@ MultiThreadedExecutor::spin()
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin() called while already spinning");
   }
-  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false););
   std::vector<std::thread> threads;
   size_t thread_id = 0;
   {
@@ -100,8 +100,7 @@ MultiThreadedExecutor::run(size_t this_thread_number)
     execute_any_executable(any_exec);
 
     if (any_exec.callback_group &&
-      any_exec.callback_group->type() == CallbackGroupType::MutuallyExclusive &&
-      any_exec.callback_group->size() > 1)
+      any_exec.callback_group->type() == CallbackGroupType::MutuallyExclusive)
     {
       try {
         interrupt_guard_condition_->trigger();

rclcpp/src/rclcpp/executors/single_threaded_executor.cpp

@@ -31,6 +31,11 @@ SingleThreadedExecutor::spin()
     throw std::runtime_error("spin() called while already spinning");
   }
   RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+
+  // Clear any previous result and rebuild the waitset
+  this->wait_result_.reset();
+  this->entities_need_rebuild_ = true;
+
   while (rclcpp::ok(this->context_) && spinning.load()) {
     rclcpp::AnyExecutable any_executable;
     if (get_next_executable(any_executable)) {

rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp

@@ -13,7 +13,6 @@
 // limitations under the License.
 
 #include "rclcpp/executors/executor_entities_collection.hpp"
-#include "rclcpp/executors/executor_notify_waitable.hpp"
 #include "rcpputils/scope_exit.hpp"
 
 #include "rclcpp/executors/static_single_threaded_executor.hpp"
@@ -40,21 +39,7 @@ StaticSingleThreadedExecutor::spin()
   // except we need to keep the wait result to reproduce the StaticSingleThreadedExecutor
   // behavior.
   while (rclcpp::ok(this->context_) && spinning.load()) {
-    std::deque<rclcpp::AnyExecutable> to_exec;
-
-    std::lock_guard<std::mutex> guard(mutex_);
-    if (current_collection_.empty() || this->entities_need_rebuild_.load()) {
-      this->collect_entities();
-    }
-
-    auto wait_result = wait_set_.wait(std::chrono::nanoseconds(-1));
-    if (wait_result.kind() == WaitResultKind::Empty) {
-      RCUTILS_LOG_WARN_NAMED(
-        "rclcpp",
-        "empty wait set received in wait(). This should never happen.");
-      continue;
-    }
-    execute_ready_executables(current_collection_, wait_result, false);
+    this->spin_once_impl(std::chrono::nanoseconds(-1));
   }
 }
 
@@ -65,7 +50,6 @@ StaticSingleThreadedExecutor::spin_some(std::chrono::nanoseconds max_duration)
   if (std::chrono::nanoseconds(0) == max_duration) {
     max_duration = std::chrono::nanoseconds::max();
   }
-
   return this->spin_some_impl(max_duration, false);
 }
 
@@ -83,41 +67,30 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati
 {
   auto start = std::chrono::steady_clock::now();
   auto max_duration_not_elapsed = [max_duration, start]() {
-      if (std::chrono::nanoseconds(0) == max_duration) {
-        // told to spin forever if need be
-        return true;
-      } else if (std::chrono::steady_clock::now() - start < max_duration) {
-        // told to spin only for some maximum amount of time
-        return true;
-      }
-      // spun too long
-      return false;
+      const auto spin_forever = std::chrono::nanoseconds(0) == max_duration;
+      const auto cur_duration = std::chrono::steady_clock::now() - start;
+      return spin_forever || (cur_duration < max_duration);
     };
 
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin_some() called while already spinning");
   }
-  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false););
 
   while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) {
     // Get executables that are ready now
     std::lock_guard<std::mutex> guard(mutex_);
-    if (current_collection_.empty() || this->entities_need_rebuild_.load()) {
-      this->collect_entities();
-    }
 
-    auto wait_result = wait_set_.wait(std::chrono::nanoseconds(0));
-    if (wait_result.kind() == WaitResultKind::Empty) {
-      RCUTILS_LOG_WARN_NAMED(
-        "rclcpp",
-        "empty wait set received in wait(). This should never happen.");
-      continue;
-    }
-
-    // Execute ready executables
-    bool work_available = execute_ready_executables(current_collection_, wait_result, false);
-    if (!work_available || !exhaustive) {
-      break;
+    auto wait_result = this->collect_and_wait(std::chrono::nanoseconds(0));
+    if (wait_result.has_value()) {
+      // Execute ready executables
+      bool work_available = this->execute_ready_executables(
+        current_collection_,
+        wait_result.value(),
+        false);
+      if (!work_available || !exhaustive) {
+        break;
+      }
     }
   }
 }
@@ -127,23 +100,36 @@ StaticSingleThreadedExecutor::spin_once_impl(std::chrono::nanoseconds timeout)
 {
   if (rclcpp::ok(context_) && spinning.load()) {
     std::lock_guard<std::mutex> guard(mutex_);
-    if (current_collection_.empty() || this->entities_need_rebuild_.load()) {
-      this->collect_entities();
+    auto wait_result = this->collect_and_wait(timeout);
+    if (wait_result.has_value()) {
+      this->execute_ready_executables(current_collection_, wait_result.value(), true);
     }
-
-    auto wait_result = wait_set_.wait(std::chrono::nanoseconds(timeout));
-    if (wait_result.kind() == WaitResultKind::Empty) {
-      RCUTILS_LOG_WARN_NAMED(
-        "rclcpp",
-        "empty wait set received in wait(). This should never happen.");
-      return;
-    }
-
-    // Execute ready executables
-    execute_ready_executables(current_collection_, wait_result, true);
   }
 }
 
+std::optional<rclcpp::WaitResult<rclcpp::WaitSet>>
+StaticSingleThreadedExecutor::collect_and_wait(std::chrono::nanoseconds timeout)
+{
+  if (this->entities_need_rebuild_.exchange(false) || current_collection_.empty()) {
+    this->collect_entities();
+  }
+  auto wait_result = wait_set_.wait(std::chrono::nanoseconds(timeout));
+  if (wait_result.kind() == WaitResultKind::Empty) {
+    RCUTILS_LOG_WARN_NAMED(
+      "rclcpp",
+      "empty wait set received in wait(). This should never happen.");
+    return {};
+  } else {
+    if (wait_result.kind() == WaitResultKind::Ready && current_notify_waitable_) {
+      auto & rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set();
+      if (current_notify_waitable_->is_ready(rcl_wait_set)) {
+        current_notify_waitable_->execute(current_notify_waitable_->take_data());
+      }
+    }
+  }
+  return wait_result;
+}
+
 // This preserves the "scheduling semantics" of the StaticSingleThreadedExecutor
 // from the original implementation.
 bool StaticSingleThreadedExecutor::execute_ready_executables(
@@ -152,95 +138,67 @@ bool StaticSingleThreadedExecutor::execute_ready_executables(
   bool spin_once)
 {
   bool any_ready_executable = false;
-
   if (wait_result.kind() != rclcpp::WaitResultKind::Ready) {
     return any_ready_executable;
   }
 
-  auto rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set();
-
-  for (size_t ii = 0; ii < rcl_wait_set.size_of_timers; ++ii) {
-    if (nullptr == rcl_wait_set.timers[ii]) {continue;}
-    auto entity_iter = collection.timers.find(rcl_wait_set.timers[ii]);
-    if (entity_iter != collection.timers.end()) {
-      auto entity = entity_iter->second.entity.lock();
-      if (!entity) {
-        continue;
-      }
-      if (!entity->call()) {
-        continue;
-      }
-      execute_timer(entity);
-      if (spin_once) {
-        return true;
-      }
-      any_ready_executable = true;
-    }
-  }
-
-  for (size_t ii = 0; ii < rcl_wait_set.size_of_subscriptions; ++ii) {
-    if (nullptr == rcl_wait_set.subscriptions[ii]) {continue;}
-    auto entity_iter = collection.subscriptions.find(rcl_wait_set.subscriptions[ii]);
+  while (auto subscription = wait_result.next_ready_subscription()) {
+    auto entity_iter = collection.subscriptions.find(subscription->get_subscription_handle().get());
     if (entity_iter != collection.subscriptions.end()) {
-      auto entity = entity_iter->second.entity.lock();
-      if (!entity) {
-        continue;
-      }
-      execute_subscription(entity);
-      if (spin_once) {
-        return true;
-      }
+      execute_subscription(subscription);
       any_ready_executable = true;
+      if (spin_once) {return any_ready_executable;}
     }
   }
 
-  for (size_t ii = 0; ii < rcl_wait_set.size_of_services; ++ii) {
-    if (nullptr == rcl_wait_set.services[ii]) {continue;}
-    auto entity_iter = collection.services.find(rcl_wait_set.services[ii]);
-    if (entity_iter != collection.services.end()) {
-      auto entity = entity_iter->second.entity.lock();
-      if (!entity) {
+  size_t current_timer_index = 0;
+  while (true) {
+    auto [timer, timer_index] = wait_result.peek_next_ready_timer(current_timer_index);
+    if (nullptr == timer) {
+      break;
+    }
+    current_timer_index = timer_index;
+    auto entity_iter = collection.timers.find(timer->get_timer_handle().get());
+    if (entity_iter != collection.timers.end()) {
+      wait_result.clear_timer_with_index(current_timer_index);
+      auto data = timer->call();
+      if (!data) {
+        // someone canceled the timer between is_ready and call
         continue;
       }
-      execute_service(entity);
-      if (spin_once) {
-        return true;
-      }
+
+      execute_timer(std::move(timer), data);
       any_ready_executable = true;
+      if (spin_once) {return any_ready_executable;}
     }
   }
 
-  for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) {
-    if (nullptr == rcl_wait_set.clients[ii]) {continue;}
-    auto entity_iter = collection.clients.find(rcl_wait_set.clients[ii]);
+  while (auto client = wait_result.next_ready_client()) {
+    auto entity_iter = collection.clients.find(client->get_client_handle().get());
     if (entity_iter != collection.clients.end()) {
-      auto entity = entity_iter->second.entity.lock();
-      if (!entity) {
-        continue;
-      }
-      execute_client(entity);
-      if (spin_once) {
-        return true;
-      }
+      execute_client(client);
       any_ready_executable = true;
+      if (spin_once) {return any_ready_executable;}
     }
   }
 
-  for (auto & [handle, entry] : collection.waitables) {
-    auto waitable = entry.entity.lock();
-    if (!waitable) {
-      continue;
-    }
-    if (!waitable->is_ready(&rcl_wait_set)) {
-      continue;
+  while (auto service = wait_result.next_ready_service()) {
+    auto entity_iter = collection.services.find(service->get_service_handle().get());
+    if (entity_iter != collection.services.end()) {
+      execute_service(service);
+      any_ready_executable = true;
+      if (spin_once) {return any_ready_executable;}
     }
+  }
 
-    auto data = waitable->take_data();
-    waitable->execute(data);
-    if (spin_once) {
-      return true;
+  while (auto waitable = wait_result.next_ready_waitable()) {
+    auto entity_iter = collection.waitables.find(waitable.get());
+    if (entity_iter != collection.waitables.end()) {
+      const auto data = waitable->take_data();
+      waitable->execute(data);
+      any_ready_executable = true;
+      if (spin_once) {return any_ready_executable;}
     }
-    any_ready_executable = true;
   }
   return any_ready_executable;
 }

rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp

@@ -40,16 +40,21 @@ EventsExecutor::EventsExecutor(
   // The timers manager can be used either to only track timers (in this case an expired
   // timer will generate an executor event and then it will be executed by the executor thread)
   // or it can also take care of executing expired timers in its dedicated thread.
-  std::function<void(const rclcpp::TimerBase *)> timer_on_ready_cb = nullptr;
+  std::function<void(const rclcpp::TimerBase *,
+    const std::shared_ptr<void> &)> timer_on_ready_cb = nullptr;
   if (!execute_timers_separate_thread) {
-    timer_on_ready_cb = [this](const rclcpp::TimerBase * timer_id) {
-        ExecutorEvent event = {timer_id, -1, ExecutorEventType::TIMER_EVENT, 1};
+    timer_on_ready_cb =
+      [this](const rclcpp::TimerBase * timer_id, const std::shared_ptr<void> & data) {
+        ExecutorEvent event = {timer_id, data, -1, ExecutorEventType::TIMER_EVENT, 1};
         this->events_queue_->enqueue(event);
       };
   }
   timers_manager_ =
     std::make_shared<rclcpp::experimental::TimersManager>(context_, timer_on_ready_cb);
 
+  this->current_entities_collection_ =
+    std::make_shared<rclcpp::executors::ExecutorEntitiesCollection>();
+
   notify_waitable_ = std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>(
     [this]() {
       // This callback is invoked when:
@@ -61,6 +66,10 @@ EventsExecutor::EventsExecutor(
       this->refresh_current_collection_from_callback_groups();
     });
 
+  // Make sure that the notify waitable is immediately added to the collection
+  // to avoid missing events
+  this->add_notify_waitable_to_collection(current_entities_collection_->waitables);
+
   notify_waitable_->add_guard_condition(interrupt_guard_condition_);
   notify_waitable_->add_guard_condition(shutdown_guard_condition_);
 
@@ -81,15 +90,12 @@ EventsExecutor::EventsExecutor(
       }
 
       ExecutorEvent event =
-      {notify_waitable_entity_id, waitable_data, ExecutorEventType::WAITABLE_EVENT, 1};
+      {notify_waitable_entity_id, nullptr, waitable_data, ExecutorEventType::WAITABLE_EVENT, 1};
       this->events_queue_->enqueue(event);
     });
 
   this->entities_collector_ =
     std::make_shared<rclcpp::executors::ExecutorEntitiesCollector>(notify_waitable_);
-
-  this->current_entities_collection_ =
-    std::make_shared<rclcpp::executors::ExecutorEntitiesCollection>();
 }
 
 EventsExecutor::~EventsExecutor()
@@ -202,11 +208,12 @@ EventsExecutor::spin_once_impl(std::chrono::nanoseconds timeout)
     timeout = std::chrono::nanoseconds::max();
   }
 
-  // Select the smallest between input timeout and timer timeout
+  // Select the smallest between input timeout and timer timeout.
+  // Cancelled timers are not considered.
   bool is_timer_timeout = false;
   auto next_timer_timeout = timers_manager_->get_head_timeout();
-  if (next_timer_timeout < timeout) {
-    timeout = next_timer_timeout;
+  if (next_timer_timeout.has_value() && next_timer_timeout.value() < timeout) {
+    timeout = next_timer_timeout.value();
     is_timer_timeout = true;
   }
 
@@ -269,10 +276,13 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
   switch (event.type) {
     case ExecutorEventType::CLIENT_EVENT:
       {
-        auto client = this->retrieve_entity(
-          static_cast<const rcl_client_t *>(event.entity_key),
-          current_entities_collection_->clients);
-
+        rclcpp::ClientBase::SharedPtr client;
+        {
+          std::lock_guard<std::recursive_mutex> lock(collection_mutex_);
+          client = this->retrieve_entity(
+            static_cast<const rcl_client_t *>(event.entity_key),
+            current_entities_collection_->clients);
+        }
         if (client) {
           for (size_t i = 0; i < event.num_events; i++) {
             execute_client(client);
@@ -283,9 +293,13 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
       }
     case ExecutorEventType::SUBSCRIPTION_EVENT:
       {
-        auto subscription = this->retrieve_entity(
-          static_cast<const rcl_subscription_t *>(event.entity_key),
-          current_entities_collection_->subscriptions);
+        rclcpp::SubscriptionBase::SharedPtr subscription;
+        {
+          std::lock_guard<std::recursive_mutex> lock(collection_mutex_);
+          subscription = this->retrieve_entity(
+            static_cast<const rcl_subscription_t *>(event.entity_key),
+            current_entities_collection_->subscriptions);
+        }
         if (subscription) {
           for (size_t i = 0; i < event.num_events; i++) {
             execute_subscription(subscription);
@@ -295,10 +309,13 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
       }
     case ExecutorEventType::SERVICE_EVENT:
       {
-        auto service = this->retrieve_entity(
-          static_cast<const rcl_service_t *>(event.entity_key),
-          current_entities_collection_->services);
-
+        rclcpp::ServiceBase::SharedPtr service;
+        {
+          std::lock_guard<std::recursive_mutex> lock(collection_mutex_);
+          service = this->retrieve_entity(
+            static_cast<const rcl_service_t *>(event.entity_key),
+            current_entities_collection_->services);
+        }
         if (service) {
           for (size_t i = 0; i < event.num_events; i++) {
             execute_service(service);
@@ -310,17 +327,21 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
     case ExecutorEventType::TIMER_EVENT:
       {
         timers_manager_->execute_ready_timer(
-          static_cast<const rclcpp::TimerBase *>(event.entity_key));
+          static_cast<const rclcpp::TimerBase *>(event.entity_key), event.data);
         break;
       }
     case ExecutorEventType::WAITABLE_EVENT:
       {
-        auto waitable = this->retrieve_entity(
-          static_cast<const rclcpp::Waitable *>(event.entity_key),
-          current_entities_collection_->waitables);
+        rclcpp::Waitable::SharedPtr waitable;
+        {
+          std::lock_guard<std::recursive_mutex> lock(collection_mutex_);
+          waitable = this->retrieve_entity(
+            static_cast<const rclcpp::Waitable *>(event.entity_key),
+            current_entities_collection_->waitables);
+        }
         if (waitable) {
           for (size_t i = 0; i < event.num_events; i++) {
-            auto data = waitable->take_data_by_entity_id(event.waitable_data);
+            const auto data = waitable->take_data_by_entity_id(event.waitable_data);
             waitable->execute(data);
           }
         }
@@ -382,6 +403,7 @@ EventsExecutor::get_automatically_added_callback_groups_from_nodes()
 void
 EventsExecutor::refresh_current_collection_from_callback_groups()
 {
+  // Build the new collection
   this->entities_collector_->update_collections();
   auto callback_groups = this->entities_collector_->get_all_callback_groups();
   rclcpp::executors::ExecutorEntitiesCollection new_collection;
@@ -395,18 +417,11 @@ EventsExecutor::refresh_current_collection_from_callback_groups()
   // retrieved in the "standard" way.
   // To do it, we need to add the notify waitable as an entry in both the new and
   // current collections such that it's neither added or removed.
-  rclcpp::CallbackGroup::WeakPtr weak_group_ptr;
-  new_collection.waitables.insert(
-  {
-    this->notify_waitable_.get(),
-    {this->notify_waitable_, weak_group_ptr}
-  });
+  this->add_notify_waitable_to_collection(new_collection.waitables);
 
-  this->current_entities_collection_->waitables.insert(
-  {
-    this->notify_waitable_.get(),
-    {this->notify_waitable_, weak_group_ptr}
-  });
+  // Acquire lock before modifying the current collection
+  std::lock_guard<std::recursive_mutex> lock(collection_mutex_);
+  this->add_notify_waitable_to_collection(current_entities_collection_->waitables);
 
   this->refresh_current_collection(new_collection);
 }
@@ -415,6 +430,9 @@ void
 EventsExecutor::refresh_current_collection(
   const rclcpp::executors::ExecutorEntitiesCollection & new_collection)
 {
+  // Acquire lock before modifying the current collection
+  std::lock_guard<std::recursive_mutex> lock(collection_mutex_);
+
   current_entities_collection_->timers.update(
     new_collection.timers,
     [this](rclcpp::TimerBase::SharedPtr timer) {timers_manager_->add_timer(timer);},
@@ -469,7 +487,7 @@ EventsExecutor::create_entity_callback(
 {
   std::function<void(size_t)>
   callback = [this, entity_key, event_type](size_t num_events) {
-      ExecutorEvent event = {entity_key, -1, event_type, num_events};
+      ExecutorEvent event = {entity_key, nullptr, -1, event_type, num_events};
       this->events_queue_->enqueue(event);
     };
   return callback;
@@ -481,8 +499,21 @@ EventsExecutor::create_waitable_callback(const rclcpp::Waitable * entity_key)
   std::function<void(size_t, int)>
   callback = [this, entity_key](size_t num_events, int waitable_data) {
       ExecutorEvent event =
-      {entity_key, waitable_data, ExecutorEventType::WAITABLE_EVENT, num_events};
+      {entity_key, nullptr, waitable_data, ExecutorEventType::WAITABLE_EVENT, num_events};
       this->events_queue_->enqueue(event);
     };
   return callback;
 }
+
+void
+EventsExecutor::add_notify_waitable_to_collection(
+  rclcpp::executors::ExecutorEntitiesCollection::WaitableCollection & collection)
+{
+  // The notify waitable is not associated to any group, so use an invalid one
+  rclcpp::CallbackGroup::WeakPtr weak_group_ptr;
+  collection.insert(
+  {
+    this->notify_waitable_.get(),
+    {this->notify_waitable_, weak_group_ptr}
+  });
+}

rclcpp/src/rclcpp/experimental/timers_manager.cpp

@@ -27,10 +27,10 @@ using rclcpp::experimental::TimersManager;
 
 TimersManager::TimersManager(
   std::shared_ptr<rclcpp::Context> context,
-  std::function<void(const rclcpp::TimerBase *)> on_ready_callback)
+  std::function<void(const rclcpp::TimerBase *, const std::shared_ptr<void> &)> on_ready_callback)
+: on_ready_callback_(on_ready_callback),
+  context_(context)
 {
-  context_ = context;
-  on_ready_callback_ = on_ready_callback;
 }
 
 TimersManager::~TimersManager()
@@ -100,7 +100,7 @@ void TimersManager::stop()
   }
 }
 
-std::chrono::nanoseconds TimersManager::get_head_timeout()
+std::optional<std::chrono::nanoseconds> TimersManager::get_head_timeout()
 {
   // Do not allow to interfere with the thread running
   if (running_) {
@@ -148,8 +148,12 @@ bool TimersManager::execute_head_timer()
   if (timer_ready) {
     // NOTE: here we always execute the timer, regardless of whether the
     // on_ready_callback is set or not.
-    head_timer->call();
-    head_timer->execute_callback();
+    auto data = head_timer->call();
+    if (!data) {
+      // someone canceled the timer between is_ready and call
+      return false;
+    }
+    head_timer->execute_callback(data);
     timers_heap.heapify_root();
     weak_timers_heap_.store(timers_heap);
   }
@@ -157,7 +161,9 @@ bool TimersManager::execute_head_timer()
   return timer_ready;
 }
 
-void TimersManager::execute_ready_timer(const rclcpp::TimerBase * timer_id)
+void TimersManager::execute_ready_timer(
+  const rclcpp::TimerBase * timer_id,
+  const std::shared_ptr<void> & data)
 {
   TimerPtr ready_timer;
   {
@@ -165,11 +171,11 @@ void TimersManager::execute_ready_timer(const rclcpp::TimerBase * timer_id)
     ready_timer = weak_timers_heap_.get_timer(timer_id);
   }
   if (ready_timer) {
-    ready_timer->execute_callback();
+    ready_timer->execute_callback(data);
   }
 }
 
-std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe()
+std::optional<std::chrono::nanoseconds> TimersManager::get_head_timeout_unsafe()
 {
   // If we don't have any weak pointer, then we just return maximum timeout
   if (weak_timers_heap_.empty()) {
@@ -191,7 +197,9 @@ std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe()
     }
     head_timer = locked_heap.front();
   }
-
+  if (head_timer->is_canceled()) {
+    return std::nullopt;
+  }
   return head_timer->time_until_trigger();
 }
 
@@ -213,11 +221,16 @@ void TimersManager::execute_ready_timers_unsafe()
   const size_t number_ready_timers = locked_heap.get_number_ready_timers();
   size_t executed_timers = 0;
   while (executed_timers < number_ready_timers && head_timer->is_ready()) {
-    head_timer->call();
-    if (on_ready_callback_) {
-      on_ready_callback_(head_timer.get());
+    auto data = head_timer->call();
+    if (data) {
+      if (on_ready_callback_) {
+        on_ready_callback_(head_timer.get(), data);
+      } else {
+        head_timer->execute_callback(data);
+      }
     } else {
-      head_timer->execute_callback();
+      // someone canceled the timer between is_ready and call
+      // we don't do anything, as the timer is now 'processed'
     }
 
     executed_timers++;
@@ -242,17 +255,34 @@ void TimersManager::run_timers()
     // Lock mutex
     std::unique_lock<std::mutex> lock(timers_mutex_);
 
-    std::chrono::nanoseconds time_to_sleep = get_head_timeout_unsafe();
+    std::optional<std::chrono::nanoseconds> time_to_sleep = get_head_timeout_unsafe();
 
-    // No need to wait if a timer is already available
-    if (time_to_sleep > std::chrono::nanoseconds::zero()) {
-      if (time_to_sleep != std::chrono::nanoseconds::max()) {
-        // Wait until timeout or notification that timers have been updated
-        timers_cv_.wait_for(lock, time_to_sleep, [this]() {return timers_updated_;});
-      } else {
-        // Wait until notification that timers have been updated
-        timers_cv_.wait(lock, [this]() {return timers_updated_;});
-      }
+    // If head timer was cancelled, try to reheap and get a new head.
+    // This avoids an edge condition where head timer is cancelled, but other
+    // valid timers remain in the heap.
+    if (!time_to_sleep.has_value()) {
+      // Re-heap to (possibly) move cancelled timer from head of heap. If
+      // entire heap is cancelled, this will still result in a nullopt.
+      TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
+      locked_heap.heapify();
+      weak_timers_heap_.store(locked_heap);
+      time_to_sleep = get_head_timeout_unsafe();
+    }
+
+    // If no timers, or all timers cancelled, wait for an update.
+    if (!time_to_sleep.has_value() || (time_to_sleep.value() == std::chrono::nanoseconds::max()) ) {
+      // Wait until notification that timers have been updated
+      timers_cv_.wait(lock, [this]() {return timers_updated_;});
+
+      // Re-heap in case ordering changed due to a cancelled timer
+      // re-activating.
+      TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
+      locked_heap.heapify();
+      weak_timers_heap_.store(locked_heap);
+    } else if (time_to_sleep.value() != std::chrono::nanoseconds::zero()) {
+      // If time_to_sleep is zero, we immediately execute. Otherwise, wait
+      // until timeout or notification that timers have been updated
+      timers_cv_.wait_for(lock, time_to_sleep.value(), [this]() {return timers_updated_;});
     }
 
     // Reset timers updated flag

rclcpp/src/rclcpp/generic_client.cpp

@@ -0,0 +1,164 @@
+// 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.
+
+#include <future>
+
+#include "rclcpp/generic_client.hpp"
+#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"
+
+namespace rclcpp
+{
+GenericClient::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)
+: ClientBase(node_base, node_graph)
+{
+  ts_lib_ = get_typesupport_library(
+    service_type, "rosidl_typesupport_cpp");
+
+  auto service_ts_ = get_service_typesupport_handle(
+    service_type, "rosidl_typesupport_cpp", *ts_lib_);
+
+  auto response_type_support_intro = get_message_typesupport_handle(
+    service_ts_->response_typesupport,
+    rosidl_typesupport_introspection_cpp::typesupport_identifier);
+  response_members_ = static_cast<const rosidl_typesupport_introspection_cpp::MessageMembers *>(
+    response_type_support_intro->data);
+
+  rcl_ret_t ret = rcl_client_init(
+    this->get_client_handle().get(),
+    this->get_rcl_node_handle(),
+    service_ts_,
+    service_name.c_str(),
+    &client_options);
+  if (ret != RCL_RET_OK) {
+    if (ret == RCL_RET_SERVICE_NAME_INVALID) {
+      auto rcl_node_handle = this->get_rcl_node_handle();
+      // this will throw on any validation problem
+      rcl_reset_error();
+      expand_topic_or_service_name(
+        service_name,
+        rcl_node_get_name(rcl_node_handle),
+        rcl_node_get_namespace(rcl_node_handle),
+        true);
+    }
+    rclcpp::exceptions::throw_from_rcl_error(ret, "could not create generic client");
+  }
+}
+
+std::shared_ptr<void>
+GenericClient::create_response()
+{
+  void * response = new uint8_t[response_members_->size_of_];
+  response_members_->init_function(response, rosidl_runtime_cpp::MessageInitialization::ZERO);
+  return std::shared_ptr<void>(
+    response,
+    [this](void * p)
+    {
+      response_members_->fini_function(p);
+      delete[] reinterpret_cast<uint8_t *>(p);
+    });
+}
+
+std::shared_ptr<rmw_request_id_t>
+GenericClient::create_request_header()
+{
+  // TODO(wjwwood): This should probably use rmw_request_id's allocator.
+  //                (since it is a C type)
+  return std::shared_ptr<rmw_request_id_t>(new rmw_request_id_t);
+}
+
+void
+GenericClient::handle_response(
+  std::shared_ptr<rmw_request_id_t> request_header,
+  std::shared_ptr<void> response)
+{
+  auto optional_pending_request =
+    this->get_and_erase_pending_request(request_header->sequence_number);
+  if (!optional_pending_request) {
+    return;
+  }
+  auto & value = *optional_pending_request;
+  if (std::holds_alternative<Promise>(value)) {
+    auto & promise = std::get<Promise>(value);
+    promise.set_value(std::move(response));
+  }
+}
+
+size_t
+GenericClient::prune_pending_requests()
+{
+  std::lock_guard guard(pending_requests_mutex_);
+  auto ret = pending_requests_.size();
+  pending_requests_.clear();
+  return ret;
+}
+
+bool
+GenericClient::remove_pending_request(int64_t request_id)
+{
+  std::lock_guard guard(pending_requests_mutex_);
+  return pending_requests_.erase(request_id) != 0u;
+}
+
+std::optional<GenericClient::CallbackInfoVariant>
+GenericClient::get_and_erase_pending_request(int64_t request_number)
+{
+  std::unique_lock<std::mutex> lock(pending_requests_mutex_);
+  auto it = pending_requests_.find(request_number);
+  if (it == pending_requests_.end()) {
+    RCUTILS_LOG_DEBUG_NAMED(
+      "rclcpp",
+      "Received invalid sequence number. Ignoring...");
+    return std::nullopt;
+  }
+  auto value = std::move(it->second.second);
+  pending_requests_.erase(request_number);
+  return value;
+}
+
+GenericClient::FutureAndRequestId
+GenericClient::async_send_request(const Request request)
+{
+  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);
+}
+
+int64_t
+GenericClient::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;
+}
+
+}  // namespace rclcpp

rclcpp/src/rclcpp/generic_publisher.cpp

@@ -23,6 +23,10 @@ namespace rclcpp
 
 void GenericPublisher::publish(const rclcpp::SerializedMessage & message)
 {
+  TRACETOOLS_TRACEPOINT(
+    rclcpp_publish,
+    nullptr,
+    static_cast<const void *>(&message.get_rcl_serialized_message()));
   auto return_code = rcl_publish_serialized_message(
     get_publisher_handle().get(), &message.get_rcl_serialized_message(), NULL);
 
@@ -68,6 +72,7 @@ void GenericPublisher::deserialize_message(
 
 void GenericPublisher::publish_loaned_message(void * loaned_message)
 {
+  TRACETOOLS_TRACEPOINT(rclcpp_publish, nullptr, static_cast<const void *>(loaned_message));
   auto return_code = rcl_publish_loaned_message(
     get_publisher_handle().get(), loaned_message, NULL);