Avoid unecessary creation of MultiThreadedExecutor (#3090) (#3096)

(cherry picked from commit 8cd4d47ec5)

Signed-off-by: solonovamax <solonovamax@12oclockpoint.com>
Co-authored-by: solo <solonovamax@12oclockpoint.com>

rclcpp/CHANGELOG.rst

@@ -2,6 +2,275 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+16.0.18 (2026-02-09)
+--------------------
+* print warning message on owner node if the parameter operation fails. (backport `#3037 <https://github.com/ros2/rclcpp/issues/3037>`_) (`#3040 <https://github.com/ros2/rclcpp/issues/3040>`_)
+* fix context in wait for message wait set (`#3030 <https://github.com/ros2/rclcpp/issues/3030>`_) (`#3033 <https://github.com/ros2/rclcpp/issues/3033>`_)
+* Improve the robustness of the TopicEndpointInfo constructor (backport `#3013 <https://github.com/ros2/rclcpp/issues/3013>`_) (`#3016 <https://github.com/ros2/rclcpp/issues/3016>`_)
+* Contributors: mergify[bot]
+
+16.0.17 (2025-12-23)
+--------------------
+* Unified Node Interfaces: Add const version of get_node_x_interface() (`#3006 <https://github.com/ros2/rclcpp/issues/3006>`_) (`#3010 <https://github.com/ros2/rclcpp/issues/3010>`_)
+* [Humble] Implement Unified Node Interface (NodeInterfaces class) (backport `#2041 <https://github.com/ros2/rclcpp/issues/2041>`_) (`#3002 <https://github.com/ros2/rclcpp/issues/3002>`_)
+* remove I/O from signal handler. (`#3000 <https://github.com/ros2/rclcpp/issues/3000>`_) (`#3005 <https://github.com/ros2/rclcpp/issues/3005>`_)
+* correct test function descriptions (backport `#2970 <https://github.com/ros2/rclcpp/issues/2970>`_) (`#2994 <https://github.com/ros2/rclcpp/issues/2994>`_)
+* Contributors: fabianhirmann, mergify[bot]
+
+16.0.16 (2025-11-18)
+--------------------
+* Fix REP url locations (backport `#2987 <https://github.com/ros2/rclcpp/issues/2987>`_) (`#2991 <https://github.com/ros2/rclcpp/issues/2991>`_)
+* Contributors: mergify[bot]
+
+16.0.15 (2025-09-11)
+--------------------
+* Allow for implicitly convertable loggers as well (`#2922 <https://github.com/ros2/rclcpp/issues/2922>`_) (`#2936 <https://github.com/ros2/rclcpp/issues/2936>`_) (`#2938 <https://github.com/ros2/rclcpp/issues/2938>`_)
+* Fix: improve exception context for parameter_value_from (backport `#2917 <https://github.com/ros2/rclcpp/issues/2917>`_) (`#2921 <https://github.com/ros2/rclcpp/issues/2921>`_)
+* Removed warning test_qos (`#2859 <https://github.com/ros2/rclcpp/issues/2859>`_) (`#2925 <https://github.com/ros2/rclcpp/issues/2925>`_)
+* Add qos parameter for wait_for_message function (`#2903 <https://github.com/ros2/rclcpp/issues/2903>`_) (`#2907 <https://github.com/ros2/rclcpp/issues/2907>`_)
+* Contributors: mergify[bot]
+
+16.0.14 (2025-07-16)
+--------------------
+* fix test_publisher_with_system_default_qos. (backport `#2881 <https://github.com/ros2/rclcpp/issues/2881>`_) (`#2882 <https://github.com/ros2/rclcpp/issues/2882>`_)
+* Contributors: mergify[bot]
+
+16.0.13 (2025-06-23)
+--------------------
+* Fix for memory leaks in rclcpp::SerializedMessage (`#2861 <https://github.com/ros2/rclcpp/issues/2861>`_) (`#2865 <https://github.com/ros2/rclcpp/issues/2865>`_)
+* Added missing chrono includes (backport `#2854 <https://github.com/ros2/rclcpp/issues/2854>`_) (`#2857 <https://github.com/ros2/rclcpp/issues/2857>`_)
+* QoSInitialization::from_rmw does not validate invalid history policy … (backport `#2841 <https://github.com/ros2/rclcpp/issues/2841>`_) (`#2844 <https://github.com/ros2/rclcpp/issues/2844>`_)
+* throws std::invalid_argument if ParameterEvent is NULL. (`#2814 <https://github.com/ros2/rclcpp/issues/2814>`_) (`#2824 <https://github.com/ros2/rclcpp/issues/2824>`_)
+* remove redundant typesupport check in serialization module (`#2808 <https://github.com/ros2/rclcpp/issues/2808>`_) (`#2816 <https://github.com/ros2/rclcpp/issues/2816>`_)
+* Contributors: mergify[bot]
+
+16.0.12 (2025-03-25)
+--------------------
+* doc: Added warning to not instantiate Clock directly with RCL_ROS_TIME (`#2768 <https://github.com/ros2/rclcpp/issues/2768>`_) (`#2770 <https://github.com/ros2/rclcpp/issues/2770>`_)
+* apply actual QoS from rmw to the IPC publisher. (backport `#2707 <https://github.com/ros2/rclcpp/issues/2707>`_) (`#2711 <https://github.com/ros2/rclcpp/issues/2711>`_)
+* Adding in topic name to logging on IPC issues (`#2706 <https://github.com/ros2/rclcpp/issues/2706>`_) (`#2709 <https://github.com/ros2/rclcpp/issues/2709>`_)
+* Contributors: mergify[bot]
+
+16.0.11 (2024-11-25)
+--------------------
+* Fix subscription.is_serialized() for callbacks with message info (`#1950 <https://github.com/ros2/rclcpp/issues/1950>`_) (`#2622 <https://github.com/ros2/rclcpp/issues/2622>`_)
+* Use the same context for the specified node in rclcpp::spin functions… (`#2618 <https://github.com/ros2/rclcpp/issues/2618>`_) (`#2620 <https://github.com/ros2/rclcpp/issues/2620>`_)
+* Contributors: mergify[bot], roscan-tech
+
+16.0.10 (2024-07-26)
+--------------------
+* 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>`_) (`#2551 <https://github.com/ros2/rclcpp/issues/2551>`_)
+* Contributors: mergify[bot]
+
+16.0.9 (2024-05-15)
+-------------------
+* Fix logging macros to build with msvc and cpp20 (`#2063 <https://github.com/ros2/rclcpp/issues/2063>`_) (`#2529 <https://github.com/ros2/rclcpp/issues/2529>`_)
+* address ambiguous auto variable. (`#2481 <https://github.com/ros2/rclcpp/issues/2481>`_) (`#2485 <https://github.com/ros2/rclcpp/issues/2485>`_)
+* Fix clang warning: bugprone-use-after-move (`#2116 <https://github.com/ros2/rclcpp/issues/2116>`_) (`#2459 <https://github.com/ros2/rclcpp/issues/2459>`_)
+* Contributors: Tamaki Nishino, mergify[bot]
+
+16.0.8 (2024-01-24)
+-------------------
+* Add missing stdexcept include (`#2186 <https://github.com/ros2/rclcpp/issues/2186>`_) (`#2394 <https://github.com/ros2/rclcpp/issues/2394>`_)
+* Contributors: gentoo90
+
+16.0.7 (2023-11-13)
+-------------------
+* Disable the loaned messages inside the executor. (backport `#2335 <https://github.com/ros2/rclcpp/issues/2335>`_) (`#2364 <https://github.com/ros2/rclcpp/issues/2364>`_)
+* Add missing 'enable_rosout' comments (`#2345 <https://github.com/ros2/rclcpp/issues/2345>`_) (`#2347 <https://github.com/ros2/rclcpp/issues/2347>`_)
+* address rate related flaky tests. (`#2329 <https://github.com/ros2/rclcpp/issues/2329>`_) (`#2342 <https://github.com/ros2/rclcpp/issues/2342>`_)
+* Update SignalHandler get_global_signal_handler to avoid complex types in static memory (`#2316 <https://github.com/ros2/rclcpp/issues/2316>`_) (`#2321 <https://github.com/ros2/rclcpp/issues/2321>`_)
+* Fix C++20 allocator construct deprecation (`#2292 <https://github.com/ros2/rclcpp/issues/2292>`_) (`#2319 <https://github.com/ros2/rclcpp/issues/2319>`_)
+* Contributors: mergify[bot]
+
+16.0.6 (2023-09-19)
+-------------------
+* Topic correct typeadapter deduction (`#2294 <https://github.com/ros2/rclcpp/issues/2294>`_) (`#2297 <https://github.com/ros2/rclcpp/issues/2297>`_)
+* check thread whether joinable before join (`#2019 <https://github.com/ros2/rclcpp/issues/2019>`_) (`#2275 <https://github.com/ros2/rclcpp/issues/2275>`_)
+* Do not crash Executor when send_response fails due to client failure. (`#2276 <https://github.com/ros2/rclcpp/issues/2276>`_) (`#2280 <https://github.com/ros2/rclcpp/issues/2280>`_)
+* Contributors: mergify[bot]
+
+16.0.5 (2023-07-17)
+-------------------
+* warning: comparison of integer expressions of different signedness (`#2219 <https://github.com/ros2/rclcpp/issues/2219>`_) (`#2223 <https://github.com/ros2/rclcpp/issues/2223>`_)
+* Trigger the intraprocess guard condition with data (`#2164 <https://github.com/ros2/rclcpp/issues/2164>`_) (`#2167 <https://github.com/ros2/rclcpp/issues/2167>`_)
+* Implement validity checks for rclcpp::Clock (`#2040 <https://github.com/ros2/rclcpp/issues/2040>`_) (`#2210 <https://github.com/ros2/rclcpp/issues/2210>`_)
+* Contributors: Tomoya Fujita, mergify[bot]
+
+16.0.4 (2023-04-25)
+-------------------
+* use allocator via init_options argument. (`#2129 <https://github.com/ros2/rclcpp/issues/2129>`_) (`#2131 <https://github.com/ros2/rclcpp/issues/2131>`_)
+* Contributors: mergify[bot]
+
+16.0.3 (2023-01-10)
+-------------------
+* Fix SharedFuture from async_send_request never becomes valid (`#2044 <https://github.com/ros2/rclcpp/issues/2044>`_) (`#2076 <https://github.com/ros2/rclcpp/issues/2076>`_)
+* do not throw exception if trying to dequeue an empty intra-process buffer (`#2061 <https://github.com/ros2/rclcpp/issues/2061>`_) (`#2070 <https://github.com/ros2/rclcpp/issues/2070>`_)
+* fix nullptr dereference in prune_requests_older_than (`#2008 <https://github.com/ros2/rclcpp/issues/2008>`_) (`#2065 <https://github.com/ros2/rclcpp/issues/2065>`_)
+* Fix bug that a callback not reached (`#1640 <https://github.com/ros2/rclcpp/issues/1640>`_) (`#2033 <https://github.com/ros2/rclcpp/issues/2033>`_)
+* Contributors: mergify[bot]
+
+16.0.2 (2022-11-07)
+-------------------
+* fix mismatched issue if using zero_allocate (`#1995 <https://github.com/ros2/rclcpp/issues/1995>`_) (`#2026 <https://github.com/ros2/rclcpp/issues/2026>`_)
+* use regex for wildcard matching (backport `#1839 <https://github.com/ros2/rclcpp/issues/1839>`_) (`#1986 <https://github.com/ros2/rclcpp/issues/1986>`_)
+* Drop wrong template specialization (`#1926 <https://github.com/ros2/rclcpp/issues/1926>`_) (`#1937 <https://github.com/ros2/rclcpp/issues/1937>`_)
+* Add statistics for handle_loaned_message (`#1927 <https://github.com/ros2/rclcpp/issues/1927>`_) (`#1932 <https://github.com/ros2/rclcpp/issues/1932>`_)
+* Contributors: mergify[bot]
+
+16.0.1 (2022-04-13)
+-------------------
+* remove DEFINE_CONTENT_FILTER cmake option (`#1914 <https://github.com/ros2/rclcpp/issues/1914>`_)
+* Contributors: Chen Lihui
+
+16.0.0 (2022-04-08)
+-------------------
+* remove things that were deprecated during galactic (`#1913 <https://github.com/ros2/rclcpp/issues/1913>`_)
+* Contributors: William Woodall
+
+15.4.0 (2022-04-05)
+-------------------
+* add take_data_by_entity_id API to waitable (`#1892 <https://github.com/ros2/rclcpp/issues/1892>`_)
+* add content-filtered-topic interfaces (`#1561 <https://github.com/ros2/rclcpp/issues/1561>`_)
+* Contributors: Alberto Soragna, Chen Lihui
+
+15.3.0 (2022-03-30)
+-------------------
+* [NodeParameters] Set name in param info pre-check (`#1908 <https://github.com/ros2/rclcpp/issues/1908>`_)
+* Add test-dep ament_cmake_google_benchmark (`#1904 <https://github.com/ros2/rclcpp/issues/1904>`_)
+* Add publish by loaned message in GenericPublisher (`#1856 <https://github.com/ros2/rclcpp/issues/1856>`_)
+* Contributors: Abrar Rahman Protyasha, Barry Xu, Gaël Écorchard
+
+15.2.0 (2022-03-24)
+-------------------
+* Add missing ament dependency on rcl_interfaces (`#1903 <https://github.com/ros2/rclcpp/issues/1903>`_)
+* Update data callback tests to account for all published samples (`#1900 <https://github.com/ros2/rclcpp/issues/1900>`_)
+* Increase timeout for acknowledgments to account for slower Connext settings (`#1901 <https://github.com/ros2/rclcpp/issues/1901>`_)
+* clang-tidy: explicit constructors (`#1782 <https://github.com/ros2/rclcpp/issues/1782>`_)
+* Add client/service QoS getters (`#1784 <https://github.com/ros2/rclcpp/issues/1784>`_)
+* Fix a bunch more rosdoc2 issues in rclcpp. (`#1897 <https://github.com/ros2/rclcpp/issues/1897>`_)
+* time_until_trigger returns max time if timer is cancelled (`#1893 <https://github.com/ros2/rclcpp/issues/1893>`_)
+* Micro-optimizations in rclcpp (`#1896 <https://github.com/ros2/rclcpp/issues/1896>`_)
+* Contributors: Andrea Sorbini, Chris Lalancette, Mauro Passerino, Scott K Logan, William Woodall
+
+15.1.0 (2022-03-01)
+-------------------
+* spin_all with a zero timeout. (`#1878 <https://github.com/ros2/rclcpp/issues/1878>`_)
+* Add RMW listener APIs (`#1579 <https://github.com/ros2/rclcpp/issues/1579>`_)
+* Remove fastrtps customization on tests (`#1887 <https://github.com/ros2/rclcpp/issues/1887>`_)
+* Install headers to include/${PROJECT_NAME} (`#1888 <https://github.com/ros2/rclcpp/issues/1888>`_)
+* Use ament_generate_version_header (`#1886 <https://github.com/ros2/rclcpp/issues/1886>`_)
+* use universal reference to support rvalue. (`#1883 <https://github.com/ros2/rclcpp/issues/1883>`_)
+* fix one subscription can wait_for_message twice (`#1870 <https://github.com/ros2/rclcpp/issues/1870>`_)
+* Add return value version of get_parameter_or (`#1813 <https://github.com/ros2/rclcpp/issues/1813>`_)
+* Cleanup time source object lifetimes (`#1867 <https://github.com/ros2/rclcpp/issues/1867>`_)
+* add is_spinning() method to executor base class
+* Contributors: Alberto Soragna, Chen Lihui, Chris Lalancette, Kenji Miyake, Miguel Company, Shane Loretz, Tomoya Fujita, iRobot ROS
+
+15.0.0 (2022-01-14)
+-------------------
+* Cleanup the TypeAdapt tests (`#1858 <https://github.com/ros2/rclcpp/issues/1858>`_)
+* Cleanup includes (`#1857 <https://github.com/ros2/rclcpp/issues/1857>`_)
+* Fix include order and relative paths for cpplint (`#1859 <https://github.com/ros2/rclcpp/issues/1859>`_)
+* Rename stringstream in macros to a more unique name (`#1862 <https://github.com/ros2/rclcpp/issues/1862>`_)
+* Add non transform capabilities for intra-process (`#1849 <https://github.com/ros2/rclcpp/issues/1849>`_)
+* Fix rclcpp documentation build (`#1779 <https://github.com/ros2/rclcpp/issues/1779>`_)
+* Contributors: Chris Lalancette, Doug Smith, Gonzo, Jacob Perron, Michel Hidalgo
+
+14.1.0 (2022-01-05)
+-------------------
+* Use UninitializedStaticallyTypedParameterException (`#1689 <https://github.com/ros2/rclcpp/issues/1689>`_)
+* Add wait_for_all_acked support (`#1662 <https://github.com/ros2/rclcpp/issues/1662>`_)
+* Add tests for function templates of declare_parameter (`#1747 <https://github.com/ros2/rclcpp/issues/1747>`_)
+* Contributors: Barry Xu, Bi0T1N, M. Mostafa Farzan
+
+14.0.0 (2021-12-17)
+-------------------
+* Fixes for uncrustify 0.72 (`#1844 <https://github.com/ros2/rclcpp/issues/1844>`_)
+* use private member to keep the all reference underneath. (`#1845 <https://github.com/ros2/rclcpp/issues/1845>`_)
+* Make node base sharable (`#1832 <https://github.com/ros2/rclcpp/issues/1832>`_)
+* Add Clock::sleep_for() (`#1828 <https://github.com/ros2/rclcpp/issues/1828>`_)
+* Synchronize rcl and std::chrono steady clocks in Clock::sleep_until (`#1830 <https://github.com/ros2/rclcpp/issues/1830>`_)
+* Use rclcpp::guard_condition (`#1612 <https://github.com/ros2/rclcpp/issues/1612>`_)
+* Call CMake function to generate version header (`#1805 <https://github.com/ros2/rclcpp/issues/1805>`_)
+* Use parantheses around logging macro parameter (`#1820 <https://github.com/ros2/rclcpp/issues/1820>`_)
+* Remove author by request (`#1818 <https://github.com/ros2/rclcpp/issues/1818>`_)
+* Update maintainers (`#1817 <https://github.com/ros2/rclcpp/issues/1817>`_)
+* min_forward & min_backward thresholds must not be disabled (`#1815 <https://github.com/ros2/rclcpp/issues/1815>`_)
+* Re-add Clock::sleep_until (`#1814 <https://github.com/ros2/rclcpp/issues/1814>`_)
+* Fix lifetime of context so it remains alive while its dependent node handles are still in use (`#1754 <https://github.com/ros2/rclcpp/issues/1754>`_)
+* Add the interface for pre-shutdown callback (`#1714 <https://github.com/ros2/rclcpp/issues/1714>`_)
+* Take message ownership from moved LoanedMessage (`#1808 <https://github.com/ros2/rclcpp/issues/1808>`_)
+* Suppress clang dead-store warnings in the benchmarks. (`#1802 <https://github.com/ros2/rclcpp/issues/1802>`_)
+* Wait for publisher and subscription to match (`#1777 <https://github.com/ros2/rclcpp/issues/1777>`_)
+* Fix unused QoS profile for clock subscription and make ClockQoS the default (`#1801 <https://github.com/ros2/rclcpp/issues/1801>`_)
+* Contributors: Abrar Rahman Protyasha, Barry Xu, Chen Lihui, Chris Lalancette, Grey, Jacob Perron, Nikolai Morin, Shane Loretz, Tomoya Fujita, mauropasse
+
+13.1.0 (2021-10-18)
+-------------------
+* Fix dangerous std::bind capture in TimeSource implementation. (`#1768 <https://github.com/ros2/rclcpp/issues/1768>`_)
+* Fix dangerous std::bind capture in ParameterEventHandler implementation. (`#1770 <https://github.com/ros2/rclcpp/issues/1770>`_)
+* Handle sigterm, in the same way sigint is being handled. (`#1771 <https://github.com/ros2/rclcpp/issues/1771>`_)
+* rclcpp::Node copy constructor: make copy of node_waitables\_ member. (`#1799 <https://github.com/ros2/rclcpp/issues/1799>`_)
+* Extend NodeGraph to match what rcl provides. (`#1484 <https://github.com/ros2/rclcpp/issues/1484>`_)
+* Context::sleep_for(): replace recursion with do-while to avoid potential stack-overflow. (`#1765 <https://github.com/ros2/rclcpp/issues/1765>`_)
+* extend_sub_namespace(): Verify string::empty() before calling string::front(). (`#1764 <https://github.com/ros2/rclcpp/issues/1764>`_)
+* Deprecate the `void shared_ptr<MessageT>` subscription callback signatures. (`#1713 <https://github.com/ros2/rclcpp/issues/1713>`_)
+* Contributors: Abrar Rahman Protyasha, Chris Lalancette, Emerson Knapp, Geoffrey Biggs, Ivan Santiago Paunovic, Jorge Perez, Tomoya Fujita, William Woodall, Yong-Hao Zou, livanov93
+
+13.0.0 (2021-08-23)
+-------------------
+* Remove can_be_nullptr assignment check for QNX case. (`#1752 <https://github.com/ros2/rclcpp/issues/1752>`_)
+* Update client API to be able to remove pending requests. (`#1734 <https://github.com/ros2/rclcpp/issues/1734>`_)
+* Fix: Allow to add a node while spinning in the StaticSingleThreadedExecutor. (`#1690 <https://github.com/ros2/rclcpp/issues/1690>`_)
+* Add tracing instrumentation for executor and message taking. (`#1738 <https://github.com/ros2/rclcpp/issues/1738>`_)
+* Fix: Reset timer trigger time before execute in StaticSingleThreadedExecutor. (`#1739 <https://github.com/ros2/rclcpp/issues/1739>`_)
+* Use FindPython3 and make python3 dependency explicit. (`#1745 <https://github.com/ros2/rclcpp/issues/1745>`_)
+* Use rosidl_get_typesupport_target(). (`#1729 <https://github.com/ros2/rclcpp/issues/1729>`_)
+* Fix returning invalid namespace if sub_namespace is empty. (`#1658 <https://github.com/ros2/rclcpp/issues/1658>`_)
+* Add free function to wait for a subscription message. (`#1705 <https://github.com/ros2/rclcpp/issues/1705>`_)
+* Use rcpputils/scope_exit.hpp and remove rclcpp/scope_exit.hpp. (`#1727 <https://github.com/ros2/rclcpp/issues/1727>`_)
+* Contributors: Ahmed Sobhy, Christophe Bedard, Ivan Santiago Paunovic, Karsten Knese, M. Hofstätter, Mauro Passerino, Shane Loretz, mauropasse
+
+12.0.0 (2021-07-26)
+-------------------
+* Remove unsafe get_callback_groups API.
+  Callers should change to using for_each_callback_group(), or
+  store the callback groups they need internally.
+* Add in callback_groups_for_each.
+  The main reason to add this method in is to make accesses to the
+  callback_groups\_ vector thread-safe.  By having a
+  callback_groups_for_each that accepts a std::function, we can
+  just have the callers give us the callback they are interested
+  in, and we can take care of the locking.
+  The rest of this fairly large PR is cleaning up all of the places
+  that use get_callback_groups() to instead use
+  callback_groups_for_each().
+* Use a different mechanism to avoid timers being scheduled multiple times by the MultiThreadedExecutor (`#1692 <https://github.com/ros2/rclcpp/issues/1692>`_)
+* Fix windows CI (`#1726 <https://github.com/ros2/rclcpp/issues/1726>`_)
+  Fix bug in AnyServiceCallback introduced in `#1709 <https://github.com/ros2/rclcpp/issues/1709>`_.
+* Contributors: Chris Lalancette, Ivan Santiago Paunovic
+
+11.2.0 (2021-07-21)
+-------------------
+* Support to defer to send a response in services. (`#1709 <https://github.com/ros2/rclcpp/issues/1709>`_)
+  Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>
+* Fix documentation bug. (`#1719 <https://github.com/ros2/rclcpp/issues/1719>`_)
+  Signed-off-by: William Woodall <william@osrfoundation.org>
+* Contributors: Ivan Santiago Paunovic, William Woodall
+
+11.1.0 (2021-07-13)
+-------------------
+* Removed left over ``is_initialized()`` implementation (`#1711 <https://github.com/ros2/rclcpp/issues/1711>`_)
+  Leftover from https://github.com/ros2/rclcpp/pull/1622
+* Fixed declare parameter methods for int and float vectors (`#1696 <https://github.com/ros2/rclcpp/issues/1696>`_)
+* Cleaned up implementation of the intra-process manager (`#1695 <https://github.com/ros2/rclcpp/issues/1695>`_)
+* Added the node name to an executor ``runtime_error`` (`#1686 <https://github.com/ros2/rclcpp/issues/1686>`_)
+* Fixed a typo "Attack" -> "Attach" (`#1687 <https://github.com/ros2/rclcpp/issues/1687>`_)
+* Removed use of std::allocator<>::rebind (`#1678 <https://github.com/ros2/rclcpp/issues/1678>`_)
+  rebind is deprecated in c++17 and removed in c++20
+* Contributors: Alberto Soragna, Chen Lihui, Chris Lalancette, Petter Nilsson, Steve Macenski, William Woodall
+
 11.0.0 (2021-05-18)
 -------------------
 * Allow declare uninitialized parameters (`#1673 <https://github.com/ros2/rclcpp/issues/1673>`_)

rclcpp/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.12)
 
 project(rclcpp)
 
@@ -41,7 +41,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/clock.cpp
   src/rclcpp/context.cpp
   src/rclcpp/contexts/default_context.cpp
-  src/rclcpp/detail/mutex_two_priorities.cpp
+  src/rclcpp/detail/add_guard_condition_to_rcl_wait_set.cpp
   src/rclcpp/detail/resolve_parameter_overrides.cpp
   src/rclcpp/detail/rmw_implementation_specific_payload.cpp
   src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp
@@ -110,6 +110,8 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/waitable.cpp
 )
 
+find_package(Python3 REQUIRED COMPONENTS Interpreter)
+
 # "watch" template for changes
 configure_file(
   "resource/logging.hpp.em"
@@ -123,7 +125,7 @@ set(python_code_logging
 string(REPLACE ";" "$<SEMICOLON>" python_code_logging "${python_code_logging}")
 add_custom_command(OUTPUT include/rclcpp/logging.hpp
   COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp"
-  COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_code_logging}"
+  COMMAND Python3::Interpreter ARGS -c "${python_code_logging}"
   DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/logging.hpp.em.watch"
   COMMENT "Expanding logging.hpp.em"
   VERBATIM
@@ -147,7 +149,7 @@ foreach(interface_file ${interface_files})
   string(REPLACE ";" "$<SEMICOLON>" python_${interface_name}_traits "${python_${interface_name}_traits}")
   add_custom_command(OUTPUT include/rclcpp/node_interfaces/${interface_name}_traits.hpp
     COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp/node_interfaces"
-    COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_${interface_name}_traits}"
+    COMMAND Python3::Interpreter ARGS -c "${python_${interface_name}_traits}"
     DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/${interface_name}_traits.hpp.em.watch"
     COMMENT "Expanding interface_traits.hpp.em into ${interface_name}_traits.hpp"
     VERBATIM
@@ -167,7 +169,7 @@ foreach(interface_file ${interface_files})
   string(REPLACE ";" "$<SEMICOLON>" python_get_${interface_name} "${python_get_${interface_name}}")
   add_custom_command(OUTPUT include/rclcpp/node_interfaces/get_${interface_name}.hpp
     COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp/node_interfaces"
-    COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_get_${interface_name}}"
+    COMMAND Python3::Interpreter ARGS -c "${python_get_${interface_name}}"
     DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/get_${interface_name}.hpp.em.watch"
     COMMENT "Expanding get_interface.hpp.em into get_${interface_file}.hpp"
     VERBATIM
@@ -185,13 +187,14 @@ endif()
 target_include_directories(${PROJECT_NAME} PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>"
-  "$<INSTALL_INTERFACE:include>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
 # specific order: dependents before dependencies
 ament_target_dependencies(${PROJECT_NAME}
   "ament_index_cpp"
   "libstatistics_collector"
   "rcl"
+  "rcl_interfaces"
   "rcl_yaml_param_parser"
   "rcpputils"
   "rcutils"
@@ -215,11 +218,14 @@ install(
   RUNTIME DESTINATION bin
 )
 
-# specific order: dependents before dependencies
-ament_export_include_directories(include)
+# Export old-style CMake variables
+ament_export_include_directories("include/${PROJECT_NAME}")
 ament_export_libraries(${PROJECT_NAME})
+
+# Export modern CMake targets
 ament_export_targets(${PROJECT_NAME})
 
+# specific order: dependents before dependencies
 ament_export_dependencies(ament_index_cpp)
 ament_export_dependencies(libstatistics_collector)
 ament_export_dependencies(rcl)
@@ -245,10 +251,12 @@ ament_package()
 
 install(
   DIRECTORY include/ ${CMAKE_CURRENT_BINARY_DIR}/include/
-  DESTINATION include
+  DESTINATION include/${PROJECT_NAME}
 )
 
 if(TEST cppcheck)
   # must set the property after ament_package()
   set_tests_properties(cppcheck PROPERTIES TIMEOUT 500)
 endif()
+
+ament_generate_version_header(${PROJECT_NAME})

rclcpp/Doxyfile

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

rclcpp/QUALITY_DECLARATION.md

@@ -1,10 +1,10 @@
-This document is a declaration of software quality for the `rclcpp` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html).
+This document is a declaration of software quality for the `rclcpp` package, based on the guidelines in [REP-2004](https://reps.openrobotics.org/rep-2004/).
 
 # rclcpp Quality Declaration
 
 The package `rclcpp` claims to be in the **Quality Level 1** category when it is used with a **Quality Level 1** middleware.
 
-Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html) of the ROS2 developer guide.
+Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://reps.openrobotics.org/rep-2004/) of the ROS2 developer guide.
 
 ## Version Policy [1]
 
@@ -55,7 +55,7 @@ All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://d
 
 ### Continuous Integration [2.iv]
 
-All pull requests must pass CI on all [tier 1 platforms](https://www.ros.org/reps/rep-2000.html#support-tiers)
+All pull requests must pass CI on all [tier 1 platforms](https://reps.openrobotics.org/rep-2000/#support-tiers)
 
 Currently nightly results can be seen here:
 
@@ -213,7 +213,7 @@ It is **Quality Level 1**, see its [Quality Declaration document](https://gitlab
 
 ## Platform Support [6]
 
-`rclcpp` supports all of the tier 1 platforms as described in [REP-2000](https://www.ros.org/reps/rep-2000.html#support-tiers), and tests each change against all of them.
+`rclcpp` supports all of the tier 1 platforms as described in [REP-2000](https://reps.openrobotics.org/rep-2000/#support-tiers), and tests each change against all of them.
 
 Currently nightly build status can be seen here:
 * [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/rclcpp/)
@@ -225,4 +225,4 @@ Currently nightly build status can be seen here:
 
 ### Vulnerability Disclosure Policy [7.i]
 
-This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://www.ros.org/reps/rep-2006.html).
+This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://reps.openrobotics.org/rep-2006/).

rclcpp/doc/wait_set_architecture.md

@@ -1,161 +0,0 @@
-# Architecture of the rclcpp::WaitSet Type
-
-The `rclcpp::WaitSet` type is actually a specific configuration of a the more general `rclcpp::WaitSetTemplate` class.
-
-The `rclcpp::WaitSetTemplate` class is design to have interchangeable implementations for controlling storage of items in the wait set, as well as to control access to the wait set functionality, i.e. whether or not it has thread-safety.
-
-In general the idea behind a wait set in rclcpp is that you gather a set of entities that can be waited on, e.g. subscriptions, timers, guard conditions, and then wait for one or more of them to be ready.
-These things are driven by asynchronous events, e.g. subscriptions become ready when new data arrives or timers become ready when their period has elapsed.
-
-At the moment, things that can be waited on are limited by what can be put into an `rcl_wait_set_t`, which includes subscriptions, service clients, service servers, timers, guard conditions, and events.
-In this context events are QoS events, as defined in the `rcl` and `rmw` interfaces.
-
-## Design Goals
-
-There are some design goals for the `rclcpp::WaitSet` family of classes:
-
-- minimize inheritance and avoid virtual functions
-  - Overhead of the executor and wait set design have been a common complaint and are often blamed for performance issues.
-  - Since the wait set is at the core of the wait-do loop of the system, aggressive optimization can actually pay off.
-- avoid unnecessary changes to the underlying `rcl_wait_set_t` instances or unnecessarily recreating it
-  - This can be expensive, because under the hood that requires changes to the middleware's object, e.g. a DDS wait set or similar.
-- minimize memory allocations in the wait() function, avoid them entirely if the set of things to wait on has not changed
-- prevent accidentally using an entity with more than wait set at the same time
-
-Several of these surround performance concerns, but others are concerned with safety and convenience for the user.
-
-## Architecture
-
-The `rclcpp::WaitSetTemplate` is the core fixture in the feature, which is using a policy-based design (see https://en.wikipedia.org/wiki/Modern_C%2B%2B_Design#Policy-based_design).
-
-It delegates storage and synchronization to policy template arguments to the `rclcpp::WaitSetTemplate` via the `StoragePolicy` and `SynchronizationPolicy` template arguments respectively.
-
-These policies can be mixed and matched to achieve different results.
-There are a few predefined combinations, `rclcpp::WaitSet` which is a non-thread-safe wait set with dynamic storage, `rclcpp::StaticWaitSet` which is a non-thread-safe wait set with fixed storage, and `rclcpp::ThreadSafeWaitSet` which is a thread-safe wait set with dynamic storage.
-They are all just aliases to `rclcpp::WaitSetTemplate` with difference combinations of policies.
-
-Users can extend the behavior by implementing their own policies if desired.
-
-This kind of design allows for specialization and code sharing without using standard polymorphism via virtual functions, and the benefit of that is that calls on the wait set class are always non-virtual, avoiding dispatching via the v-table.
-It also forces us to use templates, which means a head-only implementation, which will increase compile times, but will avoid some calls to shared libraries which on some systems has a small overhead too.
-
-These are aggressive optimizations, but it can actually be very beneficial in the inner-loop of applications, and the hope is that this approach will offer the best performance we can achieve.
-
-### StoragePolicy
-
-The storage policy is responsible for storing the entities in the wait set and providing accessors and setters (if appropriate).
-
-The storage policy is also responsible for maintaining ownership of the entities it is storing at the right times.
-For example, entities are always given to the wait set as `std::shared_ptr`'s and some policies might only maintain ownership for the lifetime of the wait set or until the entity is removed.
-Other policies might only hold ownership of the entities while waiting on them, i.e. while they are actively using them, but maintain only weak ownership otherwise.
-
-There are two built-in storage policies available, the `rclcpp::wait_set_policies::DynamicStorage` and the `rclcpp::wait_set_policies::StaticStorage<...>` classes.
-
-The `StaticStorage` policy requires that you fully specify the number of entities of each time that can be stored as template arguments and uses the `std:array` class as backing storage.
-Therefore, it also does not allow adding or removing entities after construction.
-Because entities cannot be removed and are therefore "always" in use by the wait set, it maintains shared ownership of the entities for the lifetime of the wait set.
-
-The `DynamicStorage` policy provides access to the entities and allows add and removing entities at any time after construction.
-It also only maintains shared ownership of the entities while actively waiting on them, which allows the entities to go out of scope while the wait set is idle.
-Entities that go out of scope in this manner will be removed from the wait set when it notices their weak pointers no longer lock to the entity's shared pointer.
-
-Both policies allow you to initialize the wait set with entities in the constructor.
-
-### SynchronizationPolicy
-
-The synchronization policy is responsible for synchronizing access to wait set operations that share access to the StoragePolicy.
-
-There are two built-in synchronization policies available, the `rclcpp::wait_set_policies::SequentialSynchronization` and the `rclcpp::wait_set_policies::ThreadSafeSynchronization` policies.
-
-The `rclcpp::wait_set_policies::SequentialSynchronization` policy essentially does nothing but pass through requests from the user-facing `rclcpp::WaitSetTemplate` interface to the `StoragePolicy` that is being used.
-
-The `rclcpp::wait_set_policies::ThreadSafeSynchronization` policy sequences access to the `StoragePolicy` being used so that it is safe to call methods like `add_subscription()` on the wait set while it is doing something else, like waiting with the `wait()` method.
-This synchronization comes at the expense of using mutexes and extra logic to organize the access, but provides some thread-safety.
-
-### Entity Intake and Management
-
-Entities, i.e. `rclcpp::SubscriptionBase`, `rclcpp::Waitable`, `rclcpp::ClientBase`, `rclcpp::ServiceBase`, `rclcpp::TimerBase`, `rclcpp::GuardCondition`, etc., may be passed to the wait set during construction.
-If the wait set has policies that allow for dynamic storage and supports adding and removing entities after construction, then entities can also be added and removed using method on the wait set.
-
-Either way, the entities are initially given to the wait set as a `std::shared_ptr` in order to maintain shared ownership, but depending on the storage policy it may be stored as a `std::weak_ptr` while the wait set is not actively using the entities.
-
-Also, when taking on new entities the wait set is responsible for ensuring that entity is not being used by another wait set (at least another instance of `rclcpp::WaitSetTemplate`).
-
-One more thing that plays into intake of entities is that a few entity types have some extra requirements, specifically subscriptions and waitables.
-
-Subscriptions (specifically `rclcpp::SubscriptionBase`) are complex objects that contain not only a subscription that can be waited on, but also may have `rclcpp::Waitable`'s inside in order to implement intra-process communication and for QoS events that are also implemented as `rclcpp::Waitable` instances.
-Therefore, you need a way to say which parts of the subscription should be added to the wait set, because you don't need to add all the entities that make up a subscription to a single wait set.
-The `rclcpp::SubscriptionWaitSetMask` class provides this mapping, and when adding subscriptions to a wait set you need to provide the subscription shared pointer as well as a mask instance.
-
-Waitables are complicated by the fact that they often are part of a larger piece of the API, as we saw with subscriptions, and therefore the waitable may need to keep additional things in scope while being used.
-So when adding a waitable to a wait set you may also provide an "associated entity" as a shared pointer to void, which is purely there to keep something in scope along with the waitable.
-
-#### EntityEntry classes
-
-In order to address the need to pair extra information with some entities (e.g. the subscription mask with the subscription entity) and to provide a convenient place to check if the entity is in use by another wait set and claim if not.
-
-There are a series of classes that are a variation of the `EntityEntry` concept, which wrap the incoming entities and their associated data into a single class, and also provide some safety when converting between the shared ownership and weak ownership if needed.
-
-They also provide RAII-style checking about whether or not an entity is already associated with a wait set.
-This is important because when ingesting multiple entities at the same time, e.g. via the constructor, because if you claim the first few entities to be associated with the wait set, but then find one that is already associated, then you need to abort and throw an exception.
-The RAII-style of these classes addresses this by "dissociating" the first few entities from the wait set when going out of scope due to the thrown exception.
-
-The process from intake to cleanup of the entity entries follows something like this, using waitables as an example:
-
-
-
-                                    ┌───────────────┐
-   ┌────────────────────────┐       │ WaitSet       │
-   │                        ├──────►│               ├───────────────┐
-   │ shared_ptr<Waitable>   │       │ constructor   │               │
-   │                        │       └───────────────┘  Implicit     │
-   │  and optional          │                                       │
-   │                        │       ┌───────────────┐  Conversion   │
-   │ shared_ptr<void>       │       │ add_waitable()│               │
-   │                        ├──────►│               ├───────────────┤
-   └────────────────────────┘       │ method        │               │
-                                    └───────────────┘               │
-                                                                    │
-                                                                    │
-                                                                    │
-          ┌──────────────────────┐        ┌─────────────────┐       │
-          │                      │        │                 │       │
-    ┌─────┤ ManagedWaitableEntry │◄──┬────┤  WaitableEntry  │◄──────┘
-    │     │                      │   │    │                 │
-    │     └─┬──────────────────┬─┘   │    └─┬─────────────┬─┘
-    │       │ shared_ptr+RAII  │     │      │ shared_ptr  │
-    │       └──────────────────┘     │      └─────────────┘
-    │                                │
-    │                        ┌───────┴───────────┐
-    │                        │Check that entity  │
-    │ Conversion to          │is not in use, then│
-    │ weak ownership         │claim it for this  │
-    │ if needed.             │wait set.          │
-    │                        └───────────────────┘
-    │
-    │     ┌───────────────────────────┐
-    │     │                           │
-    └────►│ WeakManagedWaitableEntry  │
-          │                           │
-          └─┬───────────────────────┬─┘
-            │ weak_ptr + RAII       │
-            └───────────────────────┘
-
-Using the diagram as a reference, you can see that the input from the user is converted into the entity entry class that, for now, just stores the entity and any associated data if it exists.
-At this point the entity comes in with shared ownership and stays as shared ownership in the basic entry class.
-Then this entry class is converted into a "managed" entry class, which will check that the entity is not associated with another wait set and associate it with the current wait set during construction.
-This managed entry class will also automatically disassociate the entity with the current wait set during destruction.
-The new managed entry class continues to keep shared ownership of the entity.
-
-Optionally, if the storage policy requires it, the managed entry class can be converted into a "weak" version, where in the entity is stored as a weak pointer instead of a shared pointer.
-In this case the entity will remain stored as a "weak managed" entry until it is removed or the wait set is destroyed.
-In the meantime, if the wait set needs to take shared ownership again, it can do that be getting a copy of the weak pointer(s) in the entry and locking them to get shared pointers.
-But the weak managed entry remains responsible for the RAII-style dissociation from the current wait set when destroyed.
-Unlike the managed entry that maintains shared ownership, the weak version will need to attempt to lock the weak pointer for the entity before trying to dissociate it from the wait set, because this association is maintained within the entity itself, through methods like `rclcpp::Waitable::exchange_in_use_by_wait_set_state()`.
-If the entity cannot be accessed via the weak pointer, then it is not explicitly dissociated by the managed entry, but it doesn't matter because the object has gone out of scope and has been deleted anyway.
-So the cleanup for the weak managed entry is best effort, but that does not pose an issue.
-
-#### The EntityEntryTemplate classes
-
-There is a template class called `rclcpp::wait_set_policies::detail::EntityEntryTemplate<EntityT>` (and associated managed and weak-managed versions) which is the foundation for most of the entity types which don't require special handling, like `rclcpp::GuardCondition` and `rclcpp::TimerBase` for example.
-Other entities like `rclcpp::SubscriptionBase` and `rclcpp::Waitable` have custom specializations of these classes to handle extra storage, custom constructors, and other extra logic to help them work.

rclcpp/include/rclcpp/allocator/allocator_common.hpp

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

rclcpp/include/rclcpp/allocator/allocator_deleter.hpp

@@ -41,7 +41,7 @@ public:
   }
 
   template<typename T>
-  AllocatorDeleter(const AllocatorDeleter<T> & a)
+  explicit AllocatorDeleter(const AllocatorDeleter<T> & a)
   {
     allocator_ = a.get_allocator();
   }

rclcpp/include/rclcpp/any_service_callback.hpp

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

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -354,12 +354,6 @@ public:
     allocator::set_allocator_for_deleter(&ros_message_type_deleter_, &ros_message_type_allocator_);
   }
 
-  [[deprecated("use AnySubscriptionCallback(const AllocatorT & allocator) instead")]]
-  explicit
-  AnySubscriptionCallback(std::shared_ptr<AllocatorT> allocator)  // NOLINT[runtime/explicit]
-  : AnySubscriptionCallback(*NotNull<AllocatorT>(allocator.get(), "invalid allocator").pointer)
-  {}
-
   AnySubscriptionCallback(const AnySubscriptionCallback &) = default;
 
   /// Generic function for setting the callback.
@@ -406,10 +400,10 @@ public:
 
   /// Function for shared_ptr to non-const MessageT, which is deprecated.
   template<typename SetT>
-  // TODO(wjwwood): enable this deprecation after Galactic
-  // [[deprecated(
-  //   "use 'void (std::shared_ptr<const MessageT>)' instead"
-  // )]]
+#if !defined(RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS)
+  // suppress deprecation warnings in `test_any_subscription_callback.cpp`
+  [[deprecated("use 'void(std::shared_ptr<const MessageT>)' instead")]]
+#endif
   void
   set_deprecated(std::function<void(std::shared_ptr<SetT>)> callback)
   {
@@ -418,10 +412,12 @@ public:
 
   /// Function for shared_ptr to non-const MessageT with MessageInfo, which is deprecated.
   template<typename SetT>
-  // TODO(wjwwood): enable this deprecation after Galactic
-  // [[deprecated(
-  //   "use 'void (std::shared_ptr<const MessageT>, const rclcpp::MessageInfo &)' instead"
-  // )]]
+#if !defined(RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS)
+  // suppress deprecation warnings in `test_any_subscription_callback.cpp`
+  [[deprecated(
+          "use 'void(std::shared_ptr<const MessageT>, const rclcpp::MessageInfo &)' instead"
+  )]]
+#endif
   void
   set_deprecated(std::function<void(std::shared_ptr<SetT>, const rclcpp::MessageInfo &)> callback)
   {
@@ -474,6 +470,22 @@ public:
     }
   }
 
+  std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>
+  convert_custom_type_to_ros_message_unique_ptr(const SubscribedType & msg)
+  {
+    if constexpr (rclcpp::TypeAdapter<MessageT>::is_specialized::value) {
+      auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator_, 1);
+      ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator_, ptr);
+      rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(msg, *ptr);
+      return std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>(ptr, ros_message_type_deleter_);
+    } else {
+      static_assert(
+        !sizeof(MessageT *),
+        "convert_custom_type_to_ros_message_unique_ptr() "
+        "unexpectedly called without specialized TypeAdapter");
+    }
+  }
+
   // Dispatch when input is a ros message and the output could be anything.
   void
   dispatch(
@@ -656,7 +668,7 @@ public:
 
   void
   dispatch_intra_process(
-    std::shared_ptr<const ROSMessageType> message,
+    std::shared_ptr<const SubscribedType> message,
     const rclcpp::MessageInfo & message_info)
   {
     TRACEPOINT(callback_start, static_cast<const void *>(this), true);
@@ -675,65 +687,89 @@ public:
 
         // conditions for custom type
         if constexpr (is_ta && std::is_same_v<T, ConstRefCallback>) {
-          auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message);
-          callback(*local_message);
+          callback(*message);
         } else if constexpr (is_ta && std::is_same_v<T, ConstRefWithInfoCallback>) {  // NOLINT
-          auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message);
-          callback(*local_message, message_info);
+          callback(*message, message_info);
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
             std::is_same_v<T, UniquePtrCallback>||
             std::is_same_v<T, SharedPtrCallback>
         ))
         {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message));
+          callback(create_custom_unique_ptr_from_custom_shared_ptr_message(message));
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
             std::is_same_v<T, UniquePtrWithInfoCallback>||
             std::is_same_v<T, SharedPtrWithInfoCallback>
         ))
         {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info);
+          callback(create_custom_unique_ptr_from_custom_shared_ptr_message(message), message_info);
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
             std::is_same_v<T, SharedConstPtrCallback>||
             std::is_same_v<T, ConstRefSharedConstPtrCallback>
         ))
         {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message));
+          callback(message);
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
             std::is_same_v<T, SharedConstPtrWithInfoCallback>||
             std::is_same_v<T, ConstRefSharedConstPtrWithInfoCallback>
         ))
         {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info);
+          callback(message, message_info);
         }
         // conditions for ros message type
-        else if constexpr (std::is_same_v<T, ConstRefROSMessageCallback>) {  // NOLINT
-          callback(*message);
-        } else if constexpr (std::is_same_v<T, ConstRefWithInfoROSMessageCallback>) {
-          callback(*message, message_info);
+        else if constexpr (std::is_same_v<T, ConstRefROSMessageCallback>) {  // NOLINT[readability/braces]
+          if constexpr (is_ta) {
+            auto local = convert_custom_type_to_ros_message_unique_ptr(*message);
+            callback(*local);
+          } else {
+            callback(*message);
+          }
+        } else if constexpr (std::is_same_v<T, ConstRefWithInfoROSMessageCallback>) {  // NOLINT[readability/braces]
+          if constexpr (is_ta) {
+            auto local = convert_custom_type_to_ros_message_unique_ptr(*message);
+            callback(*local, message_info);
+          } else {
+            callback(*message, message_info);
+          }
         } else if constexpr (  // NOLINT[readability/braces]
           std::is_same_v<T, UniquePtrROSMessageCallback>||
           std::is_same_v<T, SharedPtrROSMessageCallback>)
         {
-          callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message));
+          if constexpr (is_ta) {
+            callback(convert_custom_type_to_ros_message_unique_ptr(*message));
+          } else {
+            callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message));
+          }
         } else if constexpr (  // NOLINT[readability/braces]
           std::is_same_v<T, UniquePtrWithInfoROSMessageCallback>||
           std::is_same_v<T, SharedPtrWithInfoROSMessageCallback>)
         {
-          callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message), message_info);
+          if constexpr (is_ta) {
+            callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info);
+          } else {
+            callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message), message_info);
+          }
         } else if constexpr (  // NOLINT[readability/braces]
           std::is_same_v<T, SharedConstPtrROSMessageCallback>||
           std::is_same_v<T, ConstRefSharedConstPtrROSMessageCallback>)
         {
-          callback(message);
+          if constexpr (is_ta) {
+            callback(convert_custom_type_to_ros_message_unique_ptr(*message));
+          } else {
+            callback(message);
+          }
         } else if constexpr (  // NOLINT[readability/braces]
           std::is_same_v<T, SharedConstPtrWithInfoROSMessageCallback>||
           std::is_same_v<T, ConstRefSharedConstPtrWithInfoROSMessageCallback>)
         {
-          callback(message, message_info);
+          if constexpr (is_ta) {
+            callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info);
+          } else {
+            callback(message, message_info);
+          }
         }
         // condition to catch SerializedMessage types
         else if constexpr (  // NOLINT[readability/braces]
@@ -762,7 +798,7 @@ public:
 
   void
   dispatch_intra_process(
-    std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter> message,
+    std::unique_ptr<SubscribedType, SubscribedTypeDeleter> message,
     const rclcpp::MessageInfo & message_info)
   {
     TRACEPOINT(callback_start, static_cast<const void *>(this), true);
@@ -776,70 +812,98 @@ public:
     // Dispatch.
     std::visit(
       [&message, &message_info, this](auto && callback) {
+        // clang complains that 'this' lambda capture is unused, which is true
+        // in *some* specializations of this template, but not others.  Just
+        // quiet it down.
+        (void)this;
+
         using T = std::decay_t<decltype(callback)>;
         static constexpr bool is_ta = rclcpp::TypeAdapter<MessageT>::is_specialized::value;
 
         // conditions for custom type
         if constexpr (is_ta && std::is_same_v<T, ConstRefCallback>) {
-          auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message);
-          callback(*local_message);
+          callback(*message);
         } else if constexpr (is_ta && std::is_same_v<T, ConstRefWithInfoCallback>) {  // NOLINT
-          auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message);
-          callback(*local_message, message_info);
+          callback(*message, message_info);
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
             std::is_same_v<T, UniquePtrCallback>||
-            std::is_same_v<T, SharedPtrCallback>
-        ))
+            std::is_same_v<T, SharedPtrCallback>))
         {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message));
+          callback(std::move(message));
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
             std::is_same_v<T, UniquePtrWithInfoCallback>||
             std::is_same_v<T, SharedPtrWithInfoCallback>
         ))
         {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info);
+          callback(std::move(message), message_info);
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
             std::is_same_v<T, SharedConstPtrCallback>||
             std::is_same_v<T, ConstRefSharedConstPtrCallback>
         ))
         {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message));
+          callback(std::move(message));
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
             std::is_same_v<T, SharedConstPtrWithInfoCallback>||
             std::is_same_v<T, ConstRefSharedConstPtrWithInfoCallback>
         ))
         {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info);
+          callback(std::move(message), message_info);
         }
         // conditions for ros message type
-        else if constexpr (std::is_same_v<T, ConstRefROSMessageCallback>) {  // NOLINT
-          callback(*message);
-        } else if constexpr (std::is_same_v<T, ConstRefWithInfoROSMessageCallback>) {
-          callback(*message, message_info);
+        else if constexpr (std::is_same_v<T, ConstRefROSMessageCallback>) {  // NOLINT[readability/braces]
+          if constexpr (is_ta) {
+            auto local = convert_custom_type_to_ros_message_unique_ptr(*message);
+            callback(*local);
+          } else {
+            callback(*message);
+          }
+        } else if constexpr (std::is_same_v<T, ConstRefWithInfoROSMessageCallback>) {  // NOLINT[readability/braces]
+          if constexpr (is_ta) {
+            auto local = convert_custom_type_to_ros_message_unique_ptr(*message);
+            callback(*local, message_info);
+          } else {
+            callback(*message, message_info);
+          }
         } else if constexpr (  // NOLINT[readability/braces]
           std::is_same_v<T, UniquePtrROSMessageCallback>||
           std::is_same_v<T, SharedPtrROSMessageCallback>)
         {
-          callback(std::move(message));
+          if constexpr (is_ta) {
+            callback(convert_custom_type_to_ros_message_unique_ptr(*message));
+          } else {
+            callback(std::move(message));
+          }
         } else if constexpr (  // NOLINT[readability/braces]
           std::is_same_v<T, UniquePtrWithInfoROSMessageCallback>||
           std::is_same_v<T, SharedPtrWithInfoROSMessageCallback>)
         {
-          callback(std::move(message), message_info);
+          if constexpr (is_ta) {
+            callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info);
+          } else {
+            callback(std::move(message), message_info);
+          }
         } else if constexpr (  // NOLINT[readability/braces]
           std::is_same_v<T, SharedConstPtrROSMessageCallback>||
           std::is_same_v<T, ConstRefSharedConstPtrROSMessageCallback>)
         {
-          callback(std::move(message));
+          if constexpr (is_ta) {
+            callback(convert_custom_type_to_ros_message_unique_ptr(*message));
+          } else {
+            callback(std::move(message));
+          }
         } else if constexpr (  // NOLINT[readability/braces]
           std::is_same_v<T, SharedConstPtrWithInfoROSMessageCallback>||
           std::is_same_v<T, ConstRefSharedConstPtrWithInfoROSMessageCallback>)
         {
-          callback(std::move(message), message_info);
+          if constexpr (is_ta) {
+            callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info);
+          } else {
+            callback(std::move(message), message_info);
+          }
         }
         // condition to catch SerializedMessage types
         else if constexpr (  // NOLINT[readability/braces]
@@ -886,7 +950,13 @@ public:
       std::holds_alternative<UniquePtrSerializedMessageCallback>(callback_variant_) ||
       std::holds_alternative<SharedConstPtrSerializedMessageCallback>(callback_variant_) ||
       std::holds_alternative<ConstRefSharedConstPtrSerializedMessageCallback>(callback_variant_) ||
-      std::holds_alternative<SharedPtrSerializedMessageCallback>(callback_variant_);
+      std::holds_alternative<SharedPtrSerializedMessageCallback>(callback_variant_) ||
+      std::holds_alternative<ConstRefSerializedMessageWithInfoCallback>(callback_variant_) ||
+      std::holds_alternative<UniquePtrSerializedMessageWithInfoCallback>(callback_variant_) ||
+      std::holds_alternative<SharedConstPtrSerializedMessageWithInfoCallback>(callback_variant_) ||
+      std::holds_alternative<ConstRefSharedConstPtrSerializedMessageWithInfoCallback>(
+      callback_variant_) ||
+      std::holds_alternative<SharedPtrSerializedMessageWithInfoCallback>(callback_variant_);
   }
 
   void

rclcpp/include/rclcpp/callback_group.hpp

@@ -16,11 +16,14 @@
 #define RCLCPP__CALLBACK_GROUP_HPP_
 
 #include <atomic>
+#include <functional>
+#include <memory>
 #include <mutex>
-#include <string>
 #include <vector>
 
 #include "rclcpp/client.hpp"
+#include "rclcpp/context.hpp"
+#include "rclcpp/guard_condition.hpp"
 #include "rclcpp/publisher_base.hpp"
 #include "rclcpp/service.hpp"
 #include "rclcpp/subscription_base.hpp"
@@ -95,6 +98,10 @@ public:
     CallbackGroupType group_type,
     bool automatically_add_to_executor_with_node = true);
 
+  /// Default destructor.
+  RCLCPP_PUBLIC
+  ~CallbackGroup();
+
   template<typename Function>
   rclcpp::SubscriptionBase::SharedPtr
   find_subscription_ptrs_if(Function func) const
@@ -138,6 +145,14 @@ public:
   const CallbackGroupType &
   type() const;
 
+  RCLCPP_PUBLIC
+  void collect_all_ptrs(
+    std::function<void(const rclcpp::SubscriptionBase::SharedPtr &)> sub_func,
+    std::function<void(const rclcpp::ServiceBase::SharedPtr &)> service_func,
+    std::function<void(const rclcpp::ClientBase::SharedPtr &)> client_func,
+    std::function<void(const rclcpp::TimerBase::SharedPtr &)> timer_func,
+    std::function<void(const rclcpp::Waitable::SharedPtr &)> waitable_func) const;
+
   /// Return a reference to the 'associated with executor' atomic boolean.
   /**
    * When a callback group is added to an executor this boolean is checked
@@ -163,6 +178,16 @@ public:
   bool
   automatically_add_to_executor_with_node() const;
 
+  /// Defer creating the notify guard condition and return it.
+  RCLCPP_PUBLIC
+  rclcpp::GuardCondition::SharedPtr
+  get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr);
+
+  /// Trigger the notify guard condition.
+  RCLCPP_PUBLIC
+  void
+  trigger_notify_guard_condition();
+
 protected:
   RCLCPP_DISABLE_COPY(CallbackGroup)
 
@@ -205,6 +230,9 @@ protected:
   std::vector<rclcpp::Waitable::WeakPtr> waitable_ptrs_;
   std::atomic_bool can_be_taken_from_;
   const bool automatically_add_to_executor_with_node_;
+  // defer the creation of the guard condition
+  std::shared_ptr<rclcpp::GuardCondition> notify_guard_condition_ = nullptr;
+  std::recursive_mutex notify_guard_condition_mutex_;
 
 private:
   template<typename TypeT, typename Function>

rclcpp/include/rclcpp/client.hpp

@@ -17,34 +17,103 @@
 
 #include <atomic>
 #include <future>
-#include <map>
+#include <unordered_map>
 #include <memory>
+#include <mutex>
+#include <optional>  // NOLINT, cpplint doesn't think this is a cpp std header
 #include <sstream>
 #include <string>
 #include <tuple>
 #include <utility>
+#include <variant>  // NOLINT
+#include <vector>
 
 #include "rcl/client.h"
 #include "rcl/error_handling.h"
+#include "rcl/event_callback.h"
 #include "rcl/wait.h"
 
+#include "rclcpp/detail/cpp_callback_trampoline.hpp"
 #include "rclcpp/exceptions.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/function_traits.hpp"
+#include "rclcpp/logging.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/utilities.hpp"
-#include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/visibility_control.hpp"
 
-#include "rcutils/logging_macros.h"
-
 #include "rmw/error_handling.h"
+#include "rmw/impl/cpp/demangle.hpp"
 #include "rmw/rmw.h"
 
 namespace rclcpp
 {
 
+namespace detail
+{
+template<typename FutureT>
+struct FutureAndRequestId
+{
+  FutureT future;
+  int64_t request_id;
+
+  FutureAndRequestId(FutureT impl, int64_t req_id)
+  : future(std::move(impl)), request_id(req_id)
+  {}
+
+  /// Allow implicit conversions to `std::future` by reference.
+  operator FutureT &() {return this->future;}
+
+  /// 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().
+  auto get() {return this->future.get();}
+  /// See std::future::valid().
+  bool valid() const noexcept {return this->future.valid();}
+  /// See std::future::wait().
+  void wait() const {return this->future.wait();}
+  /// See std::future::wait_for().
+  template<class Rep, class Period>
+  std::future_status wait_for(
+    const std::chrono::duration<Rep, Period> & timeout_duration) const
+  {
+    return this->future.wait_for(timeout_duration);
+  }
+  /// See std::future::wait_until().
+  template<class Clock, class Duration>
+  std::future_status wait_until(
+    const std::chrono::time_point<Clock, Duration> & timeout_time) const
+  {
+    return this->future.wait_until(timeout_time);
+  }
+
+  // Rule of five, we could use the rule of zero here, but better be explicit as some of the
+  // methods are deleted.
+
+  /// Move constructor.
+  FutureAndRequestId(FutureAndRequestId && other) noexcept = default;
+  /// Deleted copy constructor, each instance is a unique owner of the future.
+  FutureAndRequestId(const FutureAndRequestId & other) = delete;
+  /// Move assignment.
+  FutureAndRequestId & operator=(FutureAndRequestId && other) noexcept = default;
+  /// Deleted copy assignment, each instance is a unique owner of the future.
+  FutureAndRequestId & operator=(const FutureAndRequestId & other) = delete;
+  /// Destructor.
+  ~FutureAndRequestId() = default;
+};
+}  // namespace detail
+
 namespace node_interfaces
 {
 class NodeBaseInterface;
@@ -150,6 +219,122 @@ public:
   bool
   exchange_in_use_by_wait_set_state(bool in_use_state);
 
+  /// Get the actual request publsher QoS settings, after the defaults have been determined.
+  /**
+   * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT
+   * can only be resolved after the creation of the client, and it
+   * depends on the underlying rmw implementation.
+   * If the underlying setting in use can't be represented in ROS terms,
+   * it will be set to RMW_QOS_POLICY_*_UNKNOWN.
+   * May throw runtime_error when an unexpected error occurs.
+   *
+   * \return The actual request publsher qos settings.
+   * \throws std::runtime_error if failed to get qos settings
+   */
+  RCLCPP_PUBLIC
+  rclcpp::QoS
+  get_request_publisher_actual_qos() const;
+
+  /// Get the actual response subscription QoS settings, after the defaults have been determined.
+  /**
+   * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT
+   * can only be resolved after the creation of the client, and it
+   * depends on the underlying rmw implementation.
+   * If the underlying setting in use can't be represented in ROS terms,
+   * it will be set to RMW_QOS_POLICY_*_UNKNOWN.
+   * May throw runtime_error when an unexpected error occurs.
+   *
+   * \return The actual response subscription qos settings.
+   * \throws std::runtime_error if failed to get qos settings
+   */
+  RCLCPP_PUBLIC
+  rclcpp::QoS
+  get_response_subscription_actual_qos() const;
+
+  /// Set a callback to be called when each new response is received.
+  /**
+   * The callback receives a size_t which is the number of responses received
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if responses were received before any
+   * callback was set.
+   *
+   * Since this callback is called from the middleware, you should aim to make
+   * it fast and not blocking.
+   * If you need to do a lot of work or wait for some other event, you should
+   * spin it off to another thread, otherwise you risk blocking the middleware.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * An exception will be thrown if the callback is not callable.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the client
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rmw_client_set_on_new_response_callback
+   * \sa rcl_client_set_on_new_response_callback
+   *
+   * \param[in] callback functor to be called when a new response is received
+   */
+  void
+  set_on_new_response_callback(std::function<void(size_t)> callback)
+  {
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_new_response_callback "
+              "is not callable.");
+    }
+
+    auto new_callback =
+      [callback, this](size_t number_of_responses) {
+        try {
+          callback(number_of_responses);
+        } catch (const std::exception & exception) {
+          RCLCPP_ERROR_STREAM(
+            node_logger_,
+            "rclcpp::ClientBase@" << this <<
+              " caught " << rmw::impl::cpp::demangle(exception) <<
+              " exception in user-provided callback for the 'on new response' callback: " <<
+              exception.what());
+        } catch (...) {
+          RCLCPP_ERROR_STREAM(
+            node_logger_,
+            "rclcpp::ClientBase@" << this <<
+              " caught unhandled exception in user-provided callback " <<
+              "for the 'on new response' callback");
+        }
+      };
+
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+
+    // Set it temporarily to the new callback, while we replace the old one.
+    // This two-step setting, prevents a gap where the old std::function has
+    // been replaced but the middleware hasn't been told about the new one yet.
+    set_on_new_response_callback(
+      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      static_cast<const void *>(&new_callback));
+
+    // Store the std::function to keep it in scope, also overwrites the existing one.
+    on_new_response_callback_ = new_callback;
+
+    // Set it again, now using the permanent storage.
+    set_on_new_response_callback(
+      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      static_cast<const void *>(&on_new_response_callback_));
+  }
+
+  /// Unset the callback registered for new responses, if any.
+  void
+  clear_on_new_response_callback()
+  {
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+    if (on_new_response_callback_) {
+      set_on_new_response_callback(nullptr, nullptr);
+      on_new_response_callback_ = nullptr;
+    }
+  }
+
 protected:
   RCLCPP_DISABLE_COPY(ClientBase)
 
@@ -165,19 +350,30 @@ protected:
   const rcl_node_t *
   get_rcl_node_handle() const;
 
+  RCLCPP_PUBLIC
+  void
+  set_on_new_response_callback(rcl_event_callback_t callback, const void * user_data);
+
   rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
   std::shared_ptr<rcl_node_t> node_handle_;
   std::shared_ptr<rclcpp::Context> context_;
+  rclcpp::Logger node_logger_;
 
   std::shared_ptr<rcl_client_t> client_handle_;
 
   std::atomic<bool> in_use_by_wait_set_{false};
+
+  std::recursive_mutex callback_mutex_;
+  std::function<void(size_t)> on_new_response_callback_{nullptr};
 };
 
 template<typename ServiceT>
 class Client : public ClientBase
 {
 public:
+  using Request = typename ServiceT::Request;
+  using Response = typename ServiceT::Response;
+
   using SharedRequest = typename ServiceT::Request::SharedPtr;
   using SharedResponse = typename ServiceT::Response::SharedPtr;
 
@@ -187,6 +383,7 @@ public:
   using SharedPromise = std::shared_ptr<Promise>;
   using SharedPromiseWithRequest = std::shared_ptr<PromiseWithRequest>;
 
+  using Future = std::future<SharedResponse>;
   using SharedFuture = std::shared_future<SharedResponse>;
   using SharedFutureWithRequest = std::shared_future<std::pair<SharedRequest, SharedResponse>>;
 
@@ -195,6 +392,64 @@ public:
 
   RCLCPP_SMART_PTR_DEFINITIONS(Client)
 
+  /// A convenient Client::Future and request id pair.
+  /**
+   * Public members:
+   * - future: a std::future<SharedResponse>.
+   * - request_id: the request id associated with the future.
+   *
+   * All the other methods are equivalent to the ones std::future provides.
+   */
+  struct FutureAndRequestId
+    : detail::FutureAndRequestId<std::future<SharedResponse>>
+  {
+    using detail::FutureAndRequestId<std::future<SharedResponse>>::FutureAndRequestId;
+
+    /// 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().
+    SharedFuture share() noexcept {return this->future.share();}
+  };
+
+  /// A convenient Client::SharedFuture and request id pair.
+  /**
+   * Public members:
+   * - future: a std::shared_future<SharedResponse>.
+   * - request_id: the request id associated with the future.
+   *
+   * All the other methods are equivalent to the ones std::shared_future provides.
+   */
+  struct SharedFutureAndRequestId
+    : detail::FutureAndRequestId<std::shared_future<SharedResponse>>
+  {
+    using detail::FutureAndRequestId<std::shared_future<SharedResponse>>::FutureAndRequestId;
+  };
+
+  /// A convenient Client::SharedFutureWithRequest and request id pair.
+  /**
+   * Public members:
+   * - future: a std::shared_future<SharedResponse>.
+   * - request_id: the request id associated with the future.
+   *
+   * All the other methods are equivalent to the ones std::shared_future provides.
+   */
+  struct SharedFutureWithRequestAndRequestId
+    : detail::FutureAndRequestId<std::shared_future<std::pair<SharedRequest, SharedResponse>>>
+  {
+    using detail::FutureAndRequestId<
+      std::shared_future<std::pair<SharedRequest, SharedResponse>>
+    >::FutureAndRequestId;
+  };
+
   /// Default constructor.
   /**
    * The constructor for a Client is almost never called directly.
@@ -292,34 +547,89 @@ public:
     std::shared_ptr<rmw_request_id_t> request_header,
     std::shared_ptr<void> response) override
   {
-    std::unique_lock<std::mutex> lock(pending_requests_mutex_);
-    auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(response);
-    int64_t sequence_number = request_header->sequence_number;
-    // TODO(esteve) this should throw instead since it is not expected to happen in the first place
-    if (this->pending_requests_.count(sequence_number) == 0) {
-      RCUTILS_LOG_ERROR_NAMED(
-        "rclcpp",
-        "Received invalid sequence number. Ignoring...");
+    std::optional<CallbackInfoVariant>
+    optional_pending_request = this->get_and_erase_pending_request(request_header->sequence_number);
+    if (!optional_pending_request) {
       return;
     }
-    auto tuple = this->pending_requests_[sequence_number];
-    auto call_promise = std::get<0>(tuple);
-    auto callback = std::get<1>(tuple);
-    auto future = std::get<2>(tuple);
-    this->pending_requests_.erase(sequence_number);
-    // Unlock here to allow the service to be called recursively from one of its callbacks.
-    lock.unlock();
-
-    call_promise->set_value(typed_response);
-    callback(future);
+    auto & value = *optional_pending_request;
+    auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(
+      std::move(response));
+    if (std::holds_alternative<Promise>(value)) {
+      auto & promise = std::get<Promise>(value);
+      promise.set_value(std::move(typed_response));
+    } else if (std::holds_alternative<CallbackTypeValueVariant>(value)) {
+      auto & inner = std::get<CallbackTypeValueVariant>(value);
+      const auto & callback = std::get<CallbackType>(inner);
+      auto & promise = std::get<Promise>(inner);
+      auto & future = std::get<SharedFuture>(inner);
+      promise.set_value(std::move(typed_response));
+      callback(std::move(future));
+    } else if (std::holds_alternative<CallbackWithRequestTypeValueVariant>(value)) {
+      auto & inner = std::get<CallbackWithRequestTypeValueVariant>(value);
+      const auto & callback = std::get<CallbackWithRequestType>(inner);
+      auto & promise = std::get<PromiseWithRequest>(inner);
+      auto & future = std::get<SharedFutureWithRequest>(inner);
+      auto & request = std::get<SharedRequest>(inner);
+      promise.set_value(std::make_pair(std::move(request), std::move(typed_response)));
+      callback(std::move(future));
+    }
   }
 
-  SharedFuture
+  /// Send a request to the service server.
+  /**
+   * This method returns a `FutureAndRequestId` instance
+   * that can be passed to Executor::spin_until_future_complete() to
+   * wait until it has been completed.
+   *
+   * If the future never completes,
+   * e.g. the call to Executor::spin_until_future_complete() times out,
+   * Client::remove_pending_request() must be called to clean the client internal state.
+   * Not doing so will make the `Client` instance to use more memory each time a response is not
+   * received from the service server.
+   *
+   * ```cpp
+   * auto future = client->async_send_request(my_request);
+   * if (
+   *   rclcpp::FutureReturnCode::TIMEOUT ==
+   *   executor->spin_until_future_complete(future, timeout))
+   * {
+   *   client->remove_pending_request(future);
+   *   // handle timeout
+   * } else {
+   *   handle_response(future.get());
+   * }
+   * ```
+   *
+   * \param[in] request request to be send.
+   * \return a FutureAndRequestId instance.
+   */
+  FutureAndRequestId
   async_send_request(SharedRequest request)
   {
-    return async_send_request(request, [](SharedFuture) {});
+    Promise promise;
+    auto future = promise.get_future();
+    auto req_id = async_send_request_impl(
+      *request,
+      std::move(promise));
+    return FutureAndRequestId(std::move(future), req_id);
   }
 
+  /// Send a request to the service server and schedule a callback in the executor.
+  /**
+   * Similar to the previous overload, but a callback will automatically be called when a response is received.
+   *
+   * If the callback is never called, because we never got a reply for the service server, remove_pending_request()
+   * has to be called with the returned request id or prune_pending_requests().
+   * Not doing so will make the `Client` instance use more memory each time a response is not
+   * received from the service server.
+   * In this case, it's convenient to setup a timer to cleanup the pending requests.
+   * See for example the `examples_rclcpp_async_client` package in https://github.com/ros2/examples.
+   *
+   * \param[in] request request to be send.
+   * \param[in] cb callback that will be called when we get a response for this request.
+   * \return the request id representing the request just sent.
+   */
   template<
     typename CallbackT,
     typename std::enable_if<
@@ -329,23 +639,28 @@ public:
       >::value
     >::type * = nullptr
   >
-  SharedFuture
+  SharedFutureAndRequestId
   async_send_request(SharedRequest request, CallbackT && cb)
   {
-    std::lock_guard<std::mutex> lock(pending_requests_mutex_);
-    int64_t sequence_number;
-    rcl_ret_t ret = rcl_send_request(get_client_handle().get(), request.get(), &sequence_number);
-    if (RCL_RET_OK != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");
-    }
-
-    SharedPromise call_promise = std::make_shared<Promise>();
-    SharedFuture f(call_promise->get_future());
-    pending_requests_[sequence_number] =
-      std::make_tuple(call_promise, std::forward<CallbackType>(cb), f);
-    return f;
+    Promise promise;
+    auto shared_future = promise.get_future().share();
+    auto req_id = async_send_request_impl(
+      *request,
+      std::make_tuple(
+        CallbackType{std::forward<CallbackT>(cb)},
+        shared_future,
+        std::move(promise)));
+    return SharedFutureAndRequestId{std::move(shared_future), req_id};
   }
 
+  /// Send a request to the service server and schedule a callback in the executor.
+  /**
+   * Similar to the previous method, but you can get both the request and response in the callback.
+   *
+   * \param[in] request request to be send.
+   * \param[in] cb callback that will be called when we get a response for this request.
+   * \return the request id representing the request just sent.
+   */
   template<
     typename CallbackT,
     typename std::enable_if<
@@ -355,28 +670,165 @@ public:
       >::value
     >::type * = nullptr
   >
-  SharedFutureWithRequest
+  SharedFutureWithRequestAndRequestId
   async_send_request(SharedRequest request, CallbackT && cb)
   {
-    SharedPromiseWithRequest promise = std::make_shared<PromiseWithRequest>();
-    SharedFutureWithRequest future_with_request(promise->get_future());
-
-    auto wrapping_cb = [future_with_request, promise, request,
-        cb = std::forward<CallbackWithRequestType>(cb)](SharedFuture future) {
-        auto response = future.get();
-        promise->set_value(std::make_pair(request, response));
-        cb(future_with_request);
-      };
-
-    async_send_request(request, wrapping_cb);
-
-    return future_with_request;
+    PromiseWithRequest promise;
+    auto shared_future = promise.get_future().share();
+    auto req_id = async_send_request_impl(
+      *request,
+      std::make_tuple(
+        CallbackWithRequestType{std::forward<CallbackT>(cb)},
+        request,
+        shared_future,
+        std::move(promise)));
+    return SharedFutureWithRequestAndRequestId{std::move(shared_future), req_id};
+  }
+
+  /// Cleanup a pending request.
+  /**
+   * This notifies the client that we have waited long enough for a response from the server
+   * to come, we have given up and we are not waiting for a response anymore.
+   *
+   * Not calling this will make the client start using more memory for each request
+   * that never got a reply from the server.
+   *
+   * \param[in] request_id request id returned by async_send_request().
+   * \return true when a pending request was removed, false if not (e.g. a response was received).
+   */
+  bool
+  remove_pending_request(int64_t request_id)
+  {
+    std::lock_guard guard(pending_requests_mutex_);
+    return pending_requests_.erase(request_id) != 0u;
+  }
+
+  /// Cleanup a pending request.
+  /**
+   * Convenient overload, same as:
+   *
+   * `Client::remove_pending_request(this, future.request_id)`.
+   */
+  bool
+  remove_pending_request(const FutureAndRequestId & future)
+  {
+    return this->remove_pending_request(future.request_id);
+  }
+
+  /// Cleanup a pending request.
+  /**
+   * Convenient overload, same as:
+   *
+   * `Client::remove_pending_request(this, future.request_id)`.
+   */
+  bool
+  remove_pending_request(const SharedFutureAndRequestId & future)
+  {
+    return this->remove_pending_request(future.request_id);
+  }
+
+  /// Cleanup a pending request.
+  /**
+   * Convenient overload, same as:
+   *
+   * `Client::remove_pending_request(this, future.request_id)`.
+   */
+  bool
+  remove_pending_request(const SharedFutureWithRequestAndRequestId & future)
+  {
+    return this->remove_pending_request(future.request_id);
+  }
+
+  /// Clean all pending requests.
+  /**
+   * \return number of pending requests that were removed.
+   */
+  size_t
+  prune_pending_requests()
+  {
+    std::lock_guard guard(pending_requests_mutex_);
+    auto ret = pending_requests_.size();
+    pending_requests_.clear();
+    return ret;
+  }
+
+  /// Clean all pending requests older than a time_point.
+  /**
+   * \param[in] time_point Requests that were sent before this point are going to be removed.
+   * \param[inout] pruned_requests Removed requests id will be pushed to the vector
+   *  if a pointer is provided.
+   * \return number of pending requests that were removed.
+   */
+  template<typename AllocatorT = std::allocator<int64_t>>
+  size_t
+  prune_requests_older_than(
+    std::chrono::time_point<std::chrono::system_clock> time_point,
+    std::vector<int64_t, AllocatorT> * pruned_requests = nullptr)
+  {
+    std::lock_guard guard(pending_requests_mutex_);
+    auto old_size = pending_requests_.size();
+    for (auto it = pending_requests_.begin(), last = pending_requests_.end(); it != last; ) {
+      if (it->second.first < time_point) {
+        if (pruned_requests) {
+          pruned_requests->push_back(it->first);
+        }
+        it = pending_requests_.erase(it);
+      } else {
+        ++it;
+      }
+    }
+    return old_size - pending_requests_.size();
+  }
+
+protected:
+  using CallbackTypeValueVariant = std::tuple<CallbackType, SharedFuture, Promise>;
+  using CallbackWithRequestTypeValueVariant = std::tuple<
+    CallbackWithRequestType, SharedRequest, SharedFutureWithRequest, PromiseWithRequest>;
+
+  using CallbackInfoVariant = std::variant<
+    std::promise<SharedResponse>,
+    CallbackTypeValueVariant,
+    CallbackWithRequestTypeValueVariant>;
+
+  int64_t
+  async_send_request_impl(const Request & request, CallbackInfoVariant value)
+  {
+    int64_t sequence_number;
+    std::lock_guard<std::mutex> lock(pending_requests_mutex_);
+    rcl_ret_t ret = rcl_send_request(get_client_handle().get(), &request, &sequence_number);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");
+    }
+    pending_requests_.try_emplace(
+      sequence_number,
+      std::make_pair(std::chrono::system_clock::now(), std::move(value)));
+    return sequence_number;
+  }
+
+  std::optional<CallbackInfoVariant>
+  get_and_erase_pending_request(int64_t request_number)
+  {
+    std::unique_lock<std::mutex> lock(pending_requests_mutex_);
+    auto it = this->pending_requests_.find(request_number);
+    if (it == this->pending_requests_.end()) {
+      RCUTILS_LOG_DEBUG_NAMED(
+        "rclcpp",
+        "Received invalid sequence number. Ignoring...");
+      return std::nullopt;
+    }
+    std::optional<CallbackInfoVariant> value = std::move(it->second.second);
+    this->pending_requests_.erase(request_number);
+    return value;
   }
 
-private:
   RCLCPP_DISABLE_COPY(Client)
 
-  std::map<int64_t, std::tuple<SharedPromise, CallbackType, SharedFuture>> pending_requests_;
+  std::unordered_map<
+    int64_t,
+    std::pair<
+      std::chrono::time_point<std::chrono::system_clock>,
+      CallbackInfoVariant>>
+  pending_requests_;
   std::mutex pending_requests_mutex_;
 };
 

rclcpp/include/rclcpp/clock.hpp

@@ -19,6 +19,7 @@
 #include <memory>
 #include <mutex>
 
+#include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/time.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -59,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.
    */
@@ -78,6 +86,109 @@ public:
   Time
   now();
 
+  /**
+   * Sleep until a specified Time, according to clock type.
+   *
+   * Notes for RCL_ROS_TIME clock type:
+   *   - Can sleep forever if ros time is active and received clock never reaches `until`
+   *   - If ROS time enabled state changes during the sleep, this method will immediately return
+   *     false. There is not a consistent choice of sleeping time when the time source changes,
+   *     so this is up to the caller to call again if needed.
+   *
+   * \warning When using gcc < 10 or when using gcc >= 10 and pthreads lacks the function
+   *    `pthread_cond_clockwait`, steady clocks may sleep using the system clock.
+   *    If so, steady clock sleep times can be affected by system clock time jumps.
+   *    Depending on the steady clock's epoch and resolution in comparison to the system clock's,
+   *    an overflow when converting steady clock durations to system clock times may cause
+   *    undefined behavior.
+   *    For more info see these issues:
+   *    https://gcc.gnu.org/bugzilla/show_bug.cgi?id=41861
+   *    https://gcc.gnu.org/bugzilla/show_bug.cgi?id=58931
+   *
+   * \param until absolute time according to current clock type to sleep until.
+   * \param context the rclcpp context the clock should use to check that ROS is still initialized.
+   * \return true immediately if `until` is in the past
+   * \return true when the time `until` is reached
+   * \return false if time cannot be reached reliably, for example from shutdown or a change
+   *    of time source.
+   * \throws std::runtime_error if the context is invalid
+   * \throws std::runtime_error if `until` has a different clock type from this clock
+   */
+  RCLCPP_PUBLIC
+  bool
+  sleep_until(
+    Time until,
+    Context::SharedPtr context = contexts::get_global_default_context());
+
+  /**
+   * Sleep for a specified Duration.
+   *
+   * Equivalent to
+   *
+   * ```cpp
+   * clock->sleep_until(clock->now() + rel_time, context)
+   * ```
+   *
+   * The function will return immediately if `rel_time` is zero or negative.
+   *
+   * \param rel_time the length of time to sleep for.
+   * \param context the rclcpp context the clock should use to check that ROS is still initialized.
+   * \return true when the end time is reached
+   * \return false if time cannot be reached reliably, for example from shutdown or a change
+   *    of time source.
+   * \throws std::runtime_error if the context is invalid
+   */
+  RCLCPP_PUBLIC
+  bool
+  sleep_for(
+    Duration rel_time,
+    Context::SharedPtr context = contexts::get_global_default_context());
+
+  /**
+   * Check if the clock is started.
+   *
+   * A started clock is a clock that reflects non-zero time.
+   * Typically a clock will be unstarted if it is using RCL_ROS_TIME with ROS time and
+   * nothing has been published on the clock topic yet.
+   *
+   * \return true if clock is started
+   * \throws std::runtime_error if the clock is not rcl_clock_valid
+   */
+  RCLCPP_PUBLIC
+  bool
+  started();
+
+  /**
+   * Wait until clock to start.
+   *
+   * \rclcpp::Clock::started
+   * \param context the context to wait in
+   * \return true if clock was already started or became started
+   * \throws std::runtime_error if the context is invalid or clock is not rcl_clock_valid
+   */
+  RCLCPP_PUBLIC
+  bool
+  wait_until_started(Context::SharedPtr context = contexts::get_global_default_context());
+
+  /**
+   * Wait for clock to start, with timeout.
+   *
+   * The timeout is waited in steady time.
+   *
+   * \rclcpp::Clock::started
+   * \param timeout the maximum time to wait for.
+   * \param context the context to wait in.
+   * \param wait_tick_ns the time to wait between each iteration of the wait loop (in nanoseconds).
+   * \return true if clock was or became valid
+   * \throws std::runtime_error if the context is invalid or clock is not rcl_clock_valid
+   */
+  RCLCPP_PUBLIC
+  bool
+  wait_until_started(
+    const rclcpp::Duration & timeout,
+    Context::SharedPtr context = contexts::get_global_default_context(),
+    const rclcpp::Duration & wait_tick_ns = rclcpp::Duration(0, static_cast<uint32_t>(1e7)));
+
   /**
    * Returns the clock of the type `RCL_ROS_TIME` is active.
    *

rclcpp/include/rclcpp/context.hpp

@@ -26,6 +26,7 @@
 #include <unordered_set>
 #include <utility>
 #include <vector>
+#include <stdexcept>
 
 #include "rcl/context.h"
 #include "rcl/guard_condition.h"
@@ -48,17 +49,20 @@ public:
 /// Forward declare WeakContextsWrapper
 class WeakContextsWrapper;
 
-class OnShutdownCallbackHandle
+class ShutdownCallbackHandle
 {
   friend class Context;
 
 public:
-  using OnShutdownCallbackType = std::function<void ()>;
+  using ShutdownCallbackType = std::function<void ()>;
 
 private:
-  std::weak_ptr<OnShutdownCallbackType> callback;
+  std::weak_ptr<ShutdownCallbackType> callback;
 };
 
+using OnShutdownCallbackHandle = ShutdownCallbackHandle;
+using PreShutdownCallbackHandle = ShutdownCallbackHandle;
+
 /// Context which encapsulates shared state between nodes and other similar entities.
 /**
  * A context also represents the lifecycle between init and shutdown of rclcpp.
@@ -75,7 +79,7 @@ public:
    * Every context which is constructed is added to a global vector of contexts,
    * which is used by the signal handler to conditionally shutdown each context
    * on SIGINT.
-   * See the shutdown_on_sigint option in the InitOptions class.
+   * See the shutdown_on_signal option in the InitOptions class.
    */
   RCLCPP_PUBLIC
   Context();
@@ -93,7 +97,7 @@ public:
    * Note that this function does not setup any signal handlers, so if you want
    * it to be shutdown by the signal handler, then you need to either install
    * them manually with rclcpp::install_signal_handlers() or use rclcpp::init().
-   * In addition to installing the signal handlers, the shutdown_on_sigint
+   * In addition to installing the signal handlers, the shutdown_on_signal
    * of the InitOptions needs to be `true` for this context to be shutdown by
    * the signal handler, otherwise it will be passed over.
    *
@@ -124,7 +128,7 @@ public:
   void
   init(
     int argc,
-    char const * const argv[],
+    char const * const * argv,
     const rclcpp::InitOptions & init_options = rclcpp::InitOptions());
 
   /// Return true if the context is valid, otherwise false.
@@ -189,7 +193,7 @@ public:
   bool
   shutdown(const std::string & reason);
 
-  using OnShutdownCallback = OnShutdownCallbackHandle::OnShutdownCallbackType;
+  using OnShutdownCallback = OnShutdownCallbackHandle::ShutdownCallbackType;
 
   /// Add a on_shutdown callback to be called when shutdown is called for this context.
   /**
@@ -197,7 +201,7 @@ public:
    * to last step in shutdown().
    *
    * When shutdown occurs due to the signal handler, these callbacks are run
-   * asynchronoulsy in the dedicated singal handling thread.
+   * asynchronously in the dedicated singal handling thread.
    *
    * Also, shutdown() may be called from the destructor of this function.
    * Therefore, it is not safe to throw exceptions from these callbacks.
@@ -221,7 +225,7 @@ public:
    * to last step in shutdown().
    *
    * When shutdown occurs due to the signal handler, these callbacks are run
-   * asynchronously in the dedicated singal handling thread.
+   * asynchronously in the dedicated signal handling thread.
    *
    * Also, shutdown() may be called from the destructor of this function.
    * Therefore, it is not safe to throw exceptions from these callbacks.
@@ -249,6 +253,33 @@ public:
   bool
   remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle);
 
+  using PreShutdownCallback = PreShutdownCallbackHandle::ShutdownCallbackType;
+
+  /// Add a pre_shutdown callback to be called before shutdown is called for this context.
+  /**
+   * These callbacks will be called in the order they are added.
+   *
+   * When shutdown occurs due to the signal handler, these callbacks are run
+   * asynchronously in the dedicated signal handling thread.
+   *
+   * \param[in] callback the pre_shutdown callback to be registered
+   * \return the created callback handle
+   */
+  RCLCPP_PUBLIC
+  virtual
+  PreShutdownCallbackHandle
+  add_pre_shutdown_callback(PreShutdownCallback callback);
+
+  /// Remove an registered pre_shutdown callback.
+  /**
+   * \param[in] callback_handle the pre_shutdown callback handle to be removed.
+   * \return true if the callback is found and removed, otherwise false.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  remove_pre_shutdown_callback(const PreShutdownCallbackHandle & callback_handle);
+
   /// Return the shutdown callbacks.
   /**
    * Returned callbacks are a copy of the registered callbacks.
@@ -257,6 +288,14 @@ public:
   std::vector<OnShutdownCallback>
   get_on_shutdown_callbacks() const;
 
+  /// Return the pre-shutdown callbacks.
+  /**
+   * Returned callbacks are a copy of the registered callbacks.
+   */
+  RCLCPP_PUBLIC
+  std::vector<PreShutdownCallback>
+  get_pre_shutdown_callbacks() const;
+
   /// Return the internal rcl context.
   RCLCPP_PUBLIC
   std::shared_ptr<rcl_context_t>
@@ -268,7 +307,7 @@ public:
    *
    *   - this context is shutdown()
    *   - this context is destructed (resulting in shutdown)
-   *   - this context has shutdown_on_sigint=true and SIGINT occurs (resulting in shutdown)
+   *   - this context has shutdown_on_signal=true and SIGINT/SIGTERM occurs (resulting in shutdown)
    *   - interrupt_all_sleep_for() is called
    *
    * \param[in] nanoseconds A std::chrono::duration representing how long to sleep for.
@@ -338,6 +377,9 @@ private:
   std::unordered_set<std::shared_ptr<OnShutdownCallback>> on_shutdown_callbacks_;
   mutable std::mutex on_shutdown_callbacks_mutex_;
 
+  std::unordered_set<std::shared_ptr<PreShutdownCallback>> pre_shutdown_callbacks_;
+  mutable std::mutex pre_shutdown_callbacks_mutex_;
+
   /// Condition variable for timed sleep (see sleep_for).
   std::condition_variable interrupt_condition_variable_;
   /// Mutex for protecting the global condition variable.
@@ -345,6 +387,29 @@ private:
 
   /// Keep shared ownership of global vector of weak contexts
   std::shared_ptr<WeakContextsWrapper> weak_contexts_;
+
+  enum class ShutdownType
+  {
+    pre_shutdown,
+    on_shutdown
+  };
+
+  using ShutdownCallback = ShutdownCallbackHandle::ShutdownCallbackType;
+
+  RCLCPP_LOCAL
+  ShutdownCallbackHandle
+  add_shutdown_callback(
+    ShutdownType shutdown_type,
+    ShutdownCallback callback);
+
+  RCLCPP_LOCAL
+  bool
+  remove_shutdown_callback(
+    ShutdownType shutdown_type,
+    const ShutdownCallbackHandle & callback_handle);
+
+  std::vector<rclcpp::Context::ShutdownCallback>
+  get_shutdown_callback(ShutdownType shutdown_type) const;
 };
 
 /// Return a copy of the list of context shared pointers.

rclcpp/include/rclcpp/create_publisher.hpp

@@ -92,7 +92,7 @@ template<
   typename NodeT>
 std::shared_ptr<PublisherT>
 create_publisher(
-  NodeT & node,
+  NodeT && node,
   const std::string & topic_name,
   const rclcpp::QoS & qos,
   const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options = (

rclcpp/include/rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp

@@ -12,30 +12,28 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__CLIENT_ENTRY_HPP_
-#define RCLCPP__WAIT_SET_POLICIES__DETAIL__CLIENT_ENTRY_HPP_
+#ifndef RCLCPP__DETAIL__ADD_GUARD_CONDITION_TO_RCL_WAIT_SET_HPP_
+#define RCLCPP__DETAIL__ADD_GUARD_CONDITION_TO_RCL_WAIT_SET_HPP_
 
-#include <memory>
-
-#include "rclcpp/client.hpp"
-#include "rclcpp/wait_set_policies/detail/entity_entry.hpp"
+#include "rclcpp/guard_condition.hpp"
 
 namespace rclcpp
 {
-namespace wait_set_policies
-{
 namespace detail
 {
 
-using ClientEntry =
-  // EntityEntryTemplate<rclcpp::ClientBase, std::shared_ptr<rclcpp::ClientBase>>;
-  EntityEntryTemplate<rclcpp::ClientBase>;
-using WeakClientEntry =
-  // EntityEntryTemplate<rclcpp::ClientBase, std::weak_ptr<rclcpp::ClientBase>>;
-  EntityEntryTemplate<rclcpp::ClientBase>;
+/// Adds the guard condition to a waitset
+/**
+ * \param[in] wait_set reference to a wait set where to add the guard condition
+ * \param[in] guard_condition reference to the guard_condition to be added
+ */
+RCLCPP_PUBLIC
+void
+add_guard_condition_to_rcl_wait_set(
+  rcl_wait_set_t & wait_set,
+  const rclcpp::GuardCondition & guard_condition);
 
 }  // namespace detail
-}  // namespace wait_set_policies
 }  // namespace rclcpp
 
-#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__CLIENT_ENTRY_HPP_
+#endif  // RCLCPP__DETAIL__ADD_GUARD_CONDITION_TO_RCL_WAIT_SET_HPP_

rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp

@@ -0,0 +1,68 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__DETAIL__CPP_CALLBACK_TRAMPOLINE_HPP_
+#define RCLCPP__DETAIL__CPP_CALLBACK_TRAMPOLINE_HPP_
+
+#include <functional>
+
+namespace rclcpp
+{
+
+namespace detail
+{
+
+/// Trampoline pattern for wrapping std::function into C-style callbacks.
+/**
+ * A common pattern in C is for a function to take a function pointer and a
+ * void pointer for "user data" which is passed to the function pointer when it
+ * is called from within C.
+ *
+ * It works by using the user data pointer to store a pointer to a
+ * std::function instance.
+ * So when called from C, this function will cast the user data to the right
+ * std::function type and call it.
+ *
+ * This should allow you to use free functions, lambdas with and without
+ * captures, and various kinds of std::bind instances.
+ *
+ * The interior of this function is likely to be executed within a C runtime,
+ * so no exceptions should be thrown at this point, and doing so results in
+ * undefined behavior.
+ *
+ * \tparam UserDataT Deduced type based on what is passed for user data,
+ *   usually this type is either `void *` or `const void *`.
+ * \tparam Args the arguments being passed to the callback
+ * \tparam ReturnT the return type of this function and the callback, default void
+ * \param user_data the function pointer, possibly type erased
+ * \param args the arguments to be forwarded to the callback
+ * \returns whatever the callback returns, if anything
+ */
+template<
+  typename UserDataT,
+  typename ... Args,
+  typename ReturnT = void
+>
+ReturnT
+cpp_callback_trampoline(UserDataT user_data, Args ... args) noexcept
+{
+  auto & actual_callback = *reinterpret_cast<const std::function<ReturnT(Args...)> *>(user_data);
+  return actual_callback(args ...);
+}
+
+}  // namespace detail
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__CPP_CALLBACK_TRAMPOLINE_HPP_

rclcpp/include/rclcpp/detail/mutex_two_priorities.hpp

@@ -1,76 +0,0 @@
-// Copyright 2021 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef RCLCPP__DETAIL__MUTEX_TWO_PRIORITIES_HPP_
-#define RCLCPP__DETAIL__MUTEX_TWO_PRIORITIES_HPP_
-
-#include <condition_variable>
-#include <mutex>
-
-namespace rclcpp
-{
-namespace detail
-{
-/// \internal A mutex that has two locking mechanism, one with higher priority than the other.
-/**
- * After the current mutex owner release the lock, a thread that used the high
- * priority mechanism will have priority over threads that used the low priority mechanism.
- */
-class MutexTwoPriorities
-{
-public:
-  class HighPriorityLockable
-  {
-public:
-    explicit HighPriorityLockable(MutexTwoPriorities & parent);
-
-    void lock();
-
-    void unlock();
-
-private:
-    MutexTwoPriorities & parent_;
-  };
-
-  class LowPriorityLockable
-  {
-public:
-    explicit LowPriorityLockable(MutexTwoPriorities & parent);
-
-    void lock();
-
-    void unlock();
-
-private:
-    MutexTwoPriorities & parent_;
-  };
-
-  HighPriorityLockable
-  get_high_priority_lockable();
-
-  LowPriorityLockable
-  get_low_priority_lockable();
-
-private:
-  std::condition_variable hp_cv_;
-  std::condition_variable lp_cv_;
-  std::mutex cv_mutex_;
-  size_t hp_waiting_count_{0u};
-  bool data_taken_{false};
-};
-
-}  // namespace detail
-}  // namespace rclcpp
-
-#endif  // RCLCPP__DETAIL__MUTEX_TWO_PRIORITIES_HPP_

rclcpp/include/rclcpp/detail/qos_parameters.hpp

@@ -104,6 +104,7 @@ declare_parameter_or_get(
   }
 }
 
+#ifdef DOXYGEN_ONLY
 /// \internal Declare QoS parameters for the given entity.
 /**
  * \tparam NodeT Node pointer or reference type.
@@ -116,12 +117,23 @@ declare_parameter_or_get(
  * \param default_qos User provided qos. It will be used as a default for the parameters declared.
  * \return qos profile based on the user provided parameter overrides.
  */
+template<typename NodeT, typename EntityQosParametersTraits>
+rclcpp::QoS
+  declare_qos_parameters(
+  const ::rclcpp::QosOverridingOptions & options,
+  NodeT & node,
+  const std::string & topic_name,
+  const ::rclcpp::QoS & default_qos,
+  EntityQosParametersTraits);
+
+#else
+
 template<typename NodeT, typename EntityQosParametersTraits>
 std::enable_if_t<
-  rclcpp::node_interfaces::has_node_parameters_interface<
+  (rclcpp::node_interfaces::has_node_parameters_interface<
     decltype(std::declval<typename rcpputils::remove_pointer<NodeT>::type>())>::value ||
   std::is_same<typename std::decay_t<NodeT>,
-  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr>::value,
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr>::value),
   rclcpp::QoS>
 declare_qos_parameters(
   const ::rclcpp::QosOverridingOptions & options,
@@ -204,6 +216,8 @@ declare_qos_parameters(
   return default_qos;
 }
 
+#endif
+
 /// \internal Helper function to get a rmw qos policy value from a string.
 #define RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING( \
     kind_lower, kind_upper, parameter_value, rclcpp_qos) \

rclcpp/include/rclcpp/detail/subscription_callback_type_helper.hpp

@@ -38,6 +38,7 @@ namespace detail
  *
  * This issue, with the lambda's, can be demonstrated with this minimal program:
  *
+ * \code{.cpp}
  *   #include <functional>
  *   #include <memory>
  *
@@ -52,6 +53,7 @@ namespace detail
  *     std::function<void (std::shared_ptr<int>)> cb = [](std::shared_ptr<int>){};
  *     f(cb);
  *   }
+ * \endcode
  *
  * If this program ever starts working in a future version of C++, this class
  * may become redundant.

rclcpp/include/rclcpp/detail/template_contains.hpp

@@ -0,0 +1,47 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__DETAIL__TEMPLATE_CONTAINS_HPP_
+#define RCLCPP__DETAIL__TEMPLATE_CONTAINS_HPP_
+
+#include <type_traits>
+
+namespace rclcpp
+{
+namespace detail
+{
+
+/// Template meta-function that checks if a given T is contained in the list Us.
+template<typename T, typename ... Us>
+struct template_contains;
+
+template<typename ... Args>
+inline constexpr bool template_contains_v = template_contains<Args ...>::value;
+
+template<typename T, typename NextT, typename ... Us>
+struct template_contains<T, NextT, Us ...>
+{
+  enum { value = (std::is_same_v<T, NextT>|| template_contains_v<T, Us ...>)};
+};
+
+template<typename T>
+struct template_contains<T>
+{
+  enum { value = false };
+};
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__TEMPLATE_CONTAINS_HPP_

rclcpp/include/rclcpp/detail/template_unique.hpp

@@ -0,0 +1,49 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__DETAIL__TEMPLATE_UNIQUE_HPP_
+#define RCLCPP__DETAIL__TEMPLATE_UNIQUE_HPP_
+
+#include <type_traits>
+
+#include "rclcpp/detail/template_contains.hpp"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+/// Template meta-function that checks if a given list Ts contains unique types.
+template<typename ... Ts>
+struct template_unique;
+
+template<typename ... Args>
+inline constexpr bool template_unique_v = template_unique<Args ...>::value;
+
+template<typename NextT, typename ... Ts>
+struct template_unique<NextT, Ts ...>
+{
+  enum { value = !template_contains_v<NextT, Ts ...>&& template_unique_v<Ts ...>};
+};
+
+template<typename T>
+struct template_unique<T>
+{
+  enum { value = true };
+};
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__TEMPLATE_UNIQUE_HPP_

rclcpp/include/rclcpp/detail/utilities.hpp

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

rclcpp/include/rclcpp/duration.hpp

@@ -38,13 +38,6 @@ public:
    */
   Duration(int32_t seconds, uint32_t nanoseconds);
 
-  /// Construct duration from the specified nanoseconds.
-  [[deprecated(
-    "Use Duration::from_nanoseconds instead or std::chrono_literals. For example:"
-    "rclcpp::Duration::from_nanoseconds(int64_variable);"
-    "rclcpp::Duration(0ns);")]]
-  explicit Duration(rcl_duration_value_t nanoseconds);
-
   /// Construct duration from the specified std::chrono::nanoseconds.
   explicit Duration(std::chrono::nanoseconds nanoseconds);
 

rclcpp/include/rclcpp/exceptions/exceptions.hpp

@@ -109,6 +109,8 @@ public:
   : std::runtime_error(msg) {}
 };
 
+typedef void (* reset_error_function_t)();
+
 /// Throw a C++ std::exception which was created based on an rcl error.
 /**
  * Passing nullptr for reset_error is safe and will avoid calling any function
@@ -129,7 +131,7 @@ throw_from_rcl_error [[noreturn]] (
   rcl_ret_t ret,
   const std::string & prefix = "",
   const rcl_error_state_t * error_state = nullptr,
-  void (* reset_error)() = rcl_reset_error);
+  reset_error_function_t reset_error = rcl_reset_error);
 /* *INDENT-ON* */
 
 class RCLErrorBase
@@ -254,6 +256,23 @@ public:
   {}
 };
 
+/// Thrown if user attempts to create an uninitialized statically typed parameter
+/**
+ * (see https://github.com/ros2/rclcpp/issues/1691)
+ */
+class UninitializedStaticallyTypedParameterException : public std::runtime_error
+{
+public:
+  /// Construct an instance.
+  /**
+   * \param[in] name the name of the parameter.
+   */
+  RCLCPP_PUBLIC
+  explicit UninitializedStaticallyTypedParameterException(const std::string & name)
+  : std::runtime_error("Statically typed parameter '" + name + "' must be initialized.")
+  {}
+};
+
 /// Thrown if parameter is already declared.
 class ParameterAlreadyDeclaredException : public std::runtime_error
 {
@@ -289,7 +308,6 @@ public:
   /// Construct an instance.
   /**
    * \param[in] name the name of the parameter.
-   * \param[in] message custom exception message.
    */
   explicit ParameterUninitializedException(const std::string & name)
   : std::runtime_error("parameter '" + name + "' is not initialized")

rclcpp/include/rclcpp/executor.hpp

@@ -29,6 +29,7 @@
 
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
+#include "rcpputils/scope_exit.hpp"
 
 #include "rclcpp/context.hpp"
 #include "rclcpp/contexts/default_context.hpp"
@@ -40,7 +41,6 @@
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
-#include "rclcpp/scope_exit.hpp"
 
 namespace rclcpp
 {
@@ -305,7 +305,9 @@ public:
    * If the time that waitables take to be executed is longer than the period on which new waitables
    * become ready, this method will execute work repeatedly until `max_duration` has elapsed.
    *
-   * \param[in] max_duration The maximum amount of time to spend executing work. Must be positive.
+   * \param[in] max_duration The maximum amount of time to spend executing work, must be >= 0.
+   *   `0` is potentially block forever until no more work is available.
+   * \throw std::invalid_argument if max_duration is less than 0.
    * Note that spin_all() may take longer than this time as it only returns once max_duration has
    * been exceeded.
    */
@@ -354,7 +356,7 @@ public:
     if (spinning.exchange(true)) {
       throw std::runtime_error("spin_until_future_complete() called while already spinning");
     }
-    RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
+    RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
     while (rclcpp::ok(this->context_) && spinning.load()) {
       // Do one item of work.
       spin_once_impl(timeout_left);
@@ -401,6 +403,15 @@ 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.
+   * \return True if the executor is currently spinning.
+   */
+  RCLCPP_PUBLIC
+  bool
+  is_spinning();
+
 protected:
   RCLCPP_PUBLIC
   void
@@ -455,6 +466,7 @@ protected:
   /// 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
@@ -525,7 +537,7 @@ protected:
   std::atomic_bool spinning;
 
   /// Guard condition for signaling the rmw layer to wake up for special events.
-  rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
+  rclcpp::GuardCondition interrupt_guard_condition_;
 
   std::shared_ptr<rclcpp::GuardCondition> shutdown_guard_condition_;
 
@@ -548,14 +560,14 @@ protected:
   virtual void
   spin_once_impl(std::chrono::nanoseconds timeout);
 
-  typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
-      const rcl_guard_condition_t *,
-      std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
-    WeakNodesToGuardConditionsMap;
+  typedef std::map<rclcpp::CallbackGroup::WeakPtr,
+      const rclcpp::GuardCondition *,
+      std::owner_less<rclcpp::CallbackGroup::WeakPtr>>
+    WeakCallbackGroupsToGuardConditionsMap;
 
-  /// maps nodes to guard conditions
-  WeakNodesToGuardConditionsMap
-  weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+  /// maps callback groups to guard conditions
+  WeakCallbackGroupsToGuardConditionsMap
+  weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
   /// maps callback groups associated to nodes
   WeakCallbackGroupsToNodesMap

rclcpp/include/rclcpp/executors.hpp

@@ -107,7 +107,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/multi_threaded_executor.hpp

@@ -22,7 +22,6 @@
 #include <thread>
 #include <unordered_map>
 
-#include "rclcpp/detail/mutex_two_priorities.hpp"
 #include "rclcpp/executor.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/memory_strategies.hpp"
@@ -53,7 +52,7 @@ public:
    * \param timeout maximum time to wait
    */
   RCLCPP_PUBLIC
-  MultiThreadedExecutor(
+  explicit MultiThreadedExecutor(
     const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions(),
     size_t number_of_threads = 0,
     bool yield_before_execute = false,
@@ -82,12 +81,10 @@ protected:
 private:
   RCLCPP_DISABLE_COPY(MultiThreadedExecutor)
 
-  detail::MutexTwoPriorities wait_mutex_;
+  std::mutex wait_mutex_;
   size_t number_of_threads_;
   bool yield_before_execute_;
   std::chrono::nanoseconds next_exec_timeout_;
-
-  std::set<TimerBase::SharedPtr> scheduled_timers_;
 };
 
 }  // namespace executors

rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp

@@ -57,15 +57,13 @@ public:
   /**
    * \param p_wait_set A reference to the wait set to be used in the executor
    * \param memory_strategy Shared pointer to the memory strategy to set.
-   * \param executor_guard_condition executor's guard condition
    * \throws std::runtime_error if memory strategy is null
    */
   RCLCPP_PUBLIC
   void
   init(
     rcl_wait_set_t * p_wait_set,
-    rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy,
-    rcl_guard_condition_t * executor_guard_condition);
+    rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy);
 
   /// Finalize StaticExecutorEntitiesCollector to clear resources
   RCLCPP_PUBLIC
@@ -104,7 +102,7 @@ public:
    * \throws std::runtime_error if it couldn't add guard condition to wait set
    */
   RCLCPP_PUBLIC
-  bool
+  void
   add_to_wait_set(rcl_wait_set_t * wait_set) override;
 
   RCLCPP_PUBLIC
@@ -301,7 +299,8 @@ private:
 
   /// Return true if the node belongs to the collector
   /**
-   * \param[in] group_ptr a node base interface shared pointer
+   * \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
@@ -330,7 +329,7 @@ private:
   WeakCallbackGroupsToNodesMap weak_groups_to_nodes_associated_with_executor_;
 
   typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
-      const rcl_guard_condition_t *,
+      const rclcpp::GuardCondition *,
       std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
     WeakNodesToGuardConditionsMap;
   WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_;
@@ -338,6 +337,10 @@ private:
   /// 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;
 

rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp

@@ -99,9 +99,10 @@ public:
 
   /// Static executor implementation of spin all
   /**
-   * This non-blocking function will execute entities until
-   * timeout or no more work available. If new entities get ready
-   * while executing work available, they will be executed
+   * This non-blocking function will execute entities until timeout (must be >= 0)
+   * or no more work available.
+   * If timeout is `0`, potentially it blocks forever until no more work is available.
+   * If new entities get ready while executing work available, they will be executed
    * as long as the timeout hasn't expired.
    *
    * Example:

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

@@ -16,6 +16,7 @@
 #define RCLCPP__EXPERIMENTAL__BUFFERS__INTRA_PROCESS_BUFFER_HPP_
 
 #include <memory>
+#include <stdexcept>
 #include <type_traits>
 #include <utility>
 

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

@@ -15,10 +15,6 @@
 #ifndef RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_
 #define RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_
 
-#include <algorithm>
-#include <cstddef>
-#include <cstdint>
-#include <memory>
 #include <mutex>
 #include <stdexcept>
 #include <utility>
@@ -90,8 +86,7 @@ public:
     std::lock_guard<std::mutex> lock(mutex_);
 
     if (!has_data_()) {
-      RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Calling dequeue on empty intra-process buffer");
-      throw std::runtime_error("Calling dequeue on empty intra-process buffer");
+      return BufferT();
     }
 
     auto request = std::move(ring_buffer_[read_index_]);

rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp

@@ -16,11 +16,9 @@
 #define RCLCPP__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_
 
 #include <memory>
-#include <type_traits>
+#include <stdexcept>
 #include <utility>
 
-#include "rcl/subscription.h"
-
 #include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
 #include "rclcpp/experimental/buffers/ring_buffer_implementation.hpp"
 #include "rclcpp/intra_process_buffer_type.hpp"

rclcpp/include/rclcpp/experimental/executable_list.hpp

@@ -15,7 +15,6 @@
 #ifndef RCLCPP__EXPERIMENTAL__EXECUTABLE_LIST_HPP_
 #define RCLCPP__EXPERIMENTAL__EXECUTABLE_LIST_HPP_
 
-#include <memory>
 #include <vector>
 
 #include "rclcpp/client.hpp"

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

@@ -19,18 +19,16 @@
 
 #include <shared_mutex>
 
-#include <algorithm>
-#include <atomic>
-#include <cstdint>
-#include <exception>
-#include <map>
+#include <iterator>
 #include <memory>
-#include <string>
+#include <stdexcept>
 #include <unordered_map>
 #include <utility>
 #include <vector>
+#include <typeinfo>
 
 #include "rclcpp/allocator/allocator_deleter.hpp"
+#include "rclcpp/experimental/ros_message_intra_process_buffer.hpp"
 #include "rclcpp/experimental/subscription_intra_process.hpp"
 #include "rclcpp/experimental/subscription_intra_process_base.hpp"
 #include "rclcpp/experimental/subscription_intra_process_buffer.hpp"
@@ -38,6 +36,7 @@
 #include "rclcpp/logging.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/publisher_base.hpp"
+#include "rclcpp/type_adapter.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -173,11 +172,14 @@ public:
    *
    * \param intra_process_publisher_id the id of the publisher of this message.
    * \param message the message that is being stored.
+   * \param allocator for allocations when buffering messages.
    */
   template<
     typename MessageT,
-    typename Alloc = std::allocator<void>,
-    typename Deleter = std::default_delete<MessageT>>
+    typename ROSMessageType,
+    typename Alloc,
+    typename Deleter = std::default_delete<MessageT>
+  >
   void
   do_intra_process_publish(
     uint64_t intra_process_publisher_id,
@@ -203,7 +205,7 @@ public:
       // None of the buffers require ownership, so we promote the pointer
       std::shared_ptr<MessageT> msg = std::move(message);
 
-      this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
+      this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
         msg, sub_ids.take_shared_subscriptions);
     } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT
       sub_ids.take_shared_subscriptions.size() <= 1)
@@ -217,8 +219,7 @@ public:
         concatenated_vector.end(),
         sub_ids.take_ownership_subscriptions.begin(),
         sub_ids.take_ownership_subscriptions.end());
-
-      this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter>(
+      this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
         std::move(message),
         concatenated_vector,
         allocator);
@@ -229,17 +230,19 @@ public:
       // for the buffers that do not require ownership
       auto shared_msg = std::allocate_shared<MessageT, MessageAllocatorT>(allocator, *message);
 
-      this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
+      this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
         shared_msg, sub_ids.take_shared_subscriptions);
-      this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter>(
+      this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
         std::move(message), sub_ids.take_ownership_subscriptions, allocator);
     }
   }
 
   template<
     typename MessageT,
-    typename Alloc = std::allocator<void>,
-    typename Deleter = std::default_delete<MessageT>>
+    typename ROSMessageType,
+    typename Alloc,
+    typename Deleter = std::default_delete<MessageT>
+  >
   std::shared_ptr<const MessageT>
   do_intra_process_publish_and_return_shared(
     uint64_t intra_process_publisher_id,
@@ -265,7 +268,7 @@ public:
       // If there are no owning, just convert to shared.
       std::shared_ptr<MessageT> shared_msg = std::move(message);
       if (!sub_ids.take_shared_subscriptions.empty()) {
-        this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
+        this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
           shared_msg, sub_ids.take_shared_subscriptions);
       }
       return shared_msg;
@@ -275,17 +278,16 @@ public:
       auto shared_msg = std::allocate_shared<MessageT, MessageAllocatorT>(allocator, *message);
 
       if (!sub_ids.take_shared_subscriptions.empty()) {
-        this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
+        this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
           shared_msg,
           sub_ids.take_shared_subscriptions);
       }
       if (!sub_ids.take_ownership_subscriptions.empty()) {
-        this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter>(
+        this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
           std::move(message),
           sub_ids.take_ownership_subscriptions,
           allocator);
       }
-
       return shared_msg;
     }
   }
@@ -338,41 +340,83 @@ private:
   template<
     typename MessageT,
     typename Alloc,
-    typename Deleter>
+    typename Deleter,
+    typename ROSMessageType>
   void
   add_shared_msg_to_buffers(
     std::shared_ptr<const MessageT> message,
     std::vector<uint64_t> subscription_ids)
   {
+    using ROSMessageTypeAllocatorTraits = allocator::AllocRebind<ROSMessageType, Alloc>;
+    using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
+    using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>;
+
+    using PublishedType = typename rclcpp::TypeAdapter<MessageT>::custom_type;
+    using PublishedTypeAllocatorTraits = allocator::AllocRebind<PublishedType, Alloc>;
+    using PublishedTypeAllocator = typename PublishedTypeAllocatorTraits::allocator_type;
+    using PublishedTypeDeleter = allocator::Deleter<PublishedTypeAllocator, PublishedType>;
+
     for (auto id : subscription_ids) {
       auto subscription_it = subscriptions_.find(id);
       if (subscription_it == subscriptions_.end()) {
         throw std::runtime_error("subscription has unexpectedly gone out of scope");
       }
       auto subscription_base = subscription_it->second.lock();
-      if (subscription_base) {
-        auto subscription = std::dynamic_pointer_cast<
-          rclcpp::experimental::SubscriptionIntraProcessBuffer<MessageT, Alloc, Deleter>
-          >(subscription_base);
-        if (nullptr == subscription) {
-          throw std::runtime_error(
-                  "failed to dynamic cast SubscriptionIntraProcessBase to "
-                  "SubscriptionIntraProcessBuffer<MessageT, Alloc, Deleter>, which "
-                  "can happen when the publisher and subscription use different "
-                  "allocator types, which is not supported");
-        }
-
-        subscription->provide_intra_process_message(message);
-      } else {
+      if (subscription_base == nullptr) {
         subscriptions_.erase(id);
+        continue;
+      }
+
+      auto subscription = std::dynamic_pointer_cast<
+        rclcpp::experimental::SubscriptionIntraProcessBuffer<PublishedType,
+        PublishedTypeAllocator, PublishedTypeDeleter, ROSMessageType>
+        >(subscription_base);
+      if (subscription != nullptr) {
+        subscription->provide_intra_process_data(message);
+        continue;
+      }
+
+      auto ros_message_subscription = std::dynamic_pointer_cast<
+        rclcpp::experimental::SubscriptionROSMsgIntraProcessBuffer<ROSMessageType,
+        ROSMessageTypeAllocator, ROSMessageTypeDeleter>
+        >(subscription_base);
+      if (nullptr == ros_message_subscription) {
+        throw std::runtime_error(
+                "failed to dynamic cast SubscriptionIntraProcessBase to "
+                "SubscriptionIntraProcessBuffer<MessageT, Alloc, Deleter>, or to "
+                "SubscriptionROSMsgIntraProcessBuffer<ROSMessageType,ROSMessageTypeAllocator,"
+                "ROSMessageTypeDeleter> which can happen when the publisher and "
+                "subscription use different allocator types, which is not supported");
+      }
+
+      if constexpr (rclcpp::TypeAdapter<MessageT>::is_specialized::value) {
+        ROSMessageType ros_msg;
+        rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*message, ros_msg);
+        ros_message_subscription->provide_intra_process_message(
+          std::make_shared<ROSMessageType>(ros_msg));
+      } else {
+        if constexpr (std::is_same<MessageT, ROSMessageType>::value) {
+          ros_message_subscription->provide_intra_process_message(message);
+        } else {
+          if constexpr (std::is_same<typename rclcpp::TypeAdapter<MessageT,
+            ROSMessageType>::ros_message_type, ROSMessageType>::value)
+          {
+            ROSMessageType ros_msg;
+            rclcpp::TypeAdapter<MessageT, ROSMessageType>::convert_to_ros_message(
+              *message, ros_msg);
+            ros_message_subscription->provide_intra_process_message(
+              std::make_shared<ROSMessageType>(ros_msg));
+          }
+        }
       }
     }
   }
 
   template<
     typename MessageT,
-    typename Alloc = std::allocator<void>,
-    typename Deleter = std::default_delete<MessageT>>
+    typename Alloc,
+    typename Deleter,
+    typename ROSMessageType>
   void
   add_owned_msg_to_buffers(
     std::unique_ptr<MessageT, Deleter> message,
@@ -382,39 +426,88 @@ private:
     using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
     using MessageUniquePtr = std::unique_ptr<MessageT, Deleter>;
 
+    using ROSMessageTypeAllocatorTraits = allocator::AllocRebind<ROSMessageType, Alloc>;
+    using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
+    using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>;
+
+    using PublishedType = typename rclcpp::TypeAdapter<MessageT>::custom_type;
+    using PublishedTypeAllocatorTraits = allocator::AllocRebind<PublishedType, Alloc>;
+    using PublishedTypeAllocator = typename PublishedTypeAllocatorTraits::allocator_type;
+    using PublishedTypeDeleter = allocator::Deleter<PublishedTypeAllocator, PublishedType>;
+
     for (auto it = subscription_ids.begin(); it != subscription_ids.end(); it++) {
       auto subscription_it = subscriptions_.find(*it);
       if (subscription_it == subscriptions_.end()) {
         throw std::runtime_error("subscription has unexpectedly gone out of scope");
       }
       auto subscription_base = subscription_it->second.lock();
-      if (subscription_base) {
-        auto subscription = std::dynamic_pointer_cast<
-          rclcpp::experimental::SubscriptionIntraProcessBuffer<MessageT, Alloc, Deleter>
-          >(subscription_base);
-        if (nullptr == subscription) {
-          throw std::runtime_error(
-                  "failed to dynamic cast SubscriptionIntraProcessBase to "
-                  "SubscriptionIntraProcessBuffer<MessageT, Alloc, Deleter>, which "
-                  "can happen when the publisher and subscription use different "
-                  "allocator types, which is not supported");
-        }
+      if (subscription_base == nullptr) {
+        subscriptions_.erase(subscription_it);
+        continue;
+      }
 
+      auto subscription = std::dynamic_pointer_cast<
+        rclcpp::experimental::SubscriptionIntraProcessBuffer<PublishedType,
+        PublishedTypeAllocator, PublishedTypeDeleter, ROSMessageType>
+        >(subscription_base);
+      if (subscription != nullptr) {
         if (std::next(it) == subscription_ids.end()) {
           // If this is the last subscription, give up ownership
-          subscription->provide_intra_process_message(std::move(message));
+          subscription->provide_intra_process_data(std::move(message));
+          // Last message delivered, break from for loop
+          break;
         } else {
           // Copy the message since we have additional subscriptions to serve
-          MessageUniquePtr copy_message;
           Deleter deleter = message.get_deleter();
           auto ptr = MessageAllocTraits::allocate(allocator, 1);
           MessageAllocTraits::construct(allocator, ptr, *message);
-          copy_message = MessageUniquePtr(ptr, deleter);
 
-          subscription->provide_intra_process_message(std::move(copy_message));
+          subscription->provide_intra_process_data(std::move(MessageUniquePtr(ptr, deleter)));
         }
+
+        continue;
+      }
+
+      auto ros_message_subscription = std::dynamic_pointer_cast<
+        rclcpp::experimental::SubscriptionROSMsgIntraProcessBuffer<ROSMessageType,
+        ROSMessageTypeAllocator, ROSMessageTypeDeleter>
+        >(subscription_base);
+      if (nullptr == ros_message_subscription) {
+        throw std::runtime_error(
+                "failed to dynamic cast SubscriptionIntraProcessBase to "
+                "SubscriptionIntraProcessBuffer<MessageT, Alloc, Deleter>, or to "
+                "SubscriptionROSMsgIntraProcessBuffer<ROSMessageType,ROSMessageTypeAllocator,"
+                "ROSMessageTypeDeleter> which can happen when the publisher and "
+                "subscription use different allocator types, which is not supported");
+      }
+
+      if constexpr (rclcpp::TypeAdapter<MessageT, ROSMessageType>::is_specialized::value) {
+        ROSMessageTypeAllocator ros_message_alloc(allocator);
+        auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_alloc, 1);
+        ROSMessageTypeAllocatorTraits::construct(ros_message_alloc, ptr);
+        ROSMessageTypeDeleter deleter;
+        allocator::set_allocator_for_deleter(&deleter, &allocator);
+        rclcpp::TypeAdapter<MessageT, ROSMessageType>::convert_to_ros_message(*message, *ptr);
+        auto ros_msg = std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>(ptr, deleter);
+        ros_message_subscription->provide_intra_process_message(std::move(ros_msg));
       } else {
-        subscriptions_.erase(subscription_it);
+        if constexpr (std::is_same<MessageT, ROSMessageType>::value) {
+          if (std::next(it) == subscription_ids.end()) {
+            // If this is the last subscription, give up ownership
+            ros_message_subscription->provide_intra_process_message(std::move(message));
+            // Last message delivered, break from for loop
+            break;
+          } else {
+            // Copy the message since we have additional subscriptions to serve
+            Deleter deleter = message.get_deleter();
+            allocator::set_allocator_for_deleter(&deleter, &allocator);
+            auto ptr = MessageAllocTraits::allocate(allocator, 1);
+            MessageAllocTraits::construct(allocator, ptr, *message);
+
+            ros_message_subscription->provide_intra_process_message(
+              std::move(MessageUniquePtr(ptr, deleter)));
+          }
+        }
       }
     }
   }

rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp

@@ -0,0 +1,68 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_
+#define RCLCPP__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rcl/error_handling.h"
+
+#include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/context.hpp"
+#include "rclcpp/experimental/subscription_intra_process_base.hpp"
+#include "tracetools/tracetools.h"
+
+namespace rclcpp
+{
+namespace experimental
+{
+
+template<
+  typename RosMessageT,
+  typename Alloc = std::allocator<void>,
+  typename Deleter = std::default_delete<void>
+>
+class SubscriptionROSMsgIntraProcessBuffer : public SubscriptionIntraProcessBase
+{
+public:
+  using ROSMessageTypeAllocatorTraits = allocator::AllocRebind<RosMessageT, Alloc>;
+  using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
+  using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, RosMessageT>;
+
+  using ConstMessageSharedPtr = std::shared_ptr<const RosMessageT>;
+  using MessageUniquePtr = std::unique_ptr<RosMessageT, ROSMessageTypeDeleter>;
+
+  SubscriptionROSMsgIntraProcessBuffer(
+    rclcpp::Context::SharedPtr context,
+    const std::string & topic_name,
+    const rclcpp::QoS & qos_profile)
+  : SubscriptionIntraProcessBase(context, topic_name, qos_profile)
+  {}
+
+  virtual ~SubscriptionROSMsgIntraProcessBuffer()
+  {}
+
+  virtual void
+  provide_intra_process_message(ConstMessageSharedPtr message) = 0;
+
+  virtual void
+  provide_intra_process_message(MessageUniquePtr message) = 0;
+};
+
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -15,25 +15,22 @@
 #ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_
 #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_
 
-#include <rmw/rmw.h>
+#include <rmw/types.h>
 
-#include <functional>
-#include <map>
 #include <memory>
 #include <stdexcept>
 #include <string>
+#include <type_traits>
 #include <utility>
 
-#include "rcl/error_handling.h"
+#include "rcl/types.h"
 
 #include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/context.hpp"
 #include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
-#include "rclcpp/experimental/create_intra_process_buffer.hpp"
-#include "rclcpp/experimental/subscription_intra_process_base.hpp"
 #include "rclcpp/experimental/subscription_intra_process_buffer.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
-#include "rclcpp/waitable.hpp"
 #include "tracetools/tracetools.h"
 
 namespace rclcpp
@@ -43,40 +40,47 @@ namespace experimental
 
 template<
   typename MessageT,
-  typename Alloc = std::allocator<void>,
-  typename Deleter = std::default_delete<MessageT>,
-  typename CallbackMessageT = MessageT>
+  typename SubscribedType,
+  typename SubscribedTypeAlloc = std::allocator<SubscribedType>,
+  typename SubscribedTypeDeleter = std::default_delete<SubscribedType>,
+  typename ROSMessageType = SubscribedType,
+  typename Alloc = std::allocator<void>
+>
 class SubscriptionIntraProcess
   : public SubscriptionIntraProcessBuffer<
-    MessageT,
-    Alloc,
-    Deleter
+    SubscribedType,
+    SubscribedTypeAlloc,
+    SubscribedTypeDeleter,
+    ROSMessageType
   >
 {
   using SubscriptionIntraProcessBufferT = SubscriptionIntraProcessBuffer<
-    MessageT,
-    Alloc,
-    Deleter
+    SubscribedType,
+    SubscribedTypeAlloc,
+    SubscribedTypeDeleter,
+    ROSMessageType
   >;
 
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess)
 
-  using MessageAllocTraits = typename SubscriptionIntraProcessBufferT::MessageAllocTraits;
-  using MessageAlloc = typename SubscriptionIntraProcessBufferT::MessageAlloc;
-  using ConstMessageSharedPtr = typename SubscriptionIntraProcessBufferT::ConstMessageSharedPtr;
-  using MessageUniquePtr = typename SubscriptionIntraProcessBufferT::MessageUniquePtr;
+  using MessageAllocTraits =
+    typename SubscriptionIntraProcessBufferT::SubscribedTypeAllocatorTraits;
+  using MessageAlloc = typename SubscriptionIntraProcessBufferT::SubscribedTypeAllocator;
+  using ConstMessageSharedPtr = typename SubscriptionIntraProcessBufferT::ConstDataSharedPtr;
+  using MessageUniquePtr = typename SubscriptionIntraProcessBufferT::SubscribedTypeUniquePtr;
   using BufferUniquePtr = typename SubscriptionIntraProcessBufferT::BufferUniquePtr;
 
   SubscriptionIntraProcess(
-    AnySubscriptionCallback<CallbackMessageT, Alloc> callback,
+    AnySubscriptionCallback<MessageT, Alloc> callback,
     std::shared_ptr<Alloc> allocator,
     rclcpp::Context::SharedPtr context,
     const std::string & topic_name,
     const rclcpp::QoS & qos_profile,
     rclcpp::IntraProcessBufferType buffer_type)
-  : SubscriptionIntraProcessBuffer<MessageT, Alloc, Deleter>(
-      allocator,
+  : SubscriptionIntraProcessBuffer<SubscribedType, SubscribedTypeAlloc,
+      SubscribedTypeDeleter, ROSMessageType>(
+      std::make_shared<SubscribedTypeAlloc>(*allocator),
       context,
       topic_name,
       qos_profile,
@@ -98,16 +102,29 @@ public:
   virtual ~SubscriptionIntraProcess() = default;
 
   std::shared_ptr<void>
-  take_data()
+  take_data() override
   {
     ConstMessageSharedPtr shared_msg;
     MessageUniquePtr unique_msg;
 
     if (any_callback_.use_take_shared_method()) {
       shared_msg = this->buffer_->consume_shared();
+      if (!shared_msg) {
+        return nullptr;
+      }
     } else {
       unique_msg = this->buffer_->consume_unique();
+      if (!unique_msg) {
+        return nullptr;
+      }
     }
+
+    if (this->buffer_->has_data()) {
+      // If there is data still to be processed, indicate to the
+      // executor or waitset by triggering the guard condition.
+      this->trigger_guard_condition();
+    }
+
     return std::static_pointer_cast<void>(
       std::make_shared<std::pair<ConstMessageSharedPtr, MessageUniquePtr>>(
         std::pair<ConstMessageSharedPtr, MessageUniquePtr>(
@@ -115,9 +132,9 @@ public:
     );
   }
 
-  void execute(std::shared_ptr<void> & data)
+  void execute(std::shared_ptr<void> & data) override
   {
-    execute_impl<MessageT>(data);
+    execute_impl<SubscribedType>(data);
   }
 
 protected:
@@ -134,7 +151,7 @@ protected:
   execute_impl(std::shared_ptr<void> & data)
   {
     if (!data) {
-      throw std::runtime_error("'data' is empty");
+      return;
     }
 
     rmw_message_info_t msg_info;
@@ -154,7 +171,7 @@ protected:
     shared_ptr.reset();
   }
 
-  AnySubscriptionCallback<CallbackMessageT, Alloc> any_callback_;
+  AnySubscriptionCallback<MessageT, Alloc> any_callback_;
 };
 
 }  // namespace experimental

rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp

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

rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp

@@ -15,25 +15,21 @@
 #ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_
 #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_
 
-#include <rmw/rmw.h>
-
-#include <functional>
-#include <map>
 #include <memory>
-#include <stdexcept>
 #include <string>
+#include <stdexcept>
 #include <utility>
 
 #include "rcl/error_handling.h"
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
 
-#include "rclcpp/any_subscription_callback.hpp"
 #include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
 #include "rclcpp/experimental/create_intra_process_buffer.hpp"
 #include "rclcpp/experimental/subscription_intra_process_base.hpp"
+#include "rclcpp/experimental/ros_message_intra_process_buffer.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
-#include "rclcpp/waitable.hpp"
-#include "tracetools/tracetools.h"
 
 namespace rclcpp
 {
@@ -41,24 +37,39 @@ namespace experimental
 {
 
 template<
-  typename MessageT,
-  typename Alloc = std::allocator<void>,
-  typename Deleter = std::default_delete<MessageT>
+  typename SubscribedType,
+  typename Alloc = std::allocator<SubscribedType>,
+  typename Deleter = std::default_delete<SubscribedType>,
+  /// MessageT::ros_message_type if MessageT is a TypeAdapter,
+  /// otherwise just MessageT.
+  typename ROSMessageType = SubscribedType
 >
-class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase
+class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuffer<ROSMessageType,
+    typename allocator::AllocRebind<ROSMessageType, Alloc>::allocator_type,
+    allocator::Deleter<typename allocator::AllocRebind<ROSMessageType, Alloc>::allocator_type,
+    ROSMessageType>>
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcessBuffer)
 
-  using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
-  using MessageAlloc = typename MessageAllocTraits::allocator_type;
-  using ConstMessageSharedPtr = std::shared_ptr<const MessageT>;
-  using MessageUniquePtr = std::unique_ptr<MessageT, Deleter>;
+  using SubscribedTypeAllocatorTraits = allocator::AllocRebind<SubscribedType, Alloc>;
+  using SubscribedTypeAllocator = typename SubscribedTypeAllocatorTraits::allocator_type;
+  using SubscribedTypeDeleter = allocator::Deleter<SubscribedTypeAllocator, SubscribedType>;
+
+  using ROSMessageTypeAllocatorTraits = allocator::AllocRebind<ROSMessageType, Alloc>;
+  using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
+  using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>;
+
+  using ConstMessageSharedPtr = std::shared_ptr<const ROSMessageType>;
+  using MessageUniquePtr = std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>;
+
+  using ConstDataSharedPtr = std::shared_ptr<const SubscribedType>;
+  using SubscribedTypeUniquePtr = std::unique_ptr<SubscribedType, SubscribedTypeDeleter>;
 
   using BufferUniquePtr = typename rclcpp::experimental::buffers::IntraProcessBuffer<
-    MessageT,
+    SubscribedType,
     Alloc,
-    Deleter
+    SubscribedTypeDeleter
     >::UniquePtr;
 
   SubscriptionIntraProcessBuffer(
@@ -67,74 +78,101 @@ public:
     const std::string & topic_name,
     const rclcpp::QoS & qos_profile,
     rclcpp::IntraProcessBufferType buffer_type)
-  : SubscriptionIntraProcessBase(topic_name, qos_profile)
+  : SubscriptionROSMsgIntraProcessBuffer<ROSMessageType, ROSMessageTypeAllocator,
+      ROSMessageTypeDeleter>(
+      context, topic_name, qos_profile),
+    subscribed_type_allocator_(*allocator)
   {
+    allocator::set_allocator_for_deleter(&subscribed_type_deleter_, &subscribed_type_allocator_);
+
     // Create the intra-process buffer.
-    buffer_ = rclcpp::experimental::create_intra_process_buffer<MessageT, Alloc, Deleter>(
+    buffer_ = rclcpp::experimental::create_intra_process_buffer<SubscribedType, Alloc,
+        SubscribedTypeDeleter>(
       buffer_type,
       qos_profile,
-      allocator);
-
-    // Create the guard condition.
-    rcl_guard_condition_options_t guard_condition_options =
-      rcl_guard_condition_get_default_options();
-
-    gc_ = rcl_get_zero_initialized_guard_condition();
-    rcl_ret_t ret = rcl_guard_condition_init(
-      &gc_, context->get_rcl_context().get(), guard_condition_options);
-
-    if (RCL_RET_OK != ret) {
-      throw std::runtime_error(
-              "SubscriptionIntraProcessBuffer init error initializing guard condition");
-    }
-  }
-
-  virtual ~SubscriptionIntraProcessBuffer()
-  {
-    if (rcl_guard_condition_fini(&gc_) != RCL_RET_OK) {
-      RCUTILS_LOG_ERROR_NAMED(
-        "rclcpp",
-        "Failed to destroy guard condition: %s",
-        rcutils_get_error_string().str);
-    }
+      std::make_shared<Alloc>(subscribed_type_allocator_));
   }
 
   bool
-  is_ready(rcl_wait_set_t * wait_set)
+  is_ready(rcl_wait_set_t * wait_set) override
   {
     (void) wait_set;
     return buffer_->has_data();
   }
 
+  SubscribedTypeUniquePtr
+  convert_ros_message_to_subscribed_type_unique_ptr(const ROSMessageType & msg)
+  {
+    if constexpr (!std::is_same<SubscribedType, ROSMessageType>::value) {
+      auto ptr = SubscribedTypeAllocatorTraits::allocate(subscribed_type_allocator_, 1);
+      SubscribedTypeAllocatorTraits::construct(subscribed_type_allocator_, ptr);
+      rclcpp::TypeAdapter<SubscribedType, ROSMessageType>::convert_to_custom(msg, *ptr);
+      return SubscribedTypeUniquePtr(ptr, subscribed_type_deleter_);
+    } else {
+      throw std::runtime_error(
+              "convert_ros_message_to_subscribed_type_unique_ptr "
+              "unexpectedly called without TypeAdapter");
+    }
+  }
+
   void
-  provide_intra_process_message(ConstMessageSharedPtr message)
+  provide_intra_process_message(ConstMessageSharedPtr message) override
+  {
+    if constexpr (std::is_same<SubscribedType, ROSMessageType>::value) {
+      buffer_->add_shared(std::move(message));
+      trigger_guard_condition();
+    } else {
+      buffer_->add_shared(convert_ros_message_to_subscribed_type_unique_ptr(*message));
+      trigger_guard_condition();
+    }
+    this->invoke_on_new_message();
+  }
+
+  void
+  provide_intra_process_message(MessageUniquePtr message) override
+  {
+    if constexpr (std::is_same<SubscribedType, ROSMessageType>::value) {
+      buffer_->add_unique(std::move(message));
+      trigger_guard_condition();
+    } else {
+      buffer_->add_unique(convert_ros_message_to_subscribed_type_unique_ptr(*message));
+      trigger_guard_condition();
+    }
+    this->invoke_on_new_message();
+  }
+
+  void
+  provide_intra_process_data(ConstDataSharedPtr message)
   {
     buffer_->add_shared(std::move(message));
     trigger_guard_condition();
+    this->invoke_on_new_message();
   }
 
   void
-  provide_intra_process_message(MessageUniquePtr message)
+  provide_intra_process_data(SubscribedTypeUniquePtr message)
   {
     buffer_->add_unique(std::move(message));
     trigger_guard_condition();
+    this->invoke_on_new_message();
   }
 
   bool
-  use_take_shared_method() const
+  use_take_shared_method() const override
   {
     return buffer_->use_take_shared_method();
   }
 
 protected:
   void
-  trigger_guard_condition()
+  trigger_guard_condition() override
   {
-    rcl_ret_t ret = rcl_trigger_guard_condition(&gc_);
-    (void)ret;
+    this->gc_.trigger();
   }
 
   BufferUniquePtr buffer_;
+  SubscribedTypeAllocator subscribed_type_allocator_;
+  SubscribedTypeDeleter subscribed_type_deleter_;
 };
 
 }  // namespace experimental

rclcpp/include/rclcpp/function_traits.hpp

@@ -80,10 +80,12 @@ struct function_traits<ReturnTypeT (*)(Args ...)>: function_traits<ReturnTypeT(A
 
 // std::bind for object methods
 template<typename ClassT, typename ReturnTypeT, typename ... Args, typename ... FArgs>
-#if defined _LIBCPP_VERSION  // libc++ (Clang)
+#if defined DOXYGEN_ONLY
+struct function_traits<std::bind<ReturnTypeT (ClassT::*)(Args ...), FArgs ...>>
+#elif defined _LIBCPP_VERSION  // libc++ (Clang)
 struct function_traits<std::__bind<ReturnTypeT (ClassT::*)(Args ...), FArgs ...>>
 #elif defined _GLIBCXX_RELEASE  // glibc++ (GNU C++ >= 7.1)
-struct function_traits<std::_Bind<ReturnTypeT(ClassT::*(FArgs ...))(Args ...)>>
+struct function_traits<std::_Bind<ReturnTypeT(ClassT::* (FArgs ...))(Args ...)>>
 #elif defined __GLIBCXX__  // glibc++ (GNU C++)
 struct function_traits<std::_Bind<std::_Mem_fn<ReturnTypeT (ClassT::*)(Args ...)>(FArgs ...)>>
 #elif defined _MSC_VER  // MS Visual Studio
@@ -97,10 +99,12 @@ struct function_traits<
 
 // std::bind for object const methods
 template<typename ClassT, typename ReturnTypeT, typename ... Args, typename ... FArgs>
-#if defined _LIBCPP_VERSION  // libc++ (Clang)
+#if defined DOXYGEN_ONLY
+struct function_traits<std::bind<ReturnTypeT (ClassT::*)(Args ...) const, FArgs ...>>
+#elif defined _LIBCPP_VERSION  // libc++ (Clang)
 struct function_traits<std::__bind<ReturnTypeT (ClassT::*)(Args ...) const, FArgs ...>>
 #elif defined _GLIBCXX_RELEASE  // glibc++ (GNU C++ >= 7.1)
-struct function_traits<std::_Bind<ReturnTypeT(ClassT::*(FArgs ...))(Args ...) const>>
+struct function_traits<std::_Bind<ReturnTypeT(ClassT::* (FArgs ...))(Args ...) const>>
 #elif defined __GLIBCXX__  // glibc++ (GNU C++)
 struct function_traits<std::_Bind<std::_Mem_fn<ReturnTypeT (ClassT::*)(Args ...) const>(FArgs ...)>>
 #elif defined _MSC_VER  // MS Visual Studio
@@ -114,7 +118,9 @@ struct function_traits<
 
 // std::bind for free functions
 template<typename ReturnTypeT, typename ... Args, typename ... FArgs>
-#if defined _LIBCPP_VERSION  // libc++ (Clang)
+#if defined DOXYGEN_ONLY
+struct function_traits<std::bind<ReturnTypeT( &)(Args ...), FArgs ...>>
+#elif defined _LIBCPP_VERSION  // libc++ (Clang)
 struct function_traits<std::__bind<ReturnTypeT( &)(Args ...), FArgs ...>>
 #elif defined __GLIBCXX__  // glibc++ (GNU C++)
 struct function_traits<std::_Bind<ReturnTypeT(*(FArgs ...))(Args ...)>>

rclcpp/include/rclcpp/generic_publisher.hpp

@@ -31,6 +31,8 @@
 #include "rclcpp/typesupport_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 
+#include "rmw/rmw.h"
+
 namespace rclcpp
 {
 
@@ -60,7 +62,6 @@ public:
    * \param topic_name Topic name
    * \param topic_type Topic type
    * \param qos %QoS settings
-   * \param callback Callback for new messages of serialized form
    * \param options %Publisher options.
    * Not all publisher options are currently respected, the only relevant options for this
    * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`.
@@ -117,9 +118,24 @@ public:
   RCLCPP_PUBLIC
   void publish(const rclcpp::SerializedMessage & message);
 
+  /**
+   * Publish a rclcpp::SerializedMessage via loaned message after de-serialization.
+   *
+   * \param message a serialized message
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can show
+   */
+  RCLCPP_PUBLIC
+  void publish_as_loaned_msg(const rclcpp::SerializedMessage & message);
+
 private:
   // The type support library should stay loaded, so it is stored in the GenericPublisher
   std::shared_ptr<rcpputils::SharedLibrary> ts_lib_;
+
+  void * borrow_loaned_message();
+  void deserialize_message(
+    const rmw_serialized_message_t & serialized_message,
+    void * deserialized_msg);
+  void publish_loaned_message(void * loaned_message);
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/get_message_type_support_handle.hpp

@@ -28,7 +28,17 @@
 namespace rclcpp
 {
 
-/// Specialization for when MessageT is an actual ROS message type.
+#ifdef DOXYGEN_ONLY
+
+/// Returns the message type support for the given `MessageT` type.
+/**
+ * \tparam MessageT an actual ROS message type or an adapted type using `rclcpp::TypeAdapter`
+ */
+template<typename MessageT>
+constexpr const rosidl_message_type_support_t & get_message_type_support_handle();
+
+#else
+
 template<typename MessageT>
 constexpr
 typename std::enable_if_t<
@@ -44,7 +54,6 @@ get_message_type_support_handle()
   return *handle;
 }
 
-/// Specialization for when MessageT is an adapted type using rclcpp::TypeAdapter.
 template<typename AdaptedType>
 constexpr
 typename std::enable_if_t<
@@ -63,12 +72,9 @@ get_message_type_support_handle()
   return *handle;
 }
 
-/// Specialization for when MessageT is not a ROS message nor an adapted type.
-/**
- * This specialization is a pass through runtime check, which allows a better
- * static_assert to catch this issue further down the line.
- * This should never get to be called in practice, and is purely defensive.
- */
+// This specialization is a pass through runtime check, which allows a better
+// static_assert to catch this issue further down the line.
+// This should never get to be called in practice, and is purely defensive.
 template<
   typename AdaptedType
 >
@@ -85,6 +91,8 @@ get_message_type_support_handle()
           "should never be called");
 }
 
+#endif
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__GET_MESSAGE_TYPE_SUPPORT_HANDLE_HPP_

rclcpp/include/rclcpp/graph_listener.hpp

@@ -24,6 +24,7 @@
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
 #include "rclcpp/context.hpp"
+#include "rclcpp/guard_condition.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -187,7 +188,7 @@ private:
   mutable std::mutex node_graph_interfaces_mutex_;
   std::vector<rclcpp::node_interfaces::NodeGraphInterface *> node_graph_interfaces_;
 
-  rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
+  rclcpp::GuardCondition interrupt_guard_condition_;
   rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
 };
 

rclcpp/include/rclcpp/guard_condition.hpp

@@ -40,6 +40,8 @@ public:
    *   Defaults to using the global default context singleton.
    *   Shared ownership of the context is held with the guard condition until
    *   destruction.
+   * \param[in] guard_condition_options Optional guard condition options to be used.
+   *   Defaults to using the default guard condition options.
    * \throws std::invalid_argument if the context is nullptr.
    * \throws rclcpp::exceptions::RCLError based exceptions when underlying
    *   rcl functions fail.
@@ -47,7 +49,9 @@ public:
   RCLCPP_PUBLIC
   explicit GuardCondition(
     rclcpp::Context::SharedPtr context =
-    rclcpp::contexts::get_global_default_context());
+    rclcpp::contexts::get_global_default_context(),
+    rcl_guard_condition_options_t guard_condition_options =
+    rcl_guard_condition_get_default_options());
 
   RCLCPP_PUBLIC
   virtual
@@ -58,6 +62,11 @@ public:
   rclcpp::Context::SharedPtr
   get_context() const;
 
+  /// Return the underlying rcl guard condition structure.
+  RCLCPP_PUBLIC
+  rcl_guard_condition_t &
+  get_rcl_guard_condition();
+
   /// Return the underlying rcl guard condition structure.
   RCLCPP_PUBLIC
   const rcl_guard_condition_t &
@@ -89,10 +98,27 @@ public:
   bool
   exchange_in_use_by_wait_set_state(bool in_use_state);
 
+  /// Adds the guard condition to a waitset
+  /**
+   * This function is thread-safe.
+   * \param[in] wait_set pointer to a wait set where to add the guard condition
+   */
+  RCLCPP_PUBLIC
+  void
+  add_to_wait_set(rcl_wait_set_t * wait_set);
+
+  RCLCPP_PUBLIC
+  void
+  set_on_trigger_callback(std::function<void(size_t)> callback);
+
 protected:
   rclcpp::Context::SharedPtr context_;
   rcl_guard_condition_t rcl_guard_condition_;
   std::atomic<bool> in_use_by_wait_set_{false};
+  std::recursive_mutex reentrant_mutex_;
+  std::function<void(size_t)> on_trigger_callback_{nullptr};
+  size_t unread_count_{0};
+  rcl_wait_set_t * wait_set_{nullptr};
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/init_options.hpp

@@ -29,7 +29,7 @@ class InitOptions
 {
 public:
   /// If true, the context will be shutdown on SIGINT by the signal handler (if it was installed).
-  bool shutdown_on_sigint = true;
+  bool shutdown_on_signal = true;
 
   /// Constructor
   /**

rclcpp/include/rclcpp/loaned_message.hpp

@@ -117,7 +117,9 @@ public:
   : pub_(std::move(other.pub_)),
     message_(std::move(other.message_)),
     message_allocator_(std::move(other.message_allocator_))
-  {}
+  {
+    other.message_ = nullptr;
+  }
 
   /// Destructor of the LoanedMessage class.
   /**

rclcpp/include/rclcpp/logger.hpp

@@ -79,7 +79,7 @@ get_node_logger(const rcl_node_t * node);
 /// Get the current logging directory.
 /**
  * For more details of how the logging directory is determined,
- * see \ref rcl_logging_get_logging_directory.
+ * see rcl_logging_get_logging_directory().
  *
  * \returns the logging directory being used.
  * \throws rclcpp::exceptions::RCLError if an unexpected error occurs.

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -64,9 +64,11 @@ public:
   virtual void clear_handles() = 0;
   virtual void remove_null_handles(rcl_wait_set_t * wait_set) = 0;
 
-  virtual void add_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
+  virtual void
+  add_guard_condition(const rclcpp::GuardCondition & guard_condition) = 0;
 
-  virtual void remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
+  virtual void
+  remove_guard_condition(const rclcpp::GuardCondition * guard_condition) = 0;
 
   virtual void
   get_next_subscription(
@@ -98,52 +100,52 @@ public:
 
   static rclcpp::SubscriptionBase::SharedPtr
   get_subscription_by_handle(
-    std::shared_ptr<const rcl_subscription_t> subscriber_handle,
+    const std::shared_ptr<const rcl_subscription_t> & subscriber_handle,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::ServiceBase::SharedPtr
   get_service_by_handle(
-    std::shared_ptr<const rcl_service_t> service_handle,
+    const std::shared_ptr<const rcl_service_t> & service_handle,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::ClientBase::SharedPtr
   get_client_by_handle(
-    std::shared_ptr<const rcl_client_t> client_handle,
+    const std::shared_ptr<const rcl_client_t> & client_handle,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::TimerBase::SharedPtr
   get_timer_by_handle(
-    std::shared_ptr<const rcl_timer_t> timer_handle,
+    const std::shared_ptr<const rcl_timer_t> & timer_handle,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
   get_node_by_group(
-    rclcpp::CallbackGroup::SharedPtr group,
+    const rclcpp::CallbackGroup::SharedPtr & group,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::CallbackGroup::SharedPtr
   get_group_by_subscription(
-    rclcpp::SubscriptionBase::SharedPtr subscription,
+    const rclcpp::SubscriptionBase::SharedPtr & subscription,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::CallbackGroup::SharedPtr
   get_group_by_service(
-    rclcpp::ServiceBase::SharedPtr service,
+    const rclcpp::ServiceBase::SharedPtr & service,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::CallbackGroup::SharedPtr
   get_group_by_client(
-    rclcpp::ClientBase::SharedPtr client,
+    const rclcpp::ClientBase::SharedPtr & client,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::CallbackGroup::SharedPtr
   get_group_by_timer(
-    rclcpp::TimerBase::SharedPtr timer,
+    const rclcpp::TimerBase::SharedPtr & timer,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::CallbackGroup::SharedPtr
   get_group_by_waitable(
-    rclcpp::Waitable::SharedPtr waitable,
+    const rclcpp::Waitable::SharedPtr & waitable,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 };
 

rclcpp/include/rclcpp/message_memory_strategy.hpp

@@ -18,6 +18,7 @@
 #include <memory>
 #include <stdexcept>
 
+#include "rcl/allocator.h"
 #include "rcl/types.h"
 
 #include "rclcpp/allocator/allocator_common.hpp"
@@ -61,7 +62,12 @@ public:
     message_allocator_ = std::make_shared<MessageAlloc>();
     serialized_message_allocator_ = std::make_shared<SerializedMessageAlloc>();
     buffer_allocator_ = std::make_shared<BufferAlloc>();
-    rcutils_allocator_ = allocator::get_rcl_allocator<char, BufferAlloc>(*buffer_allocator_.get());
+    if constexpr (std::is_same_v<Alloc, std::allocator<void>>) {
+      rcutils_allocator_ = rcl_get_default_allocator();
+    } else {
+      rcutils_allocator_ = allocator::get_rcl_allocator<char,
+          BufferAlloc>(*buffer_allocator_.get());
+    }
   }
 
   explicit MessageMemoryStrategy(std::shared_ptr<Alloc> allocator)
@@ -69,7 +75,12 @@ public:
     message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
     serialized_message_allocator_ = std::make_shared<SerializedMessageAlloc>(*allocator.get());
     buffer_allocator_ = std::make_shared<BufferAlloc>(*allocator.get());
-    rcutils_allocator_ = allocator::get_rcl_allocator<char, BufferAlloc>(*buffer_allocator_.get());
+    if constexpr (std::is_same_v<Alloc, std::allocator<void>>) {
+      rcutils_allocator_ = rcl_get_default_allocator();
+    } else {
+      rcutils_allocator_ = allocator::get_rcl_allocator<char,
+          BufferAlloc>(*buffer_allocator_.get());
+    }
   }
 
   virtual ~MessageMemoryStrategy() = default;

rclcpp/include/rclcpp/node.hpp

@@ -148,10 +148,16 @@ public:
     rclcpp::CallbackGroupType group_type,
     bool automatically_add_to_executor_with_node = true);
 
-  /// Return the list of callback groups in the node.
+  /// Iterate over the callback groups in the node, calling the given function on each valid one.
+  /**
+   * This method is called in a thread-safe way, and also makes sure to only call the given
+   * function on those items that are still valid.
+   *
+   * \param[in] func The callback function to call on each valid callback group.
+   */
   RCLCPP_PUBLIC
-  const std::vector<rclcpp::CallbackGroup::WeakPtr> &
-  get_callback_groups() const;
+  void
+  for_each_callback_group(const node_interfaces::NodeBaseInterface::CallbackGroupFunction & func);
 
   /// Create and return a Publisher.
   /**
@@ -389,22 +395,6 @@ public:
     rcl_interfaces::msg::ParameterDescriptor{},
     bool ignore_override = false);
 
-  /// Declare a parameter
-  [[deprecated(
-    "declare_parameter() with only a name is deprecated and will be deleted in the future.\n" \
-    "If you want to declare a parameter that won't change type without a default value use:\n" \
-    "`node->declare_parameter<ParameterT>(name)`, where e.g. ParameterT=int64_t.\n\n" \
-    "If you want to declare a parameter that can dynamically change type use:\n" \
-    "```\n" \
-    "rcl_interfaces::msg::ParameterDescriptor descriptor;\n" \
-    "descriptor.dynamic_typing = true;\n" \
-    "node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);\n" \
-    "```"
-  )]]
-  RCLCPP_PUBLIC
-  const rclcpp::ParameterValue &
-  declare_parameter(const std::string & name);
-
   /// Declare and initialize a parameter with a type.
   /**
    * See the non-templated declare_parameter() on this class for details.
@@ -714,6 +704,24 @@ public:
     ParameterT & parameter,
     const ParameterT & alternative_value) const;
 
+  /// Return the parameter value, or the "alternative_value" if not set.
+  /**
+   * If the parameter was not set, then the "alternative_value" argument is returned.
+   *
+   * This method will not throw the rclcpp::exceptions::ParameterNotDeclaredException exception.
+   *
+   * In all cases, the parameter is never set or declared within the node.
+   *
+   * \param[in] name The name of the parameter to get.
+   * \param[in] alternative_value Value to be stored in output if the parameter was not set.
+   * \returns The value of the parameter.
+   */
+  template<typename ParameterT>
+  ParameterT
+  get_parameter_or(
+    const std::string & name,
+    const ParameterT & alternative_value) const;
+
   /// Return the parameters by the given parameter names.
   /**
    * Like get_parameters(), this method may throw the
@@ -979,12 +987,15 @@ public:
   std::map<std::string, std::vector<std::string>>
   get_service_names_and_types() const;
 
-  /// Return the number of publishers that are advertised on a given topic.
+  /// Return a map of existing service names to list of service types for a specific node.
   /**
-   * \param[in] node_name the node_name on which to count the publishers.
-   * \param[in] namespace_ the namespace of the node associated with the name
-   * \return number of publishers that are advertised on a given topic.
-   * \throws std::runtime_error if publishers could not be counted
+   * This function only considers services - not clients.
+   * The returned names are the actual names used and do not have remap rules applied.
+   *
+   * \param[in] node_name name of the node.
+   * \param[in] namespace_ namespace of the node.
+   * \return a map of existing service names to list of service types.
+   * \throws std::runtime_error anything that rcl_error can throw.
    */
   RCLCPP_PUBLIC
   std::map<std::string, std::vector<std::string>>

rclcpp/include/rclcpp/node_impl.hpp

@@ -220,12 +220,16 @@ Node::declare_parameter(
   // get advantage of parameter value template magic to get
   // the correct rclcpp::ParameterType from ParameterT
   rclcpp::ParameterValue value{ParameterT{}};
-  return this->declare_parameter(
-    name,
-    value.get_type(),
-    parameter_descriptor,
-    ignore_override
-  ).get<ParameterT>();
+  try {
+    return this->declare_parameter(
+      name,
+      value.get_type(),
+      parameter_descriptor,
+      ignore_override
+    ).get<ParameterT>();
+  } catch (const ParameterTypeException &) {
+    throw exceptions::UninitializedStaticallyTypedParameterException(name);
+  }
 }
 
 template<typename ParameterT>
@@ -309,6 +313,17 @@ Node::get_parameter_or(
   return got_parameter;
 }
 
+template<typename ParameterT>
+ParameterT
+Node::get_parameter_or(
+  const std::string & name,
+  const ParameterT & alternative_value) const
+{
+  ParameterT parameter;
+  get_parameter_or(name, parameter, alternative_value);
+  return parameter;
+}
+
 // this is a partially-specialized version of get_parameter above,
 // where our concrete type for ParameterT is std::map, but the to-be-determined
 // type is the value in the map.

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

@@ -0,0 +1,210 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_
+#define RCLCPP__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_
+
+#include <memory>
+#include <tuple>
+#include <type_traits>
+#include <utility>
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+namespace detail
+{
+
+// Support and Helper template classes for the NodeInterfaces class.
+
+template<typename NodeT, typename ... Ts>
+std::tuple<std::shared_ptr<Ts>...>
+init_tuple(NodeT & n);
+
+/// Stores the interfaces in a tuple, provides constructors, and getters.
+template<typename ... InterfaceTs>
+struct NodeInterfacesStorage
+{
+  template<typename NodeT>
+  NodeInterfacesStorage(NodeT & node)  // NOLINT(runtime/explicit)
+  : interfaces_(init_tuple<decltype(node), InterfaceTs ...>(node))
+  {}
+
+  explicit NodeInterfacesStorage(std::shared_ptr<InterfaceTs>... args)
+  : interfaces_(args ...)
+  {}
+
+  /// Individual Node Interface non-const getter.
+  template<typename NodeInterfaceT>
+  std::shared_ptr<NodeInterfaceT>
+  get()
+  {
+    static_assert(
+      (std::is_same_v<NodeInterfaceT, InterfaceTs>|| ...),
+      "NodeInterfaces class does not contain given NodeInterfaceT");
+    return std::get<std::shared_ptr<NodeInterfaceT>>(interfaces_);
+  }
+
+  /// Individual Node Interface const getter.
+  template<typename NodeInterfaceT>
+  std::shared_ptr<const NodeInterfaceT>
+  get() const
+  {
+    static_assert(
+      (std::is_same_v<NodeInterfaceT, InterfaceTs>|| ...),
+      "NodeInterfaces class does not contain given NodeInterfaceT");
+    return std::get<std::shared_ptr<NodeInterfaceT>>(interfaces_);
+  }
+
+protected:
+  std::tuple<std::shared_ptr<InterfaceTs>...> interfaces_;
+};
+
+/// Prototype of NodeInterfacesSupports.
+/**
+ * Should read NodeInterfacesSupports<..., T, ...> as "NodeInterfaces supports T", and
+ * if NodeInterfacesSupport is specialized for T, the is_supported should be
+ * set to std::true_type, but by default it is std::false_type, which will
+ * lead to a compiler error when trying to use T with NodeInterfaces.
+ */
+template<typename StorageClassT, typename ... Ts>
+struct NodeInterfacesSupports;
+
+/// Prototype of NodeInterfacesSupportCheck template meta-function.
+/**
+ * This meta-function checks that all the types given are supported,
+ * throwing a more human-readable error if an unsupported type is used.
+ */
+template<typename StorageClassT, typename ... InterfaceTs>
+struct NodeInterfacesSupportCheck;
+
+/// Iterating specialization that ensures classes are supported and inherited.
+template<typename StorageClassT, typename NextInterfaceT, typename ... RemainingInterfaceTs>
+struct NodeInterfacesSupportCheck<StorageClassT, NextInterfaceT, RemainingInterfaceTs ...>
+  : public NodeInterfacesSupportCheck<StorageClassT, RemainingInterfaceTs ...>
+{
+  static_assert(
+    NodeInterfacesSupports<StorageClassT, NextInterfaceT>::is_supported::value,
+    "given NodeInterfaceT is not supported by rclcpp::node_interfaces::NodeInterfaces");
+};
+
+/// Terminating case when there are no more "RemainingInterfaceTs".
+template<typename StorageClassT>
+struct NodeInterfacesSupportCheck<StorageClassT>
+{};
+
+/// Default specialization, needs to be specialized for each supported interface.
+template<typename StorageClassT, typename ... RemainingInterfaceTs>
+struct NodeInterfacesSupports
+{
+  // Specializations need to set this to std::true_type in addition to other interfaces.
+  using is_supported = std::false_type;
+};
+
+/// Terminating specialization of NodeInterfacesSupports.
+template<typename StorageClassT>
+struct NodeInterfacesSupports<StorageClassT>
+  : public StorageClassT
+{
+  /// Perfect forwarding constructor to get arguments down to StorageClassT.
+  template<typename ... ArgsT>
+  explicit NodeInterfacesSupports(ArgsT && ... args)
+  : StorageClassT(std::forward<ArgsT>(args) ...)
+  {}
+};
+
+// Helper functions to initialize the tuple in NodeInterfaces.
+
+template<typename StorageClassT, typename ElementT, typename TupleT, typename NodeT>
+void
+init_element(TupleT & t, NodeT & n)
+{
+  std::get<std::shared_ptr<ElementT>>(t) =
+    NodeInterfacesSupports<StorageClassT, ElementT>::get_from_node_like(n);
+}
+
+template<typename NodeT, typename ... Ts>
+std::tuple<std::shared_ptr<Ts>...>
+init_tuple(NodeT & n)
+{
+  using StorageClassT = NodeInterfacesStorage<Ts ...>;
+  std::tuple<std::shared_ptr<Ts>...> t;
+  (init_element<StorageClassT, Ts>(t, n), ...);
+  return t;
+}
+
+/// Macro for creating specializations with less boilerplate.
+/**
+ * You can use this macro to add support for your interface class if:
+ *
+ * - The standard getter is get_node_{NodeInterfaceName}_interface(), and
+ * - the getter returns a non-const shared_ptr<{NodeInterfaceType}>
+ *
+ * Examples of using this can be seen in the standard node interface headers
+ * in rclcpp, e.g. rclcpp/node_interfaces/node_base_interface.hpp has:
+ *
+ *   RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)
+ *
+ * If your interface has a non-standard getter, or you want to instrument it or
+ * something like that, then you'll need to create your own specialization of
+ * the NodeInterfacesSupports struct without this macro.
+ */
+#define RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(NodeInterfaceType, NodeInterfaceName) \
+  namespace rclcpp::node_interfaces::detail { \
+  template<typename StorageClassT, typename ... RemainingInterfaceTs> \
+  struct NodeInterfacesSupports< \
+    StorageClassT, \
+    NodeInterfaceType, \
+    RemainingInterfaceTs ...> \
+    : public NodeInterfacesSupports<StorageClassT, RemainingInterfaceTs ...> \
+  { \
+    using is_supported = std::true_type; \
+ \
+    template<typename NodeT> \
+    static \
+    std::shared_ptr<NodeInterfaceType> \
+    get_from_node_like(NodeT & node_like) \
+    { \
+      return node_like.get_node_ ## NodeInterfaceName ## _interface(); \
+    } \
+ \
+    /* Perfect forwarding constructor to get arguments down to StorageClassT (eventually). */ \
+    template<typename ... ArgsT> \
+    explicit NodeInterfacesSupports(ArgsT && ... args) \
+      : NodeInterfacesSupports<StorageClassT, RemainingInterfaceTs ...>( \
+        std::forward<ArgsT>(args) ...) \
+    {} \
+ \
+    std::shared_ptr<NodeInterfaceType> \
+    get_node_ ## NodeInterfaceName ## _interface() \
+    { \
+      return StorageClassT::template get<NodeInterfaceType>(); \
+    } \
+ \
+    std::shared_ptr<const NodeInterfaceType> \
+    get_node_ ## NodeInterfaceName ## _interface() const \
+    { \
+      return StorageClassT::template get<NodeInterfaceType>(); \
+    } \
+  }; \
+  }  // namespace rclcpp::node_interfaces::detail
+
+}  // namespace detail
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -15,11 +15,14 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_
 
+#include <functional>
 #include <memory>
+#include <mutex>
 #include <string>
 #include <vector>
 
 #include "rcl/node.h"
+#include "rclcpp/callback_group.hpp"
 #include "rclcpp/context.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
@@ -31,7 +34,7 @@ namespace node_interfaces
 {
 
 /// Implementation of the NodeBase part of the Node API.
-class NodeBase : public NodeBaseInterface
+class NodeBase : public NodeBaseInterface, public std::enable_shared_from_this<NodeBase>
 {
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBase)
@@ -95,22 +98,25 @@ public:
   bool
   callback_group_in_node(rclcpp::CallbackGroup::SharedPtr group) override;
 
+  /// Iterate over the stored callback groups, calling the given function on each valid one.
+  /**
+   * This method is called in a thread-safe way, and also makes sure to only call the given
+   * function on those items that are still valid.
+   *
+   * \param[in] func The callback function to call on each valid callback group.
+   */
   RCLCPP_PUBLIC
-  const std::vector<rclcpp::CallbackGroup::WeakPtr> &
-  get_callback_groups() const override;
+  void
+  for_each_callback_group(const CallbackGroupFunction & func) override;
 
   RCLCPP_PUBLIC
   std::atomic_bool &
   get_associated_with_executor_atomic() override;
 
   RCLCPP_PUBLIC
-  rcl_guard_condition_t *
+  rclcpp::GuardCondition &
   get_notify_guard_condition() override;
 
-  RCLCPP_PUBLIC
-  std::unique_lock<std::recursive_mutex>
-  acquire_notify_guard_condition_lock() const override;
-
   RCLCPP_PUBLIC
   bool
   get_use_intra_process_default() const override;
@@ -132,13 +138,14 @@ private:
   std::shared_ptr<rcl_node_t> node_handle_;
 
   rclcpp::CallbackGroup::SharedPtr default_callback_group_;
+  std::mutex callback_groups_mutex_;
   std::vector<rclcpp::CallbackGroup::WeakPtr> callback_groups_;
 
   std::atomic_bool associated_with_executor_;
 
   /// Guard condition for notifying the Executor of changes to this node.
   mutable std::recursive_mutex notify_guard_condition_mutex_;
-  rcl_guard_condition_t notify_guard_condition_ = rcl_get_zero_initialized_guard_condition();
+  rclcpp::GuardCondition notify_guard_condition_;
   bool notify_guard_condition_is_valid_;
 };
 

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -24,7 +24,9 @@
 
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/context.hpp"
+#include "rclcpp/guard_condition.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -122,11 +124,19 @@ public:
   bool
   callback_group_in_node(rclcpp::CallbackGroup::SharedPtr group) = 0;
 
-  /// Return list of callback groups associated with this node.
+  using CallbackGroupFunction = std::function<void (rclcpp::CallbackGroup::SharedPtr)>;
+
+  /// Iterate over the stored callback groups, calling the given function on each valid one.
+  /**
+   * This method is called in a thread-safe way, and also makes sure to only call the given
+   * function on those items that are still valid.
+   *
+   * \param[in] func The callback function to call on each valid callback group.
+   */
   RCLCPP_PUBLIC
   virtual
-  const std::vector<rclcpp::CallbackGroup::WeakPtr> &
-  get_callback_groups() const = 0;
+  void
+  for_each_callback_group(const CallbackGroupFunction & func) = 0;
 
   /// Return the atomic bool which is used to ensure only one executor is used.
   RCLCPP_PUBLIC
@@ -134,24 +144,17 @@ public:
   std::atomic_bool &
   get_associated_with_executor_atomic() = 0;
 
-  /// Return guard condition that should be notified when the internal node state changes.
+  /// Return a guard condition that should be notified when the internal node state changes.
   /**
    * For example, this should be notified when a publisher is added or removed.
    *
-   * \return the rcl_guard_condition_t if it is valid, else nullptr
+   * \return the GuardCondition if it is valid, else thow runtime error
    */
   RCLCPP_PUBLIC
   virtual
-  rcl_guard_condition_t *
+  rclcpp::GuardCondition &
   get_notify_guard_condition() = 0;
 
-  /// Acquire and return a scoped lock that protects the notify guard condition.
-  /** This should be used when triggering the notify guard condition. */
-  RCLCPP_PUBLIC
-  virtual
-  std::unique_lock<std::recursive_mutex>
-  acquire_notify_guard_condition_lock() const = 0;
-
   /// Return the default preference for using intra process communication.
   RCLCPP_PUBLIC
   virtual
@@ -175,4 +178,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

@@ -20,6 +20,7 @@
 #include <memory>
 #include <mutex>
 #include <string>
+#include <tuple>
 #include <utility>
 #include <vector>
 
@@ -70,10 +71,34 @@ public:
     const std::string & node_name,
     const std::string & namespace_) const override;
 
+  RCLCPP_PUBLIC
+  std::map<std::string, std::vector<std::string>>
+  get_client_names_and_types_by_node(
+    const std::string & node_name,
+    const std::string & namespace_) const override;
+
+  RCLCPP_PUBLIC
+  std::map<std::string, std::vector<std::string>>
+  get_publisher_names_and_types_by_node(
+    const std::string & node_name,
+    const std::string & namespace_,
+    bool no_demangle = false) const override;
+
+  RCLCPP_PUBLIC
+  std::map<std::string, std::vector<std::string>>
+  get_subscriber_names_and_types_by_node(
+    const std::string & node_name,
+    const std::string & namespace_,
+    bool no_demangle = false) const override;
+
   RCLCPP_PUBLIC
   std::vector<std::string>
   get_node_names() const override;
 
+  RCLCPP_PUBLIC
+  std::vector<std::tuple<std::string, std::string, std::string>>
+  get_node_names_with_enclaves() const override;
+
   RCLCPP_PUBLIC
   std::vector<std::pair<std::string, std::string>>
   get_node_names_and_namespaces() const override;

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

@@ -20,6 +20,7 @@
 #include <chrono>
 #include <map>
 #include <string>
+#include <tuple>
 #include <utility>
 #include <vector>
 
@@ -28,6 +29,7 @@
 
 #include "rclcpp/event.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -51,12 +53,16 @@ public:
   /// Construct a TopicEndpointInfo from a rcl_topic_endpoint_info_t.
   RCLCPP_PUBLIC
   explicit TopicEndpointInfo(const rcl_topic_endpoint_info_t & info)
-  : node_name_(info.node_name),
-    node_namespace_(info.node_namespace),
-    topic_type_(info.topic_type),
-    endpoint_type_(static_cast<rclcpp::EndpointType>(info.endpoint_type)),
+  : endpoint_type_(static_cast<rclcpp::EndpointType>(info.endpoint_type)),
     qos_profile_({info.qos_profile.history, info.qos_profile.depth}, info.qos_profile)
   {
+    if (!info.node_name || !info.node_namespace || !info.topic_type) {
+      throw std::invalid_argument("Constructor TopicEndpointInfo with invalid topic endpoint info");
+    }
+    node_name_ = info.node_name;
+    node_namespace_ = info.node_namespace;
+    topic_type_ = info.topic_type;
+
     std::copy(info.endpoint_gid, info.endpoint_gid + RMW_GID_STORAGE_SIZE, endpoint_gid_.begin());
   }
 
@@ -146,7 +152,9 @@ public:
   /**
    * A topic is considered to exist when at least one publisher or subscriber
    * exists for it, whether they be local or remote to this process.
-   * The returned names are the actual names used and do not have remap rules applied.
+   * The returned names are the actual names of the topics, either announced by another nodes or by this one.
+   * Attempting to create publishers or subscribers using names returned by this function may not
+   * result in the desired topic name being used depending on the remap rules in use.
    *
    * \param[in] no_demangle if true, topic names and types are not demangled
    */
@@ -160,7 +168,9 @@ public:
    * A service is considered to exist when at least one service server or
    * service client exists for it, whether they be local or remote to this
    * process.
-   * The returned names are the actual names used and do not have remap rules applied.
+   * The returned names are the actual names of the services, either announced by another nodes or by this one.
+   * Attempting to create clients or services using names returned by this function may not result in
+   * the desired service name being used depending on the remap rules in use.
    */
   RCLCPP_PUBLIC
   virtual
@@ -170,7 +180,9 @@ public:
   /// Return a map of existing service names to list of service types for a specific node.
   /**
    * This function only considers services - not clients.
-   * The returned names are the actual names used and do not have remap rules applied.
+   * The returned names are the actual names after remap rules applied.
+   * Attempting to create service clients using names returned by this function may not
+   * result in the desired service name being used depending on the remap rules in use.
    *
    * \param[in] node_name name of the node
    * \param[in] namespace_ namespace of the node
@@ -182,18 +194,85 @@ public:
     const std::string & node_name,
     const std::string & namespace_) const = 0;
 
+  /// Return a map of existing service names and types with a specific node.
+  /**
+   * This function only considers clients - not service servers.
+   * The returned names are the actual names after remap rules applied.
+   * Attempting to create service servers using names returned by this function may not
+   * result in the desired service name being used depending on the remap rules in use.
+   *
+   * \param[in] node_name name of the node
+   * \param[in] namespace_ namespace of the node
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::map<std::string, std::vector<std::string>>
+  get_client_names_and_types_by_node(
+    const std::string & node_name,
+    const std::string & namespace_) const = 0;
+
+  /// Return a map of existing topic names to list of topic types for a specific node.
+  /**
+   * This function only considers publishers - not subscribers.
+   * The returned names are the actual names after remap rules applied.
+   * Attempting to create publishers or subscribers using names returned by this function may not
+   * result in the desired topic name being used depending on the remap rules in use.
+   *
+   * \param[in] node_name name of the node
+   * \param[in] namespace_ namespace of the node
+   * \param[in] no_demangle if true, topic names and types are not demangled
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::map<std::string, std::vector<std::string>>
+  get_publisher_names_and_types_by_node(
+    const std::string & node_name,
+    const std::string & namespace_,
+    bool no_demangle = false) const = 0;
+
+  /// Return a map of existing topic names to list of topic types for a specific node.
+  /**
+   * This function only considers subscribers - not publishers.
+   * The returned names are the actual names after remap rules applied.
+   * Attempting to create publishers or subscribers using names returned by this function may not
+   * result in the desired topic name being used depending on the remap rules in use.
+   *
+   * \param[in] node_name name of the node
+   * \param[in] namespace_ namespace of the node
+   * \param[in] no_demangle if true, topic names and types are not demangled
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::map<std::string, std::vector<std::string>>
+  get_subscriber_names_and_types_by_node(
+    const std::string & node_name,
+    const std::string & namespace_,
+    bool no_demangle = false) const = 0;
+
   /// Return a vector of existing node names (string).
   /*
-   * The returned names are the actual names used and do not have remap rules applied.
+   * The returned names are the actual names after remap rules applied.
    */
   RCLCPP_PUBLIC
   virtual
   std::vector<std::string>
   get_node_names() const = 0;
 
+  /// Return a vector of existing node names, namespaces and enclaves (tuple of string).
+  /*
+   * The returned names are the actual names after remap rules applied.
+   * The enclaves contain the runtime security artifacts, those can be
+   * used to establish secured network.
+   * See https://design.ros2.org/articles/ros2_security_enclaves.html
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<std::tuple<std::string, std::string, std::string>>
+  get_node_names_with_enclaves() const = 0;
+
   /// Return a vector of existing node names and namespaces (pair of string).
   /*
-   * The returned names are the actual names used and do not have remap rules applied.
+   * The returned names are the actual names after remap rules applied.
    */
   RCLCPP_PUBLIC
   virtual
@@ -283,6 +362,8 @@ public:
   /// Return the topic endpoint information about publishers on a given topic.
   /**
    * \param[in] topic_name the actual topic name used; it will not be automatically remapped.
+   * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name,
+   *   otherwise it should be a valid ROS topic name.
    * \sa rclcpp::Node::get_publishers_info_by_topic
    */
   RCLCPP_PUBLIC
@@ -293,6 +374,8 @@ public:
   /// Return the topic endpoint information about subscriptions on a given topic.
   /**
    * \param[in] topic_name the actual topic name used; it will not be automatically remapped.
+   * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name,
+   *   otherwise it should be a valid ROS topic name.
    * \sa rclcpp::Node::get_subscriptions_info_by_topic
    */
   RCLCPP_PUBLIC
@@ -304,4 +387,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeGraphInterface, graph)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp

@@ -0,0 +1,171 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_INTERFACES_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_INTERFACES_HPP_
+
+#include <memory>
+
+#include "rclcpp/detail/template_unique.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
+
+#define ALL_RCLCPP_NODE_INTERFACES \
+  rclcpp::node_interfaces::NodeBaseInterface, \
+  rclcpp::node_interfaces::NodeClockInterface, \
+  rclcpp::node_interfaces::NodeGraphInterface, \
+  rclcpp::node_interfaces::NodeLoggingInterface, \
+  rclcpp::node_interfaces::NodeParametersInterface, \
+  rclcpp::node_interfaces::NodeServicesInterface, \
+  rclcpp::node_interfaces::NodeTimeSourceInterface, \
+  rclcpp::node_interfaces::NodeTimersInterface, \
+  rclcpp::node_interfaces::NodeTopicsInterface, \
+  rclcpp::node_interfaces::NodeWaitablesInterface
+
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+
+/// A helper class for aggregating node interfaces.
+template<typename ... InterfaceTs>
+class NodeInterfaces
+  : public detail::NodeInterfacesSupportCheck<
+    detail::NodeInterfacesStorage<InterfaceTs ...>,
+    InterfaceTs ...
+  >,
+  public detail::NodeInterfacesSupports<
+    detail::NodeInterfacesStorage<InterfaceTs ...>,
+    InterfaceTs ...
+  >
+{
+  static_assert(
+    0 != sizeof ...(InterfaceTs),
+    "must provide at least one interface as a template argument");
+  static_assert(
+    rclcpp::detail::template_unique_v<InterfaceTs ...>,
+    "must provide unique template parameters");
+
+  using NodeInterfacesSupportsT = detail::NodeInterfacesSupports<
+    detail::NodeInterfacesStorage<InterfaceTs ...>,
+    InterfaceTs ...
+  >;
+
+public:
+  /// Create a new NodeInterfaces object using the given node-like object's interfaces.
+  /**
+   * Specify which interfaces you need by passing them as template parameters.
+   *
+   * This allows you to aggregate interfaces from different sources together to pass as a single
+   * aggregate object to any functions that take node interfaces or node-likes, without needing to
+   * templatize that function.
+   *
+   * You may also use this constructor to create a NodeInterfaces that contains a subset of
+   * another NodeInterfaces' interfaces.
+   *
+   * Finally, this class supports implicit conversion from node-like objects, allowing you to
+   * directly pass a node-like to a function that takes a NodeInterfaces object.
+   *
+   * Usage examples:
+   *   ```cpp
+   *   // Suppose we have some function:
+   *   void fn(NodeInterfaces<NodeBaseInterface, NodeClockInterface> interfaces);
+   *
+   *   // Then we can, explicitly:
+   *   rclcpp::Node node("some_node");
+   *   auto ni = NodeInterfaces<NodeBaseInterface, NodeClockInterface>(node);
+   *   fn(ni);
+   *
+   *   // But also:
+   *   fn(node);
+   *
+   *   // Subsetting a NodeInterfaces object also works!
+   *   auto ni_base = NodeInterfaces<NodeBaseInterface>(ni);
+   *
+   *   // Or aggregate them (you could aggregate interfaces from disparate node-likes)
+   *   auto ni_aggregated = NodeInterfaces<NodeBaseInterface, NodeClockInterface>(
+   *     node->get_node_base_interface(),
+   *     node->get_node_clock_interface()
+   *   )
+   *
+   *   // And then to access the interfaces:
+   *   // Get with get<>
+   *   auto base = ni.get<NodeBaseInterface>();
+   *
+   *   // Or the appropriate getter
+   *   auto clock = ni.get_clock_interface();
+   *   ```
+   *
+   * You may use any of the standard node interfaces that come with rclcpp:
+   *   - rclcpp::node_interfaces::NodeBaseInterface
+   *   - rclcpp::node_interfaces::NodeClockInterface
+   *   - rclcpp::node_interfaces::NodeGraphInterface
+   *   - rclcpp::node_interfaces::NodeLoggingInterface
+   *   - rclcpp::node_interfaces::NodeParametersInterface
+   *   - rclcpp::node_interfaces::NodeServicesInterface
+   *   - rclcpp::node_interfaces::NodeTimeSourceInterface
+   *   - rclcpp::node_interfaces::NodeTimersInterface
+   *   - rclcpp::node_interfaces::NodeTopicsInterface
+   *   - rclcpp::node_interfaces::NodeWaitablesInterface
+   *
+   * Or you use custom interfaces as long as you make a template specialization
+   * of the rclcpp::node_interfaces::detail::NodeInterfacesSupport struct using
+   * the RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT macro.
+   *
+   * Usage example:
+   *   ```RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)```
+   *
+   * If you choose not to use the helper macro, then you can specialize the
+   * template yourself, but you must:
+   *
+   *   - Provide a template specialization of the get_from_node_like method that gets the interface
+   *     from any node-like that stores the interface, using the node-like's getter
+   *   - Designate the is_supported type as std::true_type using a using directive
+   *   - Provide any number of getter methods to be used to obtain the interface with the
+   *     NodeInterface object, noting that the getters of the storage class will apply to all
+   *     supported interfaces.
+   *     - The getter method names should not clash in name with any other interface getter
+   *       specializations if those other interfaces are meant to be aggregated in the same
+   *       NodeInterfaces object.
+   *
+   * \param[in] node Node-like object from which to get the node interfaces
+   */
+  template<typename NodeT>
+  NodeInterfaces(NodeT & node)  // NOLINT(runtime/explicit)
+  : NodeInterfacesSupportsT(node)
+  {}
+
+  /// NodeT::SharedPtr Constructor
+  template<typename NodeT>
+  NodeInterfaces(std::shared_ptr<NodeT> node)  // NOLINT(runtime/explicit)
+  : NodeInterfaces(
+      [&]() -> NodeT & {
+        if (!node) {
+          throw std::invalid_argument("given node pointer is nullptr");
+        }
+        return *node;
+      }())
+  {}
+
+  explicit NodeInterfaces(std::shared_ptr<InterfaceTs>... args)
+  : NodeInterfacesSupportsT(args ...)
+  {}
+};
+
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_INTERFACES_HPP_

rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp

@@ -19,6 +19,7 @@
 
 #include "rclcpp/logger.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -54,4 +55,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeLoggingInterface, logging)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_LOGGING_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -103,25 +103,6 @@ public:
   virtual
   ~NodeParameters();
 
-// This is overriding a deprecated method, so we need to ignore the deprecation warning here.
-// Users of the method will still get a warning!
-#ifndef _WIN32
-# pragma GCC diagnostic push
-# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
-#else
-# pragma warning(push)
-# pragma warning(disable: 4996)
-#endif
-  [[deprecated(RCLCPP_INTERNAL_NODE_PARAMETERS_INTERFACE_DEPRECATE_DECLARE)]]
-  RCLCPP_PUBLIC
-  const rclcpp::ParameterValue &
-  declare_parameter(const std::string & name) override;
-#ifndef _WIN32
-# pragma GCC diagnostic pop
-#else
-# pragma warning(pop)
-#endif
-
   RCLCPP_PUBLIC
   const rclcpp::ParameterValue &
   declare_parameter(

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -25,6 +25,7 @@
 #include "rcl_interfaces/msg/set_parameters_result.hpp"
 
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -45,17 +46,6 @@ struct OnSetParametersCallbackHandle
   OnParametersSetCallbackType callback;
 };
 
-#define RCLCPP_INTERNAL_NODE_PARAMETERS_INTERFACE_DEPRECATE_DECLARE \
-  "declare_parameter() with only a name is deprecated and will be deleted in the future.\n" \
-  "If you want to declare a parameter that won't change type without a default value use:\n" \
-  "`node_params->declare_parameter(name, type)`, with e.g. type=rclcpp::PARAMETER_INTEGER.\n\n" \
-  "If you want to declare a parameter that can dynamically change type use:\n" \
-  "```\n" \
-  "rcl_interfaces::msg::ParameterDescriptor descriptor;\n" \
-  "descriptor.dynamic_typing = true;\n" \
-  "node_params->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);\n" \
-  "```"
-
 /// Pure virtual interface class for the NodeParameters part of the Node API.
 class NodeParametersInterface
 {
@@ -66,15 +56,6 @@ public:
   virtual
   ~NodeParametersInterface() = default;
 
-  /// Declare a parameter.
-  /**
-   * \sa rclcpp::Node::declare_parameter
-   */
-  [[deprecated(RCLCPP_INTERNAL_NODE_PARAMETERS_INTERFACE_DEPRECATE_DECLARE)]]
-  virtual
-  const rclcpp::ParameterValue &
-  declare_parameter(const std::string & name) = 0;
-
   /// Declare and initialize a parameter.
   /**
    * \sa rclcpp::Node::declare_parameter
@@ -235,4 +216,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeParametersInterface, parameters)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp

@@ -20,6 +20,7 @@
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/client.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/service.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -62,4 +63,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeServicesInterface, services)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp

@@ -48,7 +48,7 @@ public:
     rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
     rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
     rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters,
-    const rclcpp::QoS & qos = rclcpp::RosoutQoS(),
+    const rclcpp::QoS & qos = rclcpp::ClockQoS(),
     bool use_clock_thread = true
   );
 

rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp

@@ -16,6 +16,7 @@
 #define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_
 
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -37,4 +38,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTimeSourceInterface, time_source)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp

@@ -17,6 +17,7 @@
 
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -47,4 +48,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTimersInterface, timers)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_topics.hpp

@@ -20,12 +20,16 @@
 #include "rcl/publisher.h"
 #include "rcl/subscription.h"
 
+#include "rclcpp/callback_group.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
-#include "rclcpp/publisher.hpp"
+#include "rclcpp/publisher_base.hpp"
 #include "rclcpp/publisher_factory.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/subscription_factory.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp

rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp

@@ -24,6 +24,7 @@
 
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/publisher.hpp"
@@ -97,4 +98,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTopicsInterface, topics)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp

@@ -17,6 +17,7 @@
 
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rclcpp/waitable.hpp"
 
@@ -54,4 +55,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeWaitablesInterface, waitables)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_

rclcpp/include/rclcpp/node_options.hpp

@@ -42,6 +42,7 @@ public:
    *   - arguments = {}
    *   - parameter_overrides = {}
    *   - use_global_arguments = true
+   *   - enable_rosout = true
    *   - use_intra_process_comms = false
    *   - enable_topic_statistics = false
    *   - start_parameter_services = true
@@ -349,6 +350,9 @@ public:
    * global arguments (e.g. parameter overrides from a YAML file), which are
    * not explicitly declared will not appear on the node at all, even if
    * `allow_undeclared_parameters` is true.
+   * Parameter declaration from overrides is done in the node's base constructor,
+   * so the user must take care to check if the parameter is already (e.g.
+   * automatically) declared before declaring it themselves.
    * Already declared parameters will not be re-declared, and parameters
    * declared in this way will use the default constructed ParameterDescriptor.
    */

rclcpp/include/rclcpp/parameter.hpp

@@ -264,6 +264,7 @@ get_value_helper<rclcpp::Parameter>(const rclcpp::Parameter * parameter)
 
 }  // namespace detail
 
+/// \cond
 template<typename T>
 decltype(auto)
 Parameter::get_value() const
@@ -275,6 +276,7 @@ Parameter::get_value() const
     throw exceptions::InvalidParameterTypeException(this->name_, ex.what());
   }
 }
+/// \endcond
 
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/parameter_client.hpp

@@ -79,7 +79,7 @@ public:
    * \param[in] group (optional) The async parameter client will be added to this callback group.
    */
   template<typename NodeT>
-  AsyncParametersClient(
+  explicit AsyncParametersClient(
     const std::shared_ptr<NodeT> node,
     const std::string & remote_node_name = "",
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters,
@@ -102,7 +102,7 @@ public:
    * \param[in] group (optional) The async parameter client will be added to this callback group.
    */
   template<typename NodeT>
-  AsyncParametersClient(
+  explicit AsyncParametersClient(
     NodeT * node,
     const std::string & remote_node_name = "",
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters,
@@ -185,7 +185,7 @@ public:
   /**
    * This function filters the parameters to be set based on the node name.
    *
-   * \param yaml_filename the full name of the yaml file
+   * \param parameter_map named parameters to be loaded
    * \return the future of the set_parameter service used to load the parameters
    * \throw InvalidParametersException if there is no parameter to set
    */
@@ -333,7 +333,7 @@ public:
   {}
 
   template<typename NodeT>
-  SyncParametersClient(
+  explicit SyncParametersClient(
     NodeT * node,
     const std::string & remote_node_name = "",
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters)

rclcpp/include/rclcpp/parameter_event_handler.hpp

@@ -165,17 +165,21 @@ public:
    * \param[in] qos The QoS settings to use for any subscriptions.
    */
   template<typename NodeT>
-  ParameterEventHandler(
+  explicit ParameterEventHandler(
     NodeT node,
     const rclcpp::QoS & qos =
     rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)))
+  : node_base_(rclcpp::node_interfaces::get_node_base_interface(node))
   {
-    node_base_ = rclcpp::node_interfaces::get_node_base_interface(node);
     auto node_topics = rclcpp::node_interfaces::get_node_topics_interface(node);
 
+    callbacks_ = std::make_shared<Callbacks>();
+
     event_subscription_ = rclcpp::create_subscription<rcl_interfaces::msg::ParameterEvent>(
       node_topics, "/parameter_events", qos,
-      std::bind(&ParameterEventHandler::event_callback, this, std::placeholders::_1));
+      [callbacks = callbacks_](const rcl_interfaces::msg::ParameterEvent & event) {
+        callbacks->event_callback(event);
+      });
   }
 
   using ParameterEventCallbackType =
@@ -249,8 +253,8 @@ public:
   get_parameter_from_event(
     const rcl_interfaces::msg::ParameterEvent & event,
     rclcpp::Parameter & parameter,
-    const std::string parameter_name,
-    const std::string node_name = "");
+    const std::string & parameter_name,
+    const std::string & node_name = "");
 
   /// Get an rclcpp::Parameter from parameter event
   /**
@@ -269,8 +273,8 @@ public:
   static rclcpp::Parameter
   get_parameter_from_event(
     const rcl_interfaces::msg::ParameterEvent & event,
-    const std::string parameter_name,
-    const std::string node_name = "");
+    const std::string & parameter_name,
+    const std::string & node_name = "");
 
   /// Get all rclcpp::Parameter values from a parameter event
   /**
@@ -285,17 +289,6 @@ public:
   using CallbacksContainerType = std::list<ParameterCallbackHandle::WeakPtr>;
 
 protected:
-  /// Callback for parameter events subscriptions.
-  RCLCPP_PUBLIC
-  void
-  event_callback(const rcl_interfaces::msg::ParameterEvent & event);
-
-  // Utility function for resolving node path.
-  std::string resolve_path(const std::string & path);
-
-  // Node interface used for base functionality
-  std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base_;
-
   // *INDENT-OFF* Uncrustify doesn't handle indented public/private labels
   // Hash function for string pair required in std::unordered_map
   // See: https://stackoverflow.com/questions/35985960/c-why-is-boosthash-combine-the-best-way-to-combine-hash-values
@@ -319,18 +312,34 @@ protected:
   };
   // *INDENT-ON*
 
-  // Map container for registered parameters
-  std::unordered_map<
-    std::pair<std::string, std::string>,
-    CallbacksContainerType,
-    StringPairHash
-  > parameter_callbacks_;
+  struct Callbacks
+  {
+    std::recursive_mutex mutex_;
+
+    // Map container for registered parameters
+    std::unordered_map<
+      std::pair<std::string, std::string>,
+      CallbacksContainerType,
+      StringPairHash
+    > parameter_callbacks_;
+
+    std::list<ParameterEventCallbackHandle::WeakPtr> event_callbacks_;
+
+    /// Callback for parameter events subscriptions.
+    RCLCPP_PUBLIC
+    void
+    event_callback(const rcl_interfaces::msg::ParameterEvent & event);
+  };
+
+  std::shared_ptr<Callbacks> callbacks_;
+
+  // Utility function for resolving node path.
+  std::string resolve_path(const std::string & path);
+
+  // Node interface used for base functionality
+  std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base_;
 
   rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr event_subscription_;
-
-  std::list<ParameterEventCallbackHandle::WeakPtr> event_callbacks_;
-
-  std::recursive_mutex mutex_;
 };
 
 }  // namespace rclcpp

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_map.hpp

@@ -41,6 +41,16 @@ RCLCPP_PUBLIC
 ParameterMap
 parameter_map_from(const rcl_params_t * const c_params);
 
+/// Convert parameters from rcl_yaml_param_parser into C++ class instances.
+/// \param[in] c_params C structures containing parameters for multiple nodes.
+/// \param[in] node_fqn a Fully Qualified Name of node, default value is nullptr.
+///   If it's not nullptr, return the relative node parameters belonging to this node_fqn.
+/// \returns a map where the keys are fully qualified node names and values a list of parameters.
+/// \throws InvalidParametersException if the `rcl_params_t` is inconsistent or invalid.
+RCLCPP_PUBLIC
+ParameterMap
+parameter_map_from(const rcl_params_t * const c_params, const char * node_fqn);
+
 /// Convert parameter value from rcl_yaml_param_parser into a C++ class instance.
 /// \param[in] c_value C structure containing a value of a parameter.
 /// \returns an instance of a parameter value

rclcpp/include/rclcpp/parameter_value.hpp

@@ -301,6 +301,16 @@ public:
     return get<ParameterType::PARAMETER_BOOL_ARRAY>();
   }
 
+  template<typename type>
+  constexpr
+  typename std::enable_if<
+    std::is_convertible<
+      type, const std::vector<int> &>::value, const std::vector<int64_t> &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_INTEGER_ARRAY>();
+  }
+
   template<typename type>
   constexpr
   typename std::enable_if<
@@ -311,6 +321,16 @@ public:
     return get<ParameterType::PARAMETER_INTEGER_ARRAY>();
   }
 
+  template<typename type>
+  constexpr
+  typename std::enable_if<
+    std::is_convertible<
+      type, const std::vector<float> &>::value, const std::vector<double> &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_DOUBLE_ARRAY>();
+  }
+
   template<typename type>
   constexpr
   typename std::enable_if<

rclcpp/include/rclcpp/publisher.hpp

@@ -20,12 +20,14 @@
 #include <memory>
 #include <sstream>
 #include <string>
+#include <type_traits>
 #include <utility>
 
 #include "rcl/error_handling.h"
 #include "rcl/publisher.h"
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
+#include "rosidl_runtime_cpp/traits.hpp"
 
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/allocator/allocator_deleter.hpp"
@@ -81,7 +83,6 @@ public:
 
   /// MessageT::custom_type if MessageT is a TypeAdapter, otherwise just MessageT.
   using PublishedType = typename rclcpp::TypeAdapter<MessageT>::custom_type;
-  /// MessageT::ros_message_type if MessageT is a TypeAdapter, otherwise just MessageT.
   using ROSMessageType = typename rclcpp::TypeAdapter<MessageT>::ros_message_type;
 
   using PublishedTypeAllocatorTraits = allocator::AllocRebind<PublishedType, AllocatorT>;
@@ -176,8 +177,7 @@ public:
     const rclcpp::QoS & qos,
     const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options)
   {
-    // Topic is unused for now.
-    (void)topic;
+    (void)qos;
     (void)options;
 
     // If needed, setup intra process communication.
@@ -185,19 +185,23 @@ public:
       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::Volatile) {
+      if (qos_profile.durability() != rclcpp::DurabilityPolicy::Volatile) {
         throw std::invalid_argument(
                 "intraprocess communication allowed only with volatile durability");
       }
+      // Register the publisher with the intra process manager.
       uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this());
       this->setup_intra_process(
         intra_process_publisher_id,
@@ -263,10 +267,11 @@ public:
       get_subscription_count() > get_intra_process_subscription_count();
 
     if (inter_process_publish_needed) {
-      auto shared_msg = this->do_intra_process_publish_and_return_shared(std::move(msg));
+      auto shared_msg =
+        this->do_intra_process_ros_message_publish_and_return_shared(std::move(msg));
       this->do_inter_process_publish(*shared_msg);
     } else {
-      this->do_intra_process_publish(std::move(msg));
+      this->do_intra_process_ros_message_publish(std::move(msg));
     }
   }
 
@@ -319,13 +324,28 @@ public:
   >
   publish(std::unique_ptr<T, PublishedTypeDeleter> msg)
   {
-    // TODO(wjwwood): later update this to give the unique_ptr to the intra
-    // process manager and let it decide if it needs to be converted or not.
-    // For now, convert it unconditionally and pass it the ROSMessageType
-    // publish function specialization.
-    auto unique_ros_msg = this->create_ros_message_unique_ptr();
-    rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, *unique_ros_msg);
-    this->publish(std::move(unique_ros_msg));
+    // Avoid allocating when not using intra process.
+    if (!intra_process_is_enabled_) {
+      // In this case we're not using intra process.
+      ROSMessageType ros_msg;
+      rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, ros_msg);
+      return this->do_inter_process_publish(ros_msg);
+    }
+
+    bool inter_process_publish_needed =
+      get_subscription_count() > get_intra_process_subscription_count();
+
+    if (inter_process_publish_needed) {
+      ROSMessageType ros_msg;
+      // TODO(clalancette): This is unnecessarily doing an additional conversion
+      // that may have already been done in do_intra_process_publish_and_return_shared().
+      // We should just reuse that effort.
+      rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, ros_msg);
+      this->do_intra_process_publish(std::move(msg));
+      this->do_inter_process_publish(ros_msg);
+    } else {
+      this->do_intra_process_publish(std::move(msg));
+    }
   }
 
   /// Publish a message on the topic.
@@ -346,12 +366,7 @@ public:
   >
   publish(const T & msg)
   {
-    // TODO(wjwwood): later update this to give the unique_ptr to the intra
-    // process manager and let it decide if it needs to be converted or not.
-    // For now, convert it unconditionally and pass it the ROSMessageType
-    // publish function specialization.
-
-    // Avoid allocating when not using intra process.
+    // Avoid double allocating when not using intra process.
     if (!intra_process_is_enabled_) {
       // Convert to the ROS message equivalent and publish it.
       ROSMessageType ros_msg;
@@ -359,13 +374,12 @@ public:
       // In this case we're not using intra process.
       return this->do_inter_process_publish(ros_msg);
     }
-    // Otherwise we have to allocate memory in a unique_ptr, convert it,
-    // and pass it along.
+
+    // Otherwise we have to allocate memory in a unique_ptr and pass it along.
     // As the message is not const, a copy should be made.
     // A shared_ptr<const MessageT> could also be constructed here.
-    auto unique_ros_msg = this->create_ros_message_unique_ptr();
-    rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(msg, *unique_ros_msg);
-    this->publish(std::move(unique_ros_msg));
+    auto unique_msg = this->duplicate_type_adapt_message_as_unique_ptr(msg);
+    this->publish(std::move(unique_msg));
   }
 
   void
@@ -438,10 +452,7 @@ protected:
   void
   do_inter_process_publish(const ROSMessageType & msg)
   {
-    TRACEPOINT(
-      rclcpp_publish,
-      static_cast<const void *>(publisher_handle_.get()),
-      static_cast<const void *>(&msg));
+    TRACEPOINT(rclcpp_publish, nullptr, static_cast<const void *>(&msg));
     auto status = rcl_publish(publisher_handle_.get(), &msg, nullptr);
 
     if (RCL_RET_PUBLISHER_INVALID == status) {
@@ -494,7 +505,7 @@ protected:
   }
 
   void
-  do_intra_process_publish(std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter> msg)
+  do_intra_process_publish(std::unique_ptr<PublishedType, PublishedTypeDeleter> msg)
   {
     auto ipm = weak_ipm_.lock();
     if (!ipm) {
@@ -505,14 +516,32 @@ protected:
       throw std::runtime_error("cannot publish msg which is a null pointer");
     }
 
-    ipm->template do_intra_process_publish<ROSMessageType, AllocatorT>(
+    ipm->template do_intra_process_publish<PublishedType, ROSMessageType, AllocatorT>(
+      intra_process_publisher_id_,
+      std::move(msg),
+      published_type_allocator_);
+  }
+
+  void
+  do_intra_process_ros_message_publish(std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter> msg)
+  {
+    auto ipm = weak_ipm_.lock();
+    if (!ipm) {
+      throw std::runtime_error(
+              "intra process publish called after destruction of intra process manager");
+    }
+    if (!msg) {
+      throw std::runtime_error("cannot publish msg which is a null pointer");
+    }
+
+    ipm->template do_intra_process_publish<ROSMessageType, ROSMessageType, AllocatorT>(
       intra_process_publisher_id_,
       std::move(msg),
       ros_message_type_allocator_);
   }
 
   std::shared_ptr<const ROSMessageType>
-  do_intra_process_publish_and_return_shared(
+  do_intra_process_ros_message_publish_and_return_shared(
     std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter> msg)
   {
     auto ipm = weak_ipm_.lock();
@@ -524,13 +553,14 @@ protected:
       throw std::runtime_error("cannot publish msg which is a null pointer");
     }
 
-    return ipm->template do_intra_process_publish_and_return_shared<ROSMessageType,
+    return ipm->template do_intra_process_publish_and_return_shared<ROSMessageType, ROSMessageType,
              AllocatorT>(
       intra_process_publisher_id_,
       std::move(msg),
       ros_message_type_allocator_);
   }
 
+
   /// Return a new unique_ptr using the ROSMessageType of the publisher.
   std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>
   create_ros_message_unique_ptr()
@@ -549,6 +579,15 @@ protected:
     return std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>(ptr, ros_message_type_deleter_);
   }
 
+  /// Duplicate a given type adapted message as a unique_ptr.
+  std::unique_ptr<PublishedType, PublishedTypeDeleter>
+  duplicate_type_adapt_message_as_unique_ptr(const PublishedType & msg)
+  {
+    auto ptr = PublishedTypeAllocatorTraits::allocate(published_type_allocator_, 1);
+    PublishedTypeAllocatorTraits::construct(published_type_allocator_, ptr, msg);
+    return std::unique_ptr<PublishedType, PublishedTypeDeleter>(ptr, published_type_deleter_);
+  }
+
   /// Copy of original options passed during construction.
   /**
    * It is important to save a copy of this so that the rmw payload which it

rclcpp/include/rclcpp/publisher_base.hpp

@@ -18,11 +18,14 @@
 #include <rmw/error_handling.h>
 #include <rmw/rmw.h>
 
+#include <chrono>
 #include <functional>
 #include <iostream>
 #include <memory>
 #include <sstream>
 #include <string>
+#include <unordered_map>
+#include <utility>
 #include <vector>
 
 #include "rcl/publisher.h"
@@ -33,6 +36,7 @@
 #include "rclcpp/qos_event.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rcpputils/time.hpp"
 
 namespace rclcpp
 {
@@ -110,9 +114,10 @@ public:
   get_publisher_handle() const;
 
   /// Get all the QoS event handlers associated with this publisher.
-  /** \return The vector of QoS event handlers. */
+  /** \return The map of QoS event handlers. */
   RCLCPP_PUBLIC
-  const std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
+  const
+  std::unordered_map<rcl_publisher_event_type_t, std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
   get_event_handlers() const;
 
   /// Get subscription count
@@ -203,6 +208,111 @@ public:
   std::vector<rclcpp::NetworkFlowEndpoint>
   get_network_flow_endpoints() const;
 
+  /// Wait until all published messages are acknowledged or until the specified timeout elapses.
+  /**
+   * This method waits until all published messages are acknowledged by all matching
+   * subscriptions or the given timeout elapses.
+   *
+   * If the timeout is negative then this method will block indefinitely until all published
+   * messages are acknowledged.
+   * If the timeout is zero then this method will not block, it will check if all published
+   * messages are acknowledged and return immediately.
+   * If the timeout is greater than zero, this method will wait until all published messages are
+   * acknowledged or the timeout elapses.
+   *
+   * This method only waits for acknowledgments if the publisher's QoS profile is RELIABLE.
+   * Otherwise this method will immediately return `true`.
+   *
+   * \param[in] timeout the duration to wait for all published messages to be acknowledged.
+   * \return `true` if all published messages were acknowledged before the given timeout
+   *   elapsed, otherwise `false`.
+   * \throws rclcpp::exceptions::RCLError if middleware doesn't support or internal error occurs
+   * \throws std::invalid_argument if timeout is greater than std::chrono::nanoseconds::max() or
+   *   less than std::chrono::nanoseconds::min()
+   */
+  template<typename DurationRepT = int64_t, typename DurationT = std::milli>
+  bool
+  wait_for_all_acked(
+    std::chrono::duration<DurationRepT, DurationT> timeout =
+    std::chrono::duration<DurationRepT, DurationT>(-1)) const
+  {
+    rcl_duration_value_t rcl_timeout = rcpputils::convert_to_nanoseconds(timeout).count();
+
+    rcl_ret_t ret = rcl_publisher_wait_for_all_acked(publisher_handle_.get(), rcl_timeout);
+    if (ret == RCL_RET_OK) {
+      return true;
+    } else if (ret == RCL_RET_TIMEOUT) {
+      return false;
+    } else {
+      rclcpp::exceptions::throw_from_rcl_error(ret);
+    }
+  }
+
+  /// Set a callback to be called when each new qos event instance occurs.
+  /**
+   * The callback receives a size_t which is the number of events that occurred
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if events occurred before any
+   * callback was set.
+   *
+   * Since this callback is called from the middleware, you should aim to make
+   * it fast and not blocking.
+   * If you need to do a lot of work or wait for some other event, you should
+   * spin it off to another thread, otherwise you risk blocking the middleware.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * An exception will be thrown if the callback is not callable.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the qos event
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rclcpp::QOSEventHandlerBase::set_on_ready_callback
+   *
+   * \param[in] callback functor to be called when a new event occurs
+   * \param[in] event_type identifier for the qos event we want to attach the callback to
+   */
+  void
+  set_on_new_qos_event_callback(
+    std::function<void(size_t)> callback,
+    rcl_publisher_event_type_t event_type)
+  {
+    if (event_handlers_.count(event_type) == 0) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling set_on_new_qos_event_callback for non registered publisher event_type");
+      return;
+    }
+
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_new_qos_event_callback "
+              "is not callable.");
+    }
+
+    // The on_ready_callback signature has an extra `int` argument used to disambiguate between
+    // possible different entities within a generic waitable.
+    // We hide that detail to users of this method.
+    std::function<void(size_t, int)> new_callback = std::bind(callback, std::placeholders::_1);
+    event_handlers_[event_type]->set_on_ready_callback(new_callback);
+  }
+
+  /// Unset the callback registered for new qos events, if any.
+  void
+  clear_on_new_qos_event_callback(rcl_publisher_event_type_t event_type)
+  {
+    if (event_handlers_.count(event_type) == 0) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling clear_on_new_qos_event_callback for non registered event_type");
+      return;
+    }
+
+    event_handlers_[event_type]->clear_on_ready_callback();
+  }
+
 protected:
   template<typename EventCallbackT>
   void
@@ -216,7 +326,7 @@ protected:
       rcl_publisher_event_init,
       publisher_handle_,
       event_type);
-    event_handlers_.emplace_back(handler);
+    event_handlers_.insert(std::make_pair(event_type, handler));
   }
 
   RCLCPP_PUBLIC
@@ -226,7 +336,8 @@ protected:
 
   std::shared_ptr<rcl_publisher_t> publisher_handle_;
 
-  std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
+  std::unordered_map<rcl_publisher_event_type_t,
+    std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
 
   using IntraProcessManagerWeakPtr =
     std::weak_ptr<rclcpp::experimental::IntraProcessManager>;
@@ -235,6 +346,8 @@ protected:
   uint64_t intra_process_publisher_id_;
 
   rmw_gid_t rmw_gid_;
+
+  const rosidl_message_type_support_t type_support_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/publisher_options.hpp

@@ -72,7 +72,7 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
   /// Optional custom allocator.
   std::shared_ptr<Allocator> allocator = nullptr;
 
-  PublisherOptionsWithAllocator<Allocator>() {}
+  PublisherOptionsWithAllocator() {}
 
   /// Constructor using base class as input.
   explicit PublisherOptionsWithAllocator(const PublisherOptionsBase & publisher_options_base)
@@ -119,6 +119,10 @@ private:
   rcl_allocator_t
   get_rcl_allocator() const
   {
+    if constexpr (std::is_same_v<Allocator, std::allocator<void>>) {
+      return rcl_get_default_allocator();
+    }
+
     if (!plain_allocator_storage_) {
       plain_allocator_storage_ =
         std::make_shared<PlainAllocator>(*this->get_allocator());

rclcpp/include/rclcpp/qos_event.hpp

@@ -17,16 +17,21 @@
 
 #include <functional>
 #include <memory>
+#include <mutex>
 #include <stdexcept>
 #include <string>
 
 #include "rcl/error_handling.h"
+#include "rcl/event_callback.h"
+#include "rmw/impl/cpp/demangle.hpp"
 #include "rmw/incompatible_qos_events_statuses.h"
 
 #include "rcutils/logging_macros.h"
 
+#include "rclcpp/detail/cpp_callback_trampoline.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/function_traits.hpp"
+#include "rclcpp/logging.hpp"
 #include "rclcpp/waitable.hpp"
 
 namespace rclcpp
@@ -84,6 +89,11 @@ public:
 class QOSEventHandlerBase : public Waitable
 {
 public:
+  enum class EntityType : std::size_t
+  {
+    Event,
+  };
+
   RCLCPP_PUBLIC
   virtual ~QOSEventHandlerBase();
 
@@ -94,7 +104,7 @@ public:
 
   /// Add the Waitable to a wait set.
   RCLCPP_PUBLIC
-  bool
+  void
   add_to_wait_set(rcl_wait_set_t * wait_set) override;
 
   /// Check if the Waitable is ready.
@@ -102,9 +112,111 @@ public:
   bool
   is_ready(rcl_wait_set_t * wait_set) override;
 
+  /// Set a callback to be called when each new event instance occurs.
+  /**
+   * The callback receives a size_t which is the number of events that occurred
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if events occurred before any
+   * callback was set.
+   *
+   * The callback also receives an int identifier argument.
+   * This is needed because a Waitable may be composed of several distinct entities,
+   * such as subscriptions, services, etc.
+   * The application should provide a generic callback function that will be then
+   * forwarded by the waitable to all of its entities.
+   * Before forwarding, a different value for the identifier argument will be
+   * bond to the function.
+   * This implies that the provided callback can use the identifier to behave
+   * differently depending on which entity triggered the waitable to become ready.
+   *
+   * Since this callback is called from the middleware, you should aim to make
+   * it fast and not blocking.
+   * If you need to do a lot of work or wait for some other event, you should
+   * spin it off to another thread, otherwise you risk blocking the middleware.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * An exception will be thrown if the callback is not callable.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the qos event
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rmw_event_set_callback
+   * \sa rcl_event_set_callback
+   *
+   * \param[in] callback functor to be called when a new event occurs
+   */
+  void
+  set_on_ready_callback(std::function<void(size_t, int)> callback) override
+  {
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_ready_callback "
+              "is not callable.");
+    }
+
+    // Note: we bind the int identifier argument to this waitable's entity types
+    auto new_callback =
+      [callback, this](size_t number_of_events) {
+        try {
+          callback(number_of_events, static_cast<int>(EntityType::Event));
+        } catch (const std::exception & exception) {
+          RCLCPP_ERROR_STREAM(
+            // TODO(wjwwood): get this class access to the node logger it is associated with
+            rclcpp::get_logger("rclcpp"),
+            "rclcpp::QOSEventHandlerBase@" << this <<
+              " caught " << rmw::impl::cpp::demangle(exception) <<
+              " exception in user-provided callback for the 'on ready' callback: " <<
+              exception.what());
+        } catch (...) {
+          RCLCPP_ERROR_STREAM(
+            rclcpp::get_logger("rclcpp"),
+            "rclcpp::QOSEventHandlerBase@" << this <<
+              " caught unhandled exception in user-provided callback " <<
+              "for the 'on ready' callback");
+        }
+      };
+
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+
+    // Set it temporarily to the new callback, while we replace the old one.
+    // This two-step setting, prevents a gap where the old std::function has
+    // been replaced but the middleware hasn't been told about the new one yet.
+    set_on_new_event_callback(
+      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      static_cast<const void *>(&new_callback));
+
+    // Store the std::function to keep it in scope, also overwrites the existing one.
+    on_new_event_callback_ = new_callback;
+
+    // Set it again, now using the permanent storage.
+    set_on_new_event_callback(
+      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      static_cast<const void *>(&on_new_event_callback_));
+  }
+
+  /// Unset the callback registered for new events, if any.
+  void
+  clear_on_ready_callback() override
+  {
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+    if (on_new_event_callback_) {
+      set_on_new_event_callback(nullptr, nullptr);
+      on_new_event_callback_ = nullptr;
+    }
+  }
+
 protected:
+  RCLCPP_PUBLIC
+  void
+  set_on_new_event_callback(rcl_event_callback_t callback, const void * user_data);
+
   rcl_event_t event_handle_;
   size_t wait_set_event_index_;
+  std::recursive_mutex callback_mutex_;
+  std::function<void(size_t)> on_new_event_callback_{nullptr};
 };
 
 template<typename EventCallbackT, typename ParentHandleT>
@@ -117,9 +229,8 @@ public:
     InitFuncT init_func,
     ParentHandleT parent_handle,
     EventTypeEnum event_type)
-  : event_callback_(callback)
+  : parent_handle_(parent_handle), event_callback_(callback)
   {
-    parent_handle_ = parent_handle;
     event_handle_ = rcl_get_zero_initialized_event();
     rcl_ret_t ret = init_func(&event_handle_, parent_handle.get(), event_type);
     if (ret != RCL_RET_OK) {
@@ -148,6 +259,13 @@ public:
     return std::static_pointer_cast<void>(std::make_shared<EventCallbackInfoT>(callback_info));
   }
 
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id) override
+  {
+    (void)id;
+    return take_data();
+  }
+
   /// Execute any entities of the Waitable that are ready.
   void
   execute(std::shared_ptr<void> & data) override

rclcpp/include/rclcpp/rclcpp.hpp

@@ -140,7 +140,6 @@
  *   - rclcpp/duration.hpp
  *   - rclcpp/function_traits.hpp
  *   - rclcpp/macros.hpp
- *   - rclcpp/scope_exit.hpp
  *   - rclcpp/time.hpp
  *   - rclcpp/utilities.hpp
  *   - rclcpp/typesupport_helpers.hpp

rclcpp/include/rclcpp/scope_exit.hpp

@@ -19,6 +19,10 @@
 #ifndef RCLCPP__SCOPE_EXIT_HPP_
 #define RCLCPP__SCOPE_EXIT_HPP_
 
+// TODO(christophebedard) remove this header completely in I-turtle
+
+#warning rclcpp/scope_exit.hpp has been deprecated, please use rcpputils/scope_exit.hpp instead
+
 #include <functional>
 
 #include "rclcpp/macros.hpp"

rclcpp/include/rclcpp/service.hpp

@@ -19,23 +19,30 @@
 #include <functional>
 #include <iostream>
 #include <memory>
+#include <mutex>
 #include <sstream>
 #include <string>
 
 #include "rcl/error_handling.h"
+#include "rcl/event_callback.h"
 #include "rcl/service.h"
 
-#include "rclcpp/any_service_callback.hpp"
-#include "rclcpp/exceptions.hpp"
-#include "rclcpp/macros.hpp"
-#include "rclcpp/type_support_decl.hpp"
-#include "rclcpp/expand_topic_or_service_name.hpp"
-#include "rclcpp/visibility_control.hpp"
-#include "rclcpp/logging.hpp"
 #include "rmw/error_handling.h"
+#include "rmw/impl/cpp/demangle.hpp"
 #include "rmw/rmw.h"
+
 #include "tracetools/tracetools.h"
 
+#include "rclcpp/any_service_callback.hpp"
+#include "rclcpp/detail/cpp_callback_trampoline.hpp"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
+#include "rclcpp/logging.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/visibility_control.hpp"
+
 namespace rclcpp
 {
 
@@ -121,6 +128,123 @@ public:
   bool
   exchange_in_use_by_wait_set_state(bool in_use_state);
 
+  /// Get the actual response publisher QoS settings, after the defaults have been determined.
+  /**
+   * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT
+   * can only be resolved after the creation of the service, and it
+   * depends on the underlying rmw implementation.
+   * If the underlying setting in use can't be represented in ROS terms,
+   * it will be set to RMW_QOS_POLICY_*_UNKNOWN.
+   * May throw runtime_error when an unexpected error occurs.
+   *
+   * \return The actual response publisher qos settings.
+   * \throws std::runtime_error if failed to get qos settings
+   */
+  RCLCPP_PUBLIC
+  rclcpp::QoS
+  get_response_publisher_actual_qos() const;
+
+  /// Get the actual request subscription QoS settings, after the defaults have been determined.
+  /**
+   * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT
+   * can only be resolved after the creation of the service, and it
+   * depends on the underlying rmw implementation.
+   * If the underlying setting in use can't be represented in ROS terms,
+   * it will be set to RMW_QOS_POLICY_*_UNKNOWN.
+   * May throw runtime_error when an unexpected error occurs.
+   *
+   * \return The actual request subscription qos settings.
+   * \throws std::runtime_error if failed to get qos settings
+   */
+  RCLCPP_PUBLIC
+  rclcpp::QoS
+  get_request_subscription_actual_qos() const;
+
+  /// Set a callback to be called when each new request is received.
+  /**
+   * The callback receives a size_t which is the number of requests received
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if requests were received before any
+   * callback was set.
+   *
+   * Since this callback is called from the middleware, you should aim to make
+   * it fast and not blocking.
+   * If you need to do a lot of work or wait for some other event, you should
+   * spin it off to another thread, otherwise you risk blocking the middleware.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   *
+   * An exception will be thrown if the callback is not callable.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the service
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rmw_service_set_on_new_request_callback
+   * \sa rcl_service_set_on_new_request_callback
+   *
+   * \param[in] callback functor to be called when a new request is received
+   */
+  void
+  set_on_new_request_callback(std::function<void(size_t)> callback)
+  {
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_new_request_callback "
+              "is not callable.");
+    }
+
+    auto new_callback =
+      [callback, this](size_t number_of_requests) {
+        try {
+          callback(number_of_requests);
+        } catch (const std::exception & exception) {
+          RCLCPP_ERROR_STREAM(
+            node_logger_,
+            "rclcpp::ServiceBase@" << this <<
+              " caught " << rmw::impl::cpp::demangle(exception) <<
+              " exception in user-provided callback for the 'on new request' callback: " <<
+              exception.what());
+        } catch (...) {
+          RCLCPP_ERROR_STREAM(
+            node_logger_,
+            "rclcpp::ServiceBase@" << this <<
+              " caught unhandled exception in user-provided callback " <<
+              "for the 'on new request' callback");
+        }
+      };
+
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+
+    // Set it temporarily to the new callback, while we replace the old one.
+    // This two-step setting, prevents a gap where the old std::function has
+    // been replaced but the middleware hasn't been told about the new one yet.
+    set_on_new_request_callback(
+      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      static_cast<const void *>(&new_callback));
+
+    // Store the std::function to keep it in scope, also overwrites the existing one.
+    on_new_request_callback_ = new_callback;
+
+    // Set it again, now using the permanent storage.
+    set_on_new_request_callback(
+      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      static_cast<const void *>(&on_new_request_callback_));
+  }
+
+  /// Unset the callback registered for new requests, if any.
+  void
+  clear_on_new_request_callback()
+  {
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+    if (on_new_request_callback_) {
+      set_on_new_request_callback(nullptr, nullptr);
+      on_new_request_callback_ = nullptr;
+    }
+  }
+
 protected:
   RCLCPP_DISABLE_COPY(ServiceBase)
 
@@ -132,16 +256,27 @@ protected:
   const rcl_node_t *
   get_rcl_node_handle() const;
 
+  RCLCPP_PUBLIC
+  void
+  set_on_new_request_callback(rcl_event_callback_t callback, const void * user_data);
+
   std::shared_ptr<rcl_node_t> node_handle_;
 
   std::shared_ptr<rcl_service_t> service_handle_;
   bool owns_rcl_handle_ = true;
 
+  rclcpp::Logger node_logger_;
+
   std::atomic<bool> in_use_by_wait_set_{false};
+
+  std::recursive_mutex callback_mutex_;
+  std::function<void(size_t)> on_new_request_callback_{nullptr};
 };
 
 template<typename ServiceT>
-class Service : public ServiceBase
+class Service
+  : public ServiceBase,
+  public std::enable_shared_from_this<Service<ServiceT>>
 {
 public:
   using CallbackType = std::function<
@@ -335,9 +470,10 @@ public:
     std::shared_ptr<void> request) override
   {
     auto typed_request = std::static_pointer_cast<typename ServiceT::Request>(request);
-    auto response = std::make_shared<typename ServiceT::Response>();
-    any_callback_.dispatch(request_header, typed_request, response);
-    send_response(*request_header, *response);
+    auto response = any_callback_.dispatch(this->shared_from_this(), request_header, typed_request);
+    if (response) {
+      send_response(*request_header, *response);
+    }
   }
 
   void
@@ -345,6 +481,14 @@ public:
   {
     rcl_ret_t ret = rcl_send_response(get_service_handle().get(), &req_id, &response);
 
+    if (ret == RCL_RET_TIMEOUT) {
+      RCLCPP_WARN(
+        node_logger_.get_child("rclcpp"),
+        "failed to send response to %s (timeout): %s",
+        this->get_service_name(), rcl_get_error_string().str);
+      rcl_reset_error();
+      return;
+    }
     if (ret != RCL_RET_OK) {
       rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send response");
     }

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -21,6 +21,7 @@
 #include "rcl/allocator.h"
 
 #include "rclcpp/allocator/allocator_common.hpp"
+#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp"
 #include "rclcpp/memory_strategy.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -61,17 +62,17 @@ public:
     allocator_ = std::make_shared<VoidAlloc>();
   }
 
-  void add_guard_condition(const rcl_guard_condition_t * guard_condition) override
+  void add_guard_condition(const rclcpp::GuardCondition & guard_condition) override
   {
     for (const auto & existing_guard_condition : guard_conditions_) {
-      if (existing_guard_condition == guard_condition) {
+      if (existing_guard_condition == &guard_condition) {
         return;
       }
     }
-    guard_conditions_.push_back(guard_condition);
+    guard_conditions_.push_back(&guard_condition);
   }
 
-  void remove_guard_condition(const rcl_guard_condition_t * guard_condition) override
+  void remove_guard_condition(const rclcpp::GuardCondition * guard_condition) override
   {
     for (auto it = guard_conditions_.begin(); it != guard_conditions_.end(); ++it) {
       if (*it == guard_condition) {
@@ -163,30 +164,22 @@ public:
       if (!group || !group->can_be_taken_from().load()) {
         continue;
       }
-      group->find_subscription_ptrs_if(
+
+      group->collect_all_ptrs(
         [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
           subscription_handles_.push_back(subscription->get_subscription_handle());
-          return false;
-        });
-      group->find_service_ptrs_if(
+        },
         [this](const rclcpp::ServiceBase::SharedPtr & service) {
           service_handles_.push_back(service->get_service_handle());
-          return false;
-        });
-      group->find_client_ptrs_if(
+        },
         [this](const rclcpp::ClientBase::SharedPtr & client) {
           client_handles_.push_back(client->get_client_handle());
-          return false;
-        });
-      group->find_timer_ptrs_if(
+        },
         [this](const rclcpp::TimerBase::SharedPtr & timer) {
           timer_handles_.push_back(timer->get_timer_handle());
-          return false;
-        });
-      group->find_waitable_ptrs_if(
+        },
         [this](const rclcpp::Waitable::SharedPtr & waitable) {
           waitable_handles_.push_back(waitable);
-          return false;
         });
     }
 
@@ -203,7 +196,7 @@ public:
 
   bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) override
   {
-    for (auto subscription : subscription_handles_) {
+    for (const std::shared_ptr<const rcl_subscription_t> & subscription : subscription_handles_) {
       if (rcl_wait_set_add_subscription(wait_set, subscription.get(), NULL) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
@@ -212,7 +205,7 @@ public:
       }
     }
 
-    for (auto client : client_handles_) {
+    for (const std::shared_ptr<const rcl_client_t> & client : client_handles_) {
       if (rcl_wait_set_add_client(wait_set, client.get(), NULL) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
@@ -221,7 +214,7 @@ public:
       }
     }
 
-    for (auto service : service_handles_) {
+    for (const std::shared_ptr<const rcl_service_t> & service : service_handles_) {
       if (rcl_wait_set_add_service(wait_set, service.get(), NULL) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
@@ -230,7 +223,7 @@ public:
       }
     }
 
-    for (auto timer : timer_handles_) {
+    for (const std::shared_ptr<const rcl_timer_t> & timer : timer_handles_) {
       if (rcl_wait_set_add_timer(wait_set, timer.get(), NULL) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
@@ -240,22 +233,11 @@ public:
     }
 
     for (auto guard_condition : guard_conditions_) {
-      if (rcl_wait_set_add_guard_condition(wait_set, guard_condition, NULL) != RCL_RET_OK) {
-        RCUTILS_LOG_ERROR_NAMED(
-          "rclcpp",
-          "Couldn't add guard_condition to wait set: %s",
-          rcl_get_error_string().str);
-        return false;
-      }
+      detail::add_guard_condition_to_rcl_wait_set(*wait_set, *guard_condition);
     }
 
-    for (auto waitable : waitable_handles_) {
-      if (!waitable->add_to_wait_set(wait_set)) {
-        RCUTILS_LOG_ERROR_NAMED(
-          "rclcpp",
-          "Couldn't add waitable to wait set: %s", rcl_get_error_string().str);
-        return false;
-      }
+    for (const std::shared_ptr<Waitable> & waitable : waitable_handles_) {
+      waitable->add_to_wait_set(wait_set);
     }
     return true;
   }
@@ -388,6 +370,11 @@ public:
           ++it;
           continue;
         }
+        if (!timer->call()) {
+          // timer was cancelled, skip it.
+          ++it;
+          continue;
+        }
         // Otherwise it is safe to set and return the any_exec
         any_exec.timer = timer;
         any_exec.callback_group = group;
@@ -395,7 +382,7 @@ public:
         timer_handles_.erase(it);
         return;
       }
-      // Else, the service is no longer valid, remove it and continue
+      // Else, the timer is no longer valid, remove it and continue
       it = timer_handles_.erase(it);
     }
   }
@@ -407,7 +394,7 @@ public:
   {
     auto it = waitable_handles_.begin();
     while (it != waitable_handles_.end()) {
-      auto waitable = *it;
+      std::shared_ptr<Waitable> & waitable = *it;
       if (waitable) {
         // Find the group for this handle and see if it can be serviced
         auto group = get_group_by_waitable(waitable, weak_groups_to_nodes);
@@ -437,13 +424,17 @@ public:
 
   rcl_allocator_t get_allocator() override
   {
-    return rclcpp::allocator::get_rcl_allocator<void *, VoidAlloc>(*allocator_.get());
+    if constexpr (std::is_same_v<Alloc, std::allocator<void>>) {
+      return rcl_get_default_allocator();
+    } else {
+      return rclcpp::allocator::get_rcl_allocator<void *, VoidAlloc>(*allocator_.get());
+    }
   }
 
   size_t number_of_ready_subscriptions() const override
   {
     size_t number_of_subscriptions = subscription_handles_.size();
-    for (auto waitable : waitable_handles_) {
+    for (const std::shared_ptr<Waitable> & waitable : waitable_handles_) {
       number_of_subscriptions += waitable->get_number_of_ready_subscriptions();
     }
     return number_of_subscriptions;
@@ -452,7 +443,7 @@ public:
   size_t number_of_ready_services() const override
   {
     size_t number_of_services = service_handles_.size();
-    for (auto waitable : waitable_handles_) {
+    for (const std::shared_ptr<Waitable> & waitable : waitable_handles_) {
       number_of_services += waitable->get_number_of_ready_services();
     }
     return number_of_services;
@@ -461,7 +452,7 @@ public:
   size_t number_of_ready_events() const override
   {
     size_t number_of_events = 0;
-    for (auto waitable : waitable_handles_) {
+    for (const std::shared_ptr<Waitable> & waitable : waitable_handles_) {
       number_of_events += waitable->get_number_of_ready_events();
     }
     return number_of_events;
@@ -470,7 +461,7 @@ public:
   size_t number_of_ready_clients() const override
   {
     size_t number_of_clients = client_handles_.size();
-    for (auto waitable : waitable_handles_) {
+    for (const std::shared_ptr<Waitable> & waitable : waitable_handles_) {
       number_of_clients += waitable->get_number_of_ready_clients();
     }
     return number_of_clients;
@@ -479,7 +470,7 @@ public:
   size_t number_of_guard_conditions() const override
   {
     size_t number_of_guard_conditions = guard_conditions_.size();
-    for (auto waitable : waitable_handles_) {
+    for (const std::shared_ptr<Waitable> & waitable : waitable_handles_) {
       number_of_guard_conditions += waitable->get_number_of_ready_guard_conditions();
     }
     return number_of_guard_conditions;
@@ -488,7 +479,7 @@ public:
   size_t number_of_ready_timers() const override
   {
     size_t number_of_timers = timer_handles_.size();
-    for (auto waitable : waitable_handles_) {
+    for (const std::shared_ptr<Waitable> & waitable : waitable_handles_) {
       number_of_timers += waitable->get_number_of_ready_timers();
     }
     return number_of_timers;
@@ -504,7 +495,7 @@ private:
   using VectorRebind =
     std::vector<T, typename std::allocator_traits<Alloc>::template rebind_alloc<T>>;
 
-  VectorRebind<const rcl_guard_condition_t *> guard_conditions_;
+  VectorRebind<const rclcpp::GuardCondition *> guard_conditions_;
 
   VectorRebind<std::shared_ptr<const rcl_subscription_t>> subscription_handles_;
   VectorRebind<std::shared_ptr<const rcl_service_t>> service_handles_;

rclcpp/include/rclcpp/subscription.hpp

@@ -146,19 +146,19 @@ public:
     options_(options),
     message_memory_strategy_(message_memory_strategy)
   {
-    if (options.event_callbacks.deadline_callback) {
+    if (options_.event_callbacks.deadline_callback) {
       this->add_event_handler(
-        options.event_callbacks.deadline_callback,
+        options_.event_callbacks.deadline_callback,
         RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);
     }
-    if (options.event_callbacks.liveliness_callback) {
+    if (options_.event_callbacks.liveliness_callback) {
       this->add_event_handler(
-        options.event_callbacks.liveliness_callback,
+        options_.event_callbacks.liveliness_callback,
         RCL_SUBSCRIPTION_LIVELINESS_CHANGED);
     }
-    if (options.event_callbacks.incompatible_qos_callback) {
+    if (options_.event_callbacks.incompatible_qos_callback) {
       this->add_event_handler(
-        options.event_callbacks.incompatible_qos_callback,
+        options_.event_callbacks.incompatible_qos_callback,
         RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
     } else if (options_.use_default_callbacks) {
       // Register default callback when not specified
@@ -172,40 +172,50 @@ public:
         // pass
       }
     }
-    if (options.event_callbacks.message_lost_callback) {
+    if (options_.event_callbacks.message_lost_callback) {
       this->add_event_handler(
-        options.event_callbacks.message_lost_callback,
+        options_.event_callbacks.message_lost_callback,
         RCL_SUBSCRIPTION_MESSAGE_LOST);
     }
 
     // Setup intra process publishing if requested.
-    if (rclcpp::detail::resolve_use_intra_process(options, *node_base)) {
+    if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) {
       using rclcpp::detail::resolve_intra_process_buffer_type;
 
       // 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_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");
       }
       if (qos_profile.durability() != rclcpp::DurabilityPolicy::Volatile) {
         throw std::invalid_argument(
                 "intraprocess communication allowed only with volatile durability");
       }
 
+      using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess<
+        MessageT,
+        SubscribedType,
+        SubscribedTypeAllocator,
+        SubscribedTypeDeleter,
+        ROSMessageT,
+        AllocatorT>;
+
       // First create a SubscriptionIntraProcess which will be given to the intra-process manager.
       auto context = node_base->get_context();
       subscription_intra_process_ = std::make_shared<SubscriptionIntraProcessT>(
         callback,
-        options.get_allocator(),
+        options_.get_allocator(),
         context,
         this->get_topic_name(),  // important to get like this, as it has the fully-qualified name
         qos_profile,
-        resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback));
+        resolve_intra_process_buffer_type(options_.intra_process_buffer_type, callback));
       TRACEPOINT(
         rclcpp_subscription_init,
         static_cast<const void *>(get_subscription_handle().get()),
@@ -276,7 +286,7 @@ public:
 
   /// Take the next message from the inter-process subscription.
   /**
-   * This verison takes a SubscribedType which is different frmo the
+   * This version takes a SubscribedType which is different from the
    * ROSMessageType when a rclcpp::TypeAdapter is in used.
    *
    * \sa take(ROSMessageType &, rclcpp::MessageInfo &)
@@ -355,11 +365,31 @@ public:
     void * loaned_message,
     const rclcpp::MessageInfo & message_info) override
   {
+    if (matches_any_intra_process_publishers(&message_info.get_rmw_message_info().publisher_gid)) {
+      // In this case, the message will be delivered via intra process and
+      // we should ignore this copy of the message.
+      return;
+    }
+
     auto typed_message = static_cast<ROSMessageType *>(loaned_message);
     // message is loaned, so we have to make sure that the deleter does not deallocate the message
     auto sptr = std::shared_ptr<ROSMessageType>(
       typed_message, [](ROSMessageType * msg) {(void) msg;});
+
+    std::chrono::time_point<std::chrono::system_clock> now;
+    if (subscription_topic_statistics_) {
+      // get current time before executing callback to
+      // exclude callback duration from topic statistics result.
+      now = std::chrono::system_clock::now();
+    }
+
     any_callback_.dispatch(sptr, message_info);
+
+    if (subscription_topic_statistics_) {
+      const auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(now);
+      const auto time = rclcpp::Time(nanos.time_since_epoch().count());
+      subscription_topic_statistics_->handle_message(*typed_message, time);
+    }
   }
 
   /// Return the borrowed message.
@@ -404,13 +434,6 @@ private:
 
   /// Component which computes and publishes topic statistics for this subscriber
   SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics_{nullptr};
-
-  using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess<
-    ROSMessageType,
-    AllocatorT,
-    ROSMessageTypeDeleter,
-    MessageT>;
-  std::shared_ptr<SubscriptionIntraProcessT> subscription_intra_process_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/subscription_base.hpp

@@ -17,16 +17,20 @@
 
 #include <atomic>
 #include <memory>
+#include <mutex>
 #include <string>
 #include <unordered_map>
 #include <vector>
 #include <utility>
 
+#include "rcl/event_callback.h"
 #include "rcl/subscription.h"
 
+#include "rmw/impl/cpp/demangle.hpp"
 #include "rmw/rmw.h"
 
 #include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/detail/cpp_callback_trampoline.hpp"
 #include "rclcpp/experimental/intra_process_manager.hpp"
 #include "rclcpp/experimental/subscription_intra_process_base.hpp"
 #include "rclcpp/macros.hpp"
@@ -35,6 +39,7 @@
 #include "rclcpp/qos.hpp"
 #include "rclcpp/qos_event.hpp"
 #include "rclcpp/serialized_message.hpp"
+#include "rclcpp/subscription_content_filter_options.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -99,15 +104,16 @@ public:
   get_subscription_handle() const;
 
   /// Get all the QoS event handlers associated with this subscription.
-  /** \return The vector of QoS event handlers. */
+  /** \return The map of QoS event handlers. */
   RCLCPP_PUBLIC
-  const std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
+  const
+  std::unordered_map<rcl_subscription_event_type_t, std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
   get_event_handlers() const;
 
   /// Get the actual QoS settings, after the defaults have been determined.
   /**
    * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT
-   * can only be resolved after the creation of the publisher, and it
+   * can only be resolved after the creation of the subscription, and it
    * depends on the underlying rmw implementation.
    * If the underlying setting in use can't be represented in ROS terms,
    * it will be set to RMW_QOS_POLICY_*_UNKNOWN.
@@ -283,6 +289,243 @@ public:
   std::vector<rclcpp::NetworkFlowEndpoint>
   get_network_flow_endpoints() const;
 
+  /// Set a callback to be called when each new message is received.
+  /**
+   * The callback receives a size_t which is the number of messages received
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if messages were received before any
+   * callback was set.
+   *
+   * Since this callback is called from the middleware, you should aim to make
+   * it fast and not blocking.
+   * If you need to do a lot of work or wait for some other event, you should
+   * spin it off to another thread, otherwise you risk blocking the middleware.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the subscription
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rmw_subscription_set_on_new_message_callback
+   * \sa rcl_subscription_set_on_new_message_callback
+   *
+   * \param[in] callback functor to be called when a new message is received
+   */
+  void
+  set_on_new_message_callback(std::function<void(size_t)> callback)
+  {
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_new_message_callback "
+              "is not callable.");
+    }
+
+    auto new_callback =
+      [callback, this](size_t number_of_messages) {
+        try {
+          callback(number_of_messages);
+        } catch (const std::exception & exception) {
+          RCLCPP_ERROR_STREAM(
+            node_logger_,
+            "rclcpp::SubscriptionBase@" << this <<
+              " caught " << rmw::impl::cpp::demangle(exception) <<
+              " exception in user-provided callback for the 'on new message' callback: " <<
+              exception.what());
+        } catch (...) {
+          RCLCPP_ERROR_STREAM(
+            node_logger_,
+            "rclcpp::SubscriptionBase@" << this <<
+              " caught unhandled exception in user-provided callback " <<
+              "for the 'on new message' callback");
+        }
+      };
+
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+
+    // Set it temporarily to the new callback, while we replace the old one.
+    // This two-step setting, prevents a gap where the old std::function has
+    // been replaced but the middleware hasn't been told about the new one yet.
+    set_on_new_message_callback(
+      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      static_cast<const void *>(&new_callback));
+
+    // Store the std::function to keep it in scope, also overwrites the existing one.
+    on_new_message_callback_ = new_callback;
+
+    // Set it again, now using the permanent storage.
+    set_on_new_message_callback(
+      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      static_cast<const void *>(&on_new_message_callback_));
+  }
+
+  /// Unset the callback registered for new messages, if any.
+  void
+  clear_on_new_message_callback()
+  {
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+
+    if (on_new_message_callback_) {
+      set_on_new_message_callback(nullptr, nullptr);
+      on_new_message_callback_ = nullptr;
+    }
+  }
+
+  /// Set a callback to be called when each new intra-process message is received.
+  /**
+   * The callback receives a size_t which is the number of messages received
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if messages were received before any
+   * callback was set.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the subscription
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rclcpp::SubscriptionIntraProcessBase::set_on_ready_callback
+   *
+   * \param[in] callback functor to be called when a new message is received
+   */
+  void
+  set_on_new_intra_process_message_callback(std::function<void(size_t)> callback)
+  {
+    if (!use_intra_process_) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling set_on_new_intra_process_message_callback for subscription with IPC disabled");
+      return;
+    }
+
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_new_intra_process_message_callback "
+              "is not callable.");
+    }
+
+    // The on_ready_callback signature has an extra `int` argument used to disambiguate between
+    // possible different entities within a generic waitable.
+    // We hide that detail to users of this method.
+    std::function<void(size_t, int)> new_callback = std::bind(callback, std::placeholders::_1);
+    subscription_intra_process_->set_on_ready_callback(new_callback);
+  }
+
+  /// Unset the callback registered for new intra-process messages, if any.
+  void
+  clear_on_new_intra_process_message_callback()
+  {
+    if (!use_intra_process_) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling clear_on_new_intra_process_message_callback for subscription with IPC disabled");
+      return;
+    }
+
+    subscription_intra_process_->clear_on_ready_callback();
+  }
+
+  /// Set a callback to be called when each new qos event instance occurs.
+  /**
+   * The callback receives a size_t which is the number of events that occurred
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if events occurred before any
+   * callback was set.
+   *
+   * Since this callback is called from the middleware, you should aim to make
+   * it fast and not blocking.
+   * If you need to do a lot of work or wait for some other event, you should
+   * spin it off to another thread, otherwise you risk blocking the middleware.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * An exception will be thrown if the callback is not callable.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the qos event
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rclcpp::QOSEventHandlerBase::set_on_ready_callback
+   *
+   * \param[in] callback functor to be called when a new event occurs
+   * \param[in] event_type identifier for the qos event we want to attach the callback to
+   */
+  void
+  set_on_new_qos_event_callback(
+    std::function<void(size_t)> callback,
+    rcl_subscription_event_type_t event_type)
+  {
+    if (event_handlers_.count(event_type) == 0) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling set_on_new_qos_event_callback for non registered subscription event_type");
+      return;
+    }
+
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_new_qos_event_callback "
+              "is not callable.");
+    }
+
+    // The on_ready_callback signature has an extra `int` argument used to disambiguate between
+    // possible different entities within a generic waitable.
+    // We hide that detail to users of this method.
+    std::function<void(size_t, int)> new_callback = std::bind(callback, std::placeholders::_1);
+    event_handlers_[event_type]->set_on_ready_callback(new_callback);
+  }
+
+  /// Unset the callback registered for new qos events, if any.
+  void
+  clear_on_new_qos_event_callback(rcl_subscription_event_type_t event_type)
+  {
+    if (event_handlers_.count(event_type) == 0) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling clear_on_new_qos_event_callback for non registered event_type");
+      return;
+    }
+
+    event_handlers_[event_type]->clear_on_ready_callback();
+  }
+
+  /// Check if content filtered topic feature of the subscription instance is enabled.
+  /**
+   * \return boolean flag indicating if the content filtered topic of this subscription is enabled.
+   */
+  RCLCPP_PUBLIC
+  bool
+  is_cft_enabled() const;
+
+  /// Set the filter expression and expression parameters for the subscription.
+  /**
+   * \param[in] filter_expression A filter expression to set.
+   *   \sa ContentFilterOptions::filter_expression
+   *   An empty string ("") will clear the content filter setting of the subscription.
+   * \param[in] expression_parameters Array of expression parameters to set.
+   *   \sa ContentFilterOptions::expression_parameters
+   * \throws RCLBadAlloc if memory cannot be allocated
+   * \throws RCLError if an unexpect error occurs
+   */
+  RCLCPP_PUBLIC
+  void
+  set_content_filter(
+    const std::string & filter_expression,
+    const std::vector<std::string> & expression_parameters = {});
+
+  /// Get the filter expression and expression parameters for the subscription.
+  /**
+   * \return rclcpp::ContentFilterOptions The content filter options to get.
+   * \throws RCLBadAlloc if memory cannot be allocated
+   * \throws RCLError if an unexpect error occurs
+   */
+  RCLCPP_PUBLIC
+  rclcpp::ContentFilterOptions
+  get_content_filter() const;
+
 protected:
   template<typename EventCallbackT>
   void
@@ -297,7 +540,7 @@ protected:
       get_subscription_handle(),
       event_type);
     qos_events_in_use_by_wait_set_.insert(std::make_pair(handler.get(), false));
-    event_handlers_.emplace_back(handler);
+    event_handlers_.insert(std::make_pair(event_type, handler));
   }
 
   RCLCPP_PUBLIC
@@ -307,17 +550,24 @@ protected:
   bool
   matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const;
 
+  RCLCPP_PUBLIC
+  void
+  set_on_new_message_callback(rcl_event_callback_t callback, const void * user_data);
+
   rclcpp::node_interfaces::NodeBaseInterface * const node_base_;
 
   std::shared_ptr<rcl_node_t> node_handle_;
   std::shared_ptr<rcl_subscription_t> subscription_handle_;
   std::shared_ptr<rcl_subscription_t> intra_process_subscription_handle_;
+  rclcpp::Logger node_logger_;
 
-  std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
+  std::unordered_map<rcl_subscription_event_type_t,
+    std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
 
   bool use_intra_process_;
   IntraProcessManagerWeakPtr weak_ipm_;
   uint64_t intra_process_subscription_id_;
+  std::shared_ptr<rclcpp::experimental::SubscriptionIntraProcessBase> subscription_intra_process_;
 
 private:
   RCLCPP_DISABLE_COPY(SubscriptionBase)
@@ -329,6 +579,9 @@ private:
   std::atomic<bool> intra_process_subscription_waitable_in_use_by_wait_set_{false};
   std::unordered_map<rclcpp::QOSEventHandlerBase *,
     std::atomic<bool>> qos_events_in_use_by_wait_set_;
+
+  std::recursive_mutex callback_mutex_;
+  std::function<void(size_t)> on_new_message_callback_{nullptr};
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/subscription_content_filter_options.hpp

@@ -0,0 +1,38 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__SUBSCRIPTION_CONTENT_FILTER_OPTIONS_HPP_
+#define RCLCPP__SUBSCRIPTION_CONTENT_FILTER_OPTIONS_HPP_
+
+#include <string>
+#include <vector>
+
+namespace rclcpp
+{
+
+/// Options to configure content filtered topic in the subscription.
+struct ContentFilterOptions
+{
+  /// Filter expression is similar to the WHERE part of an SQL clause.
+  std::string filter_expression;
+  /**
+   * Expression parameters is the tokens placeholder ‘parameters’ (i.e., "%n" tokens begin from 0)
+   * in the filter_expression. The maximum expression_parameters size is 100.
+   */
+  std::vector<std::string> expression_parameters;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__SUBSCRIPTION_CONTENT_FILTER_OPTIONS_HPP_

rclcpp/include/rclcpp/subscription_factory.hpp

@@ -26,14 +26,13 @@
 
 #include "rclcpp/any_subscription_callback.hpp"
 #include "rclcpp/get_message_type_support_handle.hpp"
-#include "rclcpp/intra_process_buffer_type.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/subscription.hpp"
 #include "rclcpp/subscription_options.hpp"
 #include "rclcpp/subscription_traits.hpp"
-#include "rclcpp/visibility_control.hpp"
 #include "rclcpp/topic_statistics/subscription_topic_statistics.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {

rclcpp/include/rclcpp/subscription_options.hpp

@@ -28,6 +28,7 @@
 #include "rclcpp/qos.hpp"
 #include "rclcpp/qos_event.hpp"
 #include "rclcpp/qos_overriding_options.hpp"
+#include "rclcpp/subscription_content_filter_options.hpp"
 #include "rclcpp/topic_statistics_state.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -81,6 +82,8 @@ struct SubscriptionOptionsBase
   TopicStatisticsOptions topic_stats_options;
 
   QosOverridingOptions qos_overriding_options;
+
+  ContentFilterOptions content_filter_options;
 };
 
 /// Structure containing optional configuration for Subscriptions.
@@ -94,7 +97,7 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
   /// Optional custom allocator.
   std::shared_ptr<Allocator> allocator = nullptr;
 
-  SubscriptionOptionsWithAllocator<Allocator>() {}
+  SubscriptionOptionsWithAllocator() {}
 
   /// Constructor using base class as input.
   explicit SubscriptionOptionsWithAllocator(
@@ -123,6 +126,21 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
       rmw_implementation_payload->modify_rmw_subscription_options(result.rmw_subscription_options);
     }
 
+    // Copy content_filter_options into rcl_subscription_options.
+    if (!content_filter_options.filter_expression.empty()) {
+      std::vector<const char *> cstrings =
+        get_c_vector_string(content_filter_options.expression_parameters);
+      rcl_ret_t ret = rcl_subscription_options_set_content_filter_options(
+        get_c_string(content_filter_options.filter_expression),
+        cstrings.size(),
+        cstrings.data(),
+        &result);
+      if (RCL_RET_OK != ret) {
+        rclcpp::exceptions::throw_from_rcl_error(
+          ret, "failed to set content_filter_options");
+      }
+    }
+
     return result;
   }
 
@@ -145,11 +163,15 @@ private:
   rcl_allocator_t
   get_rcl_allocator() const
   {
-    if (!plain_allocator_storage_) {
-      plain_allocator_storage_ =
-        std::make_shared<PlainAllocator>(*this->get_allocator());
+    if constexpr (std::is_same_v<Allocator, std::allocator<void>>) {
+      return rcl_get_default_allocator();
+    } else {
+      if (!plain_allocator_storage_) {
+        plain_allocator_storage_ =
+          std::make_shared<PlainAllocator>(*this->get_allocator());
+      }
+      return rclcpp::allocator::get_rcl_allocator<char>(*plain_allocator_storage_);
     }
-    return rclcpp::allocator::get_rcl_allocator<char>(*plain_allocator_storage_);
   }
 
   // This is a temporal workaround, to make sure that get_allocator()

rclcpp/include/rclcpp/time_source.hpp

@@ -56,6 +56,7 @@ public:
    *
    * \param node std::shared pointer to a initialized node
    * \param qos QoS that will be used when creating a `/clock` subscription.
+   * \param use_clock_thread whether to spin the attached node in a separate thread
    */
   RCLCPP_PUBLIC
   explicit TimeSource(
@@ -68,12 +69,21 @@ public:
    * An Empty TimeSource class
    *
    * \param qos QoS that will be used when creating a `/clock` subscription.
+   * \param use_clock_thread whether to spin the attached node in a separate thread.
    */
   RCLCPP_PUBLIC
   explicit TimeSource(
     const rclcpp::QoS & qos = rclcpp::ClockQoS(),
     bool use_clock_thread = true);
 
+  // The TimeSource is uncopyable
+  TimeSource(const TimeSource &) = delete;
+  TimeSource & operator=(const TimeSource &) = delete;
+
+  // The TimeSource is moveable
+  TimeSource(TimeSource &&) = default;
+  TimeSource & operator=(TimeSource &&) = default;
+
   /// Attach node to the time source.
   /**
    * \param node std::shared pointer to a initialized node
@@ -116,89 +126,33 @@ public:
   RCLCPP_PUBLIC
   void attachClock(rclcpp::Clock::SharedPtr clock);
 
-  /// Detach a clock to the time source
+  /// Detach a clock from the time source
   RCLCPP_PUBLIC
   void detachClock(rclcpp::Clock::SharedPtr clock);
 
+  /// Get whether a separate clock thread is used or not
+  RCLCPP_PUBLIC
+  bool get_use_clock_thread();
+
+  /// Set whether to use a separate clock thread or not
+  RCLCPP_PUBLIC
+  void set_use_clock_thread(bool use_clock_thread);
+
+  /// Check if the clock thread is joinable
+  RCLCPP_PUBLIC
+  bool clock_thread_is_joinable();
+
   /// TimeSource Destructor
   RCLCPP_PUBLIC
   ~TimeSource();
 
-protected:
-  // Dedicated thread for clock subscription.
-  bool use_clock_thread_;
-  std::thread clock_executor_thread_;
-
 private:
-  // Preserve the node reference
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
-  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
-  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
-  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
-  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
-  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
-  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
+  class NodeState;
+  std::shared_ptr<NodeState> node_state_;
 
-  // Store (and update on node attach) logger for logging.
-  Logger logger_;
-
-  // QoS of the clock subscription.
-  rclcpp::QoS qos_;
-
-  // The subscription for the clock callback
-  using MessageT = rosgraph_msgs::msg::Clock;
-  using Alloc = std::allocator<void>;
-  using SubscriptionT = rclcpp::Subscription<MessageT, Alloc>;
-  std::shared_ptr<SubscriptionT> clock_subscription_{nullptr};
-  std::mutex clock_sub_lock_;
-  rclcpp::CallbackGroup::SharedPtr clock_callback_group_;
-  rclcpp::executors::SingleThreadedExecutor::SharedPtr clock_executor_;
-  std::promise<void> cancel_clock_executor_promise_;
-
-  // The clock callback itself
-  void clock_cb(std::shared_ptr<const rosgraph_msgs::msg::Clock> msg);
-
-  // Create the subscription for the clock topic
-  void create_clock_sub();
-
-  // Destroy the subscription for the clock topic
-  void destroy_clock_sub();
-
-  // Parameter Event subscription
-  using ParamMessageT = rcl_interfaces::msg::ParameterEvent;
-  using ParamSubscriptionT = rclcpp::Subscription<ParamMessageT, Alloc>;
-  std::shared_ptr<ParamSubscriptionT> parameter_subscription_;
-
-  // Callback for parameter updates
-  void on_parameter_event(std::shared_ptr<const rcl_interfaces::msg::ParameterEvent> event);
-
-  // An enum to hold the parameter state
-  enum UseSimTimeParameterState {UNSET, SET_TRUE, SET_FALSE};
-  UseSimTimeParameterState parameter_state_;
-
-  // An internal method to use in the clock callback that iterates and enables all clocks
-  void enable_ros_time();
-  // An internal method to use in the clock callback that iterates and disables all clocks
-  void disable_ros_time();
-
-  // Internal helper functions used inside iterators
-  static void set_clock(
-    const builtin_interfaces::msg::Time::SharedPtr msg,
-    bool set_ros_time_enabled,
-    rclcpp::Clock::SharedPtr clock);
-
-  // Local storage of validity of ROS time
-  // This is needed when new clocks are added.
-  bool ros_time_active_{false};
-  // Last set message to be passed to newly registered clocks
-  std::shared_ptr<const rosgraph_msgs::msg::Clock> last_msg_set_;
-
-  // A lock to protect iterating the associated_clocks_ field.
-  std::mutex clock_list_lock_;
-  // A vector to store references to associated clocks.
-  std::vector<rclcpp::Clock::SharedPtr> associated_clocks_;
-  // A handler for the use_sim_time parameter callback.
-  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr sim_time_cb_handler_{nullptr};
+  // Preserve the arguments received by the constructor for reuse at runtime
+  bool constructed_use_clock_thread_;
+  rclcpp::QoS constructed_qos_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/timer.hpp

@@ -91,6 +91,17 @@ public:
   void
   reset();
 
+  /// Indicate that we're about to execute the callback.
+  /**
+   * 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.
+   */
+  RCLCPP_PUBLIC
+  virtual bool
+  call() = 0;
+
   /// Call the callback function when the timer signal is emitted.
   RCLCPP_PUBLIC
   virtual void
@@ -102,7 +113,8 @@ public:
 
   /// Check how long the timer has until its next scheduled callback.
   /**
-   * \return A std::chrono::duration representing the relative time until the next callback.
+   * \return A std::chrono::duration representing the relative time until the next callback
+   * or std::chrono::nanoseconds::max() if the timer is canceled.
    * \throws std::runtime_error if the rcl_timer_get_time_until_next_call returns a failure
    */
   RCLCPP_PUBLIC
@@ -192,19 +204,28 @@ public:
   }
 
   /**
-   * \sa rclcpp::TimerBase::execute_callback
-   * \throws std::runtime_error if it failed to notify timer that callback occurred
+   * \sa rclcpp::TimerBase::call
+   * \throws std::runtime_error if it failed to notify timer that callback will occurr
    */
-  void
-  execute_callback() override
+  bool
+  call() override
   {
     rcl_ret_t ret = rcl_timer_call(timer_handle_.get());
     if (ret == RCL_RET_TIMER_CANCELED) {
-      return;
+      return false;
     }
     if (ret != RCL_RET_OK) {
       throw std::runtime_error("Failed to notify timer that callback occurred");
     }
+    return true;
+  }
+
+  /**
+   * \sa rclcpp::TimerBase::execute_callback
+   */
+  void
+  execute_callback() override
+  {
     TRACEPOINT(callback_start, static_cast<const void *>(&callback_), false);
     execute_callback_delegate<>();
     TRACEPOINT(callback_end, static_cast<const void *>(&callback_));

rclcpp/include/rclcpp/utilities.hpp

@@ -42,6 +42,20 @@ std::string to_string(T value)
 
 namespace rclcpp
 {
+
+/// Option to indicate which signal handlers rclcpp should install.
+enum class SignalHandlerOptions
+{
+  /// Install both sigint and sigterm, this is the default behavior.
+  All,
+  /// Install only a sigint handler.
+  SigInt,
+  /// Install only a sigterm handler.
+  SigTerm,
+  /// Do not install any signal handler.
+  None,
+};
+
 /// Initialize communications via the rmw implementation and set up a global signal handler.
 /**
  * Initializes the global context which is accessible via the function
@@ -50,10 +64,19 @@ namespace rclcpp
  * rclcpp::install_signal_handlers().
  *
  * \sa rclcpp::Context::init() for more details on arguments and possible exceptions
+ *
+ * \param[in] argc number of command-line arguments to parse.
+ * \param[in] argv array of command-line arguments to parse.
+ * \param[in] init_options initialization options to apply.
+ * \param[in] signal_handler_options option to indicate which signal handlers should be installed.
  */
 RCLCPP_PUBLIC
 void
-init(int argc, char const * const argv[], const InitOptions & init_options = InitOptions());
+init(
+  int argc,
+  char const * const * argv,
+  const InitOptions & init_options = InitOptions(),
+  SignalHandlerOptions signal_handler_options = SignalHandlerOptions::All);
 
 /// Install the global signal handler for rclcpp.
 /**
@@ -67,17 +90,26 @@ init(int argc, char const * const argv[], const InitOptions & init_options = Ini
  *
  * This function is thread-safe.
  *
+ * \param[in] signal_handler_options option to indicate which signal handlers should be installed.
  * \return true if signal handler was installed by this function, false if already installed.
  */
 RCLCPP_PUBLIC
 bool
-install_signal_handlers();
+install_signal_handlers(SignalHandlerOptions signal_handler_options = SignalHandlerOptions::All);
 
 /// Return true if the signal handlers are installed, otherwise false.
 RCLCPP_PUBLIC
 bool
 signal_handlers_installed();
 
+/// Get the current signal handler options.
+/**
+ * If no signal handler is installed, SignalHandlerOptions::None is returned.
+ */
+RCLCPP_PUBLIC
+SignalHandlerOptions
+get_current_signal_handler_options();
+
 /// Uninstall the global signal handler for rclcpp.
 /**
  * This function does not necessarily need to be called, but can be used to
@@ -107,7 +139,7 @@ RCLCPP_PUBLIC
 std::vector<std::string>
 init_and_remove_ros_arguments(
   int argc,
-  char const * const argv[],
+  char const * const * argv,
   const InitOptions & init_options = InitOptions());
 
 /// Remove ROS-specific arguments from argument vector.
@@ -125,7 +157,7 @@ init_and_remove_ros_arguments(
  */
 RCLCPP_PUBLIC
 std::vector<std::string>
-remove_ros_arguments(int argc, char const * const argv[]);
+remove_ros_arguments(int argc, char const * const * argv);
 
 /// Check rclcpp's status.
 /**
@@ -289,6 +321,15 @@ RCLCPP_PUBLIC
 const char *
 get_c_string(const std::string & string_in);
 
+/// Return the std::vector of C string from the given std::vector<std::string>.
+/**
+ * \param[in] strings_in is a std::vector of std::string
+ * \return the std::vector of C string from the std::vector<std::string>
+ */
+RCLCPP_PUBLIC
+std::vector<const char *>
+get_c_vector_string(const std::vector<std::string> & strings_in);
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__UTILITIES_HPP_

rclcpp/include/rclcpp/wait_for_message.hpp

@@ -0,0 +1,107 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAIT_FOR_MESSAGE_HPP_
+#define RCLCPP__WAIT_FOR_MESSAGE_HPP_
+
+#include <future>
+#include <memory>
+#include <string>
+
+#include "rcpputils/scope_exit.hpp"
+
+#include "rclcpp/node.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/wait_set.hpp"
+#include "rclcpp/qos.hpp"
+
+namespace rclcpp
+{
+/// Wait for the next incoming message.
+/**
+ * Given an already initialized subscription,
+ * wait for the next incoming message to arrive before the specified timeout.
+ *
+ * \param[out] out is the message to be filled when a new message is arriving.
+ * \param[in] subscription shared pointer to a previously initialized subscription.
+ * \param[in] context shared pointer to a context to watch for SIGINT requests.
+ * \param[in] time_to_wait parameter specifying the timeout before returning.
+ * \return true if a message was successfully received, false if message could not
+ * be obtained or shutdown was triggered asynchronously on the context.
+ */
+template<class MsgT, class Rep = int64_t, class Period = std::milli>
+bool wait_for_message(
+  MsgT & out,
+  std::shared_ptr<rclcpp::Subscription<MsgT>> subscription,
+  std::shared_ptr<rclcpp::Context> context,
+  std::chrono::duration<Rep, Period> time_to_wait = std::chrono::duration<Rep, Period>(-1))
+{
+  auto gc = std::make_shared<rclcpp::GuardCondition>(context);
+  auto shutdown_callback_handle = context->add_on_shutdown_callback(
+    [weak_gc = std::weak_ptr<rclcpp::GuardCondition>{gc}]() {
+      auto strong_gc = weak_gc.lock();
+      if (strong_gc) {
+        strong_gc->trigger();
+      }
+    });
+
+  rclcpp::WaitSet wait_set({}, {}, {}, {}, {}, {}, context);
+  wait_set.add_subscription(subscription);
+  RCPPUTILS_SCOPE_EXIT(wait_set.remove_subscription(subscription); );
+  wait_set.add_guard_condition(gc);
+  auto ret = wait_set.wait(time_to_wait);
+  if (ret.kind() != rclcpp::WaitResultKind::Ready) {
+    return false;
+  }
+
+  if (wait_set.get_rcl_wait_set().guard_conditions[0]) {
+    return false;
+  }
+
+  rclcpp::MessageInfo info;
+  if (!subscription->take(out, info)) {
+    return false;
+  }
+
+  return true;
+}
+
+/// Wait for the next incoming message.
+/**
+ * Wait for the next incoming message to arrive on a specified topic before the specified timeout.
+ *
+ * \param[out] out is the message to be filled when a new message is arriving
+ * \param[in] node the node pointer to initialize the subscription on.
+ * \param[in] topic the topic to wait for messages.
+ * \param[in] time_to_wait parameter specifying the timeout before returning.
+ * \param[in] qos parameter specifying QoS settings for the subscription.
+ * \return true if a message was successfully received, false if message could not
+ * be obtained or shutdown was triggered asynchronously on the context.
+ */
+template<class MsgT, class Rep = int64_t, class Period = std::milli>
+bool wait_for_message(
+  MsgT & out,
+  rclcpp::Node::SharedPtr node,
+  const std::string & topic,
+  std::chrono::duration<Rep, Period> time_to_wait = std::chrono::duration<Rep, Period>(-1),
+  const rclcpp::QoS & qos = rclcpp::SystemDefaultsQoS())
+{
+  auto sub = node->create_subscription<MsgT>(topic, qos, [](const std::shared_ptr<const MsgT>) {});
+  return wait_for_message<MsgT, Rep, Period>(
+    out, sub, node->get_node_options().context(), time_to_wait);
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_FOR_MESSAGE_HPP_

rclcpp/include/rclcpp/wait_set_policies/already_associated_with_wait_set_exception.hpp

@@ -1,43 +0,0 @@
-// Copyright 2021 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef RCLCPP__WAIT_SET_POLICIES__ALREADY_ASSOCIATED_WITH_WAIT_SET_EXCEPTION_HPP_
-#define RCLCPP__WAIT_SET_POLICIES__ALREADY_ASSOCIATED_WITH_WAIT_SET_EXCEPTION_HPP_
-
-#include <stdexcept>
-
-#include "rmw/impl/cpp/demangle.hpp"
-
-namespace rclcpp
-{
-namespace wait_set_policies
-{
-
-class AlreadyAssociatedWithWaitSetException : public std::runtime_error
-{
-public:
-  template<typename EntityT>
-  explicit
-  AlreadyAssociatedWithWaitSetException(const EntityT & entity_instance)
-  : std::runtime_error(
-    "cannot associate " +
-    rmw::impl::cpp::demangle(entity_instance) +
-    " with wait set because it is already in use by a wait set")
-  {}
-};
-
-}  // namespace wait_set_policies
-}  // namespace rclcpp
-
-#endif  // RCLCPP__WAIT_SET_POLICIES__ALREADY_ASSOCIATED_WITH_WAIT_SET_EXCEPTION_HPP_

rclcpp/include/rclcpp/wait_set_policies/detail/entity_entry.hpp

@@ -1,183 +0,0 @@
-// Copyright 2021 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__ENTITY_ENTRY_HPP_
-#define RCLCPP__WAIT_SET_POLICIES__DETAIL__ENTITY_ENTRY_HPP_
-
-#include <cassert>
-#include <memory>
-
-#include "rclcpp/wait_set_policies/already_associated_with_wait_set_exception.hpp"
-
-namespace rclcpp
-{
-namespace wait_set_policies
-{
-namespace detail
-{
-
-// Forward declaration for use in friend statement.
-template<typename EntityT>
-class ManagedEntityEntryTemplate;
-
-/// Encapsulating class for wait set entities, and gateway to the ManagedEntityEntryTemplate.
-/**
- * The entity is stored as a std::shared_ptr.
- *
- * This is class can be converted to a "managed" version which ensures the
- * entity is not associated with another wait set already, then associates it
- * with the current wait set, and then dissociates it on destruction.
- */
-template<typename EntityT>
-class EntityEntryTemplate
-{
-public:
-  EntityEntryTemplate(std::shared_ptr<EntityT> entity_in = nullptr)
-  : entity_(entity_in)
-  {}
-private:
-  std::shared_ptr<EntityT> entity_;
-
-  friend ManagedEntityEntryTemplate<EntityT>;
-};
-
-/// Managing class for wait set entities, with RAII-style (dis)association with the wait set.
-/**
- * The entity is stored as a std::shared_ptr, but ths class can be converted
- * (one way) into a weak version that stores it as a std::weak_ptr.
- *
- * This class will assert that the entity is not already associated with a
- * wait set, while atomically indicating it is associated with this wait set
- * to prevent other wait sets from using it, and then on destruction this class
- * will disassociate it.
- *
- * \throws rclcpp::wait_set_policies::AlreadyAssociatedWithWaitSetException if entity
- *   is already associated with a wait set.
- */
-template<typename EntityT>
-class ManagedEntityEntryTemplate
-{
-public:
-  /// The only valid way to construct this is with an unmanaged entity entry.
-  explicit ManagedEntityEntryTemplate(const EntityEntryTemplate<EntityT> & unmanaged_entity_entry)
-  : entity_(unmanaged_entity_entry.entity_)
-  {
-    if (nullptr == entity_) {
-      throw std::invalid_argument("entity cannot be nullptr for a managed entry");
-    }
-    bool already_in_use = entity_->exchange_in_use_by_wait_set_state(true);
-    if (already_in_use) {
-      throw rclcpp::wait_set_policies::AlreadyAssociatedWithWaitSetException(*entity_);
-    }
-  }
-
-  // ManagedEntityEntryTemplate(const ManagedEntityEntryTemplate<EntityT> & other)
-  // {
-  //   if (other.should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_) {
-  //     throw std::runtime_error("")
-  //   }
-  // }
-
-  ~ManagedEntityEntryTemplate()
-  {
-    if ((nullptr != entity_)) {
-      bool was_in_use = entity_->exchange_in_use_by_wait_set_state(false);
-      assert(was_in_use);
-    }
-  }
-
-  /// Return the interal entity shared pointer.
-  std::shared_ptr<EntityT>
-  get_entity() const
-  {
-    return entity_;
-  }
-
-  /// Reset the entity.
-  /**
-   * Specializations of this class may reset more than one item.
-   * Having this method in all instantiations of this class provides uniform access.
-   */
-  // void
-  // reset() noexcept
-  // {
-  //   entity_.reset();
-  // }
-
-protected:
-  std::shared_ptr<EntityT> entity_;
-
-};
-
-/// Version of ManagedEntityEntryTemplate with weak ownership and best effort disassociation.
-/**
- * The entity is stored as a std::weak_ptr, but on destruction, the entity is
- * locked, and if not nullptr, then it will be marked as not in use by a wait set.
- */
-template<typename EntityT>
-class WeakManagedEntityEntryTemplate
-{
-public:
-  /// Can only be constructed from a moved ManagedEntityEntryTemplate.
-  explicit WeakManagedEntityEntryTemplate(ManagedEntityEntryTemplate<EntityT> && moved_entity_entry)
-  : weak_entity_(moved_entity_entry.get_entity())
-  {}
-
-  ~WeakManagedEntityEntryTemplate()
-  {
-    auto entity = weak_entity_.lock();
-    if (nullptr != entity) {
-      bool was_in_use = entity->exchange_in_use_by_wait_set_state(false);
-      assert(was_in_use);
-    }
-  }
-
-  /// Return the interal entity weak pointer.
-  std::weak_ptr<EntityT>
-  get_weak_entity() const
-  {
-    return weak_entity_;
-  }
-
-  // /// Lock the entity.
-  // /**
-  //  * Specializations of this class may select from more than one item to lock.
-  //  * Having this method in all instantiations of this class provides uniform access.
-  //  */
-  // std::shared_ptr<EntityT>
-  // lock() const
-  // {
-  //   return weak_entity_.lock();
-  // }
-
-  // /// Return true if the entity has expired, otherwise false.
-  // /**
-  //  * Specializations of this class may select from more than one item to check.
-  //  * Having this method in all instantiations of this class provides uniform access.
-  //  */
-  // bool
-  // expired() const noexcept
-  // {
-  //   return weak_entity_.expired();
-  // }
-private:
-  std::weak_ptr<EntityT> weak_entity_;
-
-};
-
-}  // namespace detail
-}  // namespace wait_set_policies
-}  // namespace rclcpp
-
-#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__ENTITY_ENTRY_HPP_

rclcpp/include/rclcpp/wait_set_policies/detail/guard_condition_entry.hpp

@@ -1,41 +0,0 @@
-// Copyright 2021 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__GUARD_CONDITION_ENTRY_HPP_
-#define RCLCPP__WAIT_SET_POLICIES__DETAIL__GUARD_CONDITION_ENTRY_HPP_
-
-#include <memory>
-
-#include "rclcpp/guard_condition.hpp"
-#include "rclcpp/wait_set_policies/detail/entity_entry.hpp"
-
-namespace rclcpp
-{
-namespace wait_set_policies
-{
-namespace detail
-{
-
-using GuardConditionEntry =
-  // EntityEntryTemplate<rclcpp::GuardCondition, std::shared_ptr<rclcpp::GuardCondition>>;
-  EntityEntryTemplate<rclcpp::GuardCondition>;
-using WeakGuardConditionEntry =
-  // EntityEntryTemplate<rclcpp::GuardCondition, std::weak_ptr<rclcpp::GuardCondition>>;
-  EntityEntryTemplate<rclcpp::GuardCondition>;
-
-}  // namespace detail
-}  // namespace wait_set_policies
-}  // namespace rclcpp
-
-#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__GUARD_CONDITION_ENTRY_HPP_

rclcpp/include/rclcpp/wait_set_policies/detail/service_entry.hpp

@@ -1,41 +0,0 @@
-// Copyright 2021 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__SERVICE_ENTRY_HPP_
-#define RCLCPP__WAIT_SET_POLICIES__DETAIL__SERVICE_ENTRY_HPP_
-
-#include <memory>
-
-#include "rclcpp/service.hpp"
-#include "rclcpp/wait_set_policies/detail/entity_entry.hpp"
-
-namespace rclcpp
-{
-namespace wait_set_policies
-{
-namespace detail
-{
-
-using ServiceEntry =
-  // EntityEntryTemplate<rclcpp::ServiceBase, std::shared_ptr<rclcpp::ServiceBase>>;
-  EntityEntryTemplate<rclcpp::ServiceBase>;
-using WeakServiceEntry =
-  // EntityEntryTemplate<rclcpp::ServiceBase, std::weak_ptr<rclcpp::ServiceBase>>;
-  EntityEntryTemplate<rclcpp::ServiceBase>;
-
-}  // namespace detail
-}  // namespace wait_set_policies
-}  // namespace rclcpp
-
-#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__SERVICE_ENTRY_HPP_

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

@@ -383,10 +383,7 @@ protected:
         continue;
       }
       rclcpp::Waitable & waitable = *waitable_ptr_pair.second;
-      bool successful = waitable.add_to_wait_set(&rcl_wait_set_);
-      if (!successful) {
-        throw std::runtime_error("waitable unexpectedly failed to be added to wait set");
-      }
+      waitable.add_to_wait_set(&rcl_wait_set_);
     }
   }
 

rclcpp/include/rclcpp/wait_set_policies/detail/subscription_entry.hpp

@@ -1,403 +0,0 @@
-// Copyright 2021 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__SUBSCRIPTION_ENTRY_HPP_
-#define RCLCPP__WAIT_SET_POLICIES__DETAIL__SUBSCRIPTION_ENTRY_HPP_
-
-#include <memory>
-#include <optional>
-#include <vector>
-
-#include "rclcpp/subscription_base.hpp"
-#include "rclcpp/subscription_wait_set_mask.hpp"
-#include "rclcpp/wait_set_policies/detail/entity_entry.hpp"
-#include "rclcpp/wait_set_policies/detail/waitable_entry.hpp"
-
-namespace rclcpp
-{
-namespace wait_set_policies
-{
-namespace detail
-{
-
-/// See rclcpp::wait_set_policies::detail::EntityEntryTemplate.
-template<>
-class EntityEntryTemplate<rclcpp::SubscriptionBase>
-{
-  using EntityT = rclcpp::SubscriptionBase;
-
-public:
-  EntityEntryTemplate(
-    std::shared_ptr<EntityT> entity_in = nullptr,
-    const rclcpp::SubscriptionWaitSetMask & mask_in = {})
-  : entity_(entity_in),
-    mask_(mask_in)
-  {}
-
-  /// Create ManagedEntityEntryTemplate instances for the subscription and waitables if needed.
-  /**
-   * This method uses the SubscriptionWaitSetMask to determine how to decompose
-   * the various entities within the subscription into managed entries.
-   *
-   * It optionally returns a managed entry for the subscription, if the mask
-   * indicates it should, and then optionally any waitables for the
-   * intra-process communication and QoS events.
-   */
-  std::pair<
-    std::optional<ManagedEntityEntryTemplate<rclcpp::SubscriptionBase>>,
-    std::vector<ManagedEntityEntryTemplate<rclcpp::Waitable>>
-  >
-  manage();
-  // defined below ManagedEntityEntryTemplate<rclcpp::SubscriptionBase>.
-
-private:
-  std::shared_ptr<EntityT> entity_;
-  rclcpp::SubscriptionWaitSetMask mask_;
-};
-
-/// See rclcpp::wait_set_policies::detail::ManagedEntityEntryTemplate.
-template<>
-class ManagedEntityEntryTemplate<rclcpp::SubscriptionBase>
-{
-  using EntityT = rclcpp::SubscriptionBase;
-
-  /// Only constructed by EntityEntryTemplate<rclcpp::SubscriptionBase>::manage().
-  explicit ManagedEntityEntryTemplate(std::shared_ptr<EntityT> subscription)
-  : entity_(subscription)
-  {
-    if (nullptr == entity_) {
-      throw std::invalid_argument("entity cannot be nullptr for a managed entry");
-    }
-    bool already_in_use = entity_->exchange_in_use_by_wait_set_state(entity_.get(), true);
-    if (already_in_use) {
-      throw rclcpp::wait_set_policies::AlreadyAssociatedWithWaitSetException(*entity_);
-    }
-  }
-
-public:
-  // ManagedEntityEntryTemplate(const ManagedEntityEntryTemplate<EntityT> & other)
-  // {
-  //   if (other.should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_) {
-  //     throw std::runtime_error("")
-  //   }
-  // }
-
-  ~ManagedEntityEntryTemplate()
-  {
-    if ((nullptr != entity_)) {
-      bool was_in_use = entity_->exchange_in_use_by_wait_set_state(entity_.get(), false);
-      assert(was_in_use);
-    }
-  }
-
-  /// Return the interal entity shared pointer.
-  std::shared_ptr<EntityT>
-  get_entity() const
-  {
-    return entity_;
-  }
-
-  /// Reset the entity.
-  /**
-   * Specializations of this class may reset more than one item.
-   * Having this method in all instantiations of this class provides uniform access.
-   */
-  // void
-  // reset() noexcept
-  // {
-  //   entity_.reset();
-  // }
-
-private:
-  std::shared_ptr<EntityT> entity_;
-
-  friend EntityEntryTemplate<EntityT>;
-
-};
-
-std::pair<
-  std::optional<ManagedEntityEntryTemplate<rclcpp::SubscriptionBase>>,
-  std::vector<ManagedEntityEntryTemplate<rclcpp::Waitable>>
->
-EntityEntryTemplate<rclcpp::SubscriptionBase>::manage()
-{
-  std::optional<ManagedEntityEntryTemplate<rclcpp::SubscriptionBase>> managed_subscription_entry;
-  std::vector<ManagedEntityEntryTemplate<rclcpp::Waitable>> waitables;
-
-  if (mask_.include_subscription) {
-    managed_subscription_entry = ManagedEntityEntryTemplate<rclcpp::SubscriptionBase>(entity_);
-  }
-  if (mask_.include_events) {
-    for (const auto & event_waitable : entity_->get_event_handlers()) {
-      waitables.emplace_back(EntityEntryTemplate<rclcpp::Waitable>(event_waitable, entity_));
-    }
-  }
-  if (mask_.include_intra_process_waitable) {
-    waitables.emplace_back(
-      EntityEntryTemplate<rclcpp::Waitable>(entity_->get_intra_process_waitable(), entity_)
-    );
-  }
-
-  return {managed_subscription_entry, waitables};
-}
-
-/// See rclcpp::wait_set_policies::detail::WeakManagedEntityEntryTemplate.
-template<>
-class WeakManagedEntityEntryTemplate<rclcpp::SubscriptionBase>
-{
-  using EntityT = rclcpp::SubscriptionBase;
-
-public:
-  /// Can only be constructed from a moved ManagedEntityEntryTemplate.
-  explicit WeakManagedEntityEntryTemplate(ManagedEntityEntryTemplate<EntityT> && moved_entity_entry)
-  : weak_entity_(moved_entity_entry.get_entity())
-  {}
-
-  ~WeakManagedEntityEntryTemplate()
-  {
-    auto entity = weak_entity_.lock();
-    if (nullptr != entity) {
-      bool was_in_use = entity->exchange_in_use_by_wait_set_state(entity.get(), false);
-      assert(was_in_use);
-    }
-  }
-
-  /// Return the interal entity weak pointer.
-  std::weak_ptr<EntityT>
-  get_weak_entity() const
-  {
-    return weak_entity_;
-  }
-
-  // /// Lock the entity.
-  // /**
-  //  * Specializations of this class may select from more than one item to lock.
-  //  * Having this method in all instantiations of this class provides uniform access.
-  //  */
-  // std::shared_ptr<EntityT>
-  // lock() const
-  // {
-  //   return weak_entity_.lock();
-  // }
-
-  // /// Return true if the entity has expired, otherwise false.
-  // /**
-  //  * Specializations of this class may select from more than one item to check.
-  //  * Having this method in all instantiations of this class provides uniform access.
-  //  */
-  // bool
-  // expired() const noexcept
-  // {
-  //   return weak_entity_.expired();
-  // }
-private:
-  std::weak_ptr<EntityT> weak_entity_;
-  std::weak_ptr<void> weak_associated_entity_;
-
-};
-
-using SubscriptionEntry = EntityEntryTemplate<rclcpp::SubscriptionBase>;
-using ManagedSubscriptionEntry = ManagedEntityEntryTemplate<rclcpp::SubscriptionBase>;
-using WeakManagedSubscriptionEntry = WeakManagedEntityEntryTemplate<rclcpp::SubscriptionBase>;
-
-// /// Specialization for Subscriptions.
-// template<>
-// class EntityEntryTemplate<rclcpp::SubscriptionBase>
-// {
-//   using EntityT = rclcpp::SubscriptionBase;
-//   std::shared_ptr<EntityT> entity_;
-//   rclcpp::SubscriptionWaitSetMask mask_;
-
-// public:
-//   EntityEntryTemplate(
-//     std::shared_ptr<EntityT> entity_in = nullptr,
-//     const rclcpp::SubscriptionWaitSetMask & mask_in = {})
-//   : entity_(entity_in),
-//     mask_(mask_in)
-//   {}
-
-//   ~EntityEntryTemplate()
-//   {
-//     if (should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_) {
-//       EntityEntryTemplate<rclcpp::SubscriptionBase>::cleanup(entity_, mask_);
-//     }
-//   }
-
-//   /// See EntityEntryTemplate::get_entity().
-//   std::shared_ptr<EntityT>
-//   get_entity() const
-//   {
-//     return entity_;
-//   }
-
-//   /// Return a const reference to the subscrption mask.
-//   const rclcpp::SubscriptionWaitSetMask &
-//   get_mask() const
-//   {
-//     return mask_;
-//   }
-
-//   /// See EntityEntryTemplate::manage().
-//   std::vector<WaitableEntry>
-//   manage()
-//   {
-//     if (nullptr == entity_) {
-//       throw std::runtime_error("manage() called on EntityEntry with null entity");
-//     }
-//     auto associate = [this](void * entity_part) {
-//       bool already_in_use = entity_->exchange_in_use_by_wait_set_state(entity_part, true);
-//       if (already_in_use) {
-//         throw rclcpp::wait_set_policies::AlreadyAssociatedWithWaitSetException(*entity_);
-//       }
-//     };
-
-//     if (mask_.include_subscription) {
-//       associate(entity_.get());
-//     }
-
-//     std::vector<WaitableEntry> decomposed_waitables;
-
-//     if (mask_.include_events) {
-//       for (auto event : entity_->get_event_handlers()) {
-//         // TODO(wjwwood): do we need to use the unmanage_function argument
-//         //   to utilize the exchange_in_use_by_wait_set_state of SubscriptionBase?
-//         decomposed_waitables.emplace_back(event, entity_);
-//         decomposed_waitables.back().manage();
-//       }
-//       // mask_.include_events = false;
-//     }
-//     if (mask_.include_intra_process_waitable) {
-//       auto waitable = entity_->get_intra_process_waitable();
-//       if (nullptr != waitable) {
-//         // TODO(wjwwood): do we need to use the unmanage_function argument
-//         //   to utilize the exchange_in_use_by_wait_set_state of SubscriptionBase?
-//         decomposed_waitables.emplace_back(waitable, entity_);
-//         decomposed_waitables.back().manage();
-//         // mask_.include_intra_process_waitable = false;
-//       }
-//     }
-
-//     should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_ = true;
-
-//     return decomposed_waitables;
-//   }
-
-//   /// See EntityEntryTemplate::reset().
-//   void
-//   reset() noexcept
-//   {
-//     entity_.reset();
-//   }
-
-// protected:
-//   bool should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_{false};
-
-//   static
-//   void
-//   cleanup(std::shared_ptr<EntityT> subscription, rclcpp::SubscriptionWaitSetMask mask)
-//   {
-//     if (subscription != nullptr) {
-//       auto dissociate = [&subscription](void * entity_part) {
-//         bool was_in_use = subscription->exchange_in_use_by_wait_set_state(entity_part, false);
-//         assert(was_in_use);
-//       };
-//       if (mask.include_subscription) {
-//         dissociate(subscription.get());
-//       }
-//       if (mask.include_events) {
-//         for (auto event : subscription->get_event_handlers()) {
-//           dissociate(event.get());
-//         }
-//       }
-//       if (mask.include_intra_process_waitable) {
-//         auto waitable = subscription->get_intra_process_waitable();
-//         if (nullptr != waitable) {
-//           dissociate(waitable.get());
-//         }
-//       }
-//     }
-//   }
-
-//   friend WeakEntityEntryTemplate<EntityT>;
-
-// };
-
-// /// Specialization for Subscriptions.
-// template<>
-// class WeakEntityEntryTemplate<rclcpp::SubscriptionBase>
-// {
-//   using EntityT = rclcpp::SubscriptionBase;
-//   std::weak_ptr<EntityT> weak_entity_;
-//   rclcpp::SubscriptionWaitSetMask mask_;
-
-// public:
-//   explicit WeakEntityEntryTemplate(EntityEntryTemplate<EntityT> && moved_entity_entry)
-//   : weak_entity_(moved_entity_entry.get_entity()),
-//     mask_(moved_entity_entry.mask_),
-//     should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_(
-//       moved_entity_entry.should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_
-//     )
-//   {
-//     moved_entity_entry.should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_ = false;
-//   }
-
-//   ~WeakEntityEntryTemplate()
-//   {
-//     auto entity = weak_entity_.lock();
-//     if (should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_) {
-//       EntityEntryTemplate<rclcpp::SubscriptionBase>::cleanup(entity, mask_);
-//     }
-//   }
-
-//   /// See WeakEntityEntryTemplate::get_weak_entity().
-//   std::weak_ptr<EntityT>
-//   get_weak_entity() const
-//   {
-//     return weak_entity_;
-//   }
-
-//   /// Return a const reference to the subscrption mask.
-//   const rclcpp::SubscriptionWaitSetMask &
-//   get_mask() const
-//   {
-//     return mask_;
-//   }
-
-//   /// See WeakEntityEntryTemplate::lock().
-//   std::shared_ptr<EntityT>
-//   lock() const
-//   {
-//     return weak_entity_.lock();
-//   }
-
-//   /// See WeakEntityEntryTemplate::expired().
-//   bool
-//   expired() const noexcept
-//   {
-//     return weak_entity_.expired();
-//   }
-
-// protected:
-//   bool should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_{false};
-
-// };
-
-// using SubscriptionEntry = EntityEntryTemplate<rclcpp::SubscriptionBase>;
-// using WeakSubscriptionEntry = WeakEntityEntryTemplate<rclcpp::SubscriptionBase>;
-
-}  // namespace detail
-}  // namespace wait_set_policies
-}  // namespace rclcpp
-
-#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__SUBSCRIPTION_ENTRY_HPP_

rclcpp/include/rclcpp/wait_set_policies/detail/waitable_entry.hpp

@@ -1,183 +0,0 @@
-// Copyright 2021 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__WAITABLE_ENTRY_HPP_
-#define RCLCPP__WAIT_SET_POLICIES__DETAIL__WAITABLE_ENTRY_HPP_
-
-#include <functional>
-#include <memory>
-#include <type_traits>
-
-#include "rclcpp/waitable.hpp"
-#include "rclcpp/wait_set_policies/detail/entity_entry.hpp"
-
-namespace rclcpp
-{
-namespace wait_set_policies
-{
-namespace detail
-{
-
-/// See rclcpp::wait_set_policies::detail::EntityEntryTemplate.
-template<>
-class EntityEntryTemplate<rclcpp::Waitable>
-{
-  using EntityT = rclcpp::Waitable;
-
-public:
-  EntityEntryTemplate(
-    std::shared_ptr<EntityT> entity_in = nullptr,
-    std::shared_ptr<void> associated_entity_in = nullptr)
-  : entity_(entity_in),
-    associated_entity_(associated_entity_in)
-  {}
-private:
-  std::shared_ptr<EntityT> entity_;
-  std::shared_ptr<void> associated_entity_;
-
-  friend ManagedEntityEntryTemplate<EntityT>;
-};
-
-/// See rclcpp::wait_set_policies::detail::ManagedEntityEntryTemplate.
-template<>
-class ManagedEntityEntryTemplate<rclcpp::Waitable>
-{
-  using EntityT = rclcpp::Waitable;
-
-public:
-  /// The only valid way to construct this is with an unmanaged entity entry.
-  explicit ManagedEntityEntryTemplate(const EntityEntryTemplate<EntityT> & unmanaged_entity_entry)
-  : entity_(unmanaged_entity_entry.entity_),
-    associated_entity_(unmanaged_entity_entry.associated_entity_)
-  {
-    if (nullptr == entity_) {
-      throw std::invalid_argument("entity cannot be nullptr for a managed entry");
-    }
-    bool already_in_use = entity_->exchange_in_use_by_wait_set_state(true);
-    if (already_in_use) {
-      throw rclcpp::wait_set_policies::AlreadyAssociatedWithWaitSetException(*entity_);
-    }
-  }
-
-  // ManagedEntityEntryTemplate(const ManagedEntityEntryTemplate<EntityT> & other)
-  // {
-  //   if (other.should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_) {
-  //     throw std::runtime_error("")
-  //   }
-  // }
-
-  ~ManagedEntityEntryTemplate()
-  {
-    if ((nullptr != entity_)) {
-      bool was_in_use = entity_->exchange_in_use_by_wait_set_state(false);
-      assert(was_in_use);
-    }
-  }
-
-  /// Return the interal entity shared pointer.
-  std::shared_ptr<EntityT>
-  get_entity() const
-  {
-    return entity_;
-  }
-
-  /// Return the interal associated entity shared pointer.
-  std::shared_ptr<void>
-  get_associated_entity() const
-  {
-    return associated_entity_;
-  }
-
-  /// Reset the entity.
-  /**
-   * Specializations of this class may reset more than one item.
-   * Having this method in all instantiations of this class provides uniform access.
-   */
-  // void
-  // reset() noexcept
-  // {
-  //   entity_.reset();
-  // }
-
-protected:
-  std::shared_ptr<EntityT> entity_;
-  std::shared_ptr<void> associated_entity_;
-
-};
-
-/// See rclcpp::wait_set_policies::detail::WeakManagedEntityEntryTemplate.
-template<>
-class WeakManagedEntityEntryTemplate<rclcpp::Waitable>
-{
-  using EntityT = rclcpp::Waitable;
-
-public:
-  /// Can only be constructed from a moved ManagedEntityEntryTemplate.
-  explicit WeakManagedEntityEntryTemplate(ManagedEntityEntryTemplate<EntityT> && moved_entity_entry)
-  : weak_entity_(moved_entity_entry.get_entity()),
-    weak_associated_entity_(moved_entity_entry.get_associated_entity())
-  {}
-
-  ~WeakManagedEntityEntryTemplate()
-  {
-    auto entity = weak_entity_.lock();
-    if (nullptr != entity) {
-      bool was_in_use = entity->exchange_in_use_by_wait_set_state(false);
-      assert(was_in_use);
-    }
-  }
-
-  /// Return the interal entity weak pointer.
-  std::weak_ptr<EntityT>
-  get_weak_entity() const
-  {
-    return weak_entity_;
-  }
-
-  // /// Lock the entity.
-  // /**
-  //  * Specializations of this class may select from more than one item to lock.
-  //  * Having this method in all instantiations of this class provides uniform access.
-  //  */
-  // std::shared_ptr<EntityT>
-  // lock() const
-  // {
-  //   return weak_entity_.lock();
-  // }
-
-  // /// Return true if the entity has expired, otherwise false.
-  // /**
-  //  * Specializations of this class may select from more than one item to check.
-  //  * Having this method in all instantiations of this class provides uniform access.
-  //  */
-  // bool
-  // expired() const noexcept
-  // {
-  //   return weak_entity_.expired();
-  // }
-private:
-  std::weak_ptr<EntityT> weak_entity_;
-  std::weak_ptr<void> weak_associated_entity_;
-
-};
-
-using WaitableEntry = EntityEntryTemplate<rclcpp::Waitable>;
-using ManagedWaitableEntry = ManagedEntityEntryTemplate<rclcpp::Waitable>;
-using WeakManagedWaitableEntry = WeakManagedEntityEntryTemplate<rclcpp::Waitable>;
-
-}  // namespace detail
-}  // namespace wait_set_policies
-}  // namespace rclcpp
-
-#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__WAITABLE_ENTRY_HPP_

rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp

@@ -191,7 +191,7 @@ protected:
 
     WritePreferringReadWriteLock & parent_lock_;
 
-    friend WritePreferringReadWriteLock;
+    friend class WritePreferringReadWriteLock;
   };
 
   /// Write mutex for the WritePreferringReadWriteLock.
@@ -212,7 +212,7 @@ protected:
 
     WritePreferringReadWriteLock & parent_lock_;
 
-    friend WritePreferringReadWriteLock;
+    friend class WritePreferringReadWriteLock;
   };
 
   /// Return read mutex which can be used with standard constructs like std::lock_guard.

rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp

@@ -28,13 +28,7 @@
 #include "rclcpp/subscription_wait_set_mask.hpp"
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
-#include "rclcpp/wait_set_policies/detail/client_entry.hpp"
-#include "rclcpp/wait_set_policies/detail/guard_condition_entry.hpp"
-#include "rclcpp/wait_set_policies/detail/service_entry.hpp"
 #include "rclcpp/wait_set_policies/detail/storage_policy_common.hpp"
-#include "rclcpp/wait_set_policies/detail/subscription_entry.hpp"
-#include "rclcpp/wait_set_policies/detail/timer_entry.hpp"
-#include "rclcpp/wait_set_policies/detail/waitable_entry.hpp"
 #include "rclcpp/waitable.hpp"
 
 namespace rclcpp
@@ -48,27 +42,124 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo
 protected:
   using is_mutable = std::true_type;
 
-  using WeakManagedSubscriptionEntry = detail::WeakManagedSubscriptionEntry;
-  using SubscriptionEntry = detail::SubscriptionEntry;
+  class SubscriptionEntry
+  {
+    // (wjwwood): indent of 'public:' is weird, I know. uncrustify is dumb.
 
-  using SequenceOfWeakSubscriptions = std::vector<WeakManagedSubscriptionEntry>;
+public:
+    std::shared_ptr<rclcpp::SubscriptionBase> subscription;
+    rclcpp::SubscriptionWaitSetMask mask;
+
+    /// Conversion constructor, which is intentionally not marked explicit.
+    SubscriptionEntry(
+      std::shared_ptr<rclcpp::SubscriptionBase> subscription_in = nullptr,
+      const rclcpp::SubscriptionWaitSetMask & mask_in = {})
+    : subscription(std::move(subscription_in)),
+      mask(mask_in)
+    {}
+
+    void
+    reset() noexcept
+    {
+      subscription.reset();
+    }
+  };
+  class WeakSubscriptionEntry
+  {
+public:
+    std::weak_ptr<rclcpp::SubscriptionBase> subscription;
+    rclcpp::SubscriptionWaitSetMask mask;
+
+    explicit WeakSubscriptionEntry(
+      const std::shared_ptr<rclcpp::SubscriptionBase> & subscription_in,
+      const rclcpp::SubscriptionWaitSetMask & mask_in) noexcept
+    : subscription(subscription_in),
+      mask(mask_in)
+    {}
+
+    explicit WeakSubscriptionEntry(const SubscriptionEntry & other)
+    : subscription(other.subscription),
+      mask(other.mask)
+    {}
+
+    std::shared_ptr<rclcpp::SubscriptionBase>
+    lock() const
+    {
+      return subscription.lock();
+    }
+
+    bool
+    expired() const noexcept
+    {
+      return subscription.expired();
+    }
+  };
+  using SequenceOfWeakSubscriptions = std::vector<WeakSubscriptionEntry>;
   using SubscriptionsIterable = std::vector<SubscriptionEntry>;
 
-  using SequenceOfWeakGuardConditions = std::vector<detail::WeakGuardConditionEntry>;
-  using GuardConditionsIterable = std::vector<detail::GuardConditionEntry>;
+  using SequenceOfWeakGuardConditions = std::vector<std::weak_ptr<rclcpp::GuardCondition>>;
+  using GuardConditionsIterable = std::vector<std::shared_ptr<rclcpp::GuardCondition>>;
 
-  using SequenceOfWeakTimers = std::vector<detail::WeakTimerEntry>;
-  using TimersIterable = std::vector<detail::TimerEntry>;
+  using SequenceOfWeakTimers = std::vector<std::weak_ptr<rclcpp::TimerBase>>;
+  using TimersIterable = std::vector<std::shared_ptr<rclcpp::TimerBase>>;
 
-  using SequenceOfWeakClients = std::vector<detail::WeakClientEntry>;
-  using ClientsIterable = std::vector<detail::ClientEntry>;
+  using SequenceOfWeakClients = std::vector<std::weak_ptr<rclcpp::ClientBase>>;
+  using ClientsIterable = std::vector<std::shared_ptr<rclcpp::ClientBase>>;
 
-  using SequenceOfWeakServices = std::vector<detail::WeakServiceEntry>;
-  using ServicesIterable = std::vector<detail::ServiceEntry>;
+  using SequenceOfWeakServices = std::vector<std::weak_ptr<rclcpp::ServiceBase>>;
+  using ServicesIterable = std::vector<std::shared_ptr<rclcpp::ServiceBase>>;
 
-  using WeakWaitableEntry = detail::WeakWaitableEntry;
-  using WaitableEntry = detail::WaitableEntry;
+  class WaitableEntry
+  {
+public:
+    std::shared_ptr<rclcpp::Waitable> waitable;
+    std::shared_ptr<void> associated_entity;
 
+    /// Conversion constructor, which is intentionally not marked explicit.
+    WaitableEntry(
+      std::shared_ptr<rclcpp::Waitable> waitable_in = nullptr,
+      std::shared_ptr<void> associated_entity_in = nullptr) noexcept
+    : waitable(std::move(waitable_in)),
+      associated_entity(std::move(associated_entity_in))
+    {}
+
+    void
+    reset() noexcept
+    {
+      waitable.reset();
+      associated_entity.reset();
+    }
+  };
+  class WeakWaitableEntry
+  {
+public:
+    std::weak_ptr<rclcpp::Waitable> waitable;
+    std::weak_ptr<void> associated_entity;
+
+    explicit WeakWaitableEntry(
+      const std::shared_ptr<rclcpp::Waitable> & waitable_in,
+      const std::shared_ptr<void> & associated_entity_in) noexcept
+    : waitable(waitable_in),
+      associated_entity(associated_entity_in)
+    {}
+
+    explicit WeakWaitableEntry(const WaitableEntry & other)
+    : waitable(other.waitable),
+      associated_entity(other.associated_entity)
+    {}
+
+    std::shared_ptr<rclcpp::Waitable>
+    lock() const
+    {
+      return waitable.lock();
+    }
+
+    bool
+    expired() const noexcept
+    {
+      return waitable.expired();
+    }
+  };
   using SequenceOfWeakWaitables = std::vector<WeakWaitableEntry>;
   using WaitablesIterable = std::vector<WaitableEntry>;
 
@@ -93,9 +184,8 @@ protected:
       services,
       waitables,
       context),
-    // subscriptions_ is populated in the constructor dynamically, in order
-    // to respect the mask.
-    // shared_subscriptions_ is resized based on the result of subscriptions_.
+    subscriptions_(subscriptions.cbegin(), subscriptions.cend()),
+    shared_subscriptions_(subscriptions_.size()),
     guard_conditions_(guard_conditions.cbegin(), guard_conditions.cend()),
     shared_guard_conditions_(guard_conditions_.size()),
     timers_(timers.cbegin(), timers.cend()),
@@ -104,39 +194,9 @@ protected:
     shared_clients_(clients_.size()),
     services_(services.cbegin(), services.cend()),
     shared_services_(services_.size()),
-    waitables_(waitables.cbegin(), waitables.cend())
-    // shared_waitables_ is resized based on the result of waitables_ after
-    // waitables from subscriptions are added, if any.
-  {
-    // Ensure subscriptions are not being used by other wait sets and extract
-    // their waitables, if they have them.
-    std::vector<WaitableEntry> waitables_from_subscriptions;
-    for (auto & subscription_entry : subscriptions) {
-      std::vector<WaitableEntry> local_waitables_from_subscriptions = subscription_entry.manage();
-      if (subscription_entry.get_mask().include_subscription) {
-        subscriptions_.push_back(subscription_entry);
-      }
-    }
-    // Add subscription entries from subscriptions, if the subscription mask indicates.
-    std::copy_if(
-      subscriptions.cbegin(),
-      subscriptions.cend(),
-      std::back_inserter(subscriptions_),
-      [](const auto & subscription_entry) {
-        return subscription_entry.mask.include_subscription;
-      }
-    );
-    shared_subscriptions_.resize(subscriptions_.size());
-    // Add waitables from subscriptions, if the subscription mask indicates.
-    for (const auto & subscription_entry : subscriptions) {
-      if (subscription_entry.mask.include_events) {
-        for (const auto & event : subscription_entry.subscription->get_event_handlers()) {
-          waitables_.push_back({event, subscription_entry.subscription});
-        }
-      }
-    }
-    shared_waitables_.resize(waitables_.size());
-  }
+    waitables_(waitables.cbegin(), waitables.cend()),
+    shared_waitables_(waitables_.size())
+  {}
 
   ~DynamicStorage() = default;
 
@@ -183,7 +243,7 @@ protected:
     if (this->storage_has_entity(*subscription, subscriptions_)) {
       throw std::runtime_error("subscription already in wait set");
     }
-    WeakManagedSubscriptionEntry weak_entry{std::move(subscription), {}};
+    WeakSubscriptionEntry weak_entry{std::move(subscription), {}};
     subscriptions_.push_back(std::move(weak_entry));
     this->storage_flag_for_resize();
   }