use Reader/Writer lock for Ring Buffer.

Signed-off-by: Tomoya Fujita <Tomoya.Fujita@sony.com>

.github/ISSUE_TEMPLATE.md

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

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

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

CODEOWNERS

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

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,321 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+29.6.0 (2025-04-25)
+-------------------
+* throws std::invalid_argument if ParameterEvent is NULL. (`#2814 <https://github.com/ros2/rclcpp/issues/2814>`_)
+* Removed clang warnings (`#2823 <https://github.com/ros2/rclcpp/issues/2823>`_)
+* Contributors: Alejandro Hernández Cordero, Tomoya Fujita
+
+29.5.0 (2025-04-18)
+-------------------
+* Fix a race condition (`#2819 <https://github.com/ros2/rclcpp/issues/2819>`_)
+* Remove redundant typesupport check in serialization module (`#2808 <https://github.com/ros2/rclcpp/issues/2808>`_)
+* Remove get_typesupport_handle implementation. (`#2806 <https://github.com/ros2/rclcpp/issues/2806>`_)
+* Use NodeParameterInterface instead of /parameter_event to update "use_sim_time" (`#2378 <https://github.com/ros2/rclcpp/issues/2378>`_)
+* Remove cancel_clock_executor_promise\_. (`#2797 <https://github.com/ros2/rclcpp/issues/2797>`_)
+* Enable parameter update recursively only when QoS override parameters. (`#2742 <https://github.com/ros2/rclcpp/issues/2742>`_)
+* Contributors: Pedro de Azeredo, Tanishq Chaudhary, Tomoya Fujita
+
+29.4.0 (2025-04-04)
+-------------------
+* Removed trailing whitespace from the codebase. (`#2791 <https://github.com/ros2/rclcpp/issues/2791>`_)
+* Expanded docstring of `get_rmw_qos_profile()` (`#2787 <https://github.com/ros2/rclcpp/issues/2787>`_)
+* Set envars to run tests with rmw_zenoh_cpp with multicast discovery (`#2776 <https://github.com/ros2/rclcpp/issues/2776>`_)
+* fix: Compilefix for clang (`#2775 <https://github.com/ros2/rclcpp/issues/2775>`_)
+* add exception doc for configure_introspection. (`#2773 <https://github.com/ros2/rclcpp/issues/2773>`_)
+* feat: Add ClockWaiter and ClockConditionalVariable (`#2691 <https://github.com/ros2/rclcpp/issues/2691>`_)
+* doc: Added warning to not instantiate Clock directly with RCL_ROS_TIME (`#2768 <https://github.com/ros2/rclcpp/issues/2768>`_)
+* Use rmw_event_type_is_supported in test_qos_event (`#2761 <https://github.com/ros2/rclcpp/issues/2761>`_)
+* Support action typesupport helper (`#2750 <https://github.com/ros2/rclcpp/issues/2750>`_)
+* use maybe_unused attribute for the portability. (`#2758 <https://github.com/ros2/rclcpp/issues/2758>`_)
+* Executor strong reference fix (`#2745 <https://github.com/ros2/rclcpp/issues/2745>`_)
+* Cleanup of https://github.com/ros2/rclcpp/pull/2683 (`#2714 <https://github.com/ros2/rclcpp/issues/2714>`_)
+* Fix typo in doc section for get_service_typesupport_handle (`#2751 <https://github.com/ros2/rclcpp/issues/2751>`_)
+* Test case and fix for for https://github.com/ros2/rclcpp/issues/2652 (`#2713 <https://github.com/ros2/rclcpp/issues/2713>`_)
+* fix(timer): Delete node, after executor thread terminated (`#2737 <https://github.com/ros2/rclcpp/issues/2737>`_)
+* update doc section for spin_xxx methods. (`#2730 <https://github.com/ros2/rclcpp/issues/2730>`_)
+* fix: Expose timers used by rclcpp::Waitables (`#2699 <https://github.com/ros2/rclcpp/issues/2699>`_)
+* use rmw_qos_profile_rosout_default instead of rcl. (`#2663 <https://github.com/ros2/rclcpp/issues/2663>`_)
+* fix(Executor): Fixed entities not beeing executed after just beeing added (`#2724 <https://github.com/ros2/rclcpp/issues/2724>`_)
+* fix: make the loop condition align with the description (`#2726 <https://github.com/ros2/rclcpp/issues/2726>`_)
+* Collect log messages from rcl, and reset. (`#2720 <https://github.com/ros2/rclcpp/issues/2720>`_)
+* Contributors: Abhishek Kashyap, Alejandro Hernández Cordero, Barry Xu, Janosch Machowinski, Leander Stephen D'Souza, Tomoya Fujita, Yuyuan Yuan
+
+29.3.0 (2024-12-20)
+-------------------
+* Fix transient local IPC publish  (`#2708 <https://github.com/ros2/rclcpp/issues/2708>`_)
+* apply actual QoS from rmw to the IPC publisher. (`#2707 <https://github.com/ros2/rclcpp/issues/2707>`_)
+* Adding in topic name to logging on IPC issues (`#2706 <https://github.com/ros2/rclcpp/issues/2706>`_)
+* fix TestTimeSource.ROS_time_valid_attach_detach. (`#2700 <https://github.com/ros2/rclcpp/issues/2700>`_)
+* Update docstring for `rclcpp::Node::now()` (`#2696 <https://github.com/ros2/rclcpp/issues/2696>`_)
+* Re-enable executor test on rmw_connextdds. (`#2693 <https://github.com/ros2/rclcpp/issues/2693>`_)
+* Fix warnings on Windows. (`#2692 <https://github.com/ros2/rclcpp/issues/2692>`_)
+* Omnibus fixes for running tests with Connext. (`#2684 <https://github.com/ros2/rclcpp/issues/2684>`_)
+* fix(Executor): Fix segfault if callback group is deleted during rmw_wait (`#2683 <https://github.com/ros2/rclcpp/issues/2683>`_)
+* Contributors: Chris Lalancette, Jeffery Hsu, Patrick Roncagliolo, Steve Macenski, Tomoya Fujita, jmachowinski
+
+29.2.0 (2024-11-25)
+-------------------
+* accept custom allocator for LoanedMessage. (`#2672 <https://github.com/ros2/rclcpp/issues/2672>`_)
+* Contributors: Tomoya Fujita
+
+29.1.0 (2024-11-20)
+-------------------
+* a couple of typo fixes in doc section for LoanedMessage. (`#2676 <https://github.com/ros2/rclcpp/issues/2676>`_)
+* Make sure callback_end tracepoint is triggered in AnyServiceCallback (`#2670 <https://github.com/ros2/rclcpp/issues/2670>`_)
+* Correct the incorrect comments in generic_client.hpp (`#2662 <https://github.com/ros2/rclcpp/issues/2662>`_)
+* Fix NodeOptions assignment operator (`#2656 <https://github.com/ros2/rclcpp/issues/2656>`_)
+* set QoS History KEEP_ALL explicitly for statistics publisher. (`#2650 <https://github.com/ros2/rclcpp/issues/2650>`_)
+* Fix test_intra_process_manager.cpp with rmw_zenoh_cpp (`#2653 <https://github.com/ros2/rclcpp/issues/2653>`_)
+* Fixed test_events_executors in zenoh (`#2643 <https://github.com/ros2/rclcpp/issues/2643>`_)
+* rmw_fastrtps supports service event gid uniqueness test. (`#2638 <https://github.com/ros2/rclcpp/issues/2638>`_)
+* print warning if event callback is not supported instead of passing exception. (`#2648 <https://github.com/ros2/rclcpp/issues/2648>`_)
+* Implement callback support of async_send_request for service generic client (`#2614 <https://github.com/ros2/rclcpp/issues/2614>`_)
+* Contributors: Alejandro Hernández Cordero, Barry Xu, Chris Lalancette, Christophe Bedard, Romain DESILLE, Tomoya Fujita
+
+29.0.0 (2024-10-03)
+-------------------
+* Fixed test qos rmw zenoh (`#2639 <https://github.com/ros2/rclcpp/issues/2639>`_)
+* verify client gid uniqueness for a single service event. (`#2636 <https://github.com/ros2/rclcpp/issues/2636>`_)
+* Skip some tests in test_qos_event and run others with event types supported by rmw_zenoh (`#2626 <https://github.com/ros2/rclcpp/issues/2626>`_)
+* Shutdown the context before context's destructor is invoked in tests (`#2633 <https://github.com/ros2/rclcpp/issues/2633>`_)
+* Skip rmw zenoh content filtering tests (`#2627 <https://github.com/ros2/rclcpp/issues/2627>`_)
+* Use InvalidServiceTypeError for unavailable service type in GenericClient (`#2629 <https://github.com/ros2/rclcpp/issues/2629>`_)
+* Implement generic service (`#2617 <https://github.com/ros2/rclcpp/issues/2617>`_)
+* fix events-executor warm-up bug and add unit-tests (`#2591 <https://github.com/ros2/rclcpp/issues/2591>`_)
+* remove unnecessary gtest-skip in test_executors (`#2600 <https://github.com/ros2/rclcpp/issues/2600>`_)
+* Correct node name in service test code (`#2615 <https://github.com/ros2/rclcpp/issues/2615>`_)
+* Minor naming fixes for ParameterValue to_string() function (`#2609 <https://github.com/ros2/rclcpp/issues/2609>`_)
+* Removed clang warnings (`#2605 <https://github.com/ros2/rclcpp/issues/2605>`_)
+* Fix a couple of issues in the documentation. (`#2608 <https://github.com/ros2/rclcpp/issues/2608>`_)
+* deprecate the static single threaded executor (`#2598 <https://github.com/ros2/rclcpp/issues/2598>`_)
+* Fix name of ParameterEventHandler class in doc (`#2604 <https://github.com/ros2/rclcpp/issues/2604>`_)
+* subscriber_statistics_collectors\_ should be protected by mutex. (`#2592 <https://github.com/ros2/rclcpp/issues/2592>`_)
+* Fix bug in timers lifecycle for events executor (`#2586 <https://github.com/ros2/rclcpp/issues/2586>`_)
+* fix rclcpp/test/rclcpp/CMakeLists.txt to check for the correct targets existance (`#2596 <https://github.com/ros2/rclcpp/issues/2596>`_)
+* Shut down context during init if logging config fails (`#2594 <https://github.com/ros2/rclcpp/issues/2594>`_)
+* Make more of the Waitable API abstract (`#2593 <https://github.com/ros2/rclcpp/issues/2593>`_)
+* Contributors: Alberto Soragna, Alejandro Hernández Cordero, Alexis Pojomovsky, Barry Xu, Chris Lalancette, Christophe Bedard, Kang, Hsin-Yi, Tomoya Fujita
+
+28.3.3 (2024-07-29)
+-------------------
+* Only compile the tests once. (`#2590 <https://github.com/ros2/rclcpp/issues/2590>`_)
+* Contributors: Chris Lalancette
+
+28.3.2 (2024-07-24)
+-------------------
+* Updated rcpputils path API (`#2579 <https://github.com/ros2/rclcpp/issues/2579>`_)
+* Make the subscriber_triggered_to_receive_message test more reliable. (`#2584 <https://github.com/ros2/rclcpp/issues/2584>`_)
+  * Make the subscriber_triggered_to_receive_message test more reliable.
+  In the current code, inside of the timer we create the subscription
+  and the publisher, publish immediately, and expect the subscription
+  to get it immediately.  But it may be the case that discovery
+  hasn't even happened between the publisher and the subscription
+  by the time the publish call happens.
+  To make this more reliable, create the subscription and publish *before*
+  we ever create and spin on the timer.  This at least gives 100
+  milliseconds for discovery to happen.  That may not be quite enough
+  to make this reliable on all platforms, but in my local testing this
+  helps a lot.  Prior to this change I can make this fail one out of 10
+  times, and after the change I've run 100 times with no failures.
+* Have the EventsExecutor use more common code  (`#2570 <https://github.com/ros2/rclcpp/issues/2570>`_)
+  * move notify waitable setup to its own function
+  * move mutex lock to retrieve_entity utility
+  * use entities_need_rebuild\_ atomic bool in events-executors
+  * remove duplicated set_on_ready_callback for notify_waitable
+  * use mutex from base class rather than a new recursive mutex
+  * use current_collection\_ member in events-executor
+  * delay adding notify waitable to collection
+  * postpone clearing the current collection
+  * commonize notify waitable and collection
+  * commonize add/remove node/cbg methods
+  * fix linter errors
+  ---------
+* Removed deprecated methods and classes (`#2575 <https://github.com/ros2/rclcpp/issues/2575>`_)
+* Release ownership of entities after spinning cancelled (`#2556 <https://github.com/ros2/rclcpp/issues/2556>`_)
+  * Release ownership of entities after spinning cancelled
+  * Move release action to every exit point in different spin functions
+  * Move wait_result\_.reset() before setting spinning to false
+  * Update test code
+  * Move test code to test_executors.cpp
+  ---------
+* Split test_executors.cpp even further. (`#2572 <https://github.com/ros2/rclcpp/issues/2572>`_)
+  That's because it is too large for Windows Debug to compile,
+  so split into smaller bits.
+  Even with this split, the file is too big; that's likely
+  because we are using TYPED_TEST here, which generates multiple
+  symbols per test case.  To deal with this, without further
+  breaking up the file, also add in the /bigobj flag when
+  compiling on Windows Debug.
+* avoid adding notify waitable twice to events-executor collection (`#2564 <https://github.com/ros2/rclcpp/issues/2564>`_)
+  * avoid adding notify waitable twice to events-executor entities collection
+  * remove redundant mutex lock
+  ---------
+* Contributors: Alberto Soragna, Alejandro Hernández Cordero, Barry Xu, Chris Lalancette
+
+28.3.1 (2024-06-25)
+-------------------
+* Remove unnecessary msg includes in tests (`#2566 <https://github.com/ros2/rclcpp/issues/2566>`_)
+* Fix copy-paste errors in function docs (`#2565 <https://github.com/ros2/rclcpp/issues/2565>`_)
+* Fix typo in function doc (`#2563 <https://github.com/ros2/rclcpp/issues/2563>`_)
+* Contributors: Christophe Bedard
+
+28.3.0 (2024-06-17)
+-------------------
+* Add test creating two content filter topics with the same topic name (`#2546 <https://github.com/ros2/rclcpp/issues/2546>`_) (`#2549 <https://github.com/ros2/rclcpp/issues/2549>`_)
+* add impl pointer for ExecutorOptions (`#2523 <https://github.com/ros2/rclcpp/issues/2523>`_)
+* Fixup Executor::spin_all() regression fix (`#2517 <https://github.com/ros2/rclcpp/issues/2517>`_)
+* Add 'mimick' label to tests which use Mimick (`#2516 <https://github.com/ros2/rclcpp/issues/2516>`_)
+* Contributors: Alejandro Hernández Cordero, Scott K Logan, William Woodall
+
+28.2.0 (2024-04-26)
+-------------------
+* Check for negative time in rclcpp::Time(int64_t nanoseconds, ...) constructor (`#2510 <https://github.com/ros2/rclcpp/issues/2510>`_)
+* Revise the description of service configure_introspection() (`#2511 <https://github.com/ros2/rclcpp/issues/2511>`_)
+* Contributors: Barry Xu, Sharmin Ramli
+
+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>`_)

rclcpp/CMakeLists.txt

@@ -63,13 +63,13 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/exceptions/exceptions.cpp
   src/rclcpp/executable_list.cpp
   src/rclcpp/executor.cpp
+  src/rclcpp/executor_options.cpp
   src/rclcpp/executors.cpp
   src/rclcpp/executors/executor_entities_collection.cpp
   src/rclcpp/executors/executor_entities_collector.cpp
   src/rclcpp/executors/executor_notify_waitable.cpp
   src/rclcpp/executors/multi_threaded_executor.cpp
   src/rclcpp/executors/single_threaded_executor.cpp
-  src/rclcpp/executors/static_executor_entities_collector.cpp
   src/rclcpp/executors/static_single_threaded_executor.cpp
   src/rclcpp/expand_topic_or_service_name.cpp
   src/rclcpp/experimental/executors/events_executor/events_executor.cpp
@@ -77,6 +77,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/future_return_code.cpp
   src/rclcpp/generic_client.cpp
   src/rclcpp/generic_publisher.cpp
+  src/rclcpp/generic_service.cpp
   src/rclcpp/generic_subscription.cpp
   src/rclcpp/graph_listener.cpp
   src/rclcpp/guard_condition.cpp
@@ -299,7 +300,7 @@ install(
 
 if(TEST cppcheck)
   # must set the property after ament_package()
-  set_tests_properties(cppcheck PROPERTIES TIMEOUT 600)
+  set_tests_properties(cppcheck PROPERTIES TIMEOUT 1200)
 endif()
 
 if(TEST cpplint)

rclcpp/README.md

@@ -2,7 +2,7 @@
 
 The ROS client library in C++.
 
-The link to the latest rclcpp API documentation, which includes a complete list of its main components and features, can be found on the rclcpp package info page, at the [ROS Index](https://index.ros.org/p/rclcpp/).
+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/doc/api_review_march_2020.md

@@ -434,4 +434,3 @@
 
 - (tfoote) There should be no jumps in steady time. If there's a big change in system time, it doesn't necessarily mean that time jumped, just that you might have been sleeping for a long time. Most ntp systems adjust the slew rate these days instead of jumping but still that's an external process and I don't know of any APIs to introspect the state of the clock. I'm not sure that we have a way to detect jumps in time for system or steady time. To that end I think that we should be clear that we only provide callbacks when simulation time starts or stops, or simulation time jumps. We should also strongly recommend that operators not actively adjust their system clocks while running ROS nodes.
 - (jacobperron) I agree with Tully, if we don't have a way to detect system time jumps then I think we should just document that this only works with ROS time. In addition to documentation, we could log an info or warning message if the user registers jump callback with steady or system time, but it may be unnecessarily noisy.
-

rclcpp/doc/proposed_node_parameter_callbacks.md

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

rclcpp/include/rclcpp/allocator/allocator_deleter.hpp

@@ -69,18 +69,17 @@ private:
 };
 
 template<typename Alloc, typename T, typename D>
-void set_allocator_for_deleter(D * deleter, Alloc * alloc)
+void set_allocator_for_deleter([[maybe_unused]] D * deleter, [[maybe_unused]] Alloc * alloc)
 {
-  (void) alloc;
-  (void) deleter;
   throw std::runtime_error("Reached unexpected template specialization");
 }
 
 template<typename T, typename U>
-void set_allocator_for_deleter(std::default_delete<T> * deleter, std::allocator<U> * alloc)
+void set_allocator_for_deleter(
+  [[maybe_unused]] std::default_delete<T> * deleter,
+  [[maybe_unused]] std::allocator<U> * alloc)
 {
-  (void) deleter;
-  (void) alloc;
+  // This function is intentionally left empty.
 }
 
 template<typename Alloc, typename T>

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

@@ -165,11 +165,13 @@ public:
     if (std::holds_alternative<SharedPtrDeferResponseCallback>(callback_)) {
       const auto & cb = std::get<SharedPtrDeferResponseCallback>(callback_);
       cb(request_header, std::move(request));
+      TRACETOOLS_TRACEPOINT(callback_end, static_cast<const void *>(this));
       return nullptr;
     }
     if (std::holds_alternative<SharedPtrDeferResponseCallbackWithServiceHandle>(callback_)) {
       const auto & cb = std::get<SharedPtrDeferResponseCallbackWithServiceHandle>(callback_);
       cb(service_handle, request_header, std::move(request));
+      TRACETOOLS_TRACEPOINT(callback_end, static_cast<const void *>(this));
       return nullptr;
     }
     // auto response = allocate_shared<typename ServiceT::Response, Allocator>();

rclcpp/include/rclcpp/callback_group.hpp

@@ -59,46 +59,6 @@ class CallbackGroup
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup)
 
-  /// Constructor for CallbackGroup.
-  /**
-   * Callback Groups have a type, either 'Mutually Exclusive' or 'Reentrant'
-   * and when creating one the type must be specified.
-   *
-   * Callbacks in Reentrant Callback Groups must be able to:
-   *   - run at the same time as themselves (reentrant)
-   *   - run at the same time as other callbacks in their group
-   *   - run at the same time as other callbacks in other groups
-   *
-   * Callbacks in Mutually Exclusive Callback Groups:
-   *   - will not be run multiple times simultaneously (non-reentrant)
-   *   - will not be run at the same time as other callbacks in their group
-   *   - but must run at the same time as callbacks in other groups
-   *
-   * Additionally, callback groups have a property which determines whether or
-   * not they are added to an executor with their associated node automatically.
-   * When creating a callback group the automatically_add_to_executor_with_node
-   * argument determines this behavior, and if true it will cause the newly
-   * created callback group to be added to an executor with the node when the
-   * Executor::add_node method is used.
-   * If false, this callback group will not be added automatically and would
-   * have to be added to an executor manually using the
-   * Executor::add_callback_group method.
-   *
-   * Whether the node was added to the executor before creating the callback
-   * group, or after, is irrelevant; the callback group will be automatically
-   * added to the executor in either case.
-   *
-   * \param[in] group_type The type of the callback group.
-   * \param[in] 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.
-   */
-  [[deprecated("Use CallbackGroup constructor with context function argument")]]
-  RCLCPP_PUBLIC
-  explicit CallbackGroup(
-    CallbackGroupType group_type,
-    bool automatically_add_to_executor_with_node = true);
-
   /// Constructor for CallbackGroup.
   /**
    * Callback Groups have a type, either 'Mutually Exclusive' or 'Reentrant'
@@ -184,18 +144,41 @@ 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.
+  /**
+   * The resulting bool will be true in the case that no executor is currently
+   * using an executable entity from this group.
+   * The resulting bool will be false in the case that an executor is currently
+   * using an executable entity from this group, and the group policy doesn't
+   * allow a second take (eg mutual exclusion)
+   * \return a reference to the flag
+   */
   RCLCPP_PUBLIC
   std::atomic_bool &
   can_be_taken_from();
 
+  /// Get the group type.
+  /**
+   * \return the group type
+   */
   RCLCPP_PUBLIC
   const CallbackGroupType &
   type() const;
 
+  /// Collect all of the entity pointers contained in this callback group.
+  /**
+   * \param[in] sub_func Function to execute for each subscription
+   * \param[in] service_func Function to execute for each service
+   * \param[in] client_func Function to execute for each client
+   * \param[in] timer_func Function to execute for each timer
+   * \param[in] waitable_fuinc Function to execute for each waitable
+   */
   RCLCPP_PUBLIC
-  void collect_all_ptrs(
+  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,

rclcpp/include/rclcpp/client.hpp

@@ -70,14 +70,6 @@ struct FutureAndRequestId
   /// Allow implicit conversions to `std::future` by reference.
   operator FutureT &() {return this->future;}
 
-  /// Deprecated, use the `future` member variable instead.
-  /**
-   * Allow implicit conversions to `std::future` by value.
-   * \deprecated
-   */
-  [[deprecated("FutureAndRequestId: use .future instead of an implicit conversion")]]
-  operator FutureT() {return this->future;}
-
   // delegate future like methods in the std::future impl_
 
   /// See std::future::get().
@@ -436,15 +428,6 @@ public:
   {
     using detail::FutureAndRequestId<std::future<SharedResponse>>::FutureAndRequestId;
 
-    /// Deprecated, use `.future.share()` instead.
-    /**
-     * Allow implicit conversions to `std::shared_future` by value.
-     * \deprecated
-     */
-    [[deprecated(
-      "FutureAndRequestId: use .future.share() instead of an implicit conversion")]]
-    operator SharedFuture() {return this->future.share();}
-
     // delegate future like methods in the std::future impl_
 
     /// See std::future::share().
@@ -490,7 +473,7 @@ public:
    * \param[in] node_base NodeBaseInterface pointer that is used in part of the setup.
    * \param[in] node_graph The node graph interface of the corresponding node.
    * \param[in] service_name Name of the topic to publish to.
-   * \param[in] client_options options for the subscription.
+   * \param[in] client_options options for the client.
    */
   Client(
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
@@ -806,6 +789,9 @@ public:
    * \param[in] clock clock to use to generate introspection timestamps
    * \param[in] qos_service_event_pub QoS settings to use when creating the introspection publisher
    * \param[in] introspection_state the state to set introspection to
+   *
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw if
+   *   it failed to configure introspection.
    */
   void
   configure_introspection(
@@ -864,7 +850,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

@@ -60,6 +60,13 @@ public:
   /**
    * Initializes the clock instance with the given clock_type.
    *
+   * WARNING Don't instantiate a clock using RCL_ROS_TIME directly,
+   * unless you really know what you are doing. By default no TimeSource
+   * is attached to a new clock. This will lead to the unexpected behavior,
+   * that your RCL_ROS_TIME will run always on system time. If you want
+   * a RCL_ROS_TIME use Node::get_clock(), or make sure to attach a
+   * TimeSource yourself.
+   *
    * \param clock_type type of the clock.
    * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
    */
@@ -207,13 +214,13 @@ public:
   std::mutex &
   get_clock_mutex() noexcept;
 
-  // Add a callback to invoke if the jump threshold is exceeded.
+  /// Add a callback to invoke if the jump threshold is exceeded.
   /**
    * These callback functions must remain valid as long as the
    * returned shared pointer is valid.
    *
    * Function will register callbacks to the callback queue. On time jump all
-   * callbacks will be executed whose threshold is greater then the time jump;
+   * callbacks will be executed whose threshold is greater than the time jump;
    * The logic will first call selected pre_callbacks and then all selected
    * post_callbacks.
    *
@@ -222,7 +229,7 @@ public:
    * \param pre_callback Must be non-throwing
    * \param post_callback Must be non-throwing.
    * \param threshold Callbacks will be triggered if the time jump is greater
-   * then the threshold.
+   * than the threshold.
    * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
    * \throws std::bad_alloc if the allocation of the JumpHandler fails.
    * \warning the instance of the clock must remain valid as long as any created
@@ -250,6 +257,117 @@ private:
   std::shared_ptr<Impl> impl_;
 };
 
+/**
+ * A synchronization primitive, equal to std::conditional_variable,
+ * that works with the rclcpp::Clock.
+ *
+ * For more information on the API see https://en.cppreference.com/w/cpp/thread/condition_variable.
+ *
+ * Note, this class does not handle shutdowns, if you want to
+ * haven them handles as well, use ClockConditionalVariable.
+ */
+class ClockWaiter
+{
+private:
+  class ClockWaiterImpl;
+  std::unique_ptr<ClockWaiterImpl> impl_;
+
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(ClockWaiter)
+
+  RCLCPP_PUBLIC
+  explicit ClockWaiter(const rclcpp::Clock::SharedPtr & clock);
+
+  RCLCPP_PUBLIC
+  ~ClockWaiter();
+
+  /**
+   * Calling this function will block the current thread, until abs_time is reached,
+   * or pred returns true.
+   * @param lock A locked lock. The lock must be locked at call time, or this method will throw.
+   *             The lock will be atomically released and this thread will blocked.
+   * @param abs_time The time until which this thread shall be blocked.
+   * @param pred may be called in cased of spurious wakeups, but must be called every time
+   *             notify_one() was called. During the call to pred, the given lock will be locked.
+   *             This method will return, if pred returns true.
+   */
+  RCLCPP_PUBLIC
+  bool
+  wait_until(
+    std::unique_lock<std::mutex> & lock,
+    const rclcpp::Time & abs_time, const std::function<bool ()> & pred);
+
+  /**
+   * Notify the blocked thread, that it should reevaluate the wakeup condition.
+   * The given pred function in wait_until will be reevaluated and wait_until
+   * will return if it evaluates to true.
+   */
+  RCLCPP_PUBLIC
+  void
+  notify_one();
+};
+
+
+/**
+ * A synchronization primitive, similar to std::conditional_variable,
+ * that works with the rclcpp::Clock.
+ *
+ * For more information on the API see https://en.cppreference.com/w/cpp/thread/condition_variable.
+ *
+ * This primitive will wake up if the context was shut down.
+ */
+class ClockConditionalVariable
+{
+  class Impl;
+  std::unique_ptr<Impl> impl_;
+
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(ClockConditionalVariable)
+
+  RCLCPP_PUBLIC
+  ClockConditionalVariable(
+    const rclcpp::Clock::SharedPtr & clock,
+    rclcpp::Context::SharedPtr context = rclcpp::contexts::get_global_default_context());
+  RCLCPP_PUBLIC
+  ~ClockConditionalVariable();
+
+  /**
+   * Calling this function will block the current thread, until abs_time is reached,
+   * or pred returns true.
+   * @param lock A locked lock. The lock must be locked at call time, or this method will throw.
+   *             The lock will be atomically released and this thread will blocked.
+   *             The given lock must be created using the mutex returned my mutex().
+   * @param abs_time The time until which this thread shall be blocked.
+   * @param pred may be called in cased of spurious wakeups, but must be called every time
+   *             notify_one() was called. During the call to pred, the given lock will be locked.
+   *             This method will return, if pred returns true.
+   *
+   * @return true if until was reached.
+   */
+  RCLCPP_PUBLIC
+  bool
+  wait_until(
+    std::unique_lock<std::mutex> & lock, rclcpp::Time until,
+    const std::function<bool ()> & pred);
+
+  /**
+   * Notify the blocked thread, that is should reevaluate the wakeup condition.
+   * E.g. the given pred function in wait_until shall be reevaluated.
+   */
+  RCLCPP_PUBLIC
+  void
+  notify_one();
+
+  /**
+   * Returns the internal mutex. In order to be race free with the context shutdown,
+   * this mutex must be used for the wait_until call.
+   */
+  RCLCPP_PUBLIC
+  std::mutex &
+  mutex();
+};
+
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__CLOCK_HPP_

rclcpp/include/rclcpp/create_client.hpp

@@ -47,28 +47,9 @@ create_client(
   const std::string & service_name,
   const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
   rclcpp::CallbackGroup::SharedPtr group = nullptr)
-{
-  return create_client<ServiceT>(
-    node_base, node_graph, node_services,
-    service_name,
-    qos.get_rmw_qos_profile(),
-    group);
-}
-
-/// Create a service client with a given type.
-/// \internal
-template<typename ServiceT>
-typename rclcpp::Client<ServiceT>::SharedPtr
-create_client(
-  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
-  std::shared_ptr<node_interfaces::NodeGraphInterface> node_graph,
-  std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
-  const std::string & service_name,
-  const rmw_qos_profile_t & qos_profile,
-  rclcpp::CallbackGroup::SharedPtr group)
 {
   rcl_client_options_t options = rcl_client_get_default_options();
-  options.qos = qos_profile;
+  options.qos = qos.get_rmw_qos_profile();
 
   auto cli = rclcpp::Client<ServiceT>::make_shared(
     node_base.get(),
@@ -80,7 +61,6 @@ create_client(
   node_services->add_client(cli_base_ptr, group);
   return cli;
 }
-
 }  // namespace rclcpp
 
 #endif  // RCLCPP__CREATE_CLIENT_HPP_

rclcpp/include/rclcpp/create_generic_service.hpp

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

rclcpp/include/rclcpp/detail/qos_parameters.hpp

@@ -97,6 +97,9 @@ declare_parameter_or_get(
   rcl_interfaces::msg::ParameterDescriptor descriptor)
 {
   try {
+    // enable parameter modification to make it possible
+    // to declare QoS override parameters during parameter callbacks.
+    parameters_interface.enable_parameter_modification();
     return parameters_interface.declare_parameter(
       param_name, param_value, descriptor);
   } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) {

rclcpp/include/rclcpp/event_handler.hpp

@@ -20,6 +20,7 @@
 #include <mutex>
 #include <stdexcept>
 #include <string>
+#include <vector>
 
 #include "rcl/error_handling.h"
 #include "rcl/event_callback.h"
@@ -117,12 +118,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.
   /**
@@ -221,6 +222,13 @@ public:
     }
   }
 
+  RCLCPP_PUBLIC
+  std::vector<std::shared_ptr<rclcpp::TimerBase>>
+  get_timers() const override
+  {
+    return {};
+  }
+
 protected:
   RCLCPP_PUBLIC
   void
@@ -233,8 +241,6 @@ protected:
   size_t wait_set_event_index_;
 };
 
-using QOSEventHandlerBase [[deprecated("Use rclcpp::EventHandlerBase")]] = EventHandlerBase;
-
 template<typename EventCallbackT, typename ParentHandleT>
 class EventHandler : public EventHandlerBase
 {
@@ -280,21 +286,21 @@ public:
       RCUTILS_LOG_ERROR_NAMED(
         "rclcpp",
         "Couldn't take event info: %s", rcl_get_error_string().str);
+      rcl_reset_error();
       return nullptr;
     }
     return std::static_pointer_cast<void>(std::make_shared<EventCallbackInfoT>(callback_info));
   }
 
   std::shared_ptr<void>
-  take_data_by_entity_id(size_t id) override
+  take_data_by_entity_id([[maybe_unused]] size_t id) override
   {
-    (void)id;
     return take_data();
   }
 
   /// 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");
@@ -311,11 +317,6 @@ private:
   ParentHandleT parent_handle_;
   EventCallbackT event_callback_;
 };
-
-template<typename EventCallbackT, typename ParentHandleT>
-using QOSEventHandler [[deprecated("Use rclcpp::EventHandler")]] = EventHandler<EventCallbackT,
-    ParentHandleT>;
-
 }  // namespace rclcpp
 
 #endif  // RCLCPP__EVENT_HANDLER_HPP_

rclcpp/include/rclcpp/exceptions/exceptions.hpp

@@ -100,6 +100,15 @@ public:
   {}
 };
 
+class InvalidServiceTypeError : public std::runtime_error
+{
+public:
+  InvalidServiceTypeError()
+  : std::runtime_error("Service type is invalid.") {}
+  explicit InvalidServiceTypeError(const std::string & msg)
+  : std::runtime_error(msg) {}
+};
+
 class UnimplementedError : public std::runtime_error
 {
 public:

rclcpp/include/rclcpp/executor.hpp

@@ -29,26 +29,24 @@
 
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
+#include "rclcpp/executors/executor_notify_waitable.hpp"
 #include "rcpputils/scope_exit.hpp"
 
 #include "rclcpp/context.hpp"
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/guard_condition.hpp"
 #include "rclcpp/executor_options.hpp"
+#include "rclcpp/executors/executor_entities_collection.hpp"
+#include "rclcpp/executors/executor_entities_collector.hpp"
 #include "rclcpp/future_return_code.hpp"
-#include "rclcpp/memory_strategies.hpp"
-#include "rclcpp/memory_strategy.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/wait_set.hpp"
 
 namespace rclcpp
 {
 
-typedef std::map<rclcpp::CallbackGroup::WeakPtr,
-    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
-    std::owner_less<rclcpp::CallbackGroup::WeakPtr>> WeakCallbackGroupsToNodesMap;
-
 // Forward declaration is used in convenience method signature.
 class Node;
 class ExecutorImplementation;
@@ -274,12 +272,12 @@ 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 max duration.
@@ -289,6 +287,18 @@ public:
    * Adding subscriptions, timers, services, etc. with blocking or long running
    * callbacks may cause the function exceed the max_duration significantly.
    *
+   * Work that is ready to be done is collected only once, and when collecting that work
+   * entities which may have multiple pieces of work ready will only be executed at most
+   * one time.
+   * The reason for this is that it is not possible to tell if, for example, a ready
+   * subscription has only one message ready or multiple without checking again.
+   * Because, in order to find out if there are multiple messages, one message must
+   * be taken and executed before checking again if that subscription is still ready.
+   * However, this function only checks for ready entities to work on once,
+   * and so it will never execute a single entity more than once per call to this function.
+   * See spin_all() variants for a function that will repeatedly work on a single entity
+   * in a single call.
+   *
    * If there is no work to be done when this called, it will return immediately
    * because the collecting of available work is non-blocking.
    * Before each piece of ready work is executed this function checks if the
@@ -309,14 +319,14 @@ public:
    * \param[in] node Shared pointer to the node to add.
    */
   RCLCPP_PUBLIC
-  void
+  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
-  void
+  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.
@@ -368,52 +378,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.
@@ -422,20 +392,9 @@ public:
    * \throws std::runtime_error if there is an issue triggering the guard condition
    */
   RCLCPP_PUBLIC
-  void
+  virtual void
   cancel();
 
-  /// Support dynamic switching of the memory strategy.
-  /**
-   * Switching the memory strategy while the executor is spinning in another threading could have
-   * unintended consequences.
-   * \param[in] memory_strategy Shared pointer to the memory strategy to set.
-   * \throws std::runtime_error if memory_strategy is null
-   */
-  RCLCPP_PUBLIC
-  void
-  set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy);
-
   /// Returns true if the executor is currently spinning.
   /**
    * This function can be called asynchronously from any thread.
@@ -446,6 +405,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
@@ -460,6 +427,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.
@@ -500,7 +484,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.
   /**
@@ -520,6 +504,11 @@ protected:
   static void
   execute_client(rclcpp::ClientBase::SharedPtr client);
 
+  /// Gather all of the waitable entities from associated nodes and callback groups.
+  RCLCPP_PUBLIC
+  void
+  collect_entities();
+
   /// Block until more work becomes avilable or timeout is reached.
   /**
    * Builds a set of waitable entities, which are passed to the middleware.
@@ -531,62 +520,6 @@ protected:
   void
   wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
-  /// Find node associated with a callback group
-  /**
-   * \param[in] weak_groups_to_nodes map of callback groups to nodes
-   * \param[in] group callback group to find assocatiated node
-   * \return Pointer to associated node if found, else nullptr
-   */
-  RCLCPP_PUBLIC
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
-  get_node_by_group(
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
-    rclcpp::CallbackGroup::SharedPtr group);
-
-  /// Return true if the node has been added to this executor.
-  /**
-   * \param[in] node_ptr a shared pointer that points to a node base interface
-   * \param[in] weak_groups_to_nodes map to nodes to lookup
-   * \return true if the node is associated with the executor, otherwise false
-   */
-  RCLCPP_PUBLIC
-  bool
-  has_node(
-    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const;
-
-  /// Find the callback group associated with a timer
-  /**
-   * \param[in] timer Timer to find associated callback group
-   * \return Pointer to callback group node if found, else nullptr
-   */
-  RCLCPP_PUBLIC
-  rclcpp::CallbackGroup::SharedPtr
-  get_group_by_timer(rclcpp::TimerBase::SharedPtr timer);
-
-  /// Add a callback group to an executor
-  /**
-   * \see rclcpp::Executor::add_callback_group
-   */
-  RCLCPP_PUBLIC
-  virtual void
-  add_callback_group_to_map(
-    rclcpp::CallbackGroup::SharedPtr group_ptr,
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
-    bool notify = true) RCPPUTILS_TSA_REQUIRES(mutex_);
-
-  /// Remove a callback group from the executor.
-  /**
-   * \see rclcpp::Executor::remove_callback_group
-   */
-  RCLCPP_PUBLIC
-  virtual void
-  remove_callback_group_from_map(
-    rclcpp::CallbackGroup::SharedPtr group_ptr,
-    WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
-    bool notify = true) RCPPUTILS_TSA_REQUIRES(mutex_);
-
   /// Check for executable in ready state and populate union structure.
   /**
    * \param[out] any_executable populated union structure of ready executable
@@ -597,33 +530,6 @@ protected:
   bool
   get_next_ready_executable(AnyExecutable & any_executable);
 
-  /// Check for executable in ready state and populate union structure.
-  /**
-   * This is the implementation of get_next_ready_executable that takes into
-   * account the current state of callback groups' association with nodes and
-   * executors.
-   *
-   * This checks in a particular order for available work:
-   *   * Timers
-   *   * Subscriptions
-   *   * Services
-   *   * Clients
-   *   * Waitable
-   *
-   * If the next executable is not associated with this executor/node pair,
-   * then this method will return false.
-   *
-   * \param[out] any_executable populated union structure of ready executable
-   * \param[in] weak_groups_to_nodes mapping of callback groups to nodes
-   * \return true if an executable was ready and any_executable was populated,
-   *   otherwise false
-   */
-  RCLCPP_PUBLIC
-  bool
-  get_next_ready_executable_from_map(
-    AnyExecutable & any_executable,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
-
   /// Wait for executable in ready state and populate union structure.
   /**
    * If an executable is ready, it will return immediately, otherwise
@@ -641,20 +547,15 @@ protected:
     AnyExecutable & any_executable,
     std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
-  /// Add all callback groups that can be automatically added from associated nodes.
+  /// This function triggers a recollect of all entities that are registered to the executor.
   /**
-   * The executor, before collecting entities, verifies if any callback group from
-   * nodes associated with the executor, which is not already associated to an executor,
-   * can be automatically added to this executor.
-   * This takes care of any callback group that has been added to a node but not explicitly added
-   * to the executor.
-   * It is important to note that in order for the callback groups to be automatically added to an
-   * executor through this function, the node of the callback groups needs to have been added
-   * through the `add_node` method.
+   * Calling this function is thread safe.
+   *
+   * \param[in] notify if true will execute a trigger that will wake up a waiting executor
    */
   RCLCPP_PUBLIC
   virtual void
-  add_callback_groups_from_nodes_associated_to_executor() RCPPUTILS_TSA_REQUIRES(mutex_);
+  handle_updated_entities(bool notify);
 
   /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins.
   std::atomic_bool spinning;
@@ -665,16 +566,8 @@ protected:
   /// Guard condition for signaling the rmw layer to wake up for system shutdown.
   std::shared_ptr<rclcpp::GuardCondition> shutdown_guard_condition_;
 
-  /// Wait set for managing entities that the rmw layer waits on.
-  rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
-
-  // Mutex to protect the subsequent memory_strategy_.
   mutable std::mutex mutex_;
 
-  /// The memory strategy: an interface for handling user-defined memory allocation strategies.
-  memory_strategy::MemoryStrategy::SharedPtr
-  memory_strategy_ RCPPUTILS_TSA_PT_GUARDED_BY(mutex_);
-
   /// The context associated with this executor.
   std::shared_ptr<rclcpp::Context> context_;
 
@@ -684,39 +577,31 @@ protected:
   virtual void
   spin_once_impl(std::chrono::nanoseconds timeout);
 
-  typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
-      const rclcpp::GuardCondition *,
-      std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
-    WeakNodesToGuardConditionsMap;
+  /// Waitable containing guard conditions controlling the executor flow.
+  /**
+   * This waitable contains the interrupt and shutdown guard condition, as well
+   * as the guard condition associated with each node and callback group.
+   * By default, if any change is detected in the monitored entities, the notify
+   * waitable will awake the executor and rebuild the collections.
+   */
+  std::shared_ptr<rclcpp::executors::ExecutorNotifyWaitable> notify_waitable_;
 
-  typedef std::map<rclcpp::CallbackGroup::WeakPtr,
-      const rclcpp::GuardCondition *,
-      std::owner_less<rclcpp::CallbackGroup::WeakPtr>>
-    WeakCallbackGroupsToGuardConditionsMap;
+  std::atomic_bool entities_need_rebuild_;
 
-  /// maps nodes to guard conditions
-  WeakNodesToGuardConditionsMap
-  weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+  /// Collector used to associate executable entities from nodes and guard conditions
+  rclcpp::executors::ExecutorEntitiesCollector collector_;
 
-  /// maps callback groups to guard conditions
-  WeakCallbackGroupsToGuardConditionsMap
-  weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+  /// 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_);
 
-  /// maps callback groups associated to nodes
-  WeakCallbackGroupsToNodesMap
-  weak_groups_associated_with_executor_to_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+  /// Hold the current state of the collection being waited on by the waitset
+  rclcpp::executors::ExecutorEntitiesCollection current_collection_ RCPPUTILS_TSA_GUARDED_BY(
+    mutex_);
 
-  /// maps callback groups to nodes associated with executor
-  WeakCallbackGroupsToNodesMap
-  weak_groups_to_nodes_associated_with_executor_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
-
-  /// maps all callback groups to nodes
-  WeakCallbackGroupsToNodesMap
-  weak_groups_to_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
-
-  /// nodes that are associated with the executor
-  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>
-  weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+  /// Hold the current state of the notify waitable being waited on by the waitset
+  std::shared_ptr<rclcpp::executors::ExecutorNotifyWaitable> current_notify_waitable_
+  RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
   /// shutdown callback handle registered to Context
   rclcpp::OnShutdownCallbackHandle shutdown_callback_handle_;

rclcpp/include/rclcpp/executor_options.hpp

@@ -15,6 +15,8 @@
 #ifndef RCLCPP__EXECUTOR_OPTIONS_HPP_
 #define RCLCPP__EXECUTOR_OPTIONS_HPP_
 
+#include <memory>
+
 #include "rclcpp/context.hpp"
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/memory_strategies.hpp"
@@ -24,18 +26,30 @@
 namespace rclcpp
 {
 
+class ExecutorOptionsImplementation;
+
 /// Options to be passed to the executor constructor.
 struct ExecutorOptions
 {
-  ExecutorOptions()
-  : memory_strategy(rclcpp::memory_strategies::create_default_strategy()),
-    context(rclcpp::contexts::get_global_default_context()),
-    max_conditions(0)
-  {}
+  RCLCPP_PUBLIC
+  ExecutorOptions();
+
+  RCLCPP_PUBLIC
+  virtual ~ExecutorOptions();
+
+  RCLCPP_PUBLIC
+  ExecutorOptions(const ExecutorOptions &);
+
+  RCLCPP_PUBLIC
+  ExecutorOptions & operator=(const ExecutorOptions &);
 
   rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy;
   rclcpp::Context::SharedPtr context;
   size_t max_conditions;
+
+private:
+  /// Pointer to implementation
+  std::unique_ptr<ExecutorOptionsImplementation> impl_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/executors.hpp

@@ -120,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

@@ -19,6 +19,7 @@
 #include <memory>
 #include <mutex>
 #include <set>
+#include <vector>
 
 #include "rclcpp/guard_condition.hpp"
 #include "rclcpp/waitable.hpp"
@@ -41,18 +42,19 @@ public:
    *   of this waitable has signaled the wait_set.
    */
   RCLCPP_PUBLIC
-  explicit ExecutorNotifyWaitable(std::function<void(void)> on_execute_callback = {});
+  explicit ExecutorNotifyWaitable(
+    std::function<void(void)> on_execute_callback = {}, const rclcpp::Context::SharedPtr & context =
+    rclcpp::contexts::get_global_default_context());
 
   // Destructor
   RCLCPP_PUBLIC
   ~ExecutorNotifyWaitable() override = default;
 
   RCLCPP_PUBLIC
-  ExecutorNotifyWaitable(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 +62,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 +71,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 +80,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.
   /**
@@ -123,6 +125,14 @@ public:
   void
   clear_on_ready_callback() override;
 
+  /// Set a new callback to be called whenever this waitable is executed.
+  /**
+   * \param[in] on_execute_callback The new callback
+   */
+  RCLCPP_PUBLIC
+  void
+  set_execute_callback(std::function<void(void)> on_execute_callback);
+
   /// Remove a guard condition from being waited on.
   /**
    * \param[in] weak_guard_condition The guard condition to remove.
@@ -139,17 +149,37 @@ public:
   size_t
   get_number_of_ready_guard_conditions() override;
 
+  /// Returns the number of used Timers
+  /**
+   * Will always return an empty vector.
+   */
+  RCLCPP_PUBLIC
+  std::vector<std::shared_ptr<rclcpp::TimerBase>>
+  get_timers() const override;
+
 private:
   /// Callback to run when waitable executes
   std::function<void(void)> execute_callback_;
 
+  /// Mutex to procetect the guard conditions
   std::mutex guard_condition_mutex_;
+  /// Mutex to protect the execute callback
+  std::mutex execute_mutex_;
 
   std::function<void(size_t)> on_ready_callback_;
 
   /// The collection of guard conditions to be waited on.
-  std::set<rclcpp::GuardCondition::WeakPtr,
-    std::owner_less<rclcpp::GuardCondition::WeakPtr>> notify_guard_conditions_;
+  std::set<rclcpp::GuardCondition::SharedPtr> notify_guard_conditions_;
+
+  /// The indixes were our guard conditions were stored in the
+  /// rcl waitset
+  std::vector<size_t> idxs_of_added_guard_condition_;
+
+  /// set to true, if we got a pending trigger
+  bool needs_processing = false;
+
+  /// A guard condition needed to generate wakeups
+  rclcpp::GuardCondition::SharedPtr guard_condition_;
 };
 
 }  // namespace executors

rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp

@@ -1,357 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_
-#define RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_
-
-#include <chrono>
-#include <list>
-#include <map>
-#include <memory>
-#include <vector>
-
-#include "rcl/guard_condition.h"
-#include "rcl/wait.h"
-
-#include "rclcpp/experimental/executable_list.hpp"
-#include "rclcpp/macros.hpp"
-#include "rclcpp/memory_strategy.hpp"
-#include "rclcpp/visibility_control.hpp"
-#include "rclcpp/waitable.hpp"
-
-namespace rclcpp
-{
-namespace executors
-{
-typedef std::map<rclcpp::CallbackGroup::WeakPtr,
-    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
-    std::owner_less<rclcpp::CallbackGroup::WeakPtr>> WeakCallbackGroupsToNodesMap;
-
-class StaticExecutorEntitiesCollector final
-  : public rclcpp::Waitable,
-  public std::enable_shared_from_this<StaticExecutorEntitiesCollector>
-{
-public:
-  RCLCPP_SMART_PTR_DEFINITIONS(StaticExecutorEntitiesCollector)
-
-  // Constructor
-  RCLCPP_PUBLIC
-  StaticExecutorEntitiesCollector() = default;
-
-  // Destructor
-  RCLCPP_PUBLIC
-  ~StaticExecutorEntitiesCollector();
-
-  /// Initialize StaticExecutorEntitiesCollector
-  /**
-   * \param p_wait_set A reference to the wait set to be used in the executor
-   * \param memory_strategy Shared pointer to the memory strategy to set.
-   * \throws std::runtime_error if memory strategy is null
-   */
-  RCLCPP_PUBLIC
-  void
-  init(
-    rcl_wait_set_t * p_wait_set,
-    rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy);
-
-  /// Finalize StaticExecutorEntitiesCollector to clear resources
-  RCLCPP_PUBLIC
-  bool
-  is_init() {return initialized_;}
-
-  RCLCPP_PUBLIC
-  void
-  fini();
-
-  /// Execute the waitable.
-  RCLCPP_PUBLIC
-  void
-  execute(std::shared_ptr<void> & data) override;
-
-  /// Take the data so that it can be consumed with `execute`.
-  /**
-   * For `StaticExecutorEntitiesCollector`, this always return `nullptr`.
-   * \sa rclcpp::Waitable::take_data()
-   */
-  RCLCPP_PUBLIC
-  std::shared_ptr<void>
-  take_data() override;
-
-  /// Function to add_handles_to_wait_set and wait for work and
-  /**
-   * block until the wait set is ready or until the timeout has been exceeded.
-   * \throws std::runtime_error if wait set couldn't be cleared or filled.
-   * \throws any rcl errors from rcl_wait, \see rclcpp::exceptions::throw_from_rcl_error()
-   */
-  RCLCPP_PUBLIC
-  void
-  refresh_wait_set(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
-
-  /**
-   * \throws std::runtime_error if it couldn't add guard condition to wait set
-   */
-  RCLCPP_PUBLIC
-  void
-  add_to_wait_set(rcl_wait_set_t * wait_set) override;
-
-  RCLCPP_PUBLIC
-  size_t
-  get_number_of_ready_guard_conditions() override;
-
-  /// Add a callback group to an executor.
-  /**
-   * \see rclcpp::Executor::add_callback_group
-   */
-  RCLCPP_PUBLIC
-  bool
-  add_callback_group(
-    rclcpp::CallbackGroup::SharedPtr group_ptr,
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
-
-  /// Add a callback group to an executor.
-  /**
-   * \see rclcpp::Executor::add_callback_group
-   * \return boolean whether the node from the callback group is new
-   */
-  RCLCPP_PUBLIC
-  bool
-  add_callback_group(
-    rclcpp::CallbackGroup::SharedPtr group_ptr,
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
-
-  /// Remove a callback group from the executor.
-  /**
-   * \see rclcpp::Executor::remove_callback_group
-   */
-  RCLCPP_PUBLIC
-  bool
-  remove_callback_group(
-    rclcpp::CallbackGroup::SharedPtr group_ptr);
-
-  /// Remove a callback group from the executor.
-  /**
-   * \see rclcpp::Executor::remove_callback_group_from_map
-   */
-  RCLCPP_PUBLIC
-  bool
-  remove_callback_group_from_map(
-    rclcpp::CallbackGroup::SharedPtr group_ptr,
-    WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
-
-  /**
-   * \see rclcpp::Executor::add_node()
-   * \throw std::runtime_error if node was already added
-   */
-  RCLCPP_PUBLIC
-  bool
-  add_node(
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
-
-  /**
-   * \see rclcpp::Executor::remove_node()
-   * \throw std::runtime_error if no guard condition is associated with node.
-   */
-  RCLCPP_PUBLIC
-  bool
-  remove_node(
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
-
-  RCLCPP_PUBLIC
-  std::vector<rclcpp::CallbackGroup::WeakPtr>
-  get_all_callback_groups();
-
-  /// Get callback groups that belong to executor.
-  /**
-   * \see rclcpp::Executor::get_manually_added_callback_groups()
-   */
-  RCLCPP_PUBLIC
-  std::vector<rclcpp::CallbackGroup::WeakPtr>
-  get_manually_added_callback_groups();
-
-  /// Get callback groups that belong to executor.
-  /**
-   * \see rclcpp::Executor::get_automatically_added_callback_groups_from_nodes()
-   */
-  RCLCPP_PUBLIC
-  std::vector<rclcpp::CallbackGroup::WeakPtr>
-  get_automatically_added_callback_groups_from_nodes();
-
-  /// Complete all available queued work without blocking.
-  /**
-   * This function checks if after the guard condition was triggered
-   * (or a spurious wakeup happened) we are really ready to execute
-   * i.e. re-collect entities
-   */
-  RCLCPP_PUBLIC
-  bool
-  is_ready(rcl_wait_set_t * wait_set) override;
-
-  /// Return number of timers
-  /**
-   * \return number of timers
-   */
-  RCLCPP_PUBLIC
-  size_t
-  get_number_of_timers() {return exec_list_.number_of_timers;}
-
-  /// Return number of subscriptions
-  /**
-   * \return number of subscriptions
-   */
-  RCLCPP_PUBLIC
-  size_t
-  get_number_of_subscriptions() {return exec_list_.number_of_subscriptions;}
-
-  /// Return number of services
-  /**
-   * \return number of services
-   */
-  RCLCPP_PUBLIC
-  size_t
-  get_number_of_services() {return exec_list_.number_of_services;}
-
-  /// Return number of clients
-  /**
-   * \return number of clients
-   */
-  RCLCPP_PUBLIC
-  size_t
-  get_number_of_clients() {return exec_list_.number_of_clients;}
-
-  /// Return number of waitables
-  /**
-   * \return number of waitables
-   */
-  RCLCPP_PUBLIC
-  size_t
-  get_number_of_waitables() {return exec_list_.number_of_waitables;}
-
-  /** Return a SubscritionBase Sharedptr by index.
-   * \param[in] i The index of the SubscritionBase
-   * \return a SubscritionBase shared pointer
-   * \throws std::out_of_range if the argument is higher than the size of the structrue.
-   */
-  RCLCPP_PUBLIC
-  rclcpp::SubscriptionBase::SharedPtr
-  get_subscription(size_t i) {return exec_list_.subscription[i];}
-
-  /** Return a TimerBase Sharedptr by index.
-   * \param[in] i The index of the TimerBase
-   * \return a TimerBase shared pointer
-   * \throws std::out_of_range if the argument is higher than the size.
-   */
-  RCLCPP_PUBLIC
-  rclcpp::TimerBase::SharedPtr
-  get_timer(size_t i) {return exec_list_.timer[i];}
-
-  /** Return a ServiceBase Sharedptr by index.
-   * \param[in] i The index of the ServiceBase
-   * \return a ServiceBase shared pointer
-   * \throws std::out_of_range if the argument is higher than the size.
-   */
-  RCLCPP_PUBLIC
-  rclcpp::ServiceBase::SharedPtr
-  get_service(size_t i) {return exec_list_.service[i];}
-
-  /** Return a ClientBase Sharedptr by index
-   * \param[in] i The index of the ClientBase
-   * \return a ClientBase shared pointer
-   * \throws std::out_of_range if the argument is higher than the size.
-   */
-  RCLCPP_PUBLIC
-  rclcpp::ClientBase::SharedPtr
-  get_client(size_t i) {return exec_list_.client[i];}
-
-  /** Return a Waitable Sharedptr by index
-   * \param[in] i The index of the Waitable
-   * \return a Waitable shared pointer
-   * \throws std::out_of_range if the argument is higher than the size.
-   */
-  RCLCPP_PUBLIC
-  rclcpp::Waitable::SharedPtr
-  get_waitable(size_t i) {return exec_list_.waitable[i];}
-
-private:
-  /// Function to reallocate space for entities in the wait set.
-  /**
-   * \throws std::runtime_error if wait set couldn't be cleared or resized.
-   */
-  void
-  prepare_wait_set();
-
-  void
-  fill_executable_list();
-
-  void
-  fill_memory_strategy();
-
-  /// Return true if the node belongs to the collector
-  /**
-   * \param[in] node_ptr a node base interface shared pointer
-   * \param[in] weak_groups_to_nodes map to nodes to lookup
-   * \return boolean whether a node belongs the collector
-   */
-  bool
-  has_node(
-    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const;
-
-  /// Add all callback groups that can be automatically added by any executor
-  /// and is not already associated with an executor from nodes
-  /// that are associated with executor
-  /**
-   * \see rclcpp::Executor::add_callback_groups_from_nodes_associated_to_executor()
-   */
-  void
-  add_callback_groups_from_nodes_associated_to_executor();
-
-  void
-  fill_executable_list_from_map(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
-
-  /// Memory strategy: an interface for handling user-defined memory allocation strategies.
-  rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;
-
-  // maps callback groups to nodes.
-  WeakCallbackGroupsToNodesMap weak_groups_associated_with_executor_to_nodes_;
-  // maps callback groups to nodes.
-  WeakCallbackGroupsToNodesMap weak_groups_to_nodes_associated_with_executor_;
-
-  typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
-      const rclcpp::GuardCondition *,
-      std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
-    WeakNodesToGuardConditionsMap;
-  WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_;
-
-  /// List of weak nodes registered in the static executor
-  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
-
-  // Mutex to protect vector of new nodes.
-  std::mutex new_nodes_mutex_;
-  std::vector<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> new_nodes_;
-
-  /// Wait set for managing entities that the rmw layer waits on.
-  rcl_wait_set_t * p_wait_set_ = nullptr;
-
-  /// Executable list: timers, subscribers, clients, services and waitables
-  rclcpp::experimental::ExecutableList exec_list_;
-
-  /// Bool to check if the entities collector has been initialized
-  bool initialized_ = false;
-};
-
-}  // namespace executors
-}  // namespace rclcpp
-
-#endif  // RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_

rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp

@@ -15,24 +15,13 @@
 #ifndef RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_
 #define RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_
 
+#include <atomic>
 #include <chrono>
-#include <cassert>
-#include <cstdlib>
 #include <memory>
-#include <vector>
-#include <string>
-
-#include "rmw/rmw.h"
 
 #include "rclcpp/executor.hpp"
-#include "rclcpp/executors/static_executor_entities_collector.hpp"
-#include "rclcpp/experimental/executable_list.hpp"
-#include "rclcpp/macros.hpp"
-#include "rclcpp/memory_strategies.hpp"
-#include "rclcpp/node.hpp"
-#include "rclcpp/rate.hpp"
-#include "rclcpp/utilities.hpp"
-#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/executors/executor_entities_collection.hpp"
+#include "rclcpp/executors/single_threaded_executor.hpp"
 
 namespace rclcpp
 {
@@ -42,9 +31,15 @@ namespace executors
 /// Static executor implementation
 /**
  * This executor is a static version of the original single threaded executor.
- * It's static because it doesn't reconstruct the executable list for every iteration.
+ * It contains some performance optimization to avoid unnecessary reconstructions of
+ * the executable list for every iteration.
  * All nodes, callbackgroups, timers, subscriptions etc. are created before
  * spin() is called, and modified only when an entity is added/removed to/from a node.
+ * This executor is deprecated because these performance improvements have now been
+ * applied to all other executors.
+ * This executor is also considered unstable due to known bugs.
+ * See the unit-tests that are only applied to `StandardExecutors` for information
+ * on the known limitations.
  *
  * To run this executor instead of SingleThreadedExecutor replace:
  * rclcpp::executors::SingleThreadedExecutor exec;
@@ -55,7 +50,8 @@ namespace executors
  * exec.spin();
  * exec.remove_node(node);
  */
-class StaticSingleThreadedExecutor : public rclcpp::Executor
+class [[deprecated("Use rclcpp::executors::SingleThreadedExecutor")]] StaticSingleThreadedExecutor
+  : public rclcpp::Executor
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(StaticSingleThreadedExecutor)
@@ -65,7 +61,7 @@ public:
   explicit StaticSingleThreadedExecutor(
     const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
 
-  /// Default destrcutor.
+  /// Default destructor.
   RCLCPP_PUBLIC
   virtual ~StaticSingleThreadedExecutor();
 
@@ -116,105 +112,31 @@ public:
   void
   spin_all(std::chrono::nanoseconds max_duration) override;
 
-  /// Add a callback group to an executor.
-  /**
-   * \sa rclcpp::Executor::add_callback_group
-   */
-  RCLCPP_PUBLIC
-  void
-  add_callback_group(
-    rclcpp::CallbackGroup::SharedPtr group_ptr,
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    bool notify = true) override;
-
-  /// Remove callback group from the executor
-  /**
-   * \sa rclcpp::Executor::remove_callback_group
-   */
-  RCLCPP_PUBLIC
-  void
-  remove_callback_group(
-    rclcpp::CallbackGroup::SharedPtr group_ptr,
-    bool notify = true) override;
-
-  /// Add a node to the executor.
-  /**
-   * \sa rclcpp::Executor::add_node
-   */
-  RCLCPP_PUBLIC
-  void
-  add_node(
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    bool notify = true) override;
-
-  /// Convenience function which takes Node and forwards NodeBaseInterface.
-  /**
-   * \sa rclcpp::StaticSingleThreadedExecutor::add_node
-   */
-  RCLCPP_PUBLIC
-  void
-  add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
-
-  /// Remove a node from the executor.
-  /**
-   * \sa rclcpp::Executor::remove_node
-   */
-  RCLCPP_PUBLIC
-  void
-  remove_node(
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    bool notify = true) override;
-
-  /// Convenience function which takes Node and forwards NodeBaseInterface.
-  /**
-   * \sa rclcpp::Executor::remove_node
-   */
-  RCLCPP_PUBLIC
-  void
-  remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
-
-  RCLCPP_PUBLIC
-  std::vector<rclcpp::CallbackGroup::WeakPtr>
-  get_all_callback_groups() override;
-
-  /// Get callback groups that belong to executor.
-  /**
-   * \sa rclcpp::Executor::get_manually_added_callback_groups()
-   */
-  RCLCPP_PUBLIC
-  std::vector<rclcpp::CallbackGroup::WeakPtr>
-  get_manually_added_callback_groups() override;
-
-  /// Get callback groups that belong to executor.
-  /**
-   * \sa rclcpp::Executor::get_automatically_added_callback_groups_from_nodes()
-   */
-  RCLCPP_PUBLIC
-  std::vector<rclcpp::CallbackGroup::WeakPtr>
-  get_automatically_added_callback_groups_from_nodes() override;
-
 protected:
   /**
    * @brief Executes ready executables from wait set.
+   * @param collection entities to evaluate for ready executables.
+   * @param wait_result result to check for ready executables.
    * @param spin_once if true executes only the first ready executable.
    * @return true if any executable was ready.
    */
-  RCLCPP_PUBLIC
   bool
-  execute_ready_executables(bool spin_once = false);
+  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)
-
-  StaticExecutorEntitiesCollector::SharedPtr entities_collector_;
 };
 
 }  // namespace executors

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

@@ -17,6 +17,7 @@
 
 #include <memory>
 #include <mutex>
+#include <shared_mutex>
 #include <stdexcept>
 #include <utility>
 #include <vector>
@@ -69,7 +70,7 @@ public:
    */
   void enqueue(BufferT request) override
   {
-    std::lock_guard<std::mutex> lock(mutex_);
+    std::unique_lock lock(mutex_);
 
     write_index_ = next_(write_index_);
     ring_buffer_[write_index_] = std::move(request);
@@ -95,7 +96,7 @@ public:
    */
   BufferT dequeue() override
   {
-    std::lock_guard<std::mutex> lock(mutex_);
+    std::unique_lock lock(mutex_);
 
     if (!has_data_()) {
       return BufferT();
@@ -134,7 +135,7 @@ public:
    */
   inline size_t next(size_t val)
   {
-    std::lock_guard<std::mutex> lock(mutex_);
+    std::unique_lock lock(mutex_);
     return next_(val);
   }
 
@@ -146,7 +147,7 @@ public:
    */
   inline bool has_data() const override
   {
-    std::lock_guard<std::mutex> lock(mutex_);
+    std::shared_lock lock(mutex_);
     return has_data_();
   }
 
@@ -159,7 +160,7 @@ public:
    */
   inline bool is_full() const
   {
-    std::lock_guard<std::mutex> lock(mutex_);
+    std::shared_lock lock(mutex_);
     return is_full_();
   }
 
@@ -171,13 +172,15 @@ public:
    */
   size_t available_capacity() const override
   {
-    std::lock_guard<std::mutex> lock(mutex_);
+    std::shared_lock lock(mutex_);
     return available_capacity_();
   }
 
   void clear() override
   {
     TRACETOOLS_TRACEPOINT(rclcpp_ring_buffer_clear, static_cast<const void *>(this));
+    std::unique_lock lock(mutex_);
+    clear_();
   }
 
 private:
@@ -227,6 +230,14 @@ private:
     return capacity_ - size_;
   }
 
+  inline void clear_()
+  {
+    ring_buffer_.clear();
+    size_ = 0;
+    read_index_ = 0;
+    write_index_ = capacity_ - 1;
+  }
+
   /// Traits for checking if a type is std::unique_ptr
   template<typename ...>
   struct is_std_unique_ptr final : std::false_type {};
@@ -251,7 +262,7 @@ private:
     void> * = nullptr>
   std::vector<BufferT> get_all_data_impl()
   {
-    std::lock_guard<std::mutex> lock(mutex_);
+    std::unique_lock lock(mutex_);
     std::vector<BufferT> result_vtr;
     result_vtr.reserve(size_);
     for (size_t id = 0; id < size_; ++id) {
@@ -266,7 +277,7 @@ private:
       std::is_copy_constructible<T>::value, void> * = nullptr>
   std::vector<BufferT> get_all_data_impl()
   {
-    std::lock_guard<std::mutex> lock(mutex_);
+    std::unique_lock lock(mutex_);
     std::vector<BufferT> result_vtr;
     result_vtr.reserve(size_);
     for (size_t id = 0; id < size_; ++id) {
@@ -292,7 +303,7 @@ private:
     return {};
   }
 
-  size_t capacity_;
+  const size_t capacity_;
 
   std::vector<BufferT> ring_buffer_;
 
@@ -300,7 +311,7 @@ private:
   size_t read_index_;
   size_t size_;
 
-  mutable std::mutex mutex_;
+  mutable std::shared_mutex mutex_;
 };
 
 }  // namespace buffers

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

@@ -59,8 +59,6 @@ namespace executors
  */
 class EventsExecutor : public rclcpp::Executor
 {
-  friend class EventsExecutorEntitiesCollector;
-
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(EventsExecutor)
 
@@ -72,7 +70,7 @@ public:
    * \param[in] options Options used to configure the executor.
    */
   RCLCPP_PUBLIC
-  explicit EventsExecutor(
+  EventsExecutor(
     rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue = std::make_unique<
       rclcpp::experimental::executors::SimpleEventsQueue>(),
     bool execute_timers_separate_thread = false,
@@ -128,87 +126,6 @@ public:
   void
   spin_all(std::chrono::nanoseconds max_duration) override;
 
-  /// Add a node to the executor.
-  /**
-   * \sa rclcpp::Executor::add_node
-   */
-  RCLCPP_PUBLIC
-  void
-  add_node(
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    bool notify = true) override;
-
-  /// Convenience function which takes Node and forwards NodeBaseInterface.
-  /**
-   * \sa rclcpp::EventsExecutor::add_node
-   */
-  RCLCPP_PUBLIC
-  void
-  add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
-
-  /// Remove a node from the executor.
-  /**
-   * \sa rclcpp::Executor::remove_node
-   */
-  RCLCPP_PUBLIC
-  void
-  remove_node(
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    bool notify = true) override;
-
-  /// Convenience function which takes Node and forwards NodeBaseInterface.
-  /**
-   * \sa rclcpp::Executor::remove_node
-   */
-  RCLCPP_PUBLIC
-  void
-  remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
-
-  /// Add a callback group to an executor.
-  /**
-   * \sa rclcpp::Executor::add_callback_group
-   */
-  RCLCPP_PUBLIC
-  void
-  add_callback_group(
-    rclcpp::CallbackGroup::SharedPtr group_ptr,
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    bool notify = true) override;
-
-  /// Remove callback group from the executor
-  /**
-   * \sa rclcpp::Executor::remove_callback_group
-   */
-  RCLCPP_PUBLIC
-  void
-  remove_callback_group(
-    rclcpp::CallbackGroup::SharedPtr group_ptr,
-    bool notify = true) override;
-
-  /// Get callback groups that belong to executor.
-  /**
-   * \sa rclcpp::Executor::get_all_callback_groups()
-   */
-  RCLCPP_PUBLIC
-  std::vector<rclcpp::CallbackGroup::WeakPtr>
-  get_all_callback_groups() override;
-
-  /// Get callback groups that belong to executor.
-  /**
-   * \sa rclcpp::Executor::get_manually_added_callback_groups()
-   */
-  RCLCPP_PUBLIC
-  std::vector<rclcpp::CallbackGroup::WeakPtr>
-  get_manually_added_callback_groups() override;
-
-  /// Get callback groups that belong to executor.
-  /**
-   * \sa rclcpp::Executor::get_automatically_added_callback_groups_from_nodes()
-   */
-  RCLCPP_PUBLIC
-  std::vector<rclcpp::CallbackGroup::WeakPtr>
-  get_automatically_added_callback_groups_from_nodes() override;
-
 protected:
   /// Internal implementation of spin_once
   RCLCPP_PUBLIC
@@ -220,6 +137,11 @@ protected:
   void
   spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive);
 
+  /// Collect entities from callback groups and refresh the current collection with them
+  RCLCPP_PUBLIC
+  void
+  handle_updated_entities(bool notify) override;
+
 private:
   RCLCPP_DISABLE_COPY(EventsExecutor)
 
@@ -227,9 +149,9 @@ private:
   void
   execute_event(const ExecutorEvent & event);
 
-  /// Collect entities from callback groups and refresh the current collection with them
+  /// Rebuilds the executor's notify waitable, as we can't use the one built in the base class
   void
-  refresh_current_collection_from_callback_groups();
+  setup_notify_waitable();
 
   /// Refresh the current collection using the provided new_collection
   void
@@ -253,6 +175,11 @@ private:
   typename CollectionType::EntitySharedPtr
   retrieve_entity(typename CollectionType::Key entity_id, CollectionType & collection)
   {
+    // Note: we lock the mutex because we assume that you are trying to get an element from the
+    // current collection... If there will be a use-case to retrieve elements also from other
+    // collections, we can move the mutex back to the calling codes.
+    std::lock_guard<std::mutex> guard(mutex_);
+
     // Check if the entity_id is in the collection
     auto it = collection.find(entity_id);
     if (it == collection.end()) {
@@ -273,16 +200,6 @@ private:
   /// Queue where entities can push events
   rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue_;
 
-  std::shared_ptr<rclcpp::executors::ExecutorEntitiesCollector> entities_collector_;
-  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};
-
   /// Timers manager used to track and/or execute associated timers
   std::shared_ptr<rclcpp::experimental::TimersManager> timers_manager_;
 };

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

@@ -253,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(),

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

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

@@ -19,6 +19,7 @@
 #include <memory>
 #include <mutex>
 #include <string>
+#include <vector>
 
 #include "rcl/wait.h"
 #include "rmw/impl/cpp/demangle.hpp"
@@ -60,7 +61,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;
 
   RCLCPP_PUBLIC
   virtual
@@ -72,20 +73,19 @@ public:
   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;
 
   std::shared_ptr<void>
-  take_data_by_entity_id(size_t id) override
+  take_data_by_entity_id([[maybe_unused]] size_t id) override
   {
-    (void)id;
     return take_data();
   }
 
   void
-  execute(std::shared_ptr<void> & data) override = 0;
+  execute(const std::shared_ptr<void> & data) override = 0;
 
   virtual
   bool
@@ -180,6 +180,13 @@ public:
     on_new_message_callback_ = nullptr;
   }
 
+  RCLCPP_PUBLIC
+  std::vector<std::shared_ptr<rclcpp::TimerBase>>
+  get_timers() const override
+  {
+    return {};
+  }
+
 protected:
   std::recursive_mutex callback_mutex_;
   std::function<void(size_t)> on_new_message_callback_ {nullptr};

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"
 
@@ -99,10 +100,18 @@ public:
       static_cast<const void *>(this));
   }
 
-  bool
-  is_ready(rcl_wait_set_t * wait_set) override
+  void
+  add_to_wait_set(rcl_wait_set_t & wait_set) override
+  {
+    if (this->buffer_->has_data()) {
+      this->trigger_guard_condition();
+    }
+    detail::add_guard_condition_to_rcl_wait_set(wait_set, this->gc_);
+  }
+
+  bool
+  is_ready([[maybe_unused]] const rcl_wait_set_t & wait_set) override
   {
-    (void) wait_set;
     return buffer_->has_data();
   }
 

rclcpp/include/rclcpp/experimental/timers_manager.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,9 +165,10 @@ 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.
@@ -529,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_;
+  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

@@ -19,6 +19,7 @@
 #include <memory>
 #include <future>
 #include <string>
+#include <tuple>
 #include <vector>
 #include <utility>
 
@@ -35,8 +36,8 @@ 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 Request = void *;   // Deserialized data pointer of request message
+  using Response = void *;  // Deserialized data pointer of response message
 
   using SharedResponse = std::shared_ptr<void>;
 
@@ -46,6 +47,8 @@ public:
   using Future = std::future<SharedResponse>;
   using SharedFuture = std::shared_future<SharedResponse>;
 
+  using CallbackType = std::function<void (SharedFuture)>;
+
   RCLCPP_SMART_PTR_DEFINITIONS(GenericClient)
 
   /// A convenient GenericClient::Future and request id pair.
@@ -76,6 +79,20 @@ public:
     ~FutureAndRequestId() = default;
   };
 
+  /// A convenient GenericClient::SharedFuture and request id pair.
+  /**
+   * Public members:
+   * - future: a std::shared_future<SharedResponse>.
+   * - request_id: the request id associated with the future.
+   *
+   * All the other methods are equivalent to the ones std::shared_future provides.
+   */
+  struct SharedFutureAndRequestId
+    : detail::FutureAndRequestId<std::shared_future<SharedResponse>>
+  {
+    using detail::FutureAndRequestId<std::shared_future<SharedResponse>>::FutureAndRequestId;
+  };
+
   GenericClient(
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
     rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
@@ -106,16 +123,16 @@ public:
    * 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.
+   * Not doing so will make the `GenericClient` instance to use more memory each time a response is
+   * not received from the service server.
    *
    * ```cpp
-   * auto future = client->async_send_request(my_request);
+   * auto future = generic_client->async_send_request(my_request);
    * if (
    *   rclcpp::FutureReturnCode::TIMEOUT ==
    *   executor->spin_until_future_complete(future, timeout))
    * {
-   *   client->remove_pending_request(future);
+   *   generic_client->remove_pending_request(future);
    *   // handle timeout
    * } else {
    *   handle_response(future.get());
@@ -129,6 +146,45 @@ public:
   FutureAndRequestId
   async_send_request(const Request request);
 
+  /// Send a request to the service server and schedule a callback in the executor.
+  /**
+   * Similar to the previous overload, but a callback will automatically be called when a response
+   * is received.
+   *
+   * If the callback is never called, because we never got a reply for the service server,
+   * remove_pending_request() has to be called with the returned request id or
+   * prune_pending_requests().
+   * Not doing so will make the `GenericClient` instance use more memory each time a response is not
+   * received from the service server.
+   * In this case, it's convenient to setup a timer to cleanup the pending requests.
+   *
+   * \param[in] request request to be send.
+   * \param[in] cb callback that will be called when we get a response for this request.
+   * \return the request id representing the request just sent.
+   */
+  template<
+    typename CallbackT,
+    typename std::enable_if<
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        CallbackType
+      >::value
+    >::type * = nullptr
+  >
+  SharedFutureAndRequestId
+  async_send_request(const Request request, CallbackT && cb)
+  {
+    Promise promise;
+    auto shared_future = promise.get_future().share();
+    auto req_id = async_send_request_impl(
+      request,
+      std::make_tuple(
+        CallbackType{std::forward<CallbackT>(cb)},
+        shared_future,
+        std::move(promise)));
+    return SharedFutureAndRequestId{std::move(shared_future), req_id};
+  }
+
   /// Clean all pending requests older than a time_point.
   /**
    * \param[in] time_point Requests that were sent before this point are going to be removed.
@@ -149,15 +205,52 @@ public:
       pruned_requests);
   }
 
+  /// Clean all pending requests.
+  /**
+   * \return number of pending requests that were removed.
+   */
   RCLCPP_PUBLIC
   size_t
   prune_pending_requests();
 
+  /// Cleanup a pending request.
+  /**
+   * This notifies the client that we have waited long enough for a response from the server
+   * to come, we have given up and we are not waiting for a response anymore.
+   *
+   * Not calling this will make the client start using more memory for each request
+   * that never got a reply from the server.
+   *
+   * \param[in] request_id request id returned by async_send_request().
+   * \return true when a pending request was removed, false if not (e.g. a response was received).
+   */
   RCLCPP_PUBLIC
   bool
   remove_pending_request(
     int64_t request_id);
 
+  /// Cleanup a pending request.
+  /**
+   * Convenient overload, same as:
+   *
+   * `GenericClient::remove_pending_request(this, future.request_id)`.
+   */
+  RCLCPP_PUBLIC
+  bool
+  remove_pending_request(
+    const FutureAndRequestId & future);
+
+  /// Cleanup a pending request.
+  /**
+   * Convenient overload, same as:
+   *
+   * `GenericClient::remove_pending_request(this, future.request_id)`.
+   */
+  RCLCPP_PUBLIC
+  bool
+  remove_pending_request(
+    const SharedFutureAndRequestId & future);
+
   /// Take the next response for this client.
   /**
    * \sa ClientBase::take_type_erased_response().
@@ -179,9 +272,12 @@ public:
   }
 
 protected:
+  using CallbackTypeValueVariant = std::tuple<CallbackType, SharedFuture, Promise>;
   using CallbackInfoVariant = std::variant<
-    std::promise<SharedResponse>>;  // Use variant for extension
+    std::promise<SharedResponse>,
+    CallbackTypeValueVariant>;  // Use variant for extension
 
+  RCLCPP_PUBLIC
   int64_t
   async_send_request_impl(
     const Request request,

rclcpp/include/rclcpp/generic_service.hpp

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

rclcpp/include/rclcpp/generic_subscription.hpp

@@ -84,13 +84,22 @@ public:
       options.event_callbacks,
       options.use_default_callbacks,
       DeliveredMessageKind::SERIALIZED_MESSAGE),
-    callback_([callback](
-        std::shared_ptr<const rclcpp::SerializedMessage> serialized_message,
-        const rclcpp::MessageInfo & message_info) mutable {
-        callback.dispatch(serialized_message, message_info);
-      }),
+    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;
@@ -153,10 +162,7 @@ public:
 
 private:
   RCLCPP_DISABLE_COPY(GenericSubscription)
-
-  std::function<void(
-      std::shared_ptr<const rclcpp::SerializedMessage>,
-      const rclcpp::MessageInfo)> 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

@@ -100,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,7 +128,9 @@ protected:
   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/loaned_message.hpp

@@ -31,21 +31,20 @@ namespace rclcpp
 template<typename MessageT, typename AllocatorT = std::allocator<void>>
 class LoanedMessage
 {
+public:
   using MessageAllocatorTraits = rclcpp::allocator::AllocRebind<MessageT, AllocatorT>;
   using MessageAllocator = typename MessageAllocatorTraits::allocator_type;
 
-public:
   /// Constructor of the LoanedMessage class.
   /**
    * The constructor of this class allocates memory for a given message type
    * and associates this with a given publisher.
    *
-   * Given the publisher instance, a case differentiation is being performaned
-   * which decides whether the underlying middleware is able to allocate the appropriate
-   * memory for this message type or not.
-   * In the case that the middleware can not loan messages, the passed in allocator instance
-   * is being used to allocate the message within the scope of this class.
-   * Otherwise, the allocator is being ignored and the allocation is solely performaned
+   * The underlying middleware is queried to determine whether it is able to allocate the
+   * appropriate memory for this message type or not.
+   * In the case that the middleware cannot loan messages, the passed in allocator instance
+   * is used to allocate the message within the scope of this class.
+   * Otherwise, the allocator is ignored and the allocation is solely performed
    * in the underlying middleware with its appropriate allocation strategy.
    * The need for this arises as the user code can be written explicitly targeting a middleware
    * capable of loaning messages.
@@ -53,12 +52,12 @@ public:
    * a middleware which doesn't support message loaning in which case the allocator will be used.
    *
    * \param[in] pub rclcpp::Publisher instance to which the memory belongs
-   * \param[in] allocator Allocator instance in case middleware can not allocate messages
+   * \param[in] allocator Allocator instance in case middleware cannot allocate messages
    * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
    */
   LoanedMessage(
     const rclcpp::PublisherBase & pub,
-    std::allocator<MessageT> allocator)
+    MessageAllocator allocator)
   : pub_(pub),
     message_(nullptr),
     message_allocator_(std::move(allocator))
@@ -82,36 +81,6 @@ public:
     }
   }
 
-  /// Constructor of the LoanedMessage class.
-  /**
-   * The constructor of this class allocates memory for a given message type
-   * and associates this with a given publisher.
-   *
-   * Given the publisher instance, a case differentiation is being performaned
-   * which decides whether the underlying middleware is able to allocate the appropriate
-   * memory for this message type or not.
-   * In the case that the middleware can not loan messages, the passed in allocator instance
-   * is being used to allocate the message within the scope of this class.
-   * Otherwise, the allocator is being ignored and the allocation is solely performaned
-   * in the underlying middleware with its appropriate allocation strategy.
-   * The need for this arises as the user code can be written explicitly targeting a middleware
-   * capable of loaning messages.
-   * However, this user code is ought to be usable even when dynamically linked against
-   * a middleware which doesn't support message loaning in which case the allocator will be used.
-   *
-   * \param[in] pub rclcpp::Publisher instance to which the memory belongs
-   * \param[in] allocator Allocator instance in case middleware can not allocate messages
-   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
-   */
-  [[
-    deprecated("used the LoanedMessage constructor that does not use a shared_ptr to the allocator")
-  ]]
-  LoanedMessage(
-    const rclcpp::PublisherBase * pub,
-    std::shared_ptr<std::allocator<MessageT>> allocator)
-  : LoanedMessage(*pub, *allocator)
-  {}
-
   /// Move semantic for RVO
   LoanedMessage(LoanedMessage<MessageT> && other)
   : pub_(std::move(other.pub_)),

rclcpp/include/rclcpp/logger.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__LOGGER_HPP_
 #define RCLCPP__LOGGER_HPP_
 
+#include <filesystem>
 #include <memory>
 #include <string>
 #include <utility>
@@ -86,8 +87,8 @@ get_node_logger(const rcl_node_t * node);
  * \throws rclcpp::exceptions::RCLError if an unexpected error occurs.
  */
 RCLCPP_PUBLIC
-rcpputils::fs::path
-get_logging_directory();
+std::filesystem::path
+get_log_directory();
 
 class Logger
 {

rclcpp/include/rclcpp/node.hpp

@@ -44,6 +44,7 @@
 #include "rclcpp/event.hpp"
 #include "rclcpp/generic_client.hpp"
 #include "rclcpp/generic_publisher.hpp"
+#include "rclcpp/generic_service.hpp"
 #include "rclcpp/generic_subscription.hpp"
 #include "rclcpp/logger.hpp"
 #include "rclcpp/macros.hpp"
@@ -257,22 +258,6 @@ public:
     CallbackT callback,
     rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
-  /// Create and return a Client.
-  /**
-   * \param[in] service_name The topic to service on.
-   * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client.
-   * \param[in] group Callback group to call the service.
-   * \return Shared pointer to the created client.
-   * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t
-   */
-  template<typename ServiceT>
-  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
-  typename rclcpp::Client<ServiceT>::SharedPtr
-  create_client(
-    const std::string & service_name,
-    const rmw_qos_profile_t & qos_profile,
-    rclcpp::CallbackGroup::SharedPtr group = nullptr);
-
   /// Create and return a Client.
   /**
    * \param[in] service_name The name on which the service is accessible.
@@ -287,24 +272,6 @@ public:
     const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
     rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
-  /// Create and return a Service.
-  /**
-   * \param[in] service_name The topic to service on.
-   * \param[in] callback User-defined callback function.
-   * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client.
-   * \param[in] group Callback group to call the service.
-   * \return Shared pointer to the created service.
-   * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t
-   */
-  template<typename ServiceT, typename CallbackT>
-  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
-  typename rclcpp::Service<ServiceT>::SharedPtr
-  create_service(
-    const std::string & service_name,
-    CallbackT && callback,
-    const rmw_qos_profile_t & qos_profile,
-    rclcpp::CallbackGroup::SharedPtr group = nullptr);
-
   /// Create and return a Service.
   /**
    * \param[in] service_name The topic to service on.
@@ -337,6 +304,24 @@ public:
     const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
     rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
+  /// Create and return a GenericService.
+  /**
+   * \param[in] service_name The topic to service on.
+   * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool"
+   * \param[in] callback User-defined callback function.
+   * \param[in] qos Quality of service profile for the service.
+   * \param[in] group Callback group to call the service.
+   * \return Shared pointer to the created service.
+   */
+  template<typename CallbackT>
+  typename rclcpp::GenericService::SharedPtr
+  create_generic_service(
+    const std::string & service_name,
+    const std::string & service_type,
+    CallbackT && callback,
+    const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
+    rclcpp::CallbackGroup::SharedPtr group = nullptr);
+
   /// Create and return a GenericPublisher.
   /**
    * The returned pointer will never be empty, but this function can throw various exceptions, for
@@ -1015,8 +1000,6 @@ public:
     rclcpp::node_interfaces::OnSetParametersCallbackHandle;
   using OnSetParametersCallbackType =
     rclcpp::node_interfaces::NodeParametersInterface::OnSetParametersCallbackType;
-  using OnParametersSetCallbackType [[deprecated("use OnSetParametersCallbackType instead")]] =
-    OnSetParametersCallbackType;
 
   using PostSetParametersCallbackHandle =
     rclcpp::node_interfaces::PostSetParametersCallbackHandle;
@@ -1447,7 +1430,7 @@ public:
   rclcpp::Clock::ConstSharedPtr
   get_clock() const;
 
-  /// Returns current time from the time source specified by clock_type.
+  /// Returns current time from the node clock.
   /**
    * \sa rclcpp::Clock::now
    */
@@ -1576,6 +1559,10 @@ public:
    * which has been created using an existing instance of this class, but which
    * has an additional sub-namespace (short for subordinate namespace)
    * associated with it.
+   * A subordinate node and an instance of this class share all the node interfaces
+   * such as `rclcpp::node_interfaces::NodeParametersInterface`.
+   * Subordinate nodes are primarily used to organize namespaces and provide a
+   * hierarchical structure, but they are not meant to be completely independent nodes.
    * The sub-namespace will extend the node's namespace for the purpose of
    * creating additional entities, such as Publishers, Subscriptions, Service
    * Clients and Servers, and so on.

rclcpp/include/rclcpp/node_impl.hpp

@@ -40,6 +40,7 @@
 #include "rclcpp/create_generic_subscription.hpp"
 #include "rclcpp/create_publisher.hpp"
 #include "rclcpp/create_service.hpp"
+#include "rclcpp/create_generic_service.hpp"
 #include "rclcpp/create_subscription.hpp"
 #include "rclcpp/create_timer.hpp"
 #include "rclcpp/detail/resolve_enable_topic_statistics.hpp"
@@ -154,22 +155,6 @@ Node::create_client(
     group);
 }
 
-template<typename ServiceT>
-typename Client<ServiceT>::SharedPtr
-Node::create_client(
-  const std::string & service_name,
-  const rmw_qos_profile_t & qos_profile,
-  rclcpp::CallbackGroup::SharedPtr group)
-{
-  return rclcpp::create_client<ServiceT>(
-    node_base_,
-    node_graph_,
-    node_services_,
-    extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
-    qos_profile,
-    group);
-}
-
 template<typename ServiceT, typename CallbackT>
 typename rclcpp::Service<ServiceT>::SharedPtr
 Node::create_service(
@@ -187,20 +172,22 @@ Node::create_service(
     group);
 }
 
-template<typename ServiceT, typename CallbackT>
-typename rclcpp::Service<ServiceT>::SharedPtr
-Node::create_service(
+template<typename CallbackT>
+typename rclcpp::GenericService::SharedPtr
+Node::create_generic_service(
   const std::string & service_name,
+  const std::string & service_type,
   CallbackT && callback,
-  const rmw_qos_profile_t & qos_profile,
+  const rclcpp::QoS & qos,
   rclcpp::CallbackGroup::SharedPtr group)
 {
-  return rclcpp::create_service<ServiceT, CallbackT>(
+  return rclcpp::create_generic_service<CallbackT>(
     node_base_,
     node_services_,
     extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
+    service_type,
     std::forward<CallbackT>(callback),
-    qos_profile,
+    qos,
     group);
 }
 
@@ -336,11 +323,9 @@ template<typename ParameterT>
 bool
 Node::get_parameter(const std::string & name, ParameterT & parameter) const
 {
-  std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace());
-
   rclcpp::Parameter parameter_variant;
 
-  bool result = get_parameter(sub_name, parameter_variant);
+  bool result = get_parameter(name, parameter_variant);
   if (result) {
     parameter = static_cast<ParameterT>(parameter_variant.get_value<ParameterT>());
   }
@@ -355,9 +340,7 @@ Node::get_parameter_or(
   ParameterT & parameter,
   const ParameterT & alternative_value) const
 {
-  std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace());
-
-  bool got_parameter = get_parameter(sub_name, parameter);
+  bool got_parameter = get_parameter(name, parameter);
   if (!got_parameter) {
     parameter = alternative_value;
   }

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -214,6 +214,10 @@ public:
   const std::map<std::string, rclcpp::ParameterValue> &
   get_parameter_overrides() const override;
 
+  RCLCPP_PUBLIC
+  void
+  enable_parameter_modification() override;
+
   using PreSetCallbacksHandleContainer = std::list<PreSetParametersCallbackHandle::WeakPtr>;
   using OnSetCallbacksHandleContainer = std::list<OnSetParametersCallbackHandle::WeakPtr>;
   using PostSetCallbacksHandleContainer = std::list<PostSetParametersCallbackHandle::WeakPtr>;

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -52,8 +52,6 @@ struct OnSetParametersCallbackHandle
     std::function<
     rcl_interfaces::msg::SetParametersResult(
       const std::vector<rclcpp::Parameter> &)>;
-  using OnParametersSetCallbackType [[deprecated("use OnSetParametersCallbackType instead")]] =
-    OnSetParametersCallbackType;
 
   OnSetParametersCallbackType callback;
 };
@@ -272,6 +270,26 @@ public:
   virtual
   const std::map<std::string, rclcpp::ParameterValue> &
   get_parameter_overrides() const = 0;
+
+  /// Enable parameter modification recursively during parameter callbacks.
+  /**
+   * This function is used to enable parameter modification during parameter callbacks.
+   *
+   * There are times when it does not allow parameter modification, such as when the parameter
+   * callbacks are being called and tries to modify the parameters with set_parameter and
+   * declare_parameter to avoid recursive parameter modification.
+   * This is protected by rclcpp::node_interfaces::ParameterMutationRecursionGuard.
+   *
+   * This function is explicitly called to allow the recursive parameter operation during
+   * parameter callbacks by the application.
+   * Once it is enabled, the next parameter operation set/declare/undeclare_parameter are
+   * allowed to execute in the parameter callback. But, no more further recursive operation
+   * is allowed, unless user application calls this API again.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  enable_parameter_modification() = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/parameter_client.hpp

@@ -52,37 +52,6 @@ class AsyncParametersClient
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(AsyncParametersClient)
 
-  /// Create an async parameters client.
-  /**
-   * \param[in] node_base_interface The node base interface of the corresponding node.
-   * \param[in] node_topics_interface Node topic base interface.
-   * \param[in] node_graph_interface The node graph interface of the corresponding node.
-   * \param[in] node_services_interface Node service interface.
-   * \param[in] remote_node_name Name of the remote node
-   * \param[in] qos_profile The rmw qos profile to use to subscribe
-   * \param[in] group (optional) The async parameter client will be added to this callback group.
-   * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t
-   */
-  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
-  RCLCPP_PUBLIC
-  AsyncParametersClient(
-    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
-    const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
-    const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
-    const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
-    const std::string & remote_node_name,
-    const rmw_qos_profile_t & qos_profile,
-    rclcpp::CallbackGroup::SharedPtr group = nullptr)
-  : AsyncParametersClient(
-      node_base_interface,
-      node_topics_interface,
-      node_graph_interface,
-      node_services_interface,
-      remote_node_name,
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)),
-      group)
-  {}
-
   /// Create an async parameters client.
   /**
    * \param[in] node_base_interface The node base interface of the corresponding node.
@@ -103,31 +72,6 @@ public:
     const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS(),
     rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
-  /// Constructor
-  /**
-   * \param[in] node The async parameters client will be added to this node.
-   * \param[in] remote_node_name name of the remote node
-   * \param[in] qos_profile The rmw qos profile to use to subscribe
-   * \param[in] group (optional) The async parameter client will be added to this callback group.
-   * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t
-   */
-  template<typename NodeT>
-  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
-  AsyncParametersClient(
-    const std::shared_ptr<NodeT> node,
-    const std::string & remote_node_name,
-    const rmw_qos_profile_t & qos_profile,
-    rclcpp::CallbackGroup::SharedPtr group = nullptr)
-  : AsyncParametersClient(
-      node->get_node_base_interface(),
-      node->get_node_topics_interface(),
-      node->get_node_graph_interface(),
-      node->get_node_services_interface(),
-      remote_node_name,
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)),
-      group)
-  {}
-
   /**
    * \param[in] node The async parameters client will be added to this node.
    * \param[in] remote_node_name (optional) name of the remote node
@@ -150,31 +94,6 @@ public:
       group)
   {}
 
-  /// Constructor
-  /**
-   * \param[in] node The async parameters client will be added to this node.
-   * \param[in] remote_node_name Name of the remote node
-   * \param[in] qos_profile The rmw qos profile to use to subscribe
-   * \param[in] group (optional) The async parameter client will be added to this callback group.
-   * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t
-   */
-  template<typename NodeT>
-  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
-  AsyncParametersClient(
-    NodeT * node,
-    const std::string & remote_node_name,
-    const rmw_qos_profile_t & qos_profile,
-    rclcpp::CallbackGroup::SharedPtr group = nullptr)
-  : AsyncParametersClient(
-      node->get_node_base_interface(),
-      node->get_node_topics_interface(),
-      node->get_node_graph_interface(),
-      node->get_node_services_interface(),
-      remote_node_name,
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)),
-      group)
-  {}
-
   /**
    * \param[in] node The async parameters client will be added to this node.
    * \param[in] remote_node_name (optional) name of the remote node
@@ -383,19 +302,6 @@ class SyncParametersClient
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient)
 
-  template<typename NodeT>
-  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
-  SyncParametersClient(
-    std::shared_ptr<NodeT> node,
-    const std::string & remote_node_name,
-    const rmw_qos_profile_t & qos_profile)
-  : SyncParametersClient(
-      std::make_shared<rclcpp::executors::SingleThreadedExecutor>(),
-      node,
-      remote_node_name,
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)))
-  {}
-
   template<typename NodeT>
   explicit SyncParametersClient(
     std::shared_ptr<NodeT> node,
@@ -408,23 +314,6 @@ public:
       qos_profile)
   {}
 
-  template<typename NodeT>
-  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
-  SyncParametersClient(
-    rclcpp::Executor::SharedPtr executor,
-    std::shared_ptr<NodeT> node,
-    const std::string & remote_node_name,
-    const rmw_qos_profile_t & qos_profile)
-  : SyncParametersClient(
-      executor,
-      node->get_node_base_interface(),
-      node->get_node_topics_interface(),
-      node->get_node_graph_interface(),
-      node->get_node_services_interface(),
-      remote_node_name,
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)))
-  {}
-
   template<typename NodeT>
   SyncParametersClient(
     rclcpp::Executor::SharedPtr executor,
@@ -441,19 +330,6 @@ public:
       qos_profile)
   {}
 
-  template<typename NodeT>
-  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
-  SyncParametersClient(
-    NodeT * node,
-    const std::string & remote_node_name,
-    const rmw_qos_profile_t & qos_profile)
-  : SyncParametersClient(
-      std::make_shared<rclcpp::executors::SingleThreadedExecutor>(),
-      node,
-      remote_node_name,
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)))
-  {}
-
   template<typename NodeT>
   explicit SyncParametersClient(
     NodeT * node,
@@ -466,23 +342,6 @@ public:
       qos_profile)
   {}
 
-  template<typename NodeT>
-  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
-  SyncParametersClient(
-    rclcpp::Executor::SharedPtr executor,
-    NodeT * node,
-    const std::string & remote_node_name,
-    const rmw_qos_profile_t & qos_profile)
-  : SyncParametersClient(
-      executor,
-      node->get_node_base_interface(),
-      node->get_node_topics_interface(),
-      node->get_node_graph_interface(),
-      node->get_node_services_interface(),
-      remote_node_name,
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)))
-  {}
-
   template<typename NodeT>
   SyncParametersClient(
     rclcpp::Executor::SharedPtr executor,
@@ -499,28 +358,6 @@ public:
       qos_profile)
   {}
 
-  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
-  RCLCPP_PUBLIC
-  SyncParametersClient(
-    rclcpp::Executor::SharedPtr executor,
-    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
-    const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
-    const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
-    const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
-    const std::string & remote_node_name,
-    const rmw_qos_profile_t & qos_profile)
-  : executor_(executor), node_base_interface_(node_base_interface)
-  {
-    async_parameters_client_ =
-      std::make_shared<AsyncParametersClient>(
-      node_base_interface,
-      node_topics_interface,
-      node_graph_interface,
-      node_services_interface,
-      remote_node_name,
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)));
-  }
-
   RCLCPP_PUBLIC
   SyncParametersClient(
     rclcpp::Executor::SharedPtr executor,

rclcpp/include/rclcpp/parameter_event_handler.hpp

@@ -67,19 +67,23 @@ struct ParameterEventCallbackHandle
  * The first step is to instantiate a ParameterEventHandler, providing a ROS node to use
  * to create any required subscriptions:
  *
- *   auto param_handler = std::make_shared<rclcpp::ParameterEventHandler>(node);
+ * ```cpp
+ * auto param_handler = std::make_shared<rclcpp::ParameterEventHandler>(node);
+ * ```
  *
  * Next, you can supply a callback to the add_parameter_callback method, as follows:
  *
- *   auto cb1 = [&node](const rclcpp::Parameter & p) {
- *      RCLCPP_INFO(
- *        node->get_logger(),
- *        "cb1: Received an update to parameter \"%s\" of type %s: \"%ld\"",
- *        p.get_name().c_str(),
- *        p.get_type_name().c_str(),
- *        p.as_int());
- *   };
- *   auto handle1 = param_handler->add_parameter_callback("an_int_param", cb1);
+ * ```cpp
+ * auto cb1 = [&node](const rclcpp::Parameter & p) {
+ *    RCLCPP_INFO(
+ *      node->get_logger(),
+ *      "cb1: Received an update to parameter \"%s\" of type %s: \"%ld\"",
+ *      p.get_name().c_str(),
+ *      p.get_type_name().c_str(),
+ *      p.as_int());
+ * };
+ * auto handle1 = param_handler->add_parameter_callback("an_int_param", cb1);
+ * ```
  *
  * In this case, we didn't supply a node name (the third, optional, parameter) so the
  * default will be to monitor for changes to the "an_int_param" parameter associated with
@@ -92,16 +96,18 @@ struct ParameterEventCallbackHandle
  * You may also monitor for changes to parameters in other nodes by supplying the node
  * name to add_parameter_callback:
  *
- *   auto cb2 = [&node](const rclcpp::Parameter & p) {
- *       RCLCPP_INFO(
- *         node->get_logger(),
- *         "cb2: Received an update to parameter \"%s\" of type: %s: \"%s\"",
- *         p.get_name().c_str(),
- *         p.get_type_name().c_str(),
- *         p.as_string().c_str());
- *     };
- *   auto handle2 = param_handler->add_parameter_callback(
- *     "some_remote_param_name", cb2, "some_remote_node_name");
+ * ```cpp
+ * auto cb2 = [&node](const rclcpp::Parameter & p) {
+ *     RCLCPP_INFO(
+ *       node->get_logger(),
+ *       "cb2: Received an update to parameter \"%s\" of type: %s: \"%s\"",
+ *       p.get_name().c_str(),
+ *       p.get_type_name().c_str(),
+ *       p.as_string().c_str());
+ *   };
+ * auto handle2 = param_handler->add_parameter_callback(
+ *   "some_remote_param_name", cb2, "some_remote_node_name");
+ * ```
  *
  * In this case, the callback will be invoked whenever "some_remote_param_name" changes
  * on remote node "some_remote_node_name".
@@ -109,7 +115,9 @@ struct ParameterEventCallbackHandle
  * To remove a parameter callback, reset the callback handle smart pointer or call
  * remove_parameter_callback, passing the handle returned from add_parameter_callback:
  *
- *   param_handler->remove_parameter_callback(handle2);
+ * ```cpp
+ * param_handler->remove_parameter_callback(handle2);
+ * ```
  *
  * You can also monitor for *all* parameter changes, using add_parameter_event_callback.
  * In this case, the callback will be invoked whenever any parameter changes in the system.
@@ -117,40 +125,42 @@ struct ParameterEventCallbackHandle
  * is convenient to use a regular expression on the node names or namespaces of interest.
  * For example:
  *
- *   auto cb3 =
- *     [fqn, remote_param_name, &node](const rcl_interfaces::msg::ParameterEvent & event) {
- *       // Look for any updates to parameters in "/a_namespace" as well as any parameter changes
- *       // to our own node ("this_node")
- *       std::regex re("(/a_namespace/.*)|(/this_node)");
- *       if (regex_match(event.node, re)) {
- *         // Now that we know the event matches the regular expression we scanned for, we can
- *         // use 'get_parameter_from_event' to get a specific parameter name that we're looking for
- *         rclcpp::Parameter p;
- *         if (rclcpp::ParameterEventsSubscriber::get_parameter_from_event(
- *             event, p, remote_param_name, fqn))
- *         {
- *           RCLCPP_INFO(
- *             node->get_logger(),
- *             "cb3: Received an update to parameter \"%s\" of type: %s: \"%s\"",
- *             p.get_name().c_str(),
- *             p.get_type_name().c_str(),
- *             p.as_string().c_str());
- *         }
- *
- *         // You can also use 'get_parameter*s*_from_event' to enumerate all changes that came
- *         // in on this event
- *         auto params = rclcpp::ParameterEventsSubscriber::get_parameters_from_event(event);
- *         for (auto & p : params) {
- *           RCLCPP_INFO(
- *             node->get_logger(),
- *             "cb3: Received an update to parameter \"%s\" of type: %s: \"%s\"",
- *             p.get_name().c_str(),
- *             p.get_type_name().c_str(),
- *             p.value_to_string().c_str());
- *         }
+ * ```cpp
+ * auto cb3 =
+ *   [fqn, remote_param_name, &node](const rcl_interfaces::msg::ParameterEvent & event) {
+ *     // Look for any updates to parameters in "/a_namespace" as well as any parameter changes
+ *     // to our own node ("this_node")
+ *     std::regex re("(/a_namespace/.*)|(/this_node)");
+ *     if (regex_match(event.node, re)) {
+ *       // Now that we know the event matches the regular expression we scanned for, we can
+ *       // use 'get_parameter_from_event' to get a specific parameter name that we're looking for
+ *       rclcpp::Parameter p;
+ *       if (rclcpp::ParameterEventHandler::get_parameter_from_event(
+ *           event, p, remote_param_name, fqn))
+ *       {
+ *         RCLCPP_INFO(
+ *           node->get_logger(),
+ *           "cb3: Received an update to parameter \"%s\" of type: %s: \"%s\"",
+ *           p.get_name().c_str(),
+ *           p.get_type_name().c_str(),
+ *           p.as_string().c_str());
  *       }
- *     };
- *   auto handle3 = param_handler->add_parameter_event_callback(cb3);
+ *
+ *       // You can also use 'get_parameter*s*_from_event' to enumerate all changes that came
+ *       // in on this event
+ *       auto params = rclcpp::ParameterEventHandler::get_parameters_from_event(event);
+ *       for (auto & p : params) {
+ *         RCLCPP_INFO(
+ *           node->get_logger(),
+ *           "cb3: Received an update to parameter \"%s\" of type: %s: \"%s\"",
+ *           p.get_name().c_str(),
+ *           p.get_type_name().c_str(),
+ *           p.value_to_string().c_str());
+ *       }
+ *     }
+ *   };
+ * auto handle3 = param_handler->add_parameter_event_callback(cb3);
+ * ```
  *
  * For both parameter callbacks and parameter event callbacks, when multiple callbacks are added,
  * the callbacks are invoked last-in, first-called order (LIFO).
@@ -160,7 +170,9 @@ struct ParameterEventCallbackHandle
  *
  * To remove a parameter event callback, reset the callback smart pointer or use:
  *
- *   param_handler->remove_event_parameter_callback(handle3);
+ * ```cpp
+ * param_handler->remove_event_parameter_callback(handle3);
+ * ```
  */
 class ParameterEventHandler
 {

rclcpp/include/rclcpp/parameter_events_filter.hpp

@@ -45,6 +45,7 @@ public:
    * \param[in] names A list of parameter names of interest.
    * \param[in] types A list of the types of parameter events of iterest.
    *    EventType NEW, DELETED, or CHANGED
+   * \throws std::invalid_argument if event is NULL.
    *
    * Example Usage:
    *

rclcpp/include/rclcpp/parameter_service.hpp

@@ -40,20 +40,6 @@ class ParameterService
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(ParameterService)
 
-  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
-  RCLCPP_PUBLIC
-  ParameterService(
-    const std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
-    const std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
-    rclcpp::node_interfaces::NodeParametersInterface * node_params,
-    const rmw_qos_profile_t & qos_profile)
-  : ParameterService(
-      node_base,
-      node_services,
-      node_params,
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)))
-  {}
-
   RCLCPP_PUBLIC
   ParameterService(
     const std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,

rclcpp/include/rclcpp/parameter_value.hpp

@@ -359,7 +359,7 @@ private:
 /// Return the value of a parameter as a string
 RCLCPP_PUBLIC
 std::string
-to_string(const ParameterValue & type);
+to_string(const ParameterValue & value);
 
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/publisher.hpp

@@ -96,22 +96,6 @@ public:
   using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
   using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>;
 
-  using MessageAllocatorTraits
-  [[deprecated("use PublishedTypeAllocatorTraits")]] =
-    PublishedTypeAllocatorTraits;
-  using MessageAllocator
-  [[deprecated("use PublishedTypeAllocator")]] =
-    PublishedTypeAllocator;
-  using MessageDeleter
-  [[deprecated("use PublishedTypeDeleter")]] =
-    PublishedTypeDeleter;
-  using MessageUniquePtr
-  [[deprecated("use std::unique_ptr<PublishedType, PublishedTypeDeleter>")]] =
-    std::unique_ptr<PublishedType, PublishedTypeDeleter>;
-  using MessageSharedPtr
-  [[deprecated("use std::shared_ptr<const PublishedType>")]] =
-    std::shared_ptr<const PublishedType>;
-
   using BufferSharedPtr = typename rclcpp::experimental::buffers::IntraProcessBuffer<
     ROSMessageType,
     ROSMessageTypeAllocator,
@@ -128,8 +112,8 @@ public:
    *
    * \param[in] node_base NodeBaseInterface pointer that is used in part of the setup.
    * \param[in] topic Name of the topic to publish to.
-   * \param[in] qos QoS profile for Subcription.
-   * \param[in] options Options for the subscription.
+   * \param[in] qos QoS profile for the publisher.
+   * \param[in] options Options for the publisher.
    */
   Publisher(
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
@@ -159,34 +143,34 @@ public:
   post_init_setup(
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
     const std::string & topic,
-    const rclcpp::QoS & qos,
-    const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options)
+    [[maybe_unused]] const rclcpp::QoS & qos,
+    [[maybe_unused]] const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options)
   {
-    // Topic is unused for now.
-    (void)topic;
-    (void)options;
-
     // If needed, setup intra process communication.
     if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) {
       auto context = node_base->get_context();
       // Get the intra process manager instance for this context.
       auto ipm = context->get_sub_context<rclcpp::experimental::IntraProcessManager>();
-      // Register the publisher with the intra process manager.
-      if (qos.history() != rclcpp::HistoryPolicy::KeepLast) {
+      // Check if the QoS is compatible with intra-process.
+      auto qos_profile = get_actual_qos();
+      if (qos_profile.history() != rclcpp::HistoryPolicy::KeepLast) {
         throw std::invalid_argument(
-                "intraprocess communication allowed only with keep last history qos policy");
+                "intraprocess communication on topic '" + topic +
+                "' allowed only with keep last history qos policy");
       }
-      if (qos.depth() == 0) {
+      if (qos_profile.depth() == 0) {
         throw std::invalid_argument(
-                "intraprocess communication is not allowed with a zero qos history depth value");
+                "intraprocess communication on topic '" + topic +
+                "' is not allowed with a zero qos history depth value");
       }
-      if (qos.durability() == rclcpp::DurabilityPolicy::TransientLocal) {
+      if (qos_profile.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,
+          qos_profile,
           std::make_shared<ROSMessageTypeAllocator>(ros_message_type_allocator_));
       }
+      // Register the publisher with the intra process manager.
       uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this(), buffer_);
       this->setup_intra_process(
         intra_process_publisher_id,
@@ -203,7 +187,7 @@ public:
    * the loaned message will be directly allocated in the middleware.
    * If not, the message allocator of this rclcpp::Publisher instance is being used.
    *
-   * With a call to \sa `publish` the LoanedMessage instance is being returned to the middleware
+   * With a call to `publish` the LoanedMessage instance is being returned to the middleware
    * or free'd accordingly to the allocator.
    * If the message is not being published but processed differently, the destructor of this
    * class will either return the message to the middleware or deallocate it via the internal
@@ -248,8 +232,12 @@ public:
     // interprocess publish, resulting in lower publish-to-subscribe latency.
     // It's not possible to do that with an unique_ptr,
     // as do_intra_process_publish takes the ownership of the message.
+
+    // When durability is set to TransientLocal (i.e. there is a buffer),
+    // inter process publish should always take place to ensure
+    // late joiners receive past data.
     bool inter_process_publish_needed =
-      get_subscription_count() > get_intra_process_subscription_count();
+      get_subscription_count() > get_intra_process_subscription_count() || buffer_;
 
     if (inter_process_publish_needed) {
       auto shared_msg =
@@ -326,8 +314,11 @@ public:
       return;
     }
 
+    // When durability is set to TransientLocal (i.e. there is a buffer),
+    // inter process publish should always take place to ensure
+    // late joiners receive past data.
     bool inter_process_publish_needed =
-      get_subscription_count() > get_intra_process_subscription_count();
+      get_subscription_count() > get_intra_process_subscription_count() || buffer_;
 
     if (inter_process_publish_needed) {
       auto ros_msg_ptr = std::make_shared<ROSMessageType>();
@@ -423,13 +414,6 @@ public:
     }
   }
 
-  [[deprecated("use get_published_type_allocator() or get_ros_message_type_allocator() instead")]]
-  std::shared_ptr<PublishedTypeAllocator>
-  get_allocator() const
-  {
-    return std::make_shared<PublishedTypeAllocator>(published_type_allocator_);
-  }
-
   PublishedTypeAllocator
   get_published_type_allocator() const
   {

rclcpp/include/rclcpp/qos.hpp

@@ -146,10 +146,18 @@ public:
   QoS(size_t history_depth);  // NOLINT(runtime/explicit): conversion constructor
 
   /// Return the rmw qos profile.
+  /**
+   * The profile consists of various QoS policies such as history, reliability, and durability.
+   * Use the corresponding getter functions to retrieve individual policies.
+   */
   rmw_qos_profile_t &
   get_rmw_qos_profile();
 
   /// Return the rmw qos profile.
+  /**
+   * The profile consists of various QoS policies such as history, reliability, and durability.
+   * Use the corresponding getter functions to retrieve individual policies.
+   */
   const rmw_qos_profile_t &
   get_rmw_qos_profile() const;
 
@@ -475,7 +483,7 @@ public:
   explicit
   RosoutQoS(
     const QoSInitialization & rosout_qos_initialization = (
-      QoSInitialization::from_rmw(rcl_qos_profile_rosout_default)
+      QoSInitialization::from_rmw(rmw_qos_profile_rosout_default)
   ));
 };
 

rclcpp/include/rclcpp/rate.hpp

@@ -39,10 +39,6 @@ public:
   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;
 
@@ -54,82 +50,6 @@ using std::chrono::duration;
 using std::chrono::duration_cast;
 using std::chrono::nanoseconds;
 
-template<class Clock = std::chrono::high_resolution_clock>
-class [[deprecated("use rclcpp::Rate class instead of GenericRate")]] GenericRate : public RateBase
-{
-public:
-  RCLCPP_SMART_PTR_DEFINITIONS(GenericRate)
-
-  explicit GenericRate(double 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())
-  {}
-
-  virtual bool
-  sleep()
-  {
-    // Time coming into sleep
-    auto now = Clock::now();
-    // Time of next interval
-    auto next_interval = last_interval_ + period_;
-    // Detect backwards time flow
-    if (now < last_interval_) {
-      // Best thing to do is to set the next_interval to now + period
-      next_interval = now + period_;
-    }
-    // Calculate the time to sleep
-    auto time_to_sleep = next_interval - now;
-    // Update the interval
-    last_interval_ += period_;
-    // If the time_to_sleep is negative or zero, don't sleep
-    if (time_to_sleep <= std::chrono::seconds(0)) {
-      // If an entire cycle was missed then reset next interval.
-      // This might happen if the loop took more than a cycle.
-      // Or if time jumps forward.
-      if (now > next_interval + period_) {
-        last_interval_ = now + period_;
-      }
-      // Either way do not sleep and return false
-      return false;
-    }
-    // Sleep (will get interrupted by ctrl-c, may not sleep full time)
-    rclcpp::sleep_for(time_to_sleep);
-    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()
-  {
-    last_interval_ = Clock::now();
-  }
-
-  std::chrono::nanoseconds period() const
-  {
-    return period_;
-  }
-
-private:
-  RCLCPP_DISABLE_COPY(GenericRate)
-
-  std::chrono::nanoseconds period_;
-  using ClockDurationNano = std::chrono::duration<typename Clock::rep, std::nano>;
-  std::chrono::time_point<Clock, ClockDurationNano> last_interval_;
-};
-
 class Rate : public RateBase
 {
 public:
@@ -149,11 +69,6 @@ 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/include/rclcpp/service.hpp

@@ -307,7 +307,7 @@ public:
    * \param[in] node_handle NodeBaseInterface pointer that is used in part of the setup.
    * \param[in] service_name Name of the topic to publish to.
    * \param[in] any_callback User defined callback to call when a client request is received.
-   * \param[in] service_options options for the subscription.
+   * \param[in] service_options options for the service.
    */
   Service(
     std::shared_ptr<rcl_node_t> node_handle,
@@ -499,11 +499,14 @@ public:
     }
   }
 
-  /// Configure client introspection.
+  /// Configure service introspection.
   /**
    * \param[in] clock clock to use to generate introspection timestamps
    * \param[in] qos_service_event_pub QoS settings to use when creating the introspection publisher
    * \param[in] introspection_state the state to set introspection to
+   *
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw if
+   *   it failed to configure introspection.
    */
   void
   configure_introspection(

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
@@ -199,6 +201,7 @@ public:
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
           "Couldn't add subscription to wait set: %s", rcl_get_error_string().str);
+        rcl_reset_error();
         return false;
       }
     }
@@ -208,6 +211,7 @@ public:
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
           "Couldn't add client to wait set: %s", rcl_get_error_string().str);
+        rcl_reset_error();
         return false;
       }
     }
@@ -217,6 +221,7 @@ public:
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
           "Couldn't add service to wait set: %s", rcl_get_error_string().str);
+        rcl_reset_error();
         return false;
       }
     }
@@ -226,6 +231,7 @@ public:
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
           "Couldn't add timer to wait set: %s", rcl_get_error_string().str);
+        rcl_reset_error();
         return false;
       }
     }
@@ -235,7 +241,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 +374,8 @@ public:
           ++it;
           continue;
         }
-        if (!timer->call()) {
+        auto data = timer->call();
+        if (!data) {
           // timer was cancelled, skip it.
           ++it;
           continue;
@@ -377,6 +384,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 +398,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 +407,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 +420,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 +505,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

@@ -114,9 +114,9 @@ public:
    * all references.
    * \param[in] msg Shared pointer to the message to return.
    */
-  void return_message(std::shared_ptr<MessageT> & msg)
+  void return_message([[maybe_unused]] std::shared_ptr<MessageT> & msg)
   {
-    (void)msg;
+    // This function is intentionally left empty.
   }
 
 protected:

rclcpp/include/rclcpp/subscription.hpp

@@ -90,18 +90,6 @@ public:
   using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
   using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>;
 
-  using MessageAllocatorTraits [[deprecated("use ROSMessageTypeAllocatorTraits")]] =
-    ROSMessageTypeAllocatorTraits;
-  using MessageAllocator [[deprecated("use ROSMessageTypeAllocator")]] =
-    ROSMessageTypeAllocator;
-  using MessageDeleter [[deprecated("use ROSMessageTypeDeleter")]] =
-    ROSMessageTypeDeleter;
-
-  using ConstMessageSharedPtr [[deprecated]] = std::shared_ptr<const ROSMessageType>;
-  using MessageUniquePtr
-  [[deprecated("use std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter> instead")]] =
-    std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>;
-
 private:
   using SubscriptionTopicStatisticsSharedPtr =
     std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics>;
@@ -159,11 +147,13 @@ public:
       auto qos_profile = get_actual_qos();
       if (qos_profile.history() != rclcpp::HistoryPolicy::KeepLast) {
         throw std::invalid_argument(
-                "intraprocess communication allowed only with keep last history qos policy");
+                "intraprocess communication on topic '" + topic_name +
+                "' allowed only with keep last history qos policy");
       }
       if (qos_profile.depth() == 0) {
         throw std::invalid_argument(
-                "intraprocess communication is not allowed with 0 depth qos policy");
+                "intraprocess communication on topic '" + topic_name +
+                "' is not allowed with 0 depth qos policy");
       }
 
       using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess<
@@ -219,13 +209,11 @@ public:
   /// Called after construction to continue setup that requires shared_from_this().
   void
   post_init_setup(
-    rclcpp::node_interfaces::NodeBaseInterface * node_base,
-    const rclcpp::QoS & qos,
-    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options)
+    [[maybe_unused]] rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    [[maybe_unused]] const rclcpp::QoS & qos,
+    [[maybe_unused]] const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options)
   {
-    (void)node_base;
-    (void)qos;
-    (void)options;
+    // This function is intentionally left empty.
   }
 
   /// Take the next message from the inter-process subscription.
@@ -432,20 +420,17 @@ public:
 
   void
   return_dynamic_message(
-    rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) override
+    [[maybe_unused]] rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) override
   {
-    (void) message;
     throw rclcpp::exceptions::UnimplementedError(
             "return_dynamic_message is not implemented for Subscription");
   }
 
   void
   handle_dynamic_message(
-    const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
-    const rclcpp::MessageInfo & message_info) override
+    [[maybe_unused]] const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
+    [[maybe_unused]] const rclcpp::MessageInfo & message_info) override
   {
-    (void) message;
-    (void) message_info;
     throw rclcpp::exceptions::UnimplementedError(
             "handle_dynamic_message is not implemented for Subscription");
   }

rclcpp/include/rclcpp/subscription_base.hpp

@@ -20,8 +20,8 @@
 #include <mutex>
 #include <string>
 #include <unordered_map>
-#include <vector>
 #include <utility>
+#include <vector>
 
 #include "rcl/event_callback.h"
 #include "rcl/subscription.h"

rclcpp/include/rclcpp/subscription_options.hpp

@@ -80,7 +80,8 @@ struct SubscriptionOptionsBase
 
     // 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();
+    // Explicitly set the enough depth to avoid missing the statistics messages.
+    rclcpp::QoS qos = SystemDefaultsQoS().keep_last(10);
   };
 
   TopicStatisticsOptions topic_stats_options;

rclcpp/include/rclcpp/time.hpp

@@ -49,6 +49,7 @@ public:
   /**
    * \param nanoseconds since time epoch
    * \param clock_type clock type
+   * \throws std::runtime_error if nanoseconds are negative
    */
   RCLCPP_PUBLIC
   explicit Time(int64_t nanoseconds = 0, rcl_clock_type_t clock_type = RCL_SYSTEM_TIME);

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:
@@ -101,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>
@@ -198,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
@@ -256,27 +268,28 @@ 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
   {
     TRACETOOLS_TRACEPOINT(callback_start, reinterpret_cast<const void *>(&callback_), false);
-    execute_callback_delegate<>();
+    execute_callback_delegate<>(*static_cast<rcl_timer_call_info_t *>(data.get()));
     TRACETOOLS_TRACEPOINT(callback_end, reinterpret_cast<const void *>(&callback_));
   }
 
@@ -288,7 +301,7 @@ public:
     >::type * = nullptr
   >
   void
-  execute_callback_delegate()
+  execute_callback_delegate(const rcl_timer_call_info_t &)
   {
     callback_();
   }
@@ -300,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
@@ -323,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>

rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp

@@ -172,12 +172,11 @@ private:
   {
     auto received_message_age = std::make_unique<ReceivedMessageAge>();
     received_message_age->Start();
-    subscriber_statistics_collectors_.emplace_back(std::move(received_message_age));
-
     auto received_message_period = std::make_unique<ReceivedMessagePeriod>();
     received_message_period->Start();
     {
       std::lock_guard<std::mutex> lock(mutex_);
+      subscriber_statistics_collectors_.emplace_back(std::move(received_message_age));
       subscriber_statistics_collectors_.emplace_back(std::move(received_message_period));
     }
 

rclcpp/include/rclcpp/typesupport_helpers.hpp

@@ -21,6 +21,7 @@
 #include <tuple>
 
 #include "rcpputils/shared_library.hpp"
+#include "rosidl_runtime_cpp/action_type_support_decl.hpp"
 #include "rosidl_runtime_cpp/message_type_support_decl.hpp"
 #include "rosidl_runtime_cpp/service_type_support_decl.hpp"
 
@@ -38,25 +39,6 @@ RCLCPP_PUBLIC
 std::shared_ptr<rcpputils::SharedLibrary>
 get_typesupport_library(const std::string & type, const std::string & typesupport_identifier);
 
-/// 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(
-  const std::string & type,
-  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.
@@ -76,7 +58,7 @@ get_message_typesupport_handle(
 
 /// 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.
+ * The library needs to match the service 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"
@@ -91,6 +73,24 @@ get_service_typesupport_handle(
   const std::string & typesupport_identifier,
   rcpputils::SharedLibrary & library);
 
+/// Extract the action type support handle from the library.
+/**
+ * The library needs to match the action type. The shared library must stay loaded for the lifetime
+ * of the result.
+ *
+ * \param[in] type The action type, e.g. "example_interfaces/action/Fibonacci"
+ * \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 action type support handle
+ */
+RCLCPP_PUBLIC
+const rosidl_action_type_support_t *
+get_action_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,159 @@ 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);
+          if (ret) {
+            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;
+          if (ret) {
+            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;
+          if (ret) {
+            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;
+          if (ret) {
+            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 +312,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

@@ -104,7 +104,7 @@ protected:
       // TODO(wjwwood): support custom allocator, maybe restrict to polymorphic allocator
       rcl_get_default_allocator());
     if (RCL_RET_OK != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret);
+      rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to create wait set");
     }
 
     // (Re)build the wait set for the first time.
@@ -192,8 +192,7 @@ protected:
       size_t services_from_waitables = 0;
       size_t events_from_waitables = 0;
       for (const auto & waitable_entry : waitables) {
-        auto waitable_ptr_pair = get_raw_pointer_from_smart_pointer(waitable_entry.waitable);
-        if (nullptr == waitable_ptr_pair.second) {
+        if (!waitable_entry.waitable) {
           // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
           if (HasStrongOwnership) {
             // This will not happen in fixed sized storage, as it holds
@@ -204,7 +203,7 @@ protected:
           needs_pruning_ = true;
           continue;
         }
-        rclcpp::Waitable & waitable = *waitable_ptr_pair.second;
+        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();
@@ -222,7 +221,7 @@ protected:
         events_from_waitables
       );
       if (RCL_RET_OK != ret) {
-        rclcpp::exceptions::throw_from_rcl_error(ret);
+        rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't resize the wait set");
       }
       was_resized = true;
       // Assumption: the calling code ensures this function is not called
@@ -238,15 +237,13 @@ protected:
     if (!was_resized) {
       rcl_ret_t ret = rcl_wait_set_clear(&rcl_wait_set_);
       if (RCL_RET_OK != ret) {
-        rclcpp::exceptions::throw_from_rcl_error(ret);
+        rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't clear the wait set");
       }
     }
 
     // Add subscriptions.
     for (const auto & subscription_entry : subscriptions) {
-      auto subscription_ptr_pair =
-        get_raw_pointer_from_smart_pointer(subscription_entry.subscription);
-      if (nullptr == subscription_ptr_pair.second) {
+      if (!subscription_entry.subscription) {
         // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
         if (HasStrongOwnership) {
           // This will not happen in fixed sized storage, as it holds
@@ -257,12 +254,13 @@ protected:
         needs_pruning_ = true;
         continue;
       }
+
       rcl_ret_t ret = rcl_wait_set_add_subscription(
         &rcl_wait_set_,
-        subscription_ptr_pair.second->get_subscription_handle().get(),
+        subscription_entry.subscription->get_subscription_handle().get(),
         nullptr);
       if (RCL_RET_OK != ret) {
-        rclcpp::exceptions::throw_from_rcl_error(ret);
+        rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set");
       }
     }
 
@@ -271,8 +269,7 @@ protected:
       [this](const auto & inner_guard_conditions)
       {
         for (const auto & guard_condition : inner_guard_conditions) {
-          auto guard_condition_ptr_pair = get_raw_pointer_from_smart_pointer(guard_condition);
-          if (nullptr == guard_condition_ptr_pair.second) {
+          if (!guard_condition) {
             // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
             if (HasStrongOwnership) {
               // This will not happen in fixed sized storage, as it holds
@@ -285,10 +282,10 @@ protected:
           }
           rcl_ret_t ret = rcl_wait_set_add_guard_condition(
             &rcl_wait_set_,
-            &guard_condition_ptr_pair.second->get_rcl_guard_condition(),
+            &guard_condition->get_rcl_guard_condition(),
             nullptr);
           if (RCL_RET_OK != ret) {
-            rclcpp::exceptions::throw_from_rcl_error(ret);
+            rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set");
           }
         }
       };
@@ -301,8 +298,7 @@ protected:
 
     // Add timers.
     for (const auto & timer : timers) {
-      auto timer_ptr_pair = get_raw_pointer_from_smart_pointer(timer);
-      if (nullptr == timer_ptr_pair.second) {
+      if (!timer) {
         // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
         if (HasStrongOwnership) {
           // This will not happen in fixed sized storage, as it holds
@@ -315,17 +311,16 @@ protected:
       }
       rcl_ret_t ret = rcl_wait_set_add_timer(
         &rcl_wait_set_,
-        timer_ptr_pair.second->get_timer_handle().get(),
+        timer->get_timer_handle().get(),
         nullptr);
       if (RCL_RET_OK != ret) {
-        rclcpp::exceptions::throw_from_rcl_error(ret);
+        rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set");
       }
     }
 
     // Add clients.
     for (const auto & client : clients) {
-      auto client_ptr_pair = get_raw_pointer_from_smart_pointer(client);
-      if (nullptr == client_ptr_pair.second) {
+      if (!client) {
         // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
         if (HasStrongOwnership) {
           // This will not happen in fixed sized storage, as it holds
@@ -338,7 +333,7 @@ protected:
       }
       rcl_ret_t ret = rcl_wait_set_add_client(
         &rcl_wait_set_,
-        client_ptr_pair.second->get_client_handle().get(),
+        client->get_client_handle().get(),
         nullptr);
       if (RCL_RET_OK != ret) {
         rclcpp::exceptions::throw_from_rcl_error(ret);
@@ -347,8 +342,7 @@ protected:
 
     // Add services.
     for (const auto & service : services) {
-      auto service_ptr_pair = get_raw_pointer_from_smart_pointer(service);
-      if (nullptr == service_ptr_pair.second) {
+      if (!service) {
         // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
         if (HasStrongOwnership) {
           // This will not happen in fixed sized storage, as it holds
@@ -361,17 +355,16 @@ protected:
       }
       rcl_ret_t ret = rcl_wait_set_add_service(
         &rcl_wait_set_,
-        service_ptr_pair.second->get_service_handle().get(),
+        service->get_service_handle().get(),
         nullptr);
       if (RCL_RET_OK != ret) {
-        rclcpp::exceptions::throw_from_rcl_error(ret);
+        rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set");
       }
     }
 
     // Add waitables.
     for (auto & waitable_entry : waitables) {
-      auto waitable_ptr_pair = get_raw_pointer_from_smart_pointer(waitable_entry.waitable);
-      if (nullptr == waitable_ptr_pair.second) {
+      if (!waitable_entry.waitable) {
         // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
         if (HasStrongOwnership) {
           // This will not happen in fixed sized storage, as it holds
@@ -382,8 +375,7 @@ protected:
         needs_pruning_ = true;
         continue;
       }
-      rclcpp::Waitable & waitable = *waitable_ptr_pair.second;
-      waitable.add_to_wait_set(&rcl_wait_set_);
+      waitable_entry.waitable->add_to_wait_set(rcl_wait_set_);
     }
   }
 
@@ -405,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

@@ -204,15 +204,24 @@ public:
   void
   storage_rebuild_rcl_wait_set(const ArrayOfExtraGuardConditions & extra_guard_conditions)
   {
+    this->storage_acquire_ownerships();
+
     this->storage_rebuild_rcl_wait_set_with_sets(
-      subscriptions_,
-      guard_conditions_,
+      shared_subscriptions_,
+      shared_guard_conditions_,
       extra_guard_conditions,
-      timers_,
-      clients_,
-      services_,
-      waitables_
+      shared_timers_,
+      shared_clients_,
+      shared_services_,
+      shared_waitables_
     );
+
+    if (this->needs_pruning_) {
+      this->storage_prune_deleted_entities();
+      this->needs_pruning_ = false;
+    }
+
+    this->storage_release_ownerships();
   }
 
   template<class EntityT, class SequenceOfEntitiesT>
@@ -382,6 +391,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());
@@ -407,6 +418,7 @@ public:
         }
       };
     // Lock all the weak pointers and hold them until released.
+    lock_all(subscriptions_, shared_subscriptions_);
     lock_all(guard_conditions_, shared_guard_conditions_);
     lock_all(timers_, shared_timers_);
     lock_all(clients_, shared_clients_);
@@ -438,6 +450,7 @@ public:
           shared_ptr.reset();
         }
       };
+    reset_all(shared_subscriptions_);
     reset_all(shared_guard_conditions_);
     reset_all(shared_timers_);
     reset_all(shared_clients_);
@@ -445,6 +458,62 @@ public:
     reset_all(shared_waitables_);
   }
 
+  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();
+  }
+
+  std::shared_ptr<rclcpp::SubscriptionBase>
+  subscriptions(size_t ii) const
+  {
+    return subscriptions_[ii].lock();
+  }
+
+  std::shared_ptr<rclcpp::TimerBase>
+  timers(size_t ii) const
+  {
+    return timers_[ii].lock();
+  }
+
+  std::shared_ptr<rclcpp::ClientBase>
+  clients(size_t ii) const
+  {
+    return clients_[ii].lock();
+  }
+
+  std::shared_ptr<rclcpp::ServiceBase>
+  services(size_t ii) const
+  {
+    return services_[ii].lock();
+  }
+
+  std::shared_ptr<rclcpp::Waitable>
+  waitables(size_t ii) const
+  {
+    return waitables_[ii].lock();
+  }
+
+private:
   size_t ownership_reference_counter_ = 0;
 
   SequenceOfWeakSubscriptions subscriptions_;

rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp

@@ -290,7 +290,7 @@ protected:
         return create_wait_result(WaitResultKind::Empty);
       } else {
         // Some other error case, throw.
-        rclcpp::exceptions::throw_from_rcl_error(ret);
+        rclcpp::exceptions::throw_from_rcl_error(ret, "rcl_wait() failed");
       }
     } while (should_loop());
 

rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp

@@ -160,6 +160,15 @@ public:
       services_,
       waitables_
     );
+
+    if(this->needs_pruning_) {
+      // we need to throw here, as the indexing of the rcl_waitset is broken,
+      // in case of invalid entries
+
+      throw std::runtime_error(
+            "StaticStorage::storage_rebuild_rcl_wait_set(): entity weak_ptr "
+            "unexpectedly expired in static entity storage");
+    }
   }
 
   // storage_add_subscription() explicitly not declared here
@@ -188,6 +197,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,10 +731,9 @@ 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
-    this->storage_acquire_ownerships();
   }
 
   /// Called by the WaitResult's destructor to release resources.
@@ -734,13 +749,13 @@ private:
       throw std::runtime_error("wait_result_release() called while not holding");
     }
     wait_result_holding_ = false;
-    // this method comes from the StoragePolicy
-    this->storage_release_ownerships();
+    wait_result_dirty_ = false;
     // this method comes from the SynchronizationPolicy
     this->sync_wait_result_release();
   }
 
   bool wait_result_holding_ = false;
+  bool wait_result_dirty_ = false;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/waitable.hpp

@@ -18,6 +18,7 @@
 #include <atomic>
 #include <functional>
 #include <memory>
+#include <vector>
 
 #include "rclcpp/macros.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -27,6 +28,8 @@
 namespace rclcpp
 {
 
+class TimerBase;
+
 class Waitable
 {
 public:
@@ -109,7 +112,7 @@ 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) = 0;
 
   /// Check if the Waitable is ready.
   /**
@@ -124,7 +127,7 @@ public:
   RCLCPP_PUBLIC
   virtual
   bool
-  is_ready(rcl_wait_set_t * wait_set) = 0;
+  is_ready(const rcl_wait_set_t & wait_set) = 0;
 
   /// Take the data so that it can be consumed with `execute`.
   /**
@@ -176,7 +179,7 @@ public:
   RCLCPP_PUBLIC
   virtual
   std::shared_ptr<void>
-  take_data_by_entity_id(size_t id);
+  take_data_by_entity_id(size_t id) = 0;
 
   /// Execute data that is passed in.
   /**
@@ -203,7 +206,7 @@ public:
   RCLCPP_PUBLIC
   virtual
   void
-  execute(std::shared_ptr<void> & data) = 0;
+  execute(const std::shared_ptr<void> & data) = 0;
 
   /// Exchange the "in use by wait set" state for this timer.
   /**
@@ -246,7 +249,7 @@ public:
   RCLCPP_PUBLIC
   virtual
   void
-  set_on_ready_callback(std::function<void(size_t, int)> callback);
+  set_on_ready_callback(std::function<void(size_t, int)> callback) = 0;
 
   /// Unset any callback registered via set_on_ready_callback.
   /**
@@ -256,7 +259,18 @@ public:
   RCLCPP_PUBLIC
   virtual
   void
-  clear_on_ready_callback();
+  clear_on_ready_callback() = 0;
+
+  /// Returns all timers used by this waitable
+  /**
+   * Must return all timers used within the waitable.
+   * Note, it is not supported, that timers are added
+   * or removed over the lifetime of the waitable.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<std::shared_ptr<rclcpp::TimerBase>>
+  get_timers() const = 0;
 
 private:
   std::atomic<bool> in_use_by_wait_set_{false};

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>28.0.0</version>
+  <version>29.6.0</version>
   <description>The ROS client library in C++.</description>
 
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>

rclcpp/src/rclcpp/any_executable.cpp

@@ -21,6 +21,7 @@ AnyExecutable::AnyExecutable()
   timer(nullptr),
   service(nullptr),
   client(nullptr),
+  waitable(nullptr),
   callback_group(nullptr),
   node_base(nullptr)
 {}

rclcpp/src/rclcpp/callback_group.cpp

@@ -66,6 +66,7 @@ CallbackGroup::size() const
     timer_ptrs_.size() +
     waitable_ptrs_.size();
 }
+
 void CallbackGroup::collect_all_ptrs(
   std::function<void(const rclcpp::SubscriptionBase::SharedPtr &)> sub_func,
   std::function<void(const rclcpp::ServiceBase::SharedPtr &)> service_func,

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_;
 };
 
@@ -80,7 +84,9 @@ Clock::now() const
 }
 
 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 +97,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 +120,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 +149,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 +164,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;
     }
   }
 
@@ -340,4 +357,245 @@ Clock::create_jump_callback(
   // *INDENT-ON*
 }
 
+class ClockWaiter::ClockWaiterImpl
+{
+private:
+  std::condition_variable cv_;
+
+  rclcpp::Clock::SharedPtr clock_;
+  bool time_source_changed_ = false;
+  std::function<void(const rcl_time_jump_t &)> post_time_jump_callback;
+
+  bool
+  wait_until_system_time(
+    std::unique_lock<std::mutex> & lock,
+    const rclcpp::Time & abs_time, const std::function<bool ()> & pred)
+  {
+    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(abs_time.nanoseconds())));
+
+    return cv_.wait_until(lock, system_time, pred);
+  }
+
+  bool
+  wait_until_steady_time(
+    std::unique_lock<std::mutex> & lock,
+    const rclcpp::Time & abs_time, const std::function<bool ()> & pred)
+  {
+    // Synchronize because RCL steady clock epoch might differ from chrono::steady_clock epoch
+    const rclcpp::Time rcl_entry = clock_->now();
+    const std::chrono::steady_clock::time_point chrono_entry = std::chrono::steady_clock::now();
+    const rclcpp::Duration delta_t = abs_time - rcl_entry;
+    const std::chrono::steady_clock::time_point chrono_until =
+      chrono_entry + std::chrono::nanoseconds(delta_t.nanoseconds());
+
+    return cv_.wait_until(lock, chrono_until, pred);
+  }
+
+
+  bool
+  wait_until_ros_time(
+    std::unique_lock<std::mutex> & lock,
+    const rclcpp::Time & abs_time, const std::function<bool ()> & pred)
+  {
+    // 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
+    // - Trigger via on_clock_change to detect if time source changes, to invalidate sleep
+    rcl_jump_threshold_t threshold;
+    threshold.on_clock_change = true;
+    // 0 is disable, so -1 and 1 are smallest possible time changes
+    threshold.min_backward.nanoseconds = -1;
+    threshold.min_forward.nanoseconds = 1;
+
+    time_source_changed_ = false;
+
+    post_time_jump_callback = [this, &lock] (const rcl_time_jump_t & jump)
+      {
+        if (jump.clock_change != RCL_ROS_TIME_NO_CHANGE) {
+          std::lock_guard<std::mutex> lk(*lock.mutex());
+          time_source_changed_ = true;
+        }
+        cv_.notify_one();
+      };
+
+    // Note this is a trade-off. Adding the callback for every call
+    // is expensive for high frequency calls. For low frequency waits
+    // its more overhead to have the callback being called all the time.
+    // As we expect the use case to be low frequency calls to wait_until
+    // with relative big pauses between the calls, we install it on demand.
+    auto clock_handler = clock_->create_jump_callback(
+      nullptr,
+      post_time_jump_callback,
+      threshold);
+
+    if (!clock_->ros_time_is_active()) {
+      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(abs_time.nanoseconds())));
+
+      return cv_.wait_until(lock, system_time, [this, &pred] () {
+                 return time_source_changed_ || pred();
+      });
+    }
+
+    // RCL_ROS_TIME with ros_time_is_active.
+    // Just wait without "until" because installed
+    // jump callbacks wake the cv on every new sample.
+    cv_.wait(lock, [this, &pred, &abs_time] () {
+        return clock_->now() >= abs_time || time_source_changed_ || pred();
+    });
+
+    return clock_->now() < abs_time;
+  }
+
+public:
+  explicit ClockWaiterImpl(const rclcpp::Clock::SharedPtr & clock)
+  :clock_(clock)
+  {
+  }
+
+  bool
+  wait_until(
+    std::unique_lock<std::mutex> & lock,
+    const rclcpp::Time & abs_time, const std::function<bool ()> & pred)
+  {
+    switch(clock_->get_clock_type()) {
+      case RCL_CLOCK_UNINITIALIZED:
+        throw std::runtime_error("Error, wait on uninitialized clock called");
+      case RCL_ROS_TIME:
+        return wait_until_ros_time(lock, abs_time, pred);
+        break;
+      case RCL_STEADY_TIME:
+        return wait_until_steady_time(lock, abs_time, pred);
+        break;
+      case RCL_SYSTEM_TIME:
+        return wait_until_system_time(lock, abs_time, pred);
+        break;
+    }
+
+    return false;
+  }
+
+  void
+  notify_one()
+  {
+    cv_.notify_one();
+  }
+};
+
+ClockWaiter::ClockWaiter(const rclcpp::Clock::SharedPtr & clock)
+:impl_(std::make_unique<ClockWaiterImpl>(clock))
+{
+}
+
+ClockWaiter::~ClockWaiter() = default;
+
+bool
+ClockWaiter::wait_until(
+  std::unique_lock<std::mutex> & lock,
+  const rclcpp::Time & abs_time, const std::function<bool ()> & pred)
+{
+  return impl_->wait_until(lock, abs_time, pred);
+}
+
+void
+ClockWaiter::notify_one()
+{
+  impl_->notify_one();
+}
+
+class ClockConditionalVariable::Impl
+{
+  std::mutex pred_mutex_;
+  bool shutdown_ = false;
+  rclcpp::Context::SharedPtr context_;
+  rclcpp::OnShutdownCallbackHandle shutdown_cb_handle_;
+  ClockWaiter::UniquePtr clock_;
+
+public:
+  Impl(const rclcpp::Clock::SharedPtr & clock, rclcpp::Context::SharedPtr context)
+  :context_(context),
+    clock_(std::make_unique<ClockWaiter>(clock))
+  {
+    if (!context_ || !context_->is_valid()) {
+      throw std::runtime_error("context cannot be slept with because it's invalid");
+    }
+    // Wake this thread if the context is shutdown
+    shutdown_cb_handle_ = context_->add_on_shutdown_callback(
+      [this]() {
+        {
+          std::unique_lock lock(pred_mutex_);
+          shutdown_ = true;
+        }
+        clock_->notify_one();
+    });
+  }
+
+  ~Impl()
+  {
+    context_->remove_on_shutdown_callback(shutdown_cb_handle_);
+  }
+
+  bool
+  wait_until(
+    std::unique_lock<std::mutex> & lock, rclcpp::Time until,
+    const std::function<bool ()> & pred)
+  {
+    if(lock.mutex() != &pred_mutex_) {
+      throw std::runtime_error(
+          "ClockConditionalVariable::wait_until: Internal error, given lock does not use"
+          " mutex returned by this->mutex()");
+    }
+
+    clock_->wait_until(lock, until, [this, &pred] () -> bool {
+        return shutdown_ || pred();
+      });
+    return true;
+  }
+
+  void
+  notify_one()
+  {
+    clock_->notify_one();
+  }
+
+  std::mutex &
+  mutex()
+  {
+    return pred_mutex_;
+  }
+};
+
+ClockConditionalVariable::ClockConditionalVariable(
+  const rclcpp::Clock::SharedPtr & clock,
+  rclcpp::Context::SharedPtr context)
+:impl_(std::make_unique<Impl>(clock, context))
+{
+}
+
+ClockConditionalVariable::~ClockConditionalVariable() = default;
+
+void
+ClockConditionalVariable::notify_one()
+{
+  impl_->notify_one();
+}
+
+bool
+ClockConditionalVariable::wait_until(
+  std::unique_lock<std::mutex> & lock, rclcpp::Time until,
+  const std::function<bool ()> & pred)
+{
+  return impl_->wait_until(lock, until, pred);
+}
+
+std::mutex &
+ClockConditionalVariable::mutex()
+{
+  return impl_->mutex();
+}
+
 }  // namespace rclcpp

rclcpp/src/rclcpp/context.cpp

@@ -212,28 +212,27 @@ Context::init(
   }
   rcl_context_.reset(context, __delete_context);
 
-  if (init_options.auto_initialize_logging()) {
-    logging_mutex_ = get_global_logging_mutex();
-    std::lock_guard<std::recursive_mutex> guard(*logging_mutex_);
-    size_t & count = get_logging_reference_count();
-    if (0u == count) {
-      ret = rcl_logging_configure_with_output_handler(
-        &rcl_context_->global_arguments,
-        rcl_init_options_get_allocator(init_options.get_rcl_init_options()),
-        rclcpp_logging_output_handler);
-      if (RCL_RET_OK != ret) {
-        rcl_context_.reset();
-        rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure logging");
-      }
-    } else {
-      RCLCPP_WARN(
-        rclcpp::get_logger("rclcpp"),
-        "logging was initialized more than once");
-    }
-    ++count;
-  }
-
   try {
+    if (init_options.auto_initialize_logging()) {
+      logging_mutex_ = get_global_logging_mutex();
+      std::lock_guard<std::recursive_mutex> guard(*logging_mutex_);
+      size_t & count = get_logging_reference_count();
+      if (0u == count) {
+        ret = rcl_logging_configure_with_output_handler(
+          &rcl_context_->global_arguments,
+          rcl_init_options_get_allocator(init_options.get_rcl_init_options()),
+          rclcpp_logging_output_handler);
+        if (RCL_RET_OK != ret) {
+          rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure logging");
+        }
+      } else {
+        RCLCPP_WARN(
+          rclcpp::get_logger("rclcpp"),
+          "logging was initialized more than once");
+      }
+      ++count;
+    }
+
     std::vector<std::string> unparsed_ros_arguments = detail::get_unparsed_ros_arguments(
       argc, argv, &(rcl_context_->global_arguments), rcl_get_default_allocator());
     if (!unparsed_ros_arguments.empty()) {

rclcpp/src/rclcpp/create_generic_service.cpp

@@ -0,0 +1,49 @@
+// Copyright 2024 Sony Group Corporation.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+#include <memory>
+
+#include "rclcpp/create_generic_service.hpp"
+#include "rclcpp/generic_service.hpp"
+
+namespace rclcpp
+{
+rclcpp::GenericService::SharedPtr
+create_generic_service(
+  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,
+  GenericServiceCallback any_callback,
+  const rclcpp::QoS & qos,
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  rcl_service_options_t options = rcl_service_get_default_options();
+  options.qos = qos.get_rmw_qos_profile();
+
+  auto srv = rclcpp::GenericService::make_shared(
+    node_base.get(),
+    node_graph,
+    service_name,
+    service_type,
+    any_callback,
+    options);
+
+  auto srv_base_ptr = std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv);
+  node_services->add_service(srv_base_ptr, group);
+  return srv;
+}
+}  // namespace rclcpp

rclcpp/src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp

@@ -23,10 +23,9 @@ namespace detail
 
 void
 RMWImplementationSpecificPublisherPayload::modify_rmw_publisher_options(
-  rmw_publisher_options_t & rmw_publisher_options) const
+  [[maybe_unused]] rmw_publisher_options_t & rmw_publisher_options) const
 {
   // By default, do not mutate the rmw publisher options.
-  (void)rmw_publisher_options;
 }
 
 }  // namespace detail

rclcpp/src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp

@@ -23,10 +23,9 @@ namespace detail
 
 void
 RMWImplementationSpecificSubscriptionPayload::modify_rmw_subscription_options(
-  rmw_subscription_options_t & rmw_subscription_options) const
+  [[maybe_unused]] rmw_subscription_options_t & rmw_subscription_options) const
 {
   // By default, do not mutate the rmw subscription options.
-  (void)rmw_subscription_options;
 }
 
 }  // namespace detail

rclcpp/src/rclcpp/detail/utilities.cpp

@@ -31,11 +31,10 @@ namespace detail
 
 std::vector<std::string>
 get_unparsed_ros_arguments(
-  int argc, char const * const * argv,
+  [[maybe_unused]] int argc, char const * const * argv,
   rcl_arguments_t * arguments,
   rcl_allocator_t allocator)
 {
-  (void)argc;
   std::vector<std::string> unparsed_ros_arguments;
   int unparsed_ros_args_count = rcl_arguments_get_count_unparsed_ros(arguments);
   if (unparsed_ros_args_count > 0) {

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"
@@ -56,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");
   }
@@ -66,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/exceptions/exceptions.cpp

@@ -93,7 +93,9 @@ throw_from_rcl_error(
 RCLErrorBase::RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state)
 : ret(ret), message(error_state->message), file(error_state->file), line(error_state->line_number),
   formatted_message(rcl_get_error_string().str)
-{}
+{
+  rcl_reset_error();
+}
 
 RCLError::RCLError(
   rcl_ret_t ret,

rclcpp/src/rclcpp/executor_options.cpp

@@ -0,0 +1,55 @@
+// Copyright 2024 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/executor_options.hpp"
+
+using rclcpp::ExecutorOptions;
+
+namespace rclcpp
+{
+
+class ExecutorOptionsImplementation {};
+
+}  // namespace rclcpp
+
+ExecutorOptions::ExecutorOptions()
+: memory_strategy(rclcpp::memory_strategies::create_default_strategy()),
+  context(rclcpp::contexts::get_global_default_context()),
+  max_conditions(0),
+  impl_(nullptr)
+{}
+
+ExecutorOptions::~ExecutorOptions()
+{}
+
+ExecutorOptions::ExecutorOptions(const ExecutorOptions & other)
+{
+  *this = other;
+}
+
+ExecutorOptions & ExecutorOptions::operator=(const ExecutorOptions & other)
+{
+  if (this == &other) {
+    return *this;
+  }
+
+  this->memory_strategy = other.memory_strategy;
+  this->context = other.context;
+  this->max_conditions = other.max_conditions;
+  if (nullptr != other.impl_) {
+    this->impl_ = std::make_unique<ExecutorOptionsImplementation>(*other.impl_);
+  }
+
+  return *this;
+}

rclcpp/src/rclcpp/executors.cpp

@@ -19,7 +19,9 @@ rclcpp::spin_all(
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
   std::chrono::nanoseconds max_duration)
 {
-  rclcpp::executors::SingleThreadedExecutor exec;
+  rclcpp::ExecutorOptions options;
+  options.context = node_ptr->get_context();
+  rclcpp::executors::SingleThreadedExecutor exec(options);
   exec.spin_node_all(node_ptr, max_duration);
 }
 
@@ -32,7 +34,9 @@ rclcpp::spin_all(rclcpp::Node::SharedPtr node_ptr, std::chrono::nanoseconds max_
 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);
 }
 
@@ -45,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,28 +227,25 @@ 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++;
   }
-
   return added;
 }
-
 }  // namespace executors
 }  // namespace rclcpp

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();
@@ -105,7 +111,8 @@ void
 ExecutorEntitiesCollector::remove_node(
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
 {
-  if (!node_ptr->get_associated_with_executor_atomic().load()) {
+  std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
+  if (!has_executor.exchange(false)) {
     throw std::runtime_error(
             std::string("Node '") + node_ptr->get_fully_qualified_name() +
             "' needs to be associated with an executor.");
@@ -143,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
@@ -161,7 +173,6 @@ ExecutorEntitiesCollector::remove_callback_group(rclcpp::CallbackGroup::SharedPt
     throw std::runtime_error("Node must not be deleted before its callback group(s).");
   }
   */
-
   auto weak_group_ptr = rclcpp::CallbackGroup::WeakPtr(group_ptr);
   std::lock_guard<std::mutex> lock(mutex_);
   bool associated = manually_added_groups_.count(group_ptr) != 0;
@@ -314,7 +325,11 @@ ExecutorEntitiesCollector::process_queues()
     if (node_it != weak_nodes_.end()) {
       remove_weak_node(node_it);
     } else {
-      throw std::runtime_error("Node needs to be associated with this executor.");
+      // The node may have been destroyed and removed from the colletion before
+      // we processed the queues.  Don't throw if the pointer is already expired.
+      if (!weak_node_ptr.expired()) {
+        throw std::runtime_error("Node needs to be associated with this executor.");
+      }
     }
 
     auto node_ptr = weak_node_ptr.lock();
@@ -337,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

@@ -12,8 +12,6 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <iostream>
-
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/executors/executor_notify_waitable.hpp"
 
@@ -22,74 +20,114 @@ namespace rclcpp
 namespace executors
 {
 
-ExecutorNotifyWaitable::ExecutorNotifyWaitable(std::function<void(void)> on_execute_callback)
-: execute_callback_(on_execute_callback)
+ExecutorNotifyWaitable::ExecutorNotifyWaitable(
+  std::function<void(void)> on_execute_callback,
+  const rclcpp::Context::SharedPtr & context)
+: execute_callback_(on_execute_callback),
+  guard_condition_(std::make_shared<rclcpp::GuardCondition>(context))
 {
 }
 
-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_;
+  this->guard_condition_ = other.guard_condition_;
+  this->idxs_of_added_guard_condition_ = other.idxs_of_added_guard_condition_;
+  this->needs_processing = other.needs_processing;
 }
 
-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_;
+    this->guard_condition_ = other.guard_condition_;
+    this->idxs_of_added_guard_condition_ = other.idxs_of_added_guard_condition_;
+    this->needs_processing = other.needs_processing;
   }
   return *this;
 }
 
 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_);
 
-  for (auto weak_guard_condition : this->notify_guard_conditions_) {
-    auto guard_condition = weak_guard_condition.lock();
-    if (guard_condition) {
-      auto rcl_guard_condition = &guard_condition->get_rcl_guard_condition();
+  idxs_of_added_guard_condition_.clear();
+  idxs_of_added_guard_condition_.reserve(notify_guard_conditions_.size());
 
-      rcl_ret_t ret = rcl_wait_set_add_guard_condition(
-        wait_set,
-        rcl_guard_condition, NULL);
+  if(needs_processing) {
+    rcl_guard_condition_t * cond = &guard_condition_->get_rcl_guard_condition();
+    size_t rcl_index;
+    rcl_ret_t ret = rcl_wait_set_add_guard_condition(&wait_set, cond, &rcl_index);
 
-      if (RCL_RET_OK != ret) {
-        rclcpp::exceptions::throw_from_rcl_error(
-          ret, "failed to add guard condition to wait set");
-      }
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(
+        ret, "failed to add guard condition to wait set");
     }
+
+    idxs_of_added_guard_condition_.push_back(rcl_index);
+
+    // we want to directly wake up any way, not need to add the other guard conditions
+    guard_condition_->trigger();
+
+    return;
+  }
+
+  // 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 (const auto & guard_condition : this->notify_guard_conditions_) {
+    rcl_guard_condition_t * cond = &guard_condition->get_rcl_guard_condition();
+    size_t rcl_index;
+    rcl_ret_t ret = rcl_wait_set_add_guard_condition(&wait_set, cond, &rcl_index);
+
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(
+        ret, "failed to add guard condition to wait set");
+    }
+
+    idxs_of_added_guard_condition_.push_back(rcl_index);
   }
 }
 
 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 rcl_index : idxs_of_added_guard_condition_) {
+    if(rcl_index >= wait_set.size_of_guard_conditions) {
+      throw std::runtime_error(
+            "ExecutorNotifyWaitable::is_ready: Internal error, got index out of range");
+    }
+
+    const auto * rcl_guard_condition = wait_set.guard_conditions[rcl_index];
 
     if (nullptr == rcl_guard_condition) {
       continue;
     }
-    for (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;
-      }
-    }
+
+    any_ready = true;
+    needs_processing = true;
+    break;
   }
+
   return any_ready;
 }
 
 void
-ExecutorNotifyWaitable::execute(std::shared_ptr<void> & data)
+ExecutorNotifyWaitable::execute(const std::shared_ptr<void> & /*data*/)
 {
-  (void) data;
+  std::lock_guard<std::mutex> lock(execute_mutex_);
+
+  needs_processing = false;
+
   this->execute_callback_();
 }
 
@@ -100,9 +138,8 @@ ExecutorNotifyWaitable::take_data()
 }
 
 std::shared_ptr<void>
-ExecutorNotifyWaitable::take_data_by_entity_id(size_t id)
+ExecutorNotifyWaitable::take_data_by_entity_id([[maybe_unused]] size_t id)
 {
-  (void) id;
   return nullptr;
 }
 
@@ -120,11 +157,7 @@ ExecutorNotifyWaitable::set_on_ready_callback(std::function<void(size_t, int)> c
   std::lock_guard<std::mutex> lock(guard_condition_mutex_);
 
   on_ready_callback_ = gc_callback;
-  for (auto weak_gc : notify_guard_conditions_) {
-    auto gc = weak_gc.lock();
-    if (!gc) {
-      continue;
-    }
+  for (const auto & gc : notify_guard_conditions_) {
     gc->set_on_trigger_callback(on_ready_callback_);
   }
 }
@@ -136,22 +169,26 @@ ExecutorNotifyWaitable::clear_on_ready_callback()
   std::lock_guard<std::mutex> lock(guard_condition_mutex_);
 
   on_ready_callback_ = nullptr;
-  for (auto weak_gc : notify_guard_conditions_) {
-    auto gc = weak_gc.lock();
-    if (!gc) {
-      continue;
-    }
+  for (const auto & gc : notify_guard_conditions_) {
     gc->set_on_trigger_callback(nullptr);
   }
 }
 
+RCLCPP_PUBLIC
+void
+ExecutorNotifyWaitable::set_execute_callback(std::function<void(void)> on_execute_callback)
+{
+  std::lock_guard<std::mutex> lock(execute_mutex_);
+  execute_callback_ = on_execute_callback;
+}
+
 void
 ExecutorNotifyWaitable::add_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition)
 {
   std::lock_guard<std::mutex> lock(guard_condition_mutex_);
-  auto guard_condition = weak_guard_condition.lock();
-  if (guard_condition && notify_guard_conditions_.count(weak_guard_condition) == 0) {
-    notify_guard_conditions_.insert(weak_guard_condition);
+  const auto & guard_condition = weak_guard_condition.lock();
+  if (guard_condition && notify_guard_conditions_.count(guard_condition) == 0) {
+    notify_guard_conditions_.insert(guard_condition);
     if (on_ready_callback_) {
       guard_condition->set_on_trigger_callback(on_ready_callback_);
     }
@@ -162,11 +199,17 @@ void
 ExecutorNotifyWaitable::remove_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition)
 {
   std::lock_guard<std::mutex> lock(guard_condition_mutex_);
-  if (notify_guard_conditions_.count(weak_guard_condition) != 0) {
-    notify_guard_conditions_.erase(weak_guard_condition);
-    auto guard_condition = weak_guard_condition.lock();
+  const auto & guard_condition = weak_guard_condition.lock();
+  if (!guard_condition) {
+    // If the lock did not work, the guard condition can't be
+    // saved in the sets, therefore we don't need to remove it
+    return;
+  }
+  auto it = notify_guard_conditions_.find(guard_condition);
+  if (it != notify_guard_conditions_.end()) {
+    notify_guard_conditions_.erase(it);
     // If this notify waitable doesn't have an on_ready_callback, then there's nothing to unset
-    if (guard_condition && on_ready_callback_) {
+    if (on_ready_callback_) {
       guard_condition->set_on_trigger_callback(nullptr);
     }
   }
@@ -179,5 +222,12 @@ ExecutorNotifyWaitable::get_number_of_ready_guard_conditions()
   return notify_guard_conditions_.size();
 }
 
+std::vector<std::shared_ptr<rclcpp::TimerBase>>
+ExecutorNotifyWaitable::get_timers() const
+{
+  return {};
+}
+
+
 }  // namespace executors
 }  // namespace rclcpp

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(wait_result_.reset();this->spinning.store(false););
   std::vector<std::thread> threads;
   size_t thread_id = 0;
   {
@@ -79,9 +79,8 @@ MultiThreadedExecutor::get_number_of_threads()
 }
 
 void
-MultiThreadedExecutor::run(size_t this_thread_number)
+MultiThreadedExecutor::run([[maybe_unused]] size_t this_thread_number)
 {
-  (void)this_thread_number;
   while (rclcpp::ok(this->context_) && spinning.load()) {
     rclcpp::AnyExecutable any_exec;
     {
@@ -99,6 +98,18 @@ MultiThreadedExecutor::run(size_t this_thread_number)
 
     execute_any_executable(any_exec);
 
+    if (any_exec.callback_group &&
+      any_exec.callback_group->type() == CallbackGroupType::MutuallyExclusive)
+    {
+      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 change: ") + ex.what());
+      }
+    }
+
     // Clear the callback_group to prevent the AnyExecutable destructor from
     // resetting the callback group `can_be_taken_from`
     any_exec.callback_group.reset();

rclcpp/src/rclcpp/executors/single_threaded_executor.cpp

@@ -30,7 +30,12 @@ SingleThreadedExecutor::spin()
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin() called while already spinning");
   }
-  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+  RCPPUTILS_SCOPE_EXIT(wait_result_.reset();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_executor_entities_collector.cpp

@@ -1,527 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include "rclcpp/executors/static_executor_entities_collector.hpp"
-
-#include <algorithm>
-#include <memory>
-#include <stdexcept>
-#include <string>
-#include <utility>
-#include <vector>
-
-#include "rclcpp/memory_strategy.hpp"
-#include "rclcpp/executors/static_single_threaded_executor.hpp"
-#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp"
-
-using rclcpp::executors::StaticExecutorEntitiesCollector;
-
-StaticExecutorEntitiesCollector::~StaticExecutorEntitiesCollector()
-{
-  // Disassociate all callback groups and thus nodes.
-  for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) {
-    auto group = pair.first.lock();
-    if (group) {
-      std::atomic_bool & has_executor = group->get_associated_with_executor_atomic();
-      has_executor.store(false);
-    }
-  }
-  for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) {
-    auto group = pair.first.lock();
-    if (group) {
-      std::atomic_bool & has_executor = group->get_associated_with_executor_atomic();
-      has_executor.store(false);
-    }
-  }
-  // Disassociate all nodes
-  for (const auto & weak_node : weak_nodes_) {
-    auto node = weak_node.lock();
-    if (node) {
-      std::atomic_bool & has_executor = node->get_associated_with_executor_atomic();
-      has_executor.store(false);
-    }
-  }
-  weak_groups_associated_with_executor_to_nodes_.clear();
-  weak_groups_to_nodes_associated_with_executor_.clear();
-  exec_list_.clear();
-  weak_nodes_.clear();
-  weak_nodes_to_guard_conditions_.clear();
-}
-
-void
-StaticExecutorEntitiesCollector::init(
-  rcl_wait_set_t * p_wait_set,
-  rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy)
-{
-  // Empty initialize executable list
-  exec_list_ = rclcpp::experimental::ExecutableList();
-  // Get executor's wait_set_ pointer
-  p_wait_set_ = p_wait_set;
-  // Get executor's memory strategy ptr
-  if (memory_strategy == nullptr) {
-    throw std::runtime_error("Received NULL memory strategy in executor waitable.");
-  }
-  memory_strategy_ = memory_strategy;
-
-  // Get memory strategy and executable list. Prepare wait_set_
-  std::shared_ptr<void> shared_ptr;
-  execute(shared_ptr);
-
-  // The entities collector is now initialized
-  initialized_ = true;
-}
-
-void
-StaticExecutorEntitiesCollector::fini()
-{
-  memory_strategy_->clear_handles();
-  exec_list_.clear();
-}
-
-std::shared_ptr<void>
-StaticExecutorEntitiesCollector::take_data()
-{
-  return nullptr;
-}
-
-void
-StaticExecutorEntitiesCollector::execute(std::shared_ptr<void> & data)
-{
-  (void) data;
-  // Fill memory strategy with entities coming from weak_nodes_
-  fill_memory_strategy();
-  // Fill exec_list_ with entities coming from weak_nodes_ (same as memory strategy)
-  fill_executable_list();
-  // Resize the wait_set_ based on memory_strategy handles (rcl_wait_set_resize)
-  prepare_wait_set();
-  // Add new nodes guard conditions to map
-  std::lock_guard<std::mutex> guard{new_nodes_mutex_};
-  for (const auto & weak_node : new_nodes_) {
-    if (auto node_ptr = weak_node.lock()) {
-      weak_nodes_to_guard_conditions_[node_ptr] =
-        node_ptr->get_shared_notify_guard_condition().get();
-    }
-  }
-  new_nodes_.clear();
-}
-
-void
-StaticExecutorEntitiesCollector::fill_memory_strategy()
-{
-  memory_strategy_->clear_handles();
-  bool has_invalid_weak_groups_or_nodes =
-    memory_strategy_->collect_entities(weak_groups_to_nodes_associated_with_executor_);
-  // Clean up any invalid nodes, if they were detected
-  if (has_invalid_weak_groups_or_nodes) {
-    std::vector<rclcpp::CallbackGroup::WeakPtr> invalid_group_ptrs;
-    for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) {
-      auto & weak_group_ptr = pair.first;
-      auto & weak_node_ptr = pair.second;
-      if (weak_group_ptr.expired() || weak_node_ptr.expired()) {
-        invalid_group_ptrs.push_back(weak_group_ptr);
-      }
-    }
-    std::for_each(
-      invalid_group_ptrs.begin(), invalid_group_ptrs.end(),
-      [this](rclcpp::CallbackGroup::WeakPtr group_ptr) {
-        weak_groups_to_nodes_associated_with_executor_.erase(group_ptr);
-      });
-  }
-  has_invalid_weak_groups_or_nodes =
-    memory_strategy_->collect_entities(weak_groups_associated_with_executor_to_nodes_);
-  // Clean up any invalid nodes, if they were detected
-  if (has_invalid_weak_groups_or_nodes) {
-    std::vector<rclcpp::CallbackGroup::WeakPtr> invalid_group_ptrs;
-    for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) {
-      auto & weak_group_ptr = pair.first;
-      const auto & weak_node_ptr = pair.second;
-      if (weak_group_ptr.expired() || weak_node_ptr.expired()) {
-        invalid_group_ptrs.push_back(weak_group_ptr);
-      }
-    }
-    std::for_each(
-      invalid_group_ptrs.begin(), invalid_group_ptrs.end(),
-      [this](rclcpp::CallbackGroup::WeakPtr group_ptr) {
-        weak_groups_associated_with_executor_to_nodes_.erase(group_ptr);
-      });
-  }
-
-  // Add the static executor waitable to the memory strategy
-  memory_strategy_->add_waitable_handle(this->shared_from_this());
-}
-
-void
-StaticExecutorEntitiesCollector::fill_executable_list()
-{
-  exec_list_.clear();
-  add_callback_groups_from_nodes_associated_to_executor();
-  fill_executable_list_from_map(weak_groups_associated_with_executor_to_nodes_);
-  fill_executable_list_from_map(weak_groups_to_nodes_associated_with_executor_);
-  // Add the executor's waitable to the executable list
-  exec_list_.add_waitable(shared_from_this());
-}
-void
-StaticExecutorEntitiesCollector::fill_executable_list_from_map(
-  const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap &
-  weak_groups_to_nodes)
-{
-  for (const auto & pair : weak_groups_to_nodes) {
-    auto group = pair.first.lock();
-    auto node = pair.second.lock();
-    if (!node || !group || !group->can_be_taken_from().load()) {
-      continue;
-    }
-    group->find_timer_ptrs_if(
-      [this](const rclcpp::TimerBase::SharedPtr & timer) {
-        if (timer) {
-          exec_list_.add_timer(timer);
-        }
-        return false;
-      });
-    group->find_subscription_ptrs_if(
-      [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
-        if (subscription) {
-          exec_list_.add_subscription(subscription);
-        }
-        return false;
-      });
-    group->find_service_ptrs_if(
-      [this](const rclcpp::ServiceBase::SharedPtr & service) {
-        if (service) {
-          exec_list_.add_service(service);
-        }
-        return false;
-      });
-    group->find_client_ptrs_if(
-      [this](const rclcpp::ClientBase::SharedPtr & client) {
-        if (client) {
-          exec_list_.add_client(client);
-        }
-        return false;
-      });
-    group->find_waitable_ptrs_if(
-      [this](const rclcpp::Waitable::SharedPtr & waitable) {
-        if (waitable) {
-          exec_list_.add_waitable(waitable);
-        }
-        return false;
-      });
-  }
-}
-
-void
-StaticExecutorEntitiesCollector::prepare_wait_set()
-{
-  // clear wait set
-  if (rcl_wait_set_clear(p_wait_set_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear wait set");
-  }
-
-  // The size of waitables are accounted for in size of the other entities
-  rcl_ret_t ret = rcl_wait_set_resize(
-    p_wait_set_, memory_strategy_->number_of_ready_subscriptions(),
-    memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(),
-    memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(),
-    memory_strategy_->number_of_ready_events());
-
-  if (RCL_RET_OK != ret) {
-    throw std::runtime_error(
-            std::string("Couldn't resize the wait set: ") + rcl_get_error_string().str);
-  }
-}
-
-void
-StaticExecutorEntitiesCollector::refresh_wait_set(std::chrono::nanoseconds timeout)
-{
-  // clear wait set (memset to '0' all wait_set_ entities
-  // but keeps the wait_set_ number of entities)
-  if (rcl_wait_set_clear(p_wait_set_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear wait set");
-  }
-
-  if (!memory_strategy_->add_handles_to_wait_set(p_wait_set_)) {
-    throw std::runtime_error("Couldn't fill wait set");
-  }
-
-  rcl_ret_t status =
-    rcl_wait(p_wait_set_, std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count());
-
-  if (status == RCL_RET_WAIT_SET_EMPTY) {
-    RCUTILS_LOG_WARN_NAMED(
-      "rclcpp",
-      "empty wait set received in rcl_wait(). This should never happen.");
-  } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) {
-    using rclcpp::exceptions::throw_from_rcl_error;
-    throw_from_rcl_error(status, "rcl_wait() failed");
-  }
-}
-
-void
-StaticExecutorEntitiesCollector::add_to_wait_set(rcl_wait_set_t * wait_set)
-{
-  // Add waitable guard conditions (one for each registered node) into the wait set.
-  for (const auto & pair : weak_nodes_to_guard_conditions_) {
-    auto & gc = pair.second;
-    detail::add_guard_condition_to_rcl_wait_set(*wait_set, *gc);
-  }
-}
-
-size_t StaticExecutorEntitiesCollector::get_number_of_ready_guard_conditions()
-{
-  std::lock_guard<std::mutex> guard{new_nodes_mutex_};
-  return weak_nodes_to_guard_conditions_.size() + new_nodes_.size();
-}
-
-bool
-StaticExecutorEntitiesCollector::add_node(
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
-{
-  bool is_new_node = false;
-  // If the node already has an executor
-  std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
-  if (has_executor.exchange(true)) {
-    throw std::runtime_error("Node has already been added to an executor.");
-  }
-  node_ptr->for_each_callback_group(
-    [this, node_ptr, &is_new_node](rclcpp::CallbackGroup::SharedPtr group_ptr)
-    {
-      if (
-        !group_ptr->get_associated_with_executor_atomic().load() &&
-        group_ptr->automatically_add_to_executor_with_node())
-      {
-        is_new_node = (add_callback_group(
-          group_ptr,
-          node_ptr,
-          weak_groups_to_nodes_associated_with_executor_) ||
-        is_new_node);
-      }
-    });
-  weak_nodes_.push_back(node_ptr);
-  return is_new_node;
-}
-
-bool
-StaticExecutorEntitiesCollector::add_callback_group(
-  rclcpp::CallbackGroup::SharedPtr group_ptr,
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
-{
-  // If the callback_group already has an executor
-  std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic();
-  if (has_executor.exchange(true)) {
-    throw std::runtime_error("Callback group has already been added to an executor.");
-  }
-  bool is_new_node = !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_) &&
-    !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_);
-  rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
-  auto insert_info = weak_groups_to_nodes.insert(
-    std::make_pair(weak_group_ptr, node_ptr));
-  bool was_inserted = insert_info.second;
-  if (!was_inserted) {
-    throw std::runtime_error("Callback group was already added to executor.");
-  }
-  if (is_new_node) {
-    std::lock_guard<std::mutex> guard{new_nodes_mutex_};
-    new_nodes_.push_back(node_ptr);
-    return true;
-  }
-  return false;
-}
-
-bool
-StaticExecutorEntitiesCollector::add_callback_group(
-  rclcpp::CallbackGroup::SharedPtr group_ptr,
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
-{
-  return add_callback_group(group_ptr, node_ptr, weak_groups_associated_with_executor_to_nodes_);
-}
-
-bool
-StaticExecutorEntitiesCollector::remove_callback_group(
-  rclcpp::CallbackGroup::SharedPtr group_ptr)
-{
-  return this->remove_callback_group_from_map(
-    group_ptr,
-    weak_groups_associated_with_executor_to_nodes_);
-}
-
-bool
-StaticExecutorEntitiesCollector::remove_callback_group_from_map(
-  rclcpp::CallbackGroup::SharedPtr group_ptr,
-  rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
-{
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr;
-  rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
-  auto iter = weak_groups_to_nodes.find(weak_group_ptr);
-  if (iter != weak_groups_to_nodes.end()) {
-    node_ptr = iter->second.lock();
-    if (node_ptr == nullptr) {
-      throw std::runtime_error("Node must not be deleted before its callback group(s).");
-    }
-    weak_groups_to_nodes.erase(iter);
-  } else {
-    throw std::runtime_error("Callback group needs to be associated with executor.");
-  }
-  // If the node was matched and removed, interrupt waiting.
-  if (!has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_) &&
-    !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_))
-  {
-    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr);
-    weak_nodes_to_guard_conditions_.erase(node_weak_ptr);
-    return true;
-  }
-  return false;
-}
-
-bool
-StaticExecutorEntitiesCollector::remove_node(
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
-{
-  if (!node_ptr->get_associated_with_executor_atomic().load()) {
-    return false;
-  }
-  bool node_found = false;
-  auto node_it = weak_nodes_.begin();
-  while (node_it != weak_nodes_.end()) {
-    bool matched = (node_it->lock() == node_ptr);
-    if (matched) {
-      weak_nodes_.erase(node_it);
-      node_found = true;
-      break;
-    }
-    ++node_it;
-  }
-  if (!node_found) {
-    return false;
-  }
-  std::vector<rclcpp::CallbackGroup::SharedPtr> found_group_ptrs;
-  std::for_each(
-    weak_groups_to_nodes_associated_with_executor_.begin(),
-    weak_groups_to_nodes_associated_with_executor_.end(),
-    [&found_group_ptrs, node_ptr](
-      std::pair<rclcpp::CallbackGroup::WeakPtr,
-      rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> key_value_pair)
-    {
-      auto & weak_node_ptr = key_value_pair.second;
-      auto shared_node_ptr = weak_node_ptr.lock();
-      auto group_ptr = key_value_pair.first.lock();
-      if (shared_node_ptr == node_ptr) {
-        found_group_ptrs.push_back(group_ptr);
-      }
-    });
-  std::for_each(
-    found_group_ptrs.begin(), found_group_ptrs.end(),
-    [this](rclcpp::CallbackGroup::SharedPtr group_ptr)
-    {
-      this->remove_callback_group_from_map(
-        group_ptr,
-        weak_groups_to_nodes_associated_with_executor_);
-    });
-  std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
-  has_executor.store(false);
-  return true;
-}
-
-bool
-StaticExecutorEntitiesCollector::is_ready(rcl_wait_set_t * p_wait_set)
-{
-  // Check wait_set guard_conditions for added/removed entities to/from a node
-  for (size_t i = 0; i < p_wait_set->size_of_guard_conditions; ++i) {
-    if (p_wait_set->guard_conditions[i] != NULL) {
-      auto found_guard_condition = std::find_if(
-        weak_nodes_to_guard_conditions_.begin(), weak_nodes_to_guard_conditions_.end(),
-        [&](std::pair<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
-        const GuardCondition *> pair) -> bool {
-          const rcl_guard_condition_t & rcl_gc = pair.second->get_rcl_guard_condition();
-          return &rcl_gc == p_wait_set->guard_conditions[i];
-        });
-      if (found_guard_condition != weak_nodes_to_guard_conditions_.end()) {
-        return true;
-      }
-    }
-  }
-  // None of the guard conditions triggered belong to a registered node
-  return false;
-}
-
-// Returns true iff the weak_groups_to_nodes map has node_ptr as the value in any of its entry.
-bool
-StaticExecutorEntitiesCollector::has_node(
-  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap &
-  weak_groups_to_nodes) const
-{
-  return std::find_if(
-    weak_groups_to_nodes.begin(),
-    weak_groups_to_nodes.end(),
-    [&](const WeakCallbackGroupsToNodesMap::value_type & other) -> bool {
-      auto other_ptr = other.second.lock();
-      return other_ptr == node_ptr;
-    }) != weak_groups_to_nodes.end();
-}
-
-void
-StaticExecutorEntitiesCollector::add_callback_groups_from_nodes_associated_to_executor()
-{
-  for (const auto & weak_node : weak_nodes_) {
-    auto node = weak_node.lock();
-    if (node) {
-      node->for_each_callback_group(
-        [this, node](rclcpp::CallbackGroup::SharedPtr shared_group_ptr)
-        {
-          if (shared_group_ptr->automatically_add_to_executor_with_node() &&
-          !shared_group_ptr->get_associated_with_executor_atomic().load())
-          {
-            add_callback_group(
-              shared_group_ptr,
-              node,
-              weak_groups_to_nodes_associated_with_executor_);
-          }
-        });
-    }
-  }
-}
-
-std::vector<rclcpp::CallbackGroup::WeakPtr>
-StaticExecutorEntitiesCollector::get_all_callback_groups()
-{
-  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
-  for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
-    groups.push_back(group_node_ptr.first);
-  }
-  for (const auto & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) {
-    groups.push_back(group_node_ptr.first);
-  }
-  return groups;
-}
-
-std::vector<rclcpp::CallbackGroup::WeakPtr>
-StaticExecutorEntitiesCollector::get_manually_added_callback_groups()
-{
-  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
-  for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
-    groups.push_back(group_node_ptr.first);
-  }
-  return groups;
-}
-
-std::vector<rclcpp::CallbackGroup::WeakPtr>
-StaticExecutorEntitiesCollector::get_automatically_added_callback_groups_from_nodes()
-{
-  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
-  for (const auto & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) {
-    groups.push_back(group_node_ptr.first);
-  }
-  return groups;
-}

rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp

@@ -12,31 +12,20 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "rclcpp/executors/static_single_threaded_executor.hpp"
-
-#include <chrono>
-#include <memory>
-#include <utility>
-#include <vector>
-
+#include "rclcpp/executors/executor_entities_collection.hpp"
 #include "rcpputils/scope_exit.hpp"
 
-using rclcpp::executors::StaticSingleThreadedExecutor;
-using rclcpp::experimental::ExecutableList;
+#include "rclcpp/executors/static_single_threaded_executor.hpp"
+#include "rclcpp/any_executable.hpp"
 
-StaticSingleThreadedExecutor::StaticSingleThreadedExecutor(
-  const rclcpp::ExecutorOptions & options)
+using rclcpp::executors::StaticSingleThreadedExecutor;
+
+StaticSingleThreadedExecutor::StaticSingleThreadedExecutor(const rclcpp::ExecutorOptions & options)
 : rclcpp::Executor(options)
 {
-  entities_collector_ = std::make_shared<StaticExecutorEntitiesCollector>();
 }
 
-StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor()
-{
-  if (entities_collector_->is_init()) {
-    entities_collector_->fini();
-  }
-}
+StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor() {}
 
 void
 StaticSingleThreadedExecutor::spin()
@@ -46,14 +35,11 @@ StaticSingleThreadedExecutor::spin()
   }
   RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
 
-  // Set memory_strategy_ and exec_list_ based on weak_nodes_
-  // Prepare wait_set_ based on memory_strategy_
-  entities_collector_->init(&wait_set_, memory_strategy_);
-
+  // This is essentially the contents of the rclcpp::Executor::wait_for_work method,
+  // except we need to keep the wait result to reproduce the StaticSingleThreadedExecutor
+  // behavior.
   while (rclcpp::ok(this->context_) && spinning.load()) {
-    // Refresh wait set and wait for work
-    entities_collector_->refresh_wait_set();
-    execute_ready_executables();
+    this->spin_once_impl(std::chrono::nanoseconds(-1));
   }
 }
 
@@ -64,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);
 }
 
@@ -80,36 +65,32 @@ StaticSingleThreadedExecutor::spin_all(std::chrono::nanoseconds max_duration)
 void
 StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive)
 {
-  // Make sure the entities collector has been initialized
-  if (!entities_collector_->is_init()) {
-    entities_collector_->init(&wait_set_, memory_strategy_);
-  }
-
   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
-    entities_collector_->refresh_wait_set(std::chrono::milliseconds::zero());
-    // Execute ready executables
-    bool work_available = execute_ready_executables();
-    if (!work_available || !exhaustive) {
-      break;
+    std::lock_guard<std::mutex> guard(mutex_);
+
+    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;
+      }
     }
   }
 }
@@ -117,163 +98,125 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati
 void
 StaticSingleThreadedExecutor::spin_once_impl(std::chrono::nanoseconds timeout)
 {
-  // Make sure the entities collector has been initialized
-  if (!entities_collector_->is_init()) {
-    entities_collector_->init(&wait_set_, memory_strategy_);
-  }
-
   if (rclcpp::ok(context_) && spinning.load()) {
-    // Wait until we have a ready entity or timeout expired
-    entities_collector_->refresh_wait_set(timeout);
-    // Execute ready executables
-    execute_ready_executables(true);
+    std::lock_guard<std::mutex> guard(mutex_);
+    auto wait_result = this->collect_and_wait(timeout);
+    if (wait_result.has_value()) {
+      this->execute_ready_executables(current_collection_, wait_result.value(), true);
+    }
   }
 }
 
-void
-StaticSingleThreadedExecutor::add_callback_group(
-  rclcpp::CallbackGroup::SharedPtr group_ptr,
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  bool notify)
+std::optional<rclcpp::WaitResult<rclcpp::WaitSet>>
+StaticSingleThreadedExecutor::collect_and_wait(std::chrono::nanoseconds timeout)
 {
-  bool is_new_node = entities_collector_->add_callback_group(group_ptr, node_ptr);
-  if (is_new_node && notify) {
-    // Interrupt waiting to handle new node
-    interrupt_guard_condition_->trigger();
+  // we need to make sure that callback groups don't get out of scope
+  // during the wait. As in jazzy, they are not covered by the DynamicStorage,
+  // we explicitly hold them here as a bugfix
+  std::vector<rclcpp::CallbackGroup::SharedPtr> cbgs;
+
+  if (this->entities_need_rebuild_.exchange(false) || current_collection_.empty()) {
+    this->collect_entities();
   }
-}
 
-void
-StaticSingleThreadedExecutor::add_node(
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
-{
-  bool is_new_node = entities_collector_->add_node(node_ptr);
-  if (is_new_node && notify) {
-    // Interrupt waiting to handle new node
-    interrupt_guard_condition_->trigger();
+  auto callback_groups = this->collector_.get_all_callback_groups();
+  cbgs.resize(callback_groups.size());
+  for(const auto & w_ptr : callback_groups) {
+    auto shr_ptr = w_ptr.lock();
+    if(shr_ptr) {
+      cbgs.push_back(std::move(shr_ptr));
+    }
   }
-}
 
-void
-StaticSingleThreadedExecutor::add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify)
-{
-  this->add_node(node_ptr->get_node_base_interface(), notify);
-}
+  auto wait_result = wait_set_.wait(std::chrono::nanoseconds(timeout));
 
-void
-StaticSingleThreadedExecutor::remove_callback_group(
-  rclcpp::CallbackGroup::SharedPtr group_ptr, bool notify)
-{
-  bool node_removed = entities_collector_->remove_callback_group(group_ptr);
-  // If the node was matched and removed, interrupt waiting
-  if (node_removed && notify) {
-    interrupt_guard_condition_->trigger();
+  // drop references to the callback groups, before trying to execute anything
+  cbgs.clear();
+
+  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;
 }
 
-void
-StaticSingleThreadedExecutor::remove_node(
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
-{
-  bool node_removed = entities_collector_->remove_node(node_ptr);
-  if (!node_removed) {
-    throw std::runtime_error("Node needs to be associated with this executor.");
-  }
-  // If the node was matched and removed, interrupt waiting
-  if (notify) {
-    interrupt_guard_condition_->trigger();
-  }
-}
-
-std::vector<rclcpp::CallbackGroup::WeakPtr>
-StaticSingleThreadedExecutor::get_all_callback_groups()
-{
-  return entities_collector_->get_all_callback_groups();
-}
-
-std::vector<rclcpp::CallbackGroup::WeakPtr>
-StaticSingleThreadedExecutor::get_manually_added_callback_groups()
-{
-  return entities_collector_->get_manually_added_callback_groups();
-}
-
-std::vector<rclcpp::CallbackGroup::WeakPtr>
-StaticSingleThreadedExecutor::get_automatically_added_callback_groups_from_nodes()
-{
-  return entities_collector_->get_automatically_added_callback_groups_from_nodes();
-}
-
-void
-StaticSingleThreadedExecutor::remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify)
-{
-  this->remove_node(node_ptr->get_node_base_interface(), notify);
-}
-
-bool
-StaticSingleThreadedExecutor::execute_ready_executables(bool spin_once)
+// This preserves the "scheduling semantics" of the StaticSingleThreadedExecutor
+// from the original implementation.
+bool StaticSingleThreadedExecutor::execute_ready_executables(
+  const rclcpp::executors::ExecutorEntitiesCollection & collection,
+  rclcpp::WaitResult<rclcpp::WaitSet> & wait_result,
+  bool spin_once)
 {
   bool any_ready_executable = false;
+  if (wait_result.kind() != rclcpp::WaitResultKind::Ready) {
+    return any_ready_executable;
+  }
 
-  // Execute all the ready subscriptions
-  for (size_t i = 0; i < wait_set_.size_of_subscriptions; ++i) {
-    if (i < entities_collector_->get_number_of_subscriptions()) {
-      if (wait_set_.subscriptions[i]) {
-        execute_subscription(entities_collector_->get_subscription(i));
-        if (spin_once) {
-          return true;
-        }
-        any_ready_executable = true;
-      }
-    }
-  }
-  // Execute all the ready timers
-  for (size_t i = 0; i < wait_set_.size_of_timers; ++i) {
-    if (i < entities_collector_->get_number_of_timers()) {
-      if (wait_set_.timers[i] && entities_collector_->get_timer(i)->is_ready()) {
-        auto timer = entities_collector_->get_timer(i);
-        timer->call();
-        execute_timer(std::move(timer));
-        if (spin_once) {
-          return true;
-        }
-        any_ready_executable = true;
-      }
-    }
-  }
-  // Execute all the ready services
-  for (size_t i = 0; i < wait_set_.size_of_services; ++i) {
-    if (i < entities_collector_->get_number_of_services()) {
-      if (wait_set_.services[i]) {
-        execute_service(entities_collector_->get_service(i));
-        if (spin_once) {
-          return true;
-        }
-        any_ready_executable = true;
-      }
-    }
-  }
-  // Execute all the ready clients
-  for (size_t i = 0; i < wait_set_.size_of_clients; ++i) {
-    if (i < entities_collector_->get_number_of_clients()) {
-      if (wait_set_.clients[i]) {
-        execute_client(entities_collector_->get_client(i));
-        if (spin_once) {
-          return true;
-        }
-        any_ready_executable = true;
-      }
-    }
-  }
-  // Execute all the ready waitables
-  for (size_t i = 0; i < entities_collector_->get_number_of_waitables(); ++i) {
-    auto waitable = entities_collector_->get_waitable(i);
-    if (waitable->is_ready(&wait_set_)) {
-      auto data = waitable->take_data();
-      waitable->execute(data);
-      if (spin_once) {
-        return true;
-      }
+  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()) {
+      execute_subscription(subscription);
       any_ready_executable = true;
+      if (spin_once) {return any_ready_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 = 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_timer(std::move(timer), data);
+      any_ready_executable = true;
+      if (spin_once) {return any_ready_executable;}
+    }
+  }
+
+  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()) {
+      execute_client(client);
+      any_ready_executable = true;
+      if (spin_once) {return any_ready_executable;}
+    }
+  }
+
+  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;}
+    }
+  }
+
+  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;}
     }
   }
   return any_ready_executable;

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

@@ -40,40 +40,48 @@ 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>();
+  entities_need_rebuild_ = false;
 
-  notify_waitable_ = std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>(
+  this->setup_notify_waitable();
+
+  // Ensure that the entities collection is empty (the base class may have added elements
+  // that we are not interested in)
+  this->current_collection_.clear();
+
+  // Make sure that the notify waitable is immediately added to the collection
+  // to avoid missing events
+  this->add_notify_waitable_to_collection(current_collection_.waitables);
+}
+
+void
+EventsExecutor::setup_notify_waitable()
+{
+  // The base class already created this object but the events-executor
+  // needs different callbacks.
+  assert(notify_waitable_ && "The notify waitable should have already been constructed");
+
+  notify_waitable_->set_execute_callback(
     [this]() {
       // This callback is invoked when:
       // - the interrupt or shutdown guard condition is triggered:
       //    ---> we need to wake up the executor so that it can terminate
       // - a node or callback group guard condition is triggered:
       //    ---> the entities collection is changed, we need to update callbacks
-      notify_waitable_event_pushed_ = false;
-      this->refresh_current_collection_from_callback_groups();
+      this->handle_updated_entities(false);
     });
 
-  // 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_);
-
-  notify_waitable_->set_on_ready_callback(
-    this->create_waitable_callback(notify_waitable_.get()));
-
   auto notify_waitable_entity_id = notify_waitable_.get();
   notify_waitable_->set_on_ready_callback(
     [this, notify_waitable_entity_id](size_t num_events, int waitable_data) {
@@ -83,17 +91,14 @@ EventsExecutor::EventsExecutor(
       // For the same reason, if an event of this type has already been pushed but it has not been
       // processed yet, we avoid pushing additional events.
       (void)num_events;
-      if (notify_waitable_event_pushed_.exchange(true)) {
+      if (entities_need_rebuild_.exchange(true)) {
         return;
       }
 
       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_);
 }
 
 EventsExecutor::~EventsExecutor()
@@ -162,6 +167,14 @@ EventsExecutor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhau
       return false;
     };
 
+  // If this spin is not exhaustive (e.g. spin_some), we need to explicitly check
+  // if entities need to be rebuilt here rather than letting the notify waitable event do it.
+  // A non-exhaustive spin would not check for work a second time, thus delaying the execution
+  // of some entities to the next invocation of spin.
+  if (!exhaustive) {
+    this->handle_updated_entities(false);
+  }
+
   // Get the number of events and timers ready at start
   const size_t ready_events_at_start = events_queue_->size();
   size_t executed_events = 0;
@@ -227,46 +240,6 @@ EventsExecutor::spin_once_impl(std::chrono::nanoseconds timeout)
   }
 }
 
-void
-EventsExecutor::add_node(
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
-{
-  // This field is unused because we don't have to wake up the executor when a node is added.
-  (void) notify;
-
-  // Add node to entities collector
-  this->entities_collector_->add_node(node_ptr);
-
-  this->refresh_current_collection_from_callback_groups();
-}
-
-void
-EventsExecutor::add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify)
-{
-  this->add_node(node_ptr->get_node_base_interface(), notify);
-}
-
-void
-EventsExecutor::remove_node(
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
-{
-  // This field is unused because we don't have to wake up the executor when a node is removed.
-  (void)notify;
-
-  // Remove node from entities collector.
-  // This will result in un-setting all the event callbacks from its entities.
-  // After this function returns, this executor will not receive any more events associated
-  // to these entities.
-  this->entities_collector_->remove_node(node_ptr);
-
-  this->refresh_current_collection_from_callback_groups();
-}
-
-void
-EventsExecutor::remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify)
-{
-  this->remove_node(node_ptr->get_node_base_interface(), notify);
-}
 
 void
 EventsExecutor::execute_event(const ExecutorEvent & event)
@@ -276,10 +249,9 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
       {
         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);
+            current_collection_.clients);
         }
         if (client) {
           for (size_t i = 0; i < event.num_events; i++) {
@@ -293,10 +265,9 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
       {
         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);
+            current_collection_.subscriptions);
         }
         if (subscription) {
           for (size_t i = 0; i < event.num_events; i++) {
@@ -309,10 +280,9 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
       {
         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);
+            current_collection_.services);
         }
         if (service) {
           for (size_t i = 0; i < event.num_events; i++) {
@@ -325,21 +295,20 @@ 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:
       {
         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);
+            current_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);
           }
         }
@@ -349,61 +318,21 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
 }
 
 void
-EventsExecutor::add_callback_group(
-  rclcpp::CallbackGroup::SharedPtr group_ptr,
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  bool notify)
+EventsExecutor::handle_updated_entities([[maybe_unused]] bool notify)
 {
-  // This field is unused because we don't have to wake up
-  // the executor when a callback group is added.
-  (void)notify;
-  (void)node_ptr;
+  // Do not rebuild if we don't need to.
+  // A rebuild event could be generated, but then
+  // this function could end up being called from somewhere else
+  // before that event gets processed, for example if
+  // a node or callback group is manually added to the executor.
+  const bool notify_waitable_triggered = entities_need_rebuild_.exchange(false);
+  if (!notify_waitable_triggered && !this->collector_.has_pending()) {
+    return;
+  }
 
-  this->entities_collector_->add_callback_group(group_ptr);
-
-  this->refresh_current_collection_from_callback_groups();
-}
-
-void
-EventsExecutor::remove_callback_group(
-  rclcpp::CallbackGroup::SharedPtr group_ptr, bool notify)
-{
-  // This field is unused because we don't have to wake up
-  // the executor when a callback group is removed.
-  (void)notify;
-
-  this->entities_collector_->remove_callback_group(group_ptr);
-
-  this->refresh_current_collection_from_callback_groups();
-}
-
-std::vector<rclcpp::CallbackGroup::WeakPtr>
-EventsExecutor::get_all_callback_groups()
-{
-  this->entities_collector_->update_collections();
-  return this->entities_collector_->get_all_callback_groups();
-}
-
-std::vector<rclcpp::CallbackGroup::WeakPtr>
-EventsExecutor::get_manually_added_callback_groups()
-{
-  this->entities_collector_->update_collections();
-  return this->entities_collector_->get_manually_added_callback_groups();
-}
-
-std::vector<rclcpp::CallbackGroup::WeakPtr>
-EventsExecutor::get_automatically_added_callback_groups_from_nodes()
-{
-  this->entities_collector_->update_collections();
-  return this->entities_collector_->get_automatically_added_callback_groups();
-}
-
-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();
+  this->collector_.update_collections();
+  auto callback_groups = this->collector_.get_all_callback_groups();
   rclcpp::executors::ExecutorEntitiesCollection new_collection;
   rclcpp::executors::build_entities_collection(callback_groups, new_collection);
 
@@ -413,14 +342,11 @@ EventsExecutor::refresh_current_collection_from_callback_groups()
   // We could explicitly check for the notify waitable ID when we receive a waitable event
   // but I think that it's better if the waitable was in the collection and it could be
   // 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.
+  // To do it, we need to add the notify waitable as an entry in the new collection
+  // such that it's neither added or removed (it should have already been added
+  // to the current collection in the constructor)
   this->add_notify_waitable_to_collection(new_collection.waitables);
 
-  // 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);
 }
 
@@ -429,14 +355,19 @@ 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_);
+  std::lock_guard<std::mutex> guard(mutex_);
 
-  current_entities_collection_->timers.update(
+  // Remove expired entities to ensure re-initialized objects
+  // are updated. This fixes issues with stale state entities.
+  // See: https://github.com/ros2/rclcpp/pull/2586
+  current_collection_.remove_expired_entities();
+
+  current_collection_.timers.update(
     new_collection.timers,
     [this](rclcpp::TimerBase::SharedPtr timer) {timers_manager_->add_timer(timer);},
     [this](rclcpp::TimerBase::SharedPtr timer) {timers_manager_->remove_timer(timer);});
 
-  current_entities_collection_->subscriptions.update(
+  current_collection_.subscriptions.update(
     new_collection.subscriptions,
     [this](auto subscription) {
       subscription->set_on_new_message_callback(
@@ -445,7 +376,7 @@ EventsExecutor::refresh_current_collection(
     },
     [](auto subscription) {subscription->clear_on_new_message_callback();});
 
-  current_entities_collection_->clients.update(
+  current_collection_.clients.update(
     new_collection.clients,
     [this](auto client) {
       client->set_on_new_response_callback(
@@ -454,7 +385,7 @@ EventsExecutor::refresh_current_collection(
     },
     [](auto client) {client->clear_on_new_response_callback();});
 
-  current_entities_collection_->services.update(
+  current_collection_.services.update(
     new_collection.services,
     [this](auto service) {
       service->set_on_new_request_callback(
@@ -465,18 +396,26 @@ EventsExecutor::refresh_current_collection(
 
   // DO WE NEED THIS? WE ARE NOT DOING ANYTHING WITH GUARD CONDITIONS
   /*
-  current_entities_collection_->guard_conditions.update(new_collection.guard_conditions,
+  current_collection_.guard_conditions.update(new_collection.guard_conditions,
     [](auto guard_condition) {(void)guard_condition;},
     [](auto guard_condition) {guard_condition->set_on_trigger_callback(nullptr);});
   */
 
-  current_entities_collection_->waitables.update(
+  current_collection_.waitables.update(
     new_collection.waitables,
     [this](auto waitable) {
       waitable->set_on_ready_callback(
         this->create_waitable_callback(waitable.get()));
+      for (const auto & t : waitable->get_timers()) {
+        timers_manager_->add_timer(t);
+      }
     },
-    [](auto waitable) {waitable->clear_on_ready_callback();});
+    [this](auto waitable) {
+      waitable->clear_on_ready_callback();
+      for (const auto & t : waitable->get_timers()) {
+        timers_manager_->remove_timer(t);
+      }
+    });
 }
 
 std::function<void(size_t)>
@@ -485,7 +424,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;
@@ -497,7 +436,7 @@ 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;

rclcpp/src/rclcpp/experimental/timers_manager.cpp

@@ -27,7 +27,7 @@ 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)
 {
@@ -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,7 +171,7 @@ 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);
   }
 }
 
@@ -215,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++;

rclcpp/src/rclcpp/generic_client.cpp

@@ -31,22 +31,31 @@ GenericClient::GenericClient(
   rcl_client_options_t & client_options)
 : ClientBase(node_base, node_graph)
 {
-  ts_lib_ = get_typesupport_library(
-    service_type, "rosidl_typesupport_cpp");
+  const rosidl_service_type_support_t * service_ts;
+  try {
+    ts_lib_ = get_typesupport_library(
+      service_type, "rosidl_typesupport_cpp");
 
-  auto service_ts_ = get_service_typesupport_handle(
-    service_type, "rosidl_typesupport_cpp", *ts_lib_);
+    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);
+    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);
+  } catch (std::runtime_error & err) {
+    RCLCPP_ERROR(
+      rclcpp::get_node_logger(node_handle_.get()).get_child("rclcpp"),
+      "Invalid service type: %s",
+      err.what());
+    throw rclcpp::exceptions::InvalidServiceTypeError(err.what());
+  }
 
   rcl_ret_t ret = rcl_client_init(
     this->get_client_handle().get(),
     this->get_rcl_node_handle(),
-    service_ts_,
+    service_ts,
     service_name.c_str(),
     &client_options);
   if (ret != RCL_RET_OK) {
@@ -100,6 +109,13 @@ GenericClient::handle_response(
   if (std::holds_alternative<Promise>(value)) {
     auto & promise = std::get<Promise>(value);
     promise.set_value(std::move(response));
+  } else if (std::holds_alternative<CallbackTypeValueVariant>(value)) {
+    auto & inner = std::get<CallbackTypeValueVariant>(value);
+    const auto & callback = std::get<CallbackType>(inner);
+    auto & promise = std::get<Promise>(inner);
+    auto & future = std::get<SharedFuture>(inner);
+    promise.set_value(std::move(response));
+    callback(std::move(future));
   }
 }
 
@@ -119,6 +135,18 @@ GenericClient::remove_pending_request(int64_t request_id)
   return pending_requests_.erase(request_id) != 0u;
 }
 
+bool
+GenericClient::remove_pending_request(const FutureAndRequestId & future)
+{
+  return this->remove_pending_request(future.request_id);
+}
+
+bool
+GenericClient::remove_pending_request(const SharedFutureAndRequestId & future)
+{
+  return this->remove_pending_request(future.request_id);
+}
+
 std::optional<GenericClient::CallbackInfoVariant>
 GenericClient::get_and_erase_pending_request(int64_t request_number)
 {

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);
 

rclcpp/src/rclcpp/generic_service.cpp

@@ -0,0 +1,172 @@
+// Copyright 2024 Sony Group Corporation.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/generic_service.hpp"
+
+namespace rclcpp
+{
+GenericService::GenericService(
+  std::shared_ptr<rcl_node_t> node_handle,
+  const std::string & service_name,
+  const std::string & service_type,
+  GenericServiceCallback any_callback,
+  rcl_service_options_t & service_options)
+: ServiceBase(node_handle),
+  any_callback_(any_callback)
+{
+  const rosidl_service_type_support_t * service_ts;
+  try {
+    ts_lib_ = get_typesupport_library(
+      service_type, "rosidl_typesupport_cpp");
+
+    service_ts = get_service_typesupport_handle(
+      service_type, "rosidl_typesupport_cpp", *ts_lib_);
+
+    auto request_type_support_intro = get_message_typesupport_handle(
+      service_ts->request_typesupport,
+      rosidl_typesupport_introspection_cpp::typesupport_identifier);
+    request_members_ = static_cast<const rosidl_typesupport_introspection_cpp::MessageMembers *>(
+      request_type_support_intro->data);
+
+    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);
+  } catch (std::runtime_error & err) {
+    RCLCPP_ERROR(
+      rclcpp::get_node_logger(node_handle_.get()).get_child("rclcpp"),
+      "Invalid service type: %s",
+      err.what());
+    throw rclcpp::exceptions::InvalidServiceTypeError(err.what());
+  }
+
+  // rcl does the static memory allocation here
+  service_handle_ = std::shared_ptr<rcl_service_t>(
+    new rcl_service_t, [handle = node_handle_, service_name](rcl_service_t * service)
+    {
+      if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) {
+        RCLCPP_ERROR(
+          rclcpp::get_node_logger(handle.get()).get_child("rclcpp"),
+          "Error in destruction of rcl service handle: %s",
+          rcl_get_error_string().str);
+        rcl_reset_error();
+      }
+      delete service;
+    });
+  *service_handle_.get() = rcl_get_zero_initialized_service();
+
+  rcl_ret_t ret = rcl_service_init(
+    service_handle_.get(),
+    node_handle.get(),
+    service_ts,
+    service_name.c_str(),
+    &service_options);
+  if (ret != RCL_RET_OK) {
+    if (ret == RCL_RET_SERVICE_NAME_INVALID) {
+      auto rcl_node_handle = get_rcl_node_handle();
+      // this will throw on any validation problem
+      rcl_reset_error();
+      expand_topic_or_service_name(
+        service_name,
+        rcl_node_get_name(rcl_node_handle),
+        rcl_node_get_namespace(rcl_node_handle),
+        true);
+    }
+
+    rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service");
+  }
+  TRACETOOLS_TRACEPOINT(
+    rclcpp_service_callback_added,
+    static_cast<const void *>(get_service_handle().get()),
+    static_cast<const void *>(&any_callback_));
+#ifndef TRACETOOLS_DISABLED
+  any_callback_.register_callback_for_tracing();
+#endif
+}
+
+bool
+GenericService::take_request(
+  SharedRequest request_out,
+  rmw_request_id_t & request_id_out)
+{
+  request_out = create_request();
+  return this->take_type_erased_request(request_out.get(), request_id_out);
+}
+
+std::shared_ptr<void>
+GenericService::create_request()
+{
+  Request request = new uint8_t[request_members_->size_of_];
+  request_members_->init_function(request, rosidl_runtime_cpp::MessageInitialization::ZERO);
+  return std::shared_ptr<void>(
+    request,
+    [this](void * p)
+    {
+      request_members_->fini_function(p);
+      delete[] reinterpret_cast<uint8_t *>(p);
+    });
+}
+
+std::shared_ptr<void>
+GenericService::create_response()
+{
+  Response 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>
+GenericService::create_request_header()
+{
+  return std::make_shared<rmw_request_id_t>();
+}
+
+void
+GenericService::handle_request(
+  std::shared_ptr<rmw_request_id_t> request_header,
+  std::shared_ptr<void> request)
+{
+  auto response = any_callback_.dispatch(
+    this->shared_from_this(), request_header, request, create_response());
+  if (response) {
+    send_response(*request_header, response);
+  }
+}
+
+void
+GenericService::send_response(rmw_request_id_t & req_id, SharedResponse & response)
+{
+  rcl_ret_t ret = rcl_send_response(get_service_handle().get(), &req_id, response.get());
+
+  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");
+  }
+}
+
+}  //  namespace rclcpp

rclcpp/src/rclcpp/generic_subscription.cpp

@@ -51,15 +51,14 @@ GenericSubscription::handle_serialized_message(
   const std::shared_ptr<rclcpp::SerializedMessage> & message,
   const rclcpp::MessageInfo & message_info)
 {
-  callback_(message, message_info);
+  any_callback_.dispatch(message, message_info);
 }
 
 void
 GenericSubscription::handle_loaned_message(
-  void * message, const rclcpp::MessageInfo & message_info)
+  [[maybe_unused]] void * message,
+  [[maybe_unused]] const rclcpp::MessageInfo & message_info)
 {
-  (void) message;
-  (void) message_info;
   throw rclcpp::exceptions::UnimplementedError(
           "handle_loaned_message is not implemented for GenericSubscription");
 }
@@ -111,20 +110,17 @@ GenericSubscription::create_dynamic_message()
 
 void
 GenericSubscription::return_dynamic_message(
-  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message)
+  [[maybe_unused]] rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message)
 {
-  (void) message;
   throw rclcpp::exceptions::UnimplementedError(
           "return_dynamic_message is not implemented for GenericSubscription");
 }
 
 void
 GenericSubscription::handle_dynamic_message(
-  const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
-  const rclcpp::MessageInfo & message_info)
+  [[maybe_unused]] const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
+  [[maybe_unused]] const rclcpp::MessageInfo & message_info)
 {
-  (void) message;
-  (void) message_info;
   throw rclcpp::exceptions::UnimplementedError(
           "handle_dynamic_message is not implemented for GenericSubscription");
 }

rclcpp/src/rclcpp/guard_condition.cpp

@@ -92,19 +92,19 @@ GuardCondition::exchange_in_use_by_wait_set_state(bool in_use_state)
 }
 
 void
-GuardCondition::add_to_wait_set(rcl_wait_set_t * wait_set)
+GuardCondition::add_to_wait_set(rcl_wait_set_t & wait_set)
 {
   std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
 
   if (exchange_in_use_by_wait_set_state(true)) {
-    if (wait_set != wait_set_) {
+    if (&wait_set != wait_set_) {
       throw std::runtime_error("guard condition has already been added to a wait set.");
     }
   } else {
-    wait_set_ = wait_set;
+    wait_set_ = &wait_set;
   }
 
-  rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &this->rcl_guard_condition_, NULL);
+  rcl_ret_t ret = rcl_wait_set_add_guard_condition(&wait_set, &this->rcl_guard_condition_, NULL);
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(
       ret, "failed to add guard condition to wait set");

rclcpp/src/rclcpp/logger.cpp

@@ -12,6 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <filesystem>
 #include <memory>
 #include <string>
 #include <utility>
@@ -54,8 +55,8 @@ get_node_logger(const rcl_node_t * node)
   return rclcpp::get_logger(logger_name);
 }
 
-rcpputils::fs::path
-get_logging_directory()
+std::filesystem::path
+get_log_directory()
 {
   char * log_dir = NULL;
   auto allocator = rcutils_get_default_allocator();