28.3.0

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

README.md

@@ -8,7 +8,7 @@ rclcpp provides the standard C++ API for interacting with ROS 2.
 
 `#include "rclcpp/rclcpp.hpp"` allows use of the most common elements of the ROS 2 system.
 
-The link to the latest API documentation can be found on the rclcpp package info page, at the [ROS Index](https://index.ros.org/p/rclcpp/).
+The link to the latest API documentation can be found on the [rclcpp package info page](https://docs.ros.org/en/rolling/p/rclcpp).
 
 
 ### Examples

rclcpp/CHANGELOG.rst

@@ -2,6 +2,161 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+28.3.0 (2024-06-17)
+-------------------
+* Add test creating two content filter topics with the same topic name (`#2546 <https://github.com/ros2/rclcpp/issues/2546>`_) (`#2549 <https://github.com/ros2/rclcpp/issues/2549>`_)
+* add impl pointer for ExecutorOptions (`#2523 <https://github.com/ros2/rclcpp/issues/2523>`_)
+* Fixup Executor::spin_all() regression fix (`#2517 <https://github.com/ros2/rclcpp/issues/2517>`_)
+* Add 'mimick' label to tests which use Mimick (`#2516 <https://github.com/ros2/rclcpp/issues/2516>`_)
+* Contributors: Alejandro Hernández Cordero, Scott K Logan, William Woodall
+
+28.2.0 (2024-04-26)
+-------------------
+* Check for negative time in rclcpp::Time(int64_t nanoseconds, ...) constructor (`#2510 <https://github.com/ros2/rclcpp/issues/2510>`_)
+* Revise the description of service configure_introspection() (`#2511 <https://github.com/ros2/rclcpp/issues/2511>`_)
+* Contributors: Barry Xu, Sharmin Ramli
+
+28.1.0 (2024-04-16)
+-------------------
+* Remove references to index.ros.org. (`#2504 <https://github.com/ros2/rclcpp/issues/2504>`_)
+* Reduce overhead for inheriting from rclcpp::Executor when base functionality is not reused (`#2506 <https://github.com/ros2/rclcpp/issues/2506>`_)
+* Contributors: Chris Lalancette, William Woodall, jmachowinski
+
+28.0.1 (2024-04-16)
+-------------------
+* [wjwwood] Updated "Data race fixes" (`#2500 <https://github.com/ros2/rclcpp/issues/2500>`_)
+  * Fix callback group logic in executor
+  * fix: Fixed unnecessary copy of wait_set
+  * fix(executor): Fixed race conditions with rebuild of wait_sets
+  Before this change, the rebuild of wait set would be triggered
+  after the wait set was waken up. With bad timing, this could
+  lead to the rebuild not happening with multi threaded executor.
+  * fix(Executor): Fixed lost of entities rebuild request
+  * chore: Added assert for not set callback_group in execute_any_executable
+  * Add test for cbg getting reset
+  Co-authored-by: Janosch Machowinski <j.machowinski@nospam.org>
+  * chore: renamed test cases to snake_case
+  * style
+  * fixup test to avoid polling and short timeouts
+  * fix: Use correct notify_waitable\_ instance
+  * fix(StaticSingleThreadedExecutor): Added missing special case handling for current_notify_waitable\_
+  * fix(TestCallbackGroup): Fixed test after change to timers
+  ---------
+  Co-authored-by: Janosch Machowinski <j.machowinski@cellumation.com>
+  Co-authored-by: Michael Carroll <mjcarroll@intrinsic.ai>
+  Co-authored-by: Janosch Machowinski <j.machowinski@nospam.org>
+* fixup var names to snake case (`#2501 <https://github.com/ros2/rclcpp/issues/2501>`_)
+* Added optional TimerInfo to timer callback (`#2343 <https://github.com/ros2/rclcpp/issues/2343>`_)
+  Co-authored-by: Alexis Tsogias <a.tsogias@cellumation.com>
+  Co-authored-by: Janosch Machowinski <J.Machowinski@cellumation.com>
+* Fix uninitialized memory in test (`#2498 <https://github.com/ros2/rclcpp/issues/2498>`_)
+  When I added in the tests for large messages, I made a mistake and reserved space in the strings, but didn't actually expand it.  Thus, we were writing into uninitialized memory.  Fix this by just using the correct constructor for string, which will allocate and initialize the memory properly.
+* Ensure waitables handle guard condition retriggering (`#2483 <https://github.com/ros2/rclcpp/issues/2483>`_)
+  Co-authored-by: Michael Carroll <mjcarroll@intrinsic.ai>
+* fix: init concatenated_vector with begin() & end() (`#2492 <https://github.com/ros2/rclcpp/issues/2492>`_)
+  * this commit will fix the warning [-Wstringop-overflow=] `#2461 <https://github.com/ros2/rclcpp/issues/2461>`_
+* Use the same context for the specified node in rclcpp::spin functions (`#2433 <https://github.com/ros2/rclcpp/issues/2433>`_)
+  * Use the same conext for the specified node in rclcpp::spin_xx functions
+  * Add test for spinning with non-default-context
+  * Format code
+  ---------
+* Disable compare-function-pointers in test_utilities (`#2489 <https://github.com/ros2/rclcpp/issues/2489>`_)
+* address ambiguous auto variable. (`#2481 <https://github.com/ros2/rclcpp/issues/2481>`_)
+* Increase the cppcheck timeout to 1200 seconds (`#2484 <https://github.com/ros2/rclcpp/issues/2484>`_)
+* Removed test_timers_manager clang warning (`#2479 <https://github.com/ros2/rclcpp/issues/2479>`_)
+* Flaky timer test fix (`#2469 <https://github.com/ros2/rclcpp/issues/2469>`_)
+  * fix(time_source): Fixed possible race condition
+  * fix(test_executors_time_cancel_behaviour): Fixed multiple race conditions
+  ---------
+  Co-authored-by: Janosch Machowinski <j.machowinski@nospam.org>
+* Add tracepoint for generic publisher/subscriber (`#2448 <https://github.com/ros2/rclcpp/issues/2448>`_)
+* update rclcpp::Waitable API to use references and const (`#2467 <https://github.com/ros2/rclcpp/issues/2467>`_)
+* Utilize rclcpp::WaitSet as part of the executors (`#2142 <https://github.com/ros2/rclcpp/issues/2142>`_)
+  * Deprecate callback_group call taking context
+  * Add base executor objects that can be used by implementors
+  * Template common operations
+  * Address reviewer feedback:
+  * Add callback to EntitiesCollector constructor
+  * Make function to check automatically added callback groups take a list
+  * Lint
+  * Address reviewer feedback and fix templates
+  * Lint and docs
+  * Make executor own the notify waitable
+  * Add pending queue to collector, remove from waitable
+  Also change node's get_guard_condition to return shared_ptr
+  * Change interrupt guard condition to shared_ptr
+  Check if guard condition is valid before adding it to the waitable
+  * Lint and docs
+  * Utilize rclcpp::WaitSet as part of the executors
+  * Don't exchange atomic twice
+  * Fix add_node and add more tests
+  * Make get_notify_guard_condition follow API tick-tock
+  * Improve callback group tick-tocking
+  * Don't lock twice
+  * Address reviewer feedback
+  * Add thread safety annotations and make locks consistent
+  * @wip
+  * Reset callback groups for multithreaded executor
+  * Avoid many small function calls when building executables
+  * Re-trigger guard condition if buffer has data
+  * Address reviewer feedback
+  * Trace points
+  * Remove tracepoints
+  * Reducing diff
+  * Reduce diff
+  * Uncrustify
+  * Restore tests
+  * Back to weak_ptr and reduce test time
+  * reduce diff and lint
+  * Restore static single threaded tests that weren't working before
+  * Restore more tests
+  * Fix multithreaded test
+  * Fix assert
+  * Fix constructor test
+  * Change ready_executables signature back
+  * Don't enforce removing callback groups before nodes
+  * Remove the "add_valid_node" API
+  * Only notify if the trigger condition is valid
+  * Only trigger if valid and needed
+  * Fix spin_some/spin_all implementation
+  * Restore single threaded executor
+  * Picking ABI-incompatible executor changes
+  * Add PIMPL
+  * Additional waitset prune
+  * Fix bad merge
+  * Expand test timeout
+  * Introduce method to clear expired entities from a collection
+  * Make sure to call remove_expired_entities().
+  * Prune queued work when callback group is removed
+  * Prune subscriptions from dynamic storage
+  * Styles fixes.
+  * Re-trigger guard conditions
+  * Condense to just use watiable.take_data
+  * Lint
+  * Address reviewer comments (nits)
+  * Lock mutex when copying
+  * Refactors to static single threaded based on reviewers
+  * More small refactoring
+  * Lint
+  * Lint
+  * Add ready executable accessors to WaitResult
+  * Make use of accessors from wait_set
+  * Fix tests
+  * Fix more tests
+  * Tidy up single threaded executor implementation
+  * Don't null out timer, rely on call
+  * change how timers are checked from wait result in executors
+  * peak -> peek
+  * fix bug in next_waitable logic
+  * fix bug in StaticSTE that broke the add callback groups to executor tests
+  * style
+  ---------
+  Co-authored-by: Chris Lalancette <clalancette@gmail.com>
+  Co-authored-by: William Woodall <william@osrfoundation.org>
+* fix flakiness in TestTimersManager unit-test (`#2468 <https://github.com/ros2/rclcpp/issues/2468>`_)
+  the previous version of the test was relying on the assumption that a timer with 1ms period gets called at least 6 times if the main thread waits 15ms. this is true most of the times, but it's not guaranteed, especially when running the test on windows CI servers. the new version of the test makes no assumptions on how much time it takes for the timers manager to invoke the timers, but rather focuses on ensuring that they are called the right amount of times, which is what's important for the purpose of the test
+* Contributors: Alberto Soragna, Alejandro Hernández Cordero, Chris Lalancette, Homalozoa X, Kotaro Yoshimoto, Michael Carroll, Tomoya Fujita, William Woodall, h-suzuki-isp, jmachowinski
+
 28.0.0 (2024-03-28)
 -------------------
 * fix spin_some_max_duration unit-test for events-executor (`#2465 <https://github.com/ros2/rclcpp/issues/2465>`_)

rclcpp/CMakeLists.txt

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

rclcpp/README.md

@@ -2,7 +2,7 @@
 
 The ROS client library in C++.
 
-The link to the latest rclcpp API documentation, which includes a complete list of its main components and features, can be found on the rclcpp package info page, at the [ROS Index](https://index.ros.org/p/rclcpp/).
+The link to the latest rclcpp API documentation, which includes a complete list of its main components and features, can be found on the [rclcpp package info page](https://docs.ros.org/en/rolling/p/rclcpp).
 
 ## Quality Declaration
 

rclcpp/include/rclcpp/any_executable.hpp

@@ -45,9 +45,9 @@ struct AnyExecutable
   rclcpp::ClientBase::SharedPtr client;
   rclcpp::Waitable::SharedPtr waitable;
   // These are used to keep the scope on the containing items
-  rclcpp::CallbackGroup::SharedPtr callback_group;
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base;
-  std::shared_ptr<void> data;
+  rclcpp::CallbackGroup::SharedPtr callback_group {nullptr};
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base {nullptr};
+  std::shared_ptr<void> data {nullptr};
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/callback_group.hpp

@@ -184,18 +184,41 @@ public:
    * \return the number of entities in the callback group.
    */
   RCLCPP_PUBLIC
-  size_t size() const;
+  size_t
+  size() const;
 
+  /// Return a reference to the 'can be taken' atomic boolean.
+  /**
+   * The resulting bool will be true in the case that no executor is currently
+   * using an executable entity from this group.
+   * The resulting bool will be false in the case that an executor is currently
+   * using an executable entity from this group, and the group policy doesn't
+   * allow a second take (eg mutual exclusion)
+   * \return a reference to the flag
+   */
   RCLCPP_PUBLIC
   std::atomic_bool &
   can_be_taken_from();
 
+  /// Get the group type.
+  /**
+   * \return the group type
+   */
   RCLCPP_PUBLIC
   const CallbackGroupType &
   type() const;
 
+  /// Collect all of the entity pointers contained in this callback group.
+  /**
+   * \param[in] sub_func Function to execute for each subscription
+   * \param[in] service_func Function to execute for each service
+   * \param[in] client_func Function to execute for each client
+   * \param[in] timer_func Function to execute for each timer
+   * \param[in] waitable_fuinc Function to execute for each waitable
+   */
   RCLCPP_PUBLIC
-  void collect_all_ptrs(
+  void
+  collect_all_ptrs(
     std::function<void(const rclcpp::SubscriptionBase::SharedPtr &)> sub_func,
     std::function<void(const rclcpp::ServiceBase::SharedPtr &)> service_func,
     std::function<void(const rclcpp::ClientBase::SharedPtr &)> client_func,

rclcpp/include/rclcpp/client.hpp

@@ -864,7 +864,7 @@ protected:
         "Received invalid sequence number. Ignoring...");
       return std::nullopt;
     }
-    auto value = std::move(it->second.second);
+    std::optional<CallbackInfoVariant> value = std::move(it->second.second);
     this->pending_requests_.erase(request_number);
     return value;
   }

rclcpp/include/rclcpp/clock.hpp

@@ -193,6 +193,16 @@ public:
   bool
   ros_time_is_active();
 
+  /**
+   * Cancels an ongoing or future sleep operation of one thread.
+   *
+   * This function can be used by one thread, to wakeup another thread that is
+   * blocked using any of the sleep_ or wait_ methods of this class.
+   */
+  RCLCPP_PUBLIC
+  void
+  cancel_sleep_or_wait();
+
   /// Return the rcl_clock_t clock handle
   RCLCPP_PUBLIC
   rcl_clock_t *

rclcpp/include/rclcpp/event_handler.hpp

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

rclcpp/include/rclcpp/executor.hpp

@@ -29,26 +29,24 @@
 
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
+#include "rclcpp/executors/executor_notify_waitable.hpp"
 #include "rcpputils/scope_exit.hpp"
 
 #include "rclcpp/context.hpp"
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/guard_condition.hpp"
 #include "rclcpp/executor_options.hpp"
+#include "rclcpp/executors/executor_entities_collection.hpp"
+#include "rclcpp/executors/executor_entities_collector.hpp"
 #include "rclcpp/future_return_code.hpp"
-#include "rclcpp/memory_strategies.hpp"
-#include "rclcpp/memory_strategy.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/wait_set.hpp"
 
 namespace rclcpp
 {
 
-typedef std::map<rclcpp::CallbackGroup::WeakPtr,
-    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
-    std::owner_less<rclcpp::CallbackGroup::WeakPtr>> WeakCallbackGroupsToNodesMap;
-
 // Forward declaration is used in convenience method signature.
 class Node;
 class ExecutorImplementation;
@@ -274,12 +272,12 @@ public:
    * \param[in] node Shared pointer to the node to add.
    */
   RCLCPP_PUBLIC
-  void
+  virtual void
   spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node);
 
   /// Convenience function which takes Node and forwards NodeBaseInterface.
   RCLCPP_PUBLIC
-  void
+  virtual void
   spin_node_some(std::shared_ptr<rclcpp::Node> node);
 
   /// Collect work once and execute all available work, optionally within a max duration.
@@ -309,14 +307,14 @@ public:
    * \param[in] node Shared pointer to the node to add.
    */
   RCLCPP_PUBLIC
-  void
+  virtual void
   spin_node_all(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
     std::chrono::nanoseconds max_duration);
 
   /// Convenience function which takes Node and forwards NodeBaseInterface.
   RCLCPP_PUBLIC
-  void
+  virtual void
   spin_node_all(std::shared_ptr<rclcpp::Node> node, std::chrono::nanoseconds max_duration);
 
   /// Collect and execute work repeatedly within a duration or until no more work is available.
@@ -368,52 +366,12 @@ public:
     const FutureT & future,
     std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
   {
-    // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
-    // inside a callback executed by an executor.
-
-    // Check the future before entering the while loop.
-    // If the future is already complete, don't try to spin.
-    std::future_status status = future.wait_for(std::chrono::seconds(0));
-    if (status == std::future_status::ready) {
-      return FutureReturnCode::SUCCESS;
-    }
-
-    auto end_time = std::chrono::steady_clock::now();
-    std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
-      timeout);
-    if (timeout_ns > std::chrono::nanoseconds::zero()) {
-      end_time += timeout_ns;
-    }
-    std::chrono::nanoseconds timeout_left = timeout_ns;
-
-    if (spinning.exchange(true)) {
-      throw std::runtime_error("spin_until_future_complete() called while already spinning");
-    }
-    RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
-    while (rclcpp::ok(this->context_) && spinning.load()) {
-      // Do one item of work.
-      spin_once_impl(timeout_left);
-
-      // Check if the future is set, return SUCCESS if it is.
-      status = future.wait_for(std::chrono::seconds(0));
-      if (status == std::future_status::ready) {
-        return FutureReturnCode::SUCCESS;
+    return spin_until_future_complete_impl(
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout),
+      [&future](std::chrono::nanoseconds wait_time) {
+        return future.wait_for(wait_time);
       }
-      // If the original timeout is < 0, then this is blocking, never TIMEOUT.
-      if (timeout_ns < std::chrono::nanoseconds::zero()) {
-        continue;
-      }
-      // Otherwise check if we still have time to wait, return TIMEOUT if not.
-      auto now = std::chrono::steady_clock::now();
-      if (now >= end_time) {
-        return FutureReturnCode::TIMEOUT;
-      }
-      // Subtract the elapsed time from the original timeout.
-      timeout_left = std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - now);
-    }
-
-    // The future did not complete before ok() returned false, return INTERRUPTED.
-    return FutureReturnCode::INTERRUPTED;
+    );
   }
 
   /// Cancel any running spin* function, causing it to return.
@@ -422,20 +380,9 @@ public:
    * \throws std::runtime_error if there is an issue triggering the guard condition
    */
   RCLCPP_PUBLIC
-  void
+  virtual void
   cancel();
 
-  /// Support dynamic switching of the memory strategy.
-  /**
-   * Switching the memory strategy while the executor is spinning in another threading could have
-   * unintended consequences.
-   * \param[in] memory_strategy Shared pointer to the memory strategy to set.
-   * \throws std::runtime_error if memory_strategy is null
-   */
-  RCLCPP_PUBLIC
-  void
-  set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy);
-
   /// Returns true if the executor is currently spinning.
   /**
    * This function can be called asynchronously from any thread.
@@ -446,6 +393,14 @@ public:
   is_spinning();
 
 protected:
+  /// Constructor that will not initialize any non-trivial members.
+  /**
+  * This constructor is intended to be used by any derived executor
+  * that explicitly does not want to use the default implementation provided
+  * by this class.
+  */
+  explicit Executor(const std::shared_ptr<rclcpp::Context> & context);
+
   /// Add a node to executor, execute the next available unit of work, and remove the node.
   /**
    * Implementation of spin_node_once using std::chrono::nanoseconds
@@ -460,6 +415,23 @@ protected:
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
     std::chrono::nanoseconds timeout);
 
+  /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
+  /**
+   * \sa spin_until_future_complete()
+   * The only difference with spin_until_future_complete() is that the future's
+   * type is obscured through a std::function which lets you wait on it
+   * reguardless of type.
+   *
+   * \param[in] timeout see spin_until_future_complete() for details
+   * \param[in] wait_for_future function to wait on the future and get the
+   *   status after waiting
+   */
+  RCLCPP_PUBLIC
+  virtual FutureReturnCode
+  spin_until_future_complete_impl(
+    std::chrono::nanoseconds timeout,
+    const std::function<std::future_status(std::chrono::nanoseconds wait_time)> & wait_for_future);
+
   /// Collect work and execute available work, optionally within a duration.
   /**
    * Implementation of spin_some and spin_all.
@@ -500,7 +472,7 @@ protected:
    */
   RCLCPP_PUBLIC
   static void
-  execute_timer(rclcpp::TimerBase::SharedPtr timer);
+  execute_timer(rclcpp::TimerBase::SharedPtr timer, const std::shared_ptr<void> & data_ptr);
 
   /// Run service server executable.
   /**
@@ -520,6 +492,11 @@ protected:
   static void
   execute_client(rclcpp::ClientBase::SharedPtr client);
 
+  /// Gather all of the waitable entities from associated nodes and callback groups.
+  RCLCPP_PUBLIC
+  void
+  collect_entities();
+
   /// Block until more work becomes avilable or timeout is reached.
   /**
    * Builds a set of waitable entities, which are passed to the middleware.
@@ -531,62 +508,6 @@ protected:
   void
   wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
-  /// Find node associated with a callback group
-  /**
-   * \param[in] weak_groups_to_nodes map of callback groups to nodes
-   * \param[in] group callback group to find assocatiated node
-   * \return Pointer to associated node if found, else nullptr
-   */
-  RCLCPP_PUBLIC
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
-  get_node_by_group(
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
-    rclcpp::CallbackGroup::SharedPtr group);
-
-  /// Return true if the node has been added to this executor.
-  /**
-   * \param[in] node_ptr a shared pointer that points to a node base interface
-   * \param[in] weak_groups_to_nodes map to nodes to lookup
-   * \return true if the node is associated with the executor, otherwise false
-   */
-  RCLCPP_PUBLIC
-  bool
-  has_node(
-    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const;
-
-  /// Find the callback group associated with a timer
-  /**
-   * \param[in] timer Timer to find associated callback group
-   * \return Pointer to callback group node if found, else nullptr
-   */
-  RCLCPP_PUBLIC
-  rclcpp::CallbackGroup::SharedPtr
-  get_group_by_timer(rclcpp::TimerBase::SharedPtr timer);
-
-  /// Add a callback group to an executor
-  /**
-   * \see rclcpp::Executor::add_callback_group
-   */
-  RCLCPP_PUBLIC
-  virtual void
-  add_callback_group_to_map(
-    rclcpp::CallbackGroup::SharedPtr group_ptr,
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
-    bool notify = true) RCPPUTILS_TSA_REQUIRES(mutex_);
-
-  /// Remove a callback group from the executor.
-  /**
-   * \see rclcpp::Executor::remove_callback_group
-   */
-  RCLCPP_PUBLIC
-  virtual void
-  remove_callback_group_from_map(
-    rclcpp::CallbackGroup::SharedPtr group_ptr,
-    WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
-    bool notify = true) RCPPUTILS_TSA_REQUIRES(mutex_);
-
   /// Check for executable in ready state and populate union structure.
   /**
    * \param[out] any_executable populated union structure of ready executable
@@ -597,33 +518,6 @@ protected:
   bool
   get_next_ready_executable(AnyExecutable & any_executable);
 
-  /// Check for executable in ready state and populate union structure.
-  /**
-   * This is the implementation of get_next_ready_executable that takes into
-   * account the current state of callback groups' association with nodes and
-   * executors.
-   *
-   * This checks in a particular order for available work:
-   *   * Timers
-   *   * Subscriptions
-   *   * Services
-   *   * Clients
-   *   * Waitable
-   *
-   * If the next executable is not associated with this executor/node pair,
-   * then this method will return false.
-   *
-   * \param[out] any_executable populated union structure of ready executable
-   * \param[in] weak_groups_to_nodes mapping of callback groups to nodes
-   * \return true if an executable was ready and any_executable was populated,
-   *   otherwise false
-   */
-  RCLCPP_PUBLIC
-  bool
-  get_next_ready_executable_from_map(
-    AnyExecutable & any_executable,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
-
   /// Wait for executable in ready state and populate union structure.
   /**
    * If an executable is ready, it will return immediately, otherwise
@@ -641,20 +535,14 @@ protected:
     AnyExecutable & any_executable,
     std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
-  /// Add all callback groups that can be automatically added from associated nodes.
+  /// This function triggers a recollect of all entities that are registered to the executor.
   /**
-   * The executor, before collecting entities, verifies if any callback group from
-   * nodes associated with the executor, which is not already associated to an executor,
-   * can be automatically added to this executor.
-   * This takes care of any callback group that has been added to a node but not explicitly added
-   * to the executor.
-   * It is important to note that in order for the callback groups to be automatically added to an
-   * executor through this function, the node of the callback groups needs to have been added
-   * through the `add_node` method.
+   * Calling this function is thread safe.
+   *
+   * \param[in] notify if true will execute a trigger that will wake up a waiting executor
    */
-  RCLCPP_PUBLIC
-  virtual void
-  add_callback_groups_from_nodes_associated_to_executor() RCPPUTILS_TSA_REQUIRES(mutex_);
+  void
+  trigger_entity_recollect(bool notify);
 
   /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins.
   std::atomic_bool spinning;
@@ -665,16 +553,8 @@ protected:
   /// Guard condition for signaling the rmw layer to wake up for system shutdown.
   std::shared_ptr<rclcpp::GuardCondition> shutdown_guard_condition_;
 
-  /// Wait set for managing entities that the rmw layer waits on.
-  rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
-
-  // Mutex to protect the subsequent memory_strategy_.
   mutable std::mutex mutex_;
 
-  /// The memory strategy: an interface for handling user-defined memory allocation strategies.
-  memory_strategy::MemoryStrategy::SharedPtr
-  memory_strategy_ RCPPUTILS_TSA_PT_GUARDED_BY(mutex_);
-
   /// The context associated with this executor.
   std::shared_ptr<rclcpp::Context> context_;
 
@@ -684,39 +564,31 @@ protected:
   virtual void
   spin_once_impl(std::chrono::nanoseconds timeout);
 
-  typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
-      const rclcpp::GuardCondition *,
-      std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
-    WeakNodesToGuardConditionsMap;
+  /// Waitable containing guard conditions controlling the executor flow.
+  /**
+   * This waitable contains the interrupt and shutdown guard condition, as well
+   * as the guard condition associated with each node and callback group.
+   * By default, if any change is detected in the monitored entities, the notify
+   * waitable will awake the executor and rebuild the collections.
+   */
+  std::shared_ptr<rclcpp::executors::ExecutorNotifyWaitable> notify_waitable_;
 
-  typedef std::map<rclcpp::CallbackGroup::WeakPtr,
-      const rclcpp::GuardCondition *,
-      std::owner_less<rclcpp::CallbackGroup::WeakPtr>>
-    WeakCallbackGroupsToGuardConditionsMap;
+  std::atomic_bool entities_need_rebuild_;
 
-  /// maps nodes to guard conditions
-  WeakNodesToGuardConditionsMap
-  weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+  /// Collector used to associate executable entities from nodes and guard conditions
+  rclcpp::executors::ExecutorEntitiesCollector collector_;
 
-  /// maps callback groups to guard conditions
-  WeakCallbackGroupsToGuardConditionsMap
-  weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+  /// WaitSet to be waited on.
+  rclcpp::WaitSet wait_set_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+  std::optional<rclcpp::WaitResult<rclcpp::WaitSet>> wait_result_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
-  /// maps callback groups associated to nodes
-  WeakCallbackGroupsToNodesMap
-  weak_groups_associated_with_executor_to_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+  /// Hold the current state of the collection being waited on by the waitset
+  rclcpp::executors::ExecutorEntitiesCollection current_collection_ RCPPUTILS_TSA_GUARDED_BY(
+    mutex_);
 
-  /// maps callback groups to nodes associated with executor
-  WeakCallbackGroupsToNodesMap
-  weak_groups_to_nodes_associated_with_executor_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
-
-  /// maps all callback groups to nodes
-  WeakCallbackGroupsToNodesMap
-  weak_groups_to_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
-
-  /// nodes that are associated with the executor
-  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>
-  weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+  /// Hold the current state of the notify waitable being waited on by the waitset
+  std::shared_ptr<rclcpp::executors::ExecutorNotifyWaitable> current_notify_waitable_
+  RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
   /// shutdown callback handle registered to Context
   rclcpp::OnShutdownCallbackHandle shutdown_callback_handle_;

rclcpp/include/rclcpp/executor_options.hpp

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

rclcpp/include/rclcpp/executors.hpp

@@ -120,7 +120,9 @@ spin_until_future_complete(
   const FutureT & future,
   std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
-  rclcpp::executors::SingleThreadedExecutor executor;
+  rclcpp::ExecutorOptions options;
+  options.context = node_ptr->get_context();
+  rclcpp::executors::SingleThreadedExecutor executor(options);
   return executors::spin_node_until_future_complete<FutureT>(executor, node_ptr, future, timeout);
 }
 

rclcpp/include/rclcpp/executors/executor_entities_collection.hpp

@@ -178,6 +178,12 @@ struct ExecutorEntitiesCollection
 
   /// Clear the entities collection
   void clear();
+
+  /// Remove entities that have expired weak ownership
+  /**
+   * \return The total number of removed entities
+   */
+  size_t remove_expired_entities();
 };
 
 /// Build an entities collection from callback groups

rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp

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

rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp

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

rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp

@@ -15,24 +15,13 @@
 #ifndef RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_
 #define RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_
 
+#include <atomic>
 #include <chrono>
-#include <cassert>
-#include <cstdlib>
 #include <memory>
-#include <vector>
-#include <string>
-
-#include "rmw/rmw.h"
 
 #include "rclcpp/executor.hpp"
-#include "rclcpp/executors/static_executor_entities_collector.hpp"
-#include "rclcpp/experimental/executable_list.hpp"
-#include "rclcpp/macros.hpp"
-#include "rclcpp/memory_strategies.hpp"
-#include "rclcpp/node.hpp"
-#include "rclcpp/rate.hpp"
-#include "rclcpp/utilities.hpp"
-#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/executors/executor_entities_collection.hpp"
+#include "rclcpp/executors/single_threaded_executor.hpp"
 
 namespace rclcpp
 {
@@ -65,7 +54,7 @@ public:
   explicit StaticSingleThreadedExecutor(
     const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
 
-  /// Default destrcutor.
+  /// Default destructor.
   RCLCPP_PUBLIC
   virtual ~StaticSingleThreadedExecutor();
 
@@ -116,105 +105,31 @@ public:
   void
   spin_all(std::chrono::nanoseconds max_duration) override;
 
-  /// Add a callback group to an executor.
-  /**
-   * \sa rclcpp::Executor::add_callback_group
-   */
-  RCLCPP_PUBLIC
-  void
-  add_callback_group(
-    rclcpp::CallbackGroup::SharedPtr group_ptr,
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    bool notify = true) override;
-
-  /// Remove callback group from the executor
-  /**
-   * \sa rclcpp::Executor::remove_callback_group
-   */
-  RCLCPP_PUBLIC
-  void
-  remove_callback_group(
-    rclcpp::CallbackGroup::SharedPtr group_ptr,
-    bool notify = true) override;
-
-  /// Add a node to the executor.
-  /**
-   * \sa rclcpp::Executor::add_node
-   */
-  RCLCPP_PUBLIC
-  void
-  add_node(
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    bool notify = true) override;
-
-  /// Convenience function which takes Node and forwards NodeBaseInterface.
-  /**
-   * \sa rclcpp::StaticSingleThreadedExecutor::add_node
-   */
-  RCLCPP_PUBLIC
-  void
-  add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
-
-  /// Remove a node from the executor.
-  /**
-   * \sa rclcpp::Executor::remove_node
-   */
-  RCLCPP_PUBLIC
-  void
-  remove_node(
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    bool notify = true) override;
-
-  /// Convenience function which takes Node and forwards NodeBaseInterface.
-  /**
-   * \sa rclcpp::Executor::remove_node
-   */
-  RCLCPP_PUBLIC
-  void
-  remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
-
-  RCLCPP_PUBLIC
-  std::vector<rclcpp::CallbackGroup::WeakPtr>
-  get_all_callback_groups() override;
-
-  /// Get callback groups that belong to executor.
-  /**
-   * \sa rclcpp::Executor::get_manually_added_callback_groups()
-   */
-  RCLCPP_PUBLIC
-  std::vector<rclcpp::CallbackGroup::WeakPtr>
-  get_manually_added_callback_groups() override;
-
-  /// Get callback groups that belong to executor.
-  /**
-   * \sa rclcpp::Executor::get_automatically_added_callback_groups_from_nodes()
-   */
-  RCLCPP_PUBLIC
-  std::vector<rclcpp::CallbackGroup::WeakPtr>
-  get_automatically_added_callback_groups_from_nodes() override;
-
 protected:
   /**
    * @brief Executes ready executables from wait set.
+   * @param collection entities to evaluate for ready executables.
+   * @param wait_result result to check for ready executables.
    * @param spin_once if true executes only the first ready executable.
    * @return true if any executable was ready.
    */
-  RCLCPP_PUBLIC
   bool
-  execute_ready_executables(bool spin_once = false);
+  execute_ready_executables(
+    const rclcpp::executors::ExecutorEntitiesCollection & collection,
+    rclcpp::WaitResult<rclcpp::WaitSet> & wait_result,
+    bool spin_once);
 
-  RCLCPP_PUBLIC
   void
   spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive);
 
-  RCLCPP_PUBLIC
   void
   spin_once_impl(std::chrono::nanoseconds timeout) override;
 
+  std::optional<rclcpp::WaitResult<rclcpp::WaitSet>>
+  collect_and_wait(std::chrono::nanoseconds timeout);
+
 private:
   RCLCPP_DISABLE_COPY(StaticSingleThreadedExecutor)
-
-  StaticExecutorEntitiesCollector::SharedPtr entities_collector_;
 };
 
 }  // namespace executors

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

@@ -15,6 +15,8 @@
 #ifndef RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_EVENT_TYPES_HPP_
 #define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_EVENT_TYPES_HPP_
 
+#include <memory>
+
 namespace rclcpp
 {
 namespace experimental
@@ -34,6 +36,7 @@ enum ExecutorEventType
 struct ExecutorEvent
 {
   const void * entity_key;
+  std::shared_ptr<void> data;
   int waitable_data;
   ExecutorEventType type;
   size_t num_events;

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

@@ -253,7 +253,8 @@ public:
       // So this case is equivalent to all the buffers requiring ownership
 
       // Merge the two vector of ids into a unique one
-      std::vector<uint64_t> concatenated_vector(sub_ids.take_shared_subscriptions);
+      std::vector<uint64_t> concatenated_vector(
+        sub_ids.take_shared_subscriptions.begin(), sub_ids.take_shared_subscriptions.end());
       concatenated_vector.insert(
         concatenated_vector.end(),
         sub_ids.take_ownership_subscriptions.begin(),

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

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

rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp

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

rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp

@@ -30,6 +30,7 @@
 #include "rclcpp/experimental/ros_message_intra_process_buffer.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp"
 
 #include "tracetools/tracetools.h"
 
@@ -99,8 +100,17 @@ public:
       static_cast<const void *>(this));
   }
 
+  void
+  add_to_wait_set(rcl_wait_set_t & wait_set) override
+  {
+    if (this->buffer_->has_data()) {
+      this->trigger_guard_condition();
+    }
+    detail::add_guard_condition_to_rcl_wait_set(wait_set, this->gc_);
+  }
+
   bool
-  is_ready(rcl_wait_set_t * wait_set) override
+  is_ready(const rcl_wait_set_t & wait_set) override
   {
     (void) wait_set;
     return buffer_->has_data();

rclcpp/include/rclcpp/experimental/timers_manager.hpp

@@ -86,7 +86,8 @@ public:
   RCLCPP_PUBLIC
   TimersManager(
     std::shared_ptr<rclcpp::Context> context,
-    std::function<void(const rclcpp::TimerBase *)> on_ready_callback = nullptr);
+    std::function<void(const rclcpp::TimerBase *,
+    const std::shared_ptr<void> &)> on_ready_callback = nullptr);
 
   /**
    * @brief Destruct the TimersManager object making sure to stop thread and release memory.
@@ -164,9 +165,10 @@ public:
    * the TimersManager on_ready_callback was passed during construction.
    *
    * @param timer_id the timer ID of the timer to execute
+   * @param data internal data of the timer
    */
   RCLCPP_PUBLIC
-  void execute_ready_timer(const rclcpp::TimerBase * timer_id);
+  void execute_ready_timer(const rclcpp::TimerBase * timer_id, const std::shared_ptr<void> & data);
 
   /**
    * @brief Get the amount of time before the next timer triggers.
@@ -529,7 +531,8 @@ private:
   void execute_ready_timers_unsafe();
 
   // Callback to be called when timer is ready
-  std::function<void(const rclcpp::TimerBase *)> on_ready_callback_;
+  std::function<void(const rclcpp::TimerBase *,
+    const std::shared_ptr<void> &)> on_ready_callback_ = nullptr;
 
   // Thread used to run the timers execution task
   std::thread timers_thread_;

rclcpp/include/rclcpp/generic_subscription.hpp

@@ -84,13 +84,22 @@ public:
       options.event_callbacks,
       options.use_default_callbacks,
       DeliveredMessageKind::SERIALIZED_MESSAGE),
-    callback_([callback](
-        std::shared_ptr<const rclcpp::SerializedMessage> serialized_message,
-        const rclcpp::MessageInfo & message_info) mutable {
-        callback.dispatch(serialized_message, message_info);
-      }),
+    any_callback_(callback),
     ts_lib_(ts_lib)
-  {}
+  {
+    TRACETOOLS_TRACEPOINT(
+      rclcpp_subscription_init,
+      static_cast<const void *>(get_subscription_handle().get()),
+      static_cast<const void *>(this));
+    TRACETOOLS_TRACEPOINT(
+      rclcpp_subscription_callback_added,
+      static_cast<const void *>(this),
+      static_cast<const void *>(&any_callback_));
+
+#ifndef TRACETOOLS_DISABLED
+    any_callback_.register_callback_for_tracing();
+#endif
+  }
 
   RCLCPP_PUBLIC
   virtual ~GenericSubscription() = default;
@@ -153,10 +162,7 @@ public:
 
 private:
   RCLCPP_DISABLE_COPY(GenericSubscription)
-
-  std::function<void(
-      std::shared_ptr<const rclcpp::SerializedMessage>,
-      const rclcpp::MessageInfo)> callback_;
+  AnySubscriptionCallback<rclcpp::SerializedMessage, std::allocator<void>> any_callback_;
   // The type support library should stay loaded, so it is stored in the GenericSubscription
   std::shared_ptr<rcpputils::SharedLibrary> ts_lib_;
 };

rclcpp/include/rclcpp/guard_condition.hpp

@@ -100,7 +100,7 @@ public:
    */
   RCLCPP_PUBLIC
   void
-  add_to_wait_set(rcl_wait_set_t * wait_set);
+  add_to_wait_set(rcl_wait_set_t & wait_set);
 
   /// Set a callback to be called whenever the guard condition is triggered.
   /**
@@ -128,7 +128,9 @@ protected:
   std::recursive_mutex reentrant_mutex_;
   std::function<void(size_t)> on_trigger_callback_{nullptr};
   size_t unread_count_{0};
-  rcl_wait_set_t * wait_set_{nullptr};
+  // the type of wait_set_ is actually rcl_wait_set_t *, but it's never
+  // dereferenced, only compared to, so make it void * to avoid accidental use
+  void * wait_set_{nullptr};
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/service.hpp

@@ -499,7 +499,7 @@ public:
     }
   }
 
-  /// Configure client introspection.
+  /// Configure service introspection.
   /**
    * \param[in] clock clock to use to generate introspection timestamps
    * \param[in] qos_service_event_pub QoS settings to use when creating the introspection publisher

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

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

rclcpp/include/rclcpp/time.hpp

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

rclcpp/include/rclcpp/timer.hpp

@@ -18,6 +18,7 @@
 #include <atomic>
 #include <chrono>
 #include <functional>
+#include <optional>
 #include <memory>
 #include <sstream>
 #include <thread>
@@ -43,6 +44,12 @@
 namespace rclcpp
 {
 
+struct TimerInfo
+{
+  Time expected_call_time;
+  Time actual_call_time;
+};
+
 class TimerBase
 {
 public:
@@ -101,16 +108,20 @@ public:
    * The multithreaded executor takes advantage of this to avoid scheduling
    * the callback multiple times.
    *
-   * \return `true` if the callback should be executed, `false` if the timer was canceled.
+   * \return a valid shared_ptr if the callback should be executed,
+   *         an invalid shared_ptr (nullptr) if the timer was canceled.
    */
   RCLCPP_PUBLIC
-  virtual bool
+  virtual std::shared_ptr<void>
   call() = 0;
 
   /// Call the callback function when the timer signal is emitted.
+  /**
+   * \param[in] data the pointer returned by the call function
+   */
   RCLCPP_PUBLIC
   virtual void
-  execute_callback() = 0;
+  execute_callback(const std::shared_ptr<void> & data) = 0;
 
   RCLCPP_PUBLIC
   std::shared_ptr<const rcl_timer_t>
@@ -198,16 +209,17 @@ protected:
   set_on_reset_callback(rcl_event_callback_t callback, const void * user_data);
 };
 
-
 using VoidCallbackType = std::function<void ()>;
 using TimerCallbackType = std::function<void (TimerBase &)>;
+using TimerInfoCallbackType = std::function<void (const TimerInfo &)>;
 
 /// Generic timer. Periodically executes a user-specified callback.
 template<
   typename FunctorT,
   typename std::enable_if<
     rclcpp::function_traits::same_arguments<FunctorT, VoidCallbackType>::value ||
-    rclcpp::function_traits::same_arguments<FunctorT, TimerCallbackType>::value
+    rclcpp::function_traits::same_arguments<FunctorT, TimerCallbackType>::value ||
+    rclcpp::function_traits::same_arguments<FunctorT, TimerInfoCallbackType>::value
   >::type * = nullptr
 >
 class GenericTimer : public TimerBase
@@ -256,27 +268,28 @@ public:
    * \sa rclcpp::TimerBase::call
    * \throws std::runtime_error if it failed to notify timer that callback will occurr
    */
-  bool
+  std::shared_ptr<void>
   call() override
   {
-    rcl_ret_t ret = rcl_timer_call(timer_handle_.get());
+    auto timer_call_info_ = std::make_shared<rcl_timer_call_info_t>();
+    rcl_ret_t ret = rcl_timer_call_with_info(timer_handle_.get(), timer_call_info_.get());
     if (ret == RCL_RET_TIMER_CANCELED) {
-      return false;
+      return nullptr;
     }
     if (ret != RCL_RET_OK) {
       throw std::runtime_error("Failed to notify timer that callback occurred");
     }
-    return true;
+    return timer_call_info_;
   }
 
   /**
    * \sa rclcpp::TimerBase::execute_callback
    */
   void
-  execute_callback() override
+  execute_callback(const std::shared_ptr<void> & data) override
   {
     TRACETOOLS_TRACEPOINT(callback_start, reinterpret_cast<const void *>(&callback_), false);
-    execute_callback_delegate<>();
+    execute_callback_delegate<>(*static_cast<rcl_timer_call_info_t *>(data.get()));
     TRACETOOLS_TRACEPOINT(callback_end, reinterpret_cast<const void *>(&callback_));
   }
 
@@ -288,7 +301,7 @@ public:
     >::type * = nullptr
   >
   void
-  execute_callback_delegate()
+  execute_callback_delegate(const rcl_timer_call_info_t &)
   {
     callback_();
   }
@@ -300,11 +313,26 @@ public:
     >::type * = nullptr
   >
   void
-  execute_callback_delegate()
+  execute_callback_delegate(const rcl_timer_call_info_t &)
   {
     callback_(*this);
   }
 
+
+  template<
+    typename CallbackT = FunctorT,
+    typename std::enable_if<
+      rclcpp::function_traits::same_arguments<CallbackT, TimerInfoCallbackType>::value
+    >::type * = nullptr
+  >
+  void
+  execute_callback_delegate(const rcl_timer_call_info_t & timer_call_info)
+  {
+    const TimerInfo info{Time{timer_call_info.expected_call_time, clock_->get_clock_type()},
+      Time{timer_call_info.actual_call_time, clock_->get_clock_type()}};
+    callback_(info);
+  }
+
   /// Is the clock steady (i.e. is the time between ticks constant?)
   /** \return True if the clock used by this timer is steady. */
   bool
@@ -323,7 +351,8 @@ template<
   typename FunctorT,
   typename std::enable_if<
     rclcpp::function_traits::same_arguments<FunctorT, VoidCallbackType>::value ||
-    rclcpp::function_traits::same_arguments<FunctorT, TimerCallbackType>::value
+    rclcpp::function_traits::same_arguments<FunctorT, TimerCallbackType>::value ||
+    rclcpp::function_traits::same_arguments<FunctorT, TimerInfoCallbackType>::value
   >::type * = nullptr
 >
 class WallTimer : public GenericTimer<FunctorT>

rclcpp/include/rclcpp/wait_result.hpp

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

rclcpp/include/rclcpp/wait_result_kind.hpp

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

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

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

rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp

@@ -204,15 +204,19 @@ public:
   void
   storage_rebuild_rcl_wait_set(const ArrayOfExtraGuardConditions & extra_guard_conditions)
   {
+    this->storage_acquire_ownerships();
+
     this->storage_rebuild_rcl_wait_set_with_sets(
-      subscriptions_,
-      guard_conditions_,
+      shared_subscriptions_,
+      shared_guard_conditions_,
       extra_guard_conditions,
-      timers_,
-      clients_,
-      services_,
-      waitables_
+      shared_timers_,
+      shared_clients_,
+      shared_services_,
+      shared_waitables_
     );
+
+    this->storage_release_ownerships();
   }
 
   template<class EntityT, class SequenceOfEntitiesT>
@@ -382,6 +386,8 @@ public:
         return weak_ptr.expired();
       };
     // remove guard conditions which have been deleted
+    subscriptions_.erase(
+      std::remove_if(subscriptions_.begin(), subscriptions_.end(), p), subscriptions_.end());
     guard_conditions_.erase(
       std::remove_if(guard_conditions_.begin(), guard_conditions_.end(), p),
       guard_conditions_.end());
@@ -407,6 +413,7 @@ public:
         }
       };
     // Lock all the weak pointers and hold them until released.
+    lock_all(subscriptions_, shared_subscriptions_);
     lock_all(guard_conditions_, shared_guard_conditions_);
     lock_all(timers_, shared_timers_);
     lock_all(clients_, shared_clients_);
@@ -438,6 +445,7 @@ public:
           shared_ptr.reset();
         }
       };
+    reset_all(shared_subscriptions_);
     reset_all(shared_guard_conditions_);
     reset_all(shared_timers_);
     reset_all(shared_clients_);
@@ -445,6 +453,61 @@ public:
     reset_all(shared_waitables_);
   }
 
+  size_t size_of_subscriptions() const
+  {
+    return shared_subscriptions_.size();
+  }
+
+  size_t size_of_timers() const
+  {
+    return shared_timers_.size();
+  }
+
+  size_t size_of_clients() const
+  {
+    return shared_clients_.size();
+  }
+
+  size_t size_of_services() const
+  {
+    return shared_services_.size();
+  }
+
+  size_t size_of_waitables() const
+  {
+    return shared_waitables_.size();
+  }
+
+  std::shared_ptr<rclcpp::SubscriptionBase>
+  subscriptions(size_t ii) const
+  {
+    return shared_subscriptions_[ii].subscription;
+  }
+
+  std::shared_ptr<rclcpp::TimerBase>
+  timers(size_t ii) const
+  {
+    return shared_timers_[ii];
+  }
+
+  std::shared_ptr<rclcpp::ClientBase>
+  clients(size_t ii) const
+  {
+    return shared_clients_[ii];
+  }
+
+  std::shared_ptr<rclcpp::ServiceBase>
+  services(size_t ii) const
+  {
+    return shared_services_[ii];
+  }
+
+  std::shared_ptr<rclcpp::Waitable>
+  waitables(size_t ii) const
+  {
+    return shared_waitables_[ii].waitable;
+  }
+
   size_t ownership_reference_counter_ = 0;
 
   SequenceOfWeakSubscriptions subscriptions_;

rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp

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

rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp

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

rclcpp/include/rclcpp/wait_set_template.hpp

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

rclcpp/include/rclcpp/waitable.hpp

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

rclcpp/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp</name>
-  <version>28.0.0</version>
+  <version>28.3.0</version>
   <description>The ROS client library in C++.</description>
 
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>

rclcpp/src/rclcpp/any_executable.cpp

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

rclcpp/src/rclcpp/callback_group.cpp

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

rclcpp/src/rclcpp/clock.cpp

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

rclcpp/src/rclcpp/event_handler.cpp

@@ -15,6 +15,7 @@
 #include <stdexcept>
 #include <string>
 
+#include "rcl/error_handling.h"
 #include "rcl/event.h"
 
 #include "rclcpp/event_handler.hpp"
@@ -56,9 +57,9 @@ EventHandlerBase::get_number_of_ready_events()
 
 /// Add the Waitable to a wait set.
 void
-EventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set)
+EventHandlerBase::add_to_wait_set(rcl_wait_set_t & wait_set)
 {
-  rcl_ret_t ret = rcl_wait_set_add_event(wait_set, &event_handle_, &wait_set_event_index_);
+  rcl_ret_t ret = rcl_wait_set_add_event(&wait_set, &event_handle_, &wait_set_event_index_);
   if (RCL_RET_OK != ret) {
     exceptions::throw_from_rcl_error(ret, "Couldn't add event to wait set");
   }
@@ -66,9 +67,9 @@ EventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set)
 
 /// Check if the Waitable is ready.
 bool
-EventHandlerBase::is_ready(rcl_wait_set_t * wait_set)
+EventHandlerBase::is_ready(const rcl_wait_set_t & wait_set)
 {
-  return wait_set->events[wait_set_event_index_] == &event_handle_;
+  return wait_set.events[wait_set_event_index_] == &event_handle_;
 }
 
 void

rclcpp/src/rclcpp/executor_options.cpp

@@ -0,0 +1,55 @@
+// Copyright 2024 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/executor_options.hpp"
+
+using rclcpp::ExecutorOptions;
+
+namespace rclcpp
+{
+
+class ExecutorOptionsImplementation {};
+
+}  // namespace rclcpp
+
+ExecutorOptions::ExecutorOptions()
+: memory_strategy(rclcpp::memory_strategies::create_default_strategy()),
+  context(rclcpp::contexts::get_global_default_context()),
+  max_conditions(0),
+  impl_(nullptr)
+{}
+
+ExecutorOptions::~ExecutorOptions()
+{}
+
+ExecutorOptions::ExecutorOptions(const ExecutorOptions & other)
+{
+  *this = other;
+}
+
+ExecutorOptions & ExecutorOptions::operator=(const ExecutorOptions & other)
+{
+  if (this == &other) {
+    return *this;
+  }
+
+  this->memory_strategy = other.memory_strategy;
+  this->context = other.context;
+  this->max_conditions = other.max_conditions;
+  if (nullptr != other.impl_) {
+    this->impl_ = std::make_unique<ExecutorOptionsImplementation>(*other.impl_);
+  }
+
+  return *this;
+}

rclcpp/src/rclcpp/executors.cpp

@@ -19,7 +19,9 @@ rclcpp::spin_all(
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
   std::chrono::nanoseconds max_duration)
 {
-  rclcpp::executors::SingleThreadedExecutor exec;
+  rclcpp::ExecutorOptions options;
+  options.context = node_ptr->get_context();
+  rclcpp::executors::SingleThreadedExecutor exec(options);
   exec.spin_node_all(node_ptr, max_duration);
 }
 
@@ -32,7 +34,9 @@ rclcpp::spin_all(rclcpp::Node::SharedPtr node_ptr, std::chrono::nanoseconds max_
 void
 rclcpp::spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
 {
-  rclcpp::executors::SingleThreadedExecutor exec;
+  rclcpp::ExecutorOptions options;
+  options.context = node_ptr->get_context();
+  rclcpp::executors::SingleThreadedExecutor exec(options);
   exec.spin_node_some(node_ptr);
 }
 
@@ -45,7 +49,9 @@ rclcpp::spin_some(rclcpp::Node::SharedPtr node_ptr)
 void
 rclcpp::spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
 {
-  rclcpp::executors::SingleThreadedExecutor exec;
+  rclcpp::ExecutorOptions options;
+  options.context = node_ptr->get_context();
+  rclcpp::executors::SingleThreadedExecutor exec(options);
   exec.add_node(node_ptr);
   exec.spin();
   exec.remove_node(node_ptr);

rclcpp/src/rclcpp/executors/executor_entities_collection.cpp

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

rclcpp/src/rclcpp/executors/executor_entities_collector.cpp

@@ -61,6 +61,12 @@ ExecutorEntitiesCollector::~ExecutorEntitiesCollector()
     if (group_ptr) {
       group_ptr->get_associated_with_executor_atomic().store(false);
     }
+    // Disassociate the guard condition from the executor notify waitable
+    auto guard_condition_it = weak_groups_to_guard_conditions_.find(weak_group_ptr);
+    if (guard_condition_it != weak_groups_to_guard_conditions_.end()) {
+      this->notify_waitable_->remove_guard_condition(guard_condition_it->second);
+      weak_groups_to_guard_conditions_.erase(guard_condition_it);
+    }
   }
   pending_manually_added_groups_.clear();
   pending_manually_removed_groups_.clear();
@@ -105,7 +111,8 @@ void
 ExecutorEntitiesCollector::remove_node(
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
 {
-  if (!node_ptr->get_associated_with_executor_atomic().load()) {
+  std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
+  if (!has_executor.exchange(false)) {
     throw std::runtime_error(
             std::string("Node '") + node_ptr->get_fully_qualified_name() +
             "' needs to be associated with an executor.");
@@ -143,6 +150,11 @@ ExecutorEntitiesCollector::add_callback_group(rclcpp::CallbackGroup::SharedPtr g
   }
 
   this->pending_manually_added_groups_.insert(group_ptr);
+
+  // Store callback group notify guard condition in map and add it to the notify waitable
+  auto group_guard_condition = group_ptr->get_notify_guard_condition();
+  weak_groups_to_guard_conditions_.insert({group_ptr, group_guard_condition});
+  this->notify_waitable_->add_guard_condition(group_guard_condition);
 }
 
 void
@@ -161,7 +173,6 @@ ExecutorEntitiesCollector::remove_callback_group(rclcpp::CallbackGroup::SharedPt
     throw std::runtime_error("Node must not be deleted before its callback group(s).");
   }
   */
-
   auto weak_group_ptr = rclcpp::CallbackGroup::WeakPtr(group_ptr);
   std::lock_guard<std::mutex> lock(mutex_);
   bool associated = manually_added_groups_.count(group_ptr) != 0;
@@ -314,7 +325,11 @@ ExecutorEntitiesCollector::process_queues()
     if (node_it != weak_nodes_.end()) {
       remove_weak_node(node_it);
     } else {
-      throw std::runtime_error("Node needs to be associated with this executor.");
+      // The node may have been destroyed and removed from the colletion before
+      // we processed the queues.  Don't throw if the pointer is already expired.
+      if (!weak_node_ptr.expired()) {
+        throw std::runtime_error("Node needs to be associated with this executor.");
+      }
     }
 
     auto node_ptr = weak_node_ptr.lock();
@@ -337,6 +352,13 @@ ExecutorEntitiesCollector::process_queues()
     auto group_ptr = weak_group_ptr.lock();
     if (group_ptr) {
       this->add_callback_group_to_collection(group_ptr, manually_added_groups_);
+    } else {
+      // Disassociate the guard condition from the executor notify waitable
+      auto guard_condition_it = weak_groups_to_guard_conditions_.find(weak_group_ptr);
+      if (guard_condition_it != weak_groups_to_guard_conditions_.end()) {
+        this->notify_waitable_->remove_guard_condition(guard_condition_it->second);
+        weak_groups_to_guard_conditions_.erase(guard_condition_it);
+      }
     }
   }
   pending_manually_added_groups_.clear();

rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp

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

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -55,7 +55,7 @@ MultiThreadedExecutor::spin()
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin() called while already spinning");
   }
-  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false););
   std::vector<std::thread> threads;
   size_t thread_id = 0;
   {
@@ -99,6 +99,18 @@ MultiThreadedExecutor::run(size_t this_thread_number)
 
     execute_any_executable(any_exec);
 
+    if (any_exec.callback_group &&
+      any_exec.callback_group->type() == CallbackGroupType::MutuallyExclusive)
+    {
+      try {
+        interrupt_guard_condition_->trigger();
+      } catch (const rclcpp::exceptions::RCLError & ex) {
+        throw std::runtime_error(
+                std::string(
+                  "Failed to trigger guard condition on callback group change: ") + ex.what());
+      }
+    }
+
     // Clear the callback_group to prevent the AnyExecutable destructor from
     // resetting the callback group `can_be_taken_from`
     any_exec.callback_group.reset();

rclcpp/src/rclcpp/executors/single_threaded_executor.cpp

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

rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp

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

rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp

@@ -12,31 +12,20 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "rclcpp/executors/static_single_threaded_executor.hpp"
-
-#include <chrono>
-#include <memory>
-#include <utility>
-#include <vector>
-
+#include "rclcpp/executors/executor_entities_collection.hpp"
 #include "rcpputils/scope_exit.hpp"
 
-using rclcpp::executors::StaticSingleThreadedExecutor;
-using rclcpp::experimental::ExecutableList;
+#include "rclcpp/executors/static_single_threaded_executor.hpp"
+#include "rclcpp/any_executable.hpp"
 
-StaticSingleThreadedExecutor::StaticSingleThreadedExecutor(
-  const rclcpp::ExecutorOptions & options)
+using rclcpp::executors::StaticSingleThreadedExecutor;
+
+StaticSingleThreadedExecutor::StaticSingleThreadedExecutor(const rclcpp::ExecutorOptions & options)
 : rclcpp::Executor(options)
 {
-  entities_collector_ = std::make_shared<StaticExecutorEntitiesCollector>();
 }
 
-StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor()
-{
-  if (entities_collector_->is_init()) {
-    entities_collector_->fini();
-  }
-}
+StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor() {}
 
 void
 StaticSingleThreadedExecutor::spin()
@@ -46,14 +35,11 @@ StaticSingleThreadedExecutor::spin()
   }
   RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
 
-  // Set memory_strategy_ and exec_list_ based on weak_nodes_
-  // Prepare wait_set_ based on memory_strategy_
-  entities_collector_->init(&wait_set_, memory_strategy_);
-
+  // This is essentially the contents of the rclcpp::Executor::wait_for_work method,
+  // except we need to keep the wait result to reproduce the StaticSingleThreadedExecutor
+  // behavior.
   while (rclcpp::ok(this->context_) && spinning.load()) {
-    // Refresh wait set and wait for work
-    entities_collector_->refresh_wait_set();
-    execute_ready_executables();
+    this->spin_once_impl(std::chrono::nanoseconds(-1));
   }
 }
 
@@ -64,7 +50,6 @@ StaticSingleThreadedExecutor::spin_some(std::chrono::nanoseconds max_duration)
   if (std::chrono::nanoseconds(0) == max_duration) {
     max_duration = std::chrono::nanoseconds::max();
   }
-
   return this->spin_some_impl(max_duration, false);
 }
 
@@ -80,36 +65,32 @@ StaticSingleThreadedExecutor::spin_all(std::chrono::nanoseconds max_duration)
 void
 StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive)
 {
-  // Make sure the entities collector has been initialized
-  if (!entities_collector_->is_init()) {
-    entities_collector_->init(&wait_set_, memory_strategy_);
-  }
-
   auto start = std::chrono::steady_clock::now();
   auto max_duration_not_elapsed = [max_duration, start]() {
-      if (std::chrono::nanoseconds(0) == max_duration) {
-        // told to spin forever if need be
-        return true;
-      } else if (std::chrono::steady_clock::now() - start < max_duration) {
-        // told to spin only for some maximum amount of time
-        return true;
-      }
-      // spun too long
-      return false;
+      const auto spin_forever = std::chrono::nanoseconds(0) == max_duration;
+      const auto cur_duration = std::chrono::steady_clock::now() - start;
+      return spin_forever || (cur_duration < max_duration);
     };
 
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin_some() called while already spinning");
   }
-  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false););
 
   while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) {
     // Get executables that are ready now
-    entities_collector_->refresh_wait_set(std::chrono::milliseconds::zero());
-    // Execute ready executables
-    bool work_available = execute_ready_executables();
-    if (!work_available || !exhaustive) {
-      break;
+    std::lock_guard<std::mutex> guard(mutex_);
+
+    auto wait_result = this->collect_and_wait(std::chrono::nanoseconds(0));
+    if (wait_result.has_value()) {
+      // Execute ready executables
+      bool work_available = this->execute_ready_executables(
+        current_collection_,
+        wait_result.value(),
+        false);
+      if (!work_available || !exhaustive) {
+        break;
+      }
     }
   }
 }
@@ -117,163 +98,106 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati
 void
 StaticSingleThreadedExecutor::spin_once_impl(std::chrono::nanoseconds timeout)
 {
-  // Make sure the entities collector has been initialized
-  if (!entities_collector_->is_init()) {
-    entities_collector_->init(&wait_set_, memory_strategy_);
-  }
-
   if (rclcpp::ok(context_) && spinning.load()) {
-    // Wait until we have a ready entity or timeout expired
-    entities_collector_->refresh_wait_set(timeout);
-    // Execute ready executables
-    execute_ready_executables(true);
+    std::lock_guard<std::mutex> guard(mutex_);
+    auto wait_result = this->collect_and_wait(timeout);
+    if (wait_result.has_value()) {
+      this->execute_ready_executables(current_collection_, wait_result.value(), true);
+    }
   }
 }
 
-void
-StaticSingleThreadedExecutor::add_callback_group(
-  rclcpp::CallbackGroup::SharedPtr group_ptr,
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  bool notify)
+std::optional<rclcpp::WaitResult<rclcpp::WaitSet>>
+StaticSingleThreadedExecutor::collect_and_wait(std::chrono::nanoseconds timeout)
 {
-  bool is_new_node = entities_collector_->add_callback_group(group_ptr, node_ptr);
-  if (is_new_node && notify) {
-    // Interrupt waiting to handle new node
-    interrupt_guard_condition_->trigger();
+  if (this->entities_need_rebuild_.exchange(false) || current_collection_.empty()) {
+    this->collect_entities();
   }
-}
-
-void
-StaticSingleThreadedExecutor::add_node(
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
-{
-  bool is_new_node = entities_collector_->add_node(node_ptr);
-  if (is_new_node && notify) {
-    // Interrupt waiting to handle new node
-    interrupt_guard_condition_->trigger();
+  auto wait_result = wait_set_.wait(std::chrono::nanoseconds(timeout));
+  if (wait_result.kind() == WaitResultKind::Empty) {
+    RCUTILS_LOG_WARN_NAMED(
+      "rclcpp",
+      "empty wait set received in wait(). This should never happen.");
+    return {};
+  } else {
+    if (wait_result.kind() == WaitResultKind::Ready && current_notify_waitable_) {
+      auto & rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set();
+      if (current_notify_waitable_->is_ready(rcl_wait_set)) {
+        current_notify_waitable_->execute(current_notify_waitable_->take_data());
+      }
+    }
   }
+  return wait_result;
 }
 
-void
-StaticSingleThreadedExecutor::add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify)
-{
-  this->add_node(node_ptr->get_node_base_interface(), notify);
-}
-
-void
-StaticSingleThreadedExecutor::remove_callback_group(
-  rclcpp::CallbackGroup::SharedPtr group_ptr, bool notify)
-{
-  bool node_removed = entities_collector_->remove_callback_group(group_ptr);
-  // If the node was matched and removed, interrupt waiting
-  if (node_removed && notify) {
-    interrupt_guard_condition_->trigger();
-  }
-}
-
-void
-StaticSingleThreadedExecutor::remove_node(
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
-{
-  bool node_removed = entities_collector_->remove_node(node_ptr);
-  if (!node_removed) {
-    throw std::runtime_error("Node needs to be associated with this executor.");
-  }
-  // If the node was matched and removed, interrupt waiting
-  if (notify) {
-    interrupt_guard_condition_->trigger();
-  }
-}
-
-std::vector<rclcpp::CallbackGroup::WeakPtr>
-StaticSingleThreadedExecutor::get_all_callback_groups()
-{
-  return entities_collector_->get_all_callback_groups();
-}
-
-std::vector<rclcpp::CallbackGroup::WeakPtr>
-StaticSingleThreadedExecutor::get_manually_added_callback_groups()
-{
-  return entities_collector_->get_manually_added_callback_groups();
-}
-
-std::vector<rclcpp::CallbackGroup::WeakPtr>
-StaticSingleThreadedExecutor::get_automatically_added_callback_groups_from_nodes()
-{
-  return entities_collector_->get_automatically_added_callback_groups_from_nodes();
-}
-
-void
-StaticSingleThreadedExecutor::remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify)
-{
-  this->remove_node(node_ptr->get_node_base_interface(), notify);
-}
-
-bool
-StaticSingleThreadedExecutor::execute_ready_executables(bool spin_once)
+// This preserves the "scheduling semantics" of the StaticSingleThreadedExecutor
+// from the original implementation.
+bool StaticSingleThreadedExecutor::execute_ready_executables(
+  const rclcpp::executors::ExecutorEntitiesCollection & collection,
+  rclcpp::WaitResult<rclcpp::WaitSet> & wait_result,
+  bool spin_once)
 {
   bool any_ready_executable = false;
+  if (wait_result.kind() != rclcpp::WaitResultKind::Ready) {
+    return any_ready_executable;
+  }
 
-  // Execute all the ready subscriptions
-  for (size_t i = 0; i < wait_set_.size_of_subscriptions; ++i) {
-    if (i < entities_collector_->get_number_of_subscriptions()) {
-      if (wait_set_.subscriptions[i]) {
-        execute_subscription(entities_collector_->get_subscription(i));
-        if (spin_once) {
-          return true;
-        }
-        any_ready_executable = true;
-      }
-    }
-  }
-  // Execute all the ready timers
-  for (size_t i = 0; i < wait_set_.size_of_timers; ++i) {
-    if (i < entities_collector_->get_number_of_timers()) {
-      if (wait_set_.timers[i] && entities_collector_->get_timer(i)->is_ready()) {
-        auto timer = entities_collector_->get_timer(i);
-        timer->call();
-        execute_timer(std::move(timer));
-        if (spin_once) {
-          return true;
-        }
-        any_ready_executable = true;
-      }
-    }
-  }
-  // Execute all the ready services
-  for (size_t i = 0; i < wait_set_.size_of_services; ++i) {
-    if (i < entities_collector_->get_number_of_services()) {
-      if (wait_set_.services[i]) {
-        execute_service(entities_collector_->get_service(i));
-        if (spin_once) {
-          return true;
-        }
-        any_ready_executable = true;
-      }
-    }
-  }
-  // Execute all the ready clients
-  for (size_t i = 0; i < wait_set_.size_of_clients; ++i) {
-    if (i < entities_collector_->get_number_of_clients()) {
-      if (wait_set_.clients[i]) {
-        execute_client(entities_collector_->get_client(i));
-        if (spin_once) {
-          return true;
-        }
-        any_ready_executable = true;
-      }
-    }
-  }
-  // Execute all the ready waitables
-  for (size_t i = 0; i < entities_collector_->get_number_of_waitables(); ++i) {
-    auto waitable = entities_collector_->get_waitable(i);
-    if (waitable->is_ready(&wait_set_)) {
-      auto data = waitable->take_data();
-      waitable->execute(data);
-      if (spin_once) {
-        return true;
-      }
+  while (auto subscription = wait_result.next_ready_subscription()) {
+    auto entity_iter = collection.subscriptions.find(subscription->get_subscription_handle().get());
+    if (entity_iter != collection.subscriptions.end()) {
+      execute_subscription(subscription);
       any_ready_executable = true;
+      if (spin_once) {return any_ready_executable;}
+    }
+  }
+
+  size_t current_timer_index = 0;
+  while (true) {
+    auto [timer, timer_index] = wait_result.peek_next_ready_timer(current_timer_index);
+    if (nullptr == timer) {
+      break;
+    }
+    current_timer_index = timer_index;
+    auto entity_iter = collection.timers.find(timer->get_timer_handle().get());
+    if (entity_iter != collection.timers.end()) {
+      wait_result.clear_timer_with_index(current_timer_index);
+      auto data = timer->call();
+      if (!data) {
+        // someone canceled the timer between is_ready and call
+        continue;
+      }
+
+      execute_timer(std::move(timer), data);
+      any_ready_executable = true;
+      if (spin_once) {return any_ready_executable;}
+    }
+  }
+
+  while (auto client = wait_result.next_ready_client()) {
+    auto entity_iter = collection.clients.find(client->get_client_handle().get());
+    if (entity_iter != collection.clients.end()) {
+      execute_client(client);
+      any_ready_executable = true;
+      if (spin_once) {return any_ready_executable;}
+    }
+  }
+
+  while (auto service = wait_result.next_ready_service()) {
+    auto entity_iter = collection.services.find(service->get_service_handle().get());
+    if (entity_iter != collection.services.end()) {
+      execute_service(service);
+      any_ready_executable = true;
+      if (spin_once) {return any_ready_executable;}
+    }
+  }
+
+  while (auto waitable = wait_result.next_ready_waitable()) {
+    auto entity_iter = collection.waitables.find(waitable.get());
+    if (entity_iter != collection.waitables.end()) {
+      const auto data = waitable->take_data();
+      waitable->execute(data);
+      any_ready_executable = true;
+      if (spin_once) {return any_ready_executable;}
     }
   }
   return any_ready_executable;

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

@@ -40,10 +40,12 @@ EventsExecutor::EventsExecutor(
   // The timers manager can be used either to only track timers (in this case an expired
   // timer will generate an executor event and then it will be executed by the executor thread)
   // or it can also take care of executing expired timers in its dedicated thread.
-  std::function<void(const rclcpp::TimerBase *)> timer_on_ready_cb = nullptr;
+  std::function<void(const rclcpp::TimerBase *,
+    const std::shared_ptr<void> &)> timer_on_ready_cb = nullptr;
   if (!execute_timers_separate_thread) {
-    timer_on_ready_cb = [this](const rclcpp::TimerBase * timer_id) {
-        ExecutorEvent event = {timer_id, -1, ExecutorEventType::TIMER_EVENT, 1};
+    timer_on_ready_cb =
+      [this](const rclcpp::TimerBase * timer_id, const std::shared_ptr<void> & data) {
+        ExecutorEvent event = {timer_id, data, -1, ExecutorEventType::TIMER_EVENT, 1};
         this->events_queue_->enqueue(event);
       };
   }
@@ -88,7 +90,7 @@ EventsExecutor::EventsExecutor(
       }
 
       ExecutorEvent event =
-      {notify_waitable_entity_id, waitable_data, ExecutorEventType::WAITABLE_EVENT, 1};
+      {notify_waitable_entity_id, nullptr, waitable_data, ExecutorEventType::WAITABLE_EVENT, 1};
       this->events_queue_->enqueue(event);
     });
 
@@ -325,7 +327,7 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
     case ExecutorEventType::TIMER_EVENT:
       {
         timers_manager_->execute_ready_timer(
-          static_cast<const rclcpp::TimerBase *>(event.entity_key));
+          static_cast<const rclcpp::TimerBase *>(event.entity_key), event.data);
         break;
       }
     case ExecutorEventType::WAITABLE_EVENT:
@@ -339,7 +341,7 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
         }
         if (waitable) {
           for (size_t i = 0; i < event.num_events; i++) {
-            auto data = waitable->take_data_by_entity_id(event.waitable_data);
+            const auto data = waitable->take_data_by_entity_id(event.waitable_data);
             waitable->execute(data);
           }
         }
@@ -485,7 +487,7 @@ EventsExecutor::create_entity_callback(
 {
   std::function<void(size_t)>
   callback = [this, entity_key, event_type](size_t num_events) {
-      ExecutorEvent event = {entity_key, -1, event_type, num_events};
+      ExecutorEvent event = {entity_key, nullptr, -1, event_type, num_events};
       this->events_queue_->enqueue(event);
     };
   return callback;
@@ -497,7 +499,7 @@ EventsExecutor::create_waitable_callback(const rclcpp::Waitable * entity_key)
   std::function<void(size_t, int)>
   callback = [this, entity_key](size_t num_events, int waitable_data) {
       ExecutorEvent event =
-      {entity_key, waitable_data, ExecutorEventType::WAITABLE_EVENT, num_events};
+      {entity_key, nullptr, waitable_data, ExecutorEventType::WAITABLE_EVENT, num_events};
       this->events_queue_->enqueue(event);
     };
   return callback;

rclcpp/src/rclcpp/experimental/timers_manager.cpp

@@ -27,7 +27,7 @@ using rclcpp::experimental::TimersManager;
 
 TimersManager::TimersManager(
   std::shared_ptr<rclcpp::Context> context,
-  std::function<void(const rclcpp::TimerBase *)> on_ready_callback)
+  std::function<void(const rclcpp::TimerBase *, const std::shared_ptr<void> &)> on_ready_callback)
 : on_ready_callback_(on_ready_callback),
   context_(context)
 {
@@ -148,8 +148,12 @@ bool TimersManager::execute_head_timer()
   if (timer_ready) {
     // NOTE: here we always execute the timer, regardless of whether the
     // on_ready_callback is set or not.
-    head_timer->call();
-    head_timer->execute_callback();
+    auto data = head_timer->call();
+    if (!data) {
+      // someone canceled the timer between is_ready and call
+      return false;
+    }
+    head_timer->execute_callback(data);
     timers_heap.heapify_root();
     weak_timers_heap_.store(timers_heap);
   }
@@ -157,7 +161,9 @@ bool TimersManager::execute_head_timer()
   return timer_ready;
 }
 
-void TimersManager::execute_ready_timer(const rclcpp::TimerBase * timer_id)
+void TimersManager::execute_ready_timer(
+  const rclcpp::TimerBase * timer_id,
+  const std::shared_ptr<void> & data)
 {
   TimerPtr ready_timer;
   {
@@ -165,7 +171,7 @@ void TimersManager::execute_ready_timer(const rclcpp::TimerBase * timer_id)
     ready_timer = weak_timers_heap_.get_timer(timer_id);
   }
   if (ready_timer) {
-    ready_timer->execute_callback();
+    ready_timer->execute_callback(data);
   }
 }
 
@@ -215,11 +221,16 @@ void TimersManager::execute_ready_timers_unsafe()
   const size_t number_ready_timers = locked_heap.get_number_ready_timers();
   size_t executed_timers = 0;
   while (executed_timers < number_ready_timers && head_timer->is_ready()) {
-    head_timer->call();
-    if (on_ready_callback_) {
-      on_ready_callback_(head_timer.get());
+    auto data = head_timer->call();
+    if (data) {
+      if (on_ready_callback_) {
+        on_ready_callback_(head_timer.get(), data);
+      } else {
+        head_timer->execute_callback(data);
+      }
     } else {
-      head_timer->execute_callback();
+      // someone canceled the timer between is_ready and call
+      // we don't do anything, as the timer is now 'processed'
     }
 
     executed_timers++;

rclcpp/src/rclcpp/generic_publisher.cpp

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

rclcpp/src/rclcpp/generic_subscription.cpp

@@ -51,7 +51,7 @@ GenericSubscription::handle_serialized_message(
   const std::shared_ptr<rclcpp::SerializedMessage> & message,
   const rclcpp::MessageInfo & message_info)
 {
-  callback_(message, message_info);
+  any_callback_.dispatch(message, message_info);
 }
 
 void

rclcpp/src/rclcpp/guard_condition.cpp

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

rclcpp/src/rclcpp/subscription_base.cpp

@@ -244,6 +244,9 @@ SubscriptionBase::take_serialized(
     &message_out.get_rcl_serialized_message(),
     &message_info_out.get_rmw_message_info(),
     nullptr);
+  TRACETOOLS_TRACEPOINT(
+    rclcpp_take,
+    static_cast<const void *>(&message_out.get_rcl_serialized_message()));
   if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) {
     return false;
   } else if (RCL_RET_OK != ret) {

rclcpp/src/rclcpp/subscription_intra_process_base.cpp

@@ -18,9 +18,9 @@
 using rclcpp::experimental::SubscriptionIntraProcessBase;
 
 void
-SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t * wait_set)
+SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t & wait_set)
 {
-  detail::add_guard_condition_to_rcl_wait_set(*wait_set, gc_);
+  detail::add_guard_condition_to_rcl_wait_set(wait_set, gc_);
 }
 
 const char *

rclcpp/src/rclcpp/time.cpp

@@ -60,6 +60,10 @@ Time::Time(int32_t seconds, uint32_t nanoseconds, rcl_clock_type_t clock_type)
 Time::Time(int64_t nanoseconds, rcl_clock_type_t clock_type)
 : rcl_time_(init_time_point(clock_type))
 {
+  if (nanoseconds < 0) {
+    throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
+  }
+
   rcl_time_.nanoseconds = nanoseconds;
 }
 
@@ -249,6 +253,9 @@ Time::operator+=(const rclcpp::Duration & rhs)
   }
 
   rcl_time_.nanoseconds += rhs.nanoseconds();
+  if (rcl_time_.nanoseconds < 0) {
+    throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
+  }
 
   return *this;
 }
@@ -264,6 +271,9 @@ Time::operator-=(const rclcpp::Duration & rhs)
   }
 
   rcl_time_.nanoseconds -= rhs.nanoseconds();
+  if (rcl_time_.nanoseconds < 0) {
+    throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
+  }
 
   return *this;
 }

rclcpp/src/rclcpp/time_source.cpp

@@ -288,10 +288,10 @@ public:
   // Detach the attached node
   void detachNode()
   {
-    std::lock_guard<std::mutex> guard(node_base_lock_);
     // destroy_clock_sub() *must* be first here, to ensure that the executor
     // can't possibly call any of the callbacks as we are cleaning up.
     destroy_clock_sub();
+    std::lock_guard<std::mutex> guard(node_base_lock_);
     clocks_state_.disable_ros_time();
     if (on_set_parameters_callback_) {
       node_parameters_->remove_on_set_parameters_callback(on_set_parameters_callback_.get());
@@ -409,9 +409,14 @@ private:
       "/clock",
       qos_,
       [this](std::shared_ptr<const rosgraph_msgs::msg::Clock> msg) {
-        // We are using node_base_ as an indication if there is a node attached.
-        // Only call the clock_cb if that is the case.
-        if (node_base_ != nullptr) {
+        bool execute_cb = false;
+        {
+          std::lock_guard<std::mutex> guard(node_base_lock_);
+          // We are using node_base_ as an indication if there is a node attached.
+          // Only call the clock_cb if that is the case.
+          execute_cb = node_base_ != nullptr;
+        }
+        if (execute_cb) {
           clock_cb(msg);
         }
       },

rclcpp/src/rclcpp/waitable.cpp

@@ -86,3 +86,82 @@ Waitable::clear_on_ready_callback()
           "Custom waitables should override clear_on_ready_callback if they "
           "want to use it and make sure to call it on the waitable destructor.");
 }
+
+void
+Waitable::add_to_wait_set(rcl_wait_set_t * wait_set)
+{
+  this->add_to_wait_set(*wait_set);
+}
+
+void
+Waitable::add_to_wait_set(rcl_wait_set_t & wait_set)
+{
+#if !defined(_WIN32)
+# pragma GCC diagnostic push
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#else  // !defined(_WIN32)
+# pragma warning(push)
+# pragma warning(disable: 4996)
+#endif
+  this->add_to_wait_set(&wait_set);
+// remove warning suppression
+#if !defined(_WIN32)
+# pragma GCC diagnostic pop
+#else  // !defined(_WIN32)
+# pragma warning(pop)
+#endif
+}
+
+bool
+Waitable::is_ready(rcl_wait_set_t * wait_set)
+{
+  const rcl_wait_set_t & const_wait_set_ref = *wait_set;
+  return this->is_ready(const_wait_set_ref);
+}
+
+bool
+Waitable::is_ready(const rcl_wait_set_t & wait_set)
+{
+#if !defined(_WIN32)
+# pragma GCC diagnostic push
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#else  // !defined(_WIN32)
+# pragma warning(push)
+# pragma warning(disable: 4996)
+#endif
+  // note this const cast is only required to support a deprecated function
+  return this->is_ready(&const_cast<rcl_wait_set_t &>(wait_set));
+// remove warning suppression
+#if !defined(_WIN32)
+# pragma GCC diagnostic pop
+#else  // !defined(_WIN32)
+# pragma warning(pop)
+#endif
+}
+
+void
+Waitable::execute(std::shared_ptr<void> & data)
+{
+  const std::shared_ptr<void> & const_data = data;
+  this->execute(const_data);
+}
+
+void
+Waitable::execute(const std::shared_ptr<void> & data)
+{
+#if !defined(_WIN32)
+# pragma GCC diagnostic push
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#else  // !defined(_WIN32)
+# pragma warning(push)
+# pragma warning(disable: 4996)
+#endif
+  // note this const cast is only required to support a deprecated function
+  this->execute(const_cast<std::shared_ptr<void> &>(data));
+// remove warning suppression
+#if !defined(_WIN32)
+# pragma GCC diagnostic pop
+#else  // !defined(_WIN32)
+# pragma warning(pop)
+#endif
+}

rclcpp/test/benchmark/benchmark_executor.cpp

@@ -362,42 +362,3 @@ BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_future_complete)(benchmark
     }
   }
 }
-
-BENCHMARK_F(
-  PerformanceTestExecutorSimple,
-  static_executor_entities_collector_execute)(benchmark::State & st)
-{
-  rclcpp::executors::StaticExecutorEntitiesCollector::SharedPtr entities_collector_ =
-    std::make_shared<rclcpp::executors::StaticExecutorEntitiesCollector>();
-  entities_collector_->add_node(node->get_node_base_interface());
-
-  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  rcl_allocator_t allocator = rcl_get_default_allocator();
-  auto shared_context = node->get_node_base_interface()->get_context();
-  rcl_context_t * context = shared_context->get_rcl_context().get();
-  rcl_ret_t ret = rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator);
-  if (ret != RCL_RET_OK) {
-    st.SkipWithError(rcutils_get_error_string().str);
-  }
-  RCPPUTILS_SCOPE_EXIT(
-  {
-    rcl_ret_t ret = rcl_wait_set_fini(&wait_set);
-    if (ret != RCL_RET_OK) {
-      st.SkipWithError(rcutils_get_error_string().str);
-    }
-  });
-
-  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
-  rclcpp::GuardCondition guard_condition(shared_context);
-
-  entities_collector_->init(&wait_set, memory_strategy);
-  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
-
-  reset_heap_counters();
-
-  for (auto _ : st) {
-    (void)_;
-    std::shared_ptr<void> data = entities_collector_->take_data();
-    entities_collector_->execute(data);
-  }
-}

rclcpp/test/rclcpp/CMakeLists.txt

@@ -31,6 +31,7 @@ endif()
 ament_add_gtest(
   test_exceptions
   exceptions/test_exceptions.cpp)
+ament_add_test_label(test_exceptions mimick)
 if(TARGET test_exceptions)
   target_link_libraries(test_exceptions ${PROJECT_NAME} mimick)
 endif()
@@ -52,9 +53,15 @@ if(TARGET test_any_subscription_callback)
   target_link_libraries(test_any_subscription_callback ${PROJECT_NAME} ${test_msgs_TARGETS})
 endif()
 ament_add_gtest(test_client test_client.cpp)
+ament_add_test_label(test_client mimick)
 if(TARGET test_client)
   target_link_libraries(test_client ${PROJECT_NAME} mimick ${rcl_interfaces_TARGETS} ${test_msgs_TARGETS})
 endif()
+ament_add_gtest(test_clock test_clock.cpp)
+ament_add_test_label(test_clock mimick)
+if(TARGET test_clock)
+  target_link_libraries(test_clock ${PROJECT_NAME} mimick ${rcl_interfaces_TARGETS} ${test_msgs_TARGETS})
+endif()
 ament_add_gtest(test_copy_all_parameter_values test_copy_all_parameter_values.cpp)
 if(TARGET test_copy_all_parameter_values)
   target_link_libraries(test_copy_all_parameter_values ${PROJECT_NAME})
@@ -65,6 +72,7 @@ if(TARGET test_create_timer)
   target_include_directories(test_create_timer PRIVATE ./)
 endif()
 ament_add_gtest(test_generic_client test_generic_client.cpp)
+ament_add_test_label(test_generic_client mimick)
 if(TARGET test_generic_client)
   target_link_libraries(test_generic_client ${PROJECT_NAME}
     mimick
@@ -76,6 +84,7 @@ if(TARGET test_generic_client)
   )
 endif()
 ament_add_gtest(test_client_common test_client_common.cpp)
+ament_add_test_label(test_client_common mimick)
 if(TARGET test_client_common)
   target_link_libraries(test_client_common ${PROJECT_NAME}
     mimick
@@ -102,6 +111,7 @@ function(test_add_callback_groups_to_executor_for_rmw_implementation)
 endfunction()
 call_for_each_rmw_implementation(test_add_callback_groups_to_executor_for_rmw_implementation)
 ament_add_gtest(test_expand_topic_or_service_name test_expand_topic_or_service_name.cpp)
+ament_add_test_label(test_expand_topic_or_service_name mimick)
 if(TARGET test_expand_topic_or_service_name)
   target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME} mimick rcl::rcl rmw::rmw)
 endif()
@@ -133,6 +143,7 @@ if(TARGET test_intra_process_buffer)
 endif()
 
 ament_add_gtest(test_loaned_message test_loaned_message.cpp)
+ament_add_test_label(test_loaned_message mimick)
 target_link_libraries(test_loaned_message ${PROJECT_NAME} mimick ${test_msgs_TARGETS})
 
 ament_add_gtest(test_memory_strategy test_memory_strategy.cpp)
@@ -142,6 +153,7 @@ ament_add_gtest(test_message_memory_strategy test_message_memory_strategy.cpp)
 target_link_libraries(test_message_memory_strategy ${PROJECT_NAME} ${test_msgs_TARGETS})
 
 ament_add_gtest(test_node test_node.cpp TIMEOUT 240)
+ament_add_test_label(test_node mimick)
 if(TARGET test_node)
   target_link_libraries(test_node ${PROJECT_NAME} mimick rcpputils::rcpputils rmw::rmw ${test_msgs_TARGETS})
 endif()
@@ -153,6 +165,7 @@ if(TARGET test_node_interfaces__get_node_interfaces)
 endif()
 ament_add_gtest(test_node_interfaces__node_base
   node_interfaces/test_node_base.cpp)
+ament_add_test_label(test_node_interfaces__node_base mimick)
 if(TARGET test_node_interfaces__node_base)
   target_link_libraries(test_node_interfaces__node_base ${PROJECT_NAME} mimick rcl::rcl rmw::rmw)
 endif()
@@ -164,6 +177,7 @@ endif()
 ament_add_gtest(test_node_interfaces__node_graph
   node_interfaces/test_node_graph.cpp
   TIMEOUT 120)
+ament_add_test_label(test_node_interfaces__node_graph mimick)
 if(TARGET test_node_interfaces__node_graph)
   target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME} mimick rcl::rcl ${test_msgs_TARGETS})
 endif()
@@ -174,21 +188,25 @@ if(TARGET test_node_interfaces__node_interfaces)
 endif()
 ament_add_gtest(test_node_interfaces__node_parameters
   node_interfaces/test_node_parameters.cpp)
+ament_add_test_label(test_node_interfaces__node_parameters mimick)
 if(TARGET test_node_interfaces__node_parameters)
   target_link_libraries(test_node_interfaces__node_parameters ${PROJECT_NAME} mimick rcpputils::rcpputils)
 endif()
 ament_add_gtest(test_node_interfaces__node_services
   node_interfaces/test_node_services.cpp)
+ament_add_test_label(test_node_interfaces__node_services mimick)
 if(TARGET test_node_interfaces__node_services)
   target_link_libraries(test_node_interfaces__node_services ${PROJECT_NAME} mimick rcl::rcl)
 endif()
 ament_add_gtest(test_node_interfaces__node_timers
   node_interfaces/test_node_timers.cpp)
+ament_add_test_label(test_node_interfaces__node_timers mimick)
 if(TARGET test_node_interfaces__node_timers)
   target_link_libraries(test_node_interfaces__node_timers ${PROJECT_NAME} mimick rcl::rcl)
 endif()
 ament_add_gtest(test_node_interfaces__node_topics
   node_interfaces/test_node_topics.cpp)
+ament_add_test_label(test_node_interfaces__node_topics mimick)
 if(TARGET test_node_interfaces__node_topics)
   target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME} mimick rcl::rcl ${test_msgs_TARGETS})
 endif()
@@ -199,6 +217,7 @@ if(TARGET test_node_interfaces__node_type_descriptions)
 endif()
 ament_add_gtest(test_node_interfaces__node_waitables
   node_interfaces/test_node_waitables.cpp)
+ament_add_test_label(test_node_interfaces__node_waitables mimick)
 if(TARGET test_node_interfaces__node_waitables)
   target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME} mimick rcl::rcl)
 endif()
@@ -234,10 +253,12 @@ if(TARGET test_node_global_args)
   target_link_libraries(test_node_global_args ${PROJECT_NAME})
 endif()
 ament_add_gtest(test_node_options test_node_options.cpp)
+ament_add_test_label(test_node_options mimick)
 if(TARGET test_node_options)
   target_link_libraries(test_node_options ${PROJECT_NAME} mimick rcl::rcl)
 endif()
 ament_add_gtest(test_init_options test_init_options.cpp)
+ament_add_test_label(test_init_options mimick)
 if(TARGET test_init_options)
   target_link_libraries(test_init_options ${PROJECT_NAME} mimick rcl::rcl)
 endif()
@@ -266,6 +287,7 @@ if(TARGET test_parameter_map)
   target_link_libraries(test_parameter_map ${PROJECT_NAME} rcl::rcl rcl_yaml_param_parser::rcl_yaml_param_parser rcutils::rcutils)
 endif()
 ament_add_gtest(test_publisher test_publisher.cpp TIMEOUT 120)
+ament_add_test_label(test_publisher mimick)
 if(TARGET test_publisher)
   target_link_libraries(test_publisher ${PROJECT_NAME} mimick rcl::rcl rcutils::rcutils ${test_msgs_TARGETS})
 endif()
@@ -331,6 +353,7 @@ function(test_qos_event_for_rmw_implementation)
   ament_add_gmock(test_qos_event${target_suffix} test_qos_event.cpp
     ENV ${rmw_implementation_env_var}
   )
+  ament_add_test_label(test_qos_event${target_suffix} mimick)
   if(TARGET test_qos_event${target_suffix})
     target_link_libraries(test_qos_event${target_suffix} ${PROJECT_NAME} mimick rcutils::rcutils rmw::rmw ${test_msgs_TARGETS})
   endif()
@@ -358,15 +381,18 @@ if(TARGET test_serialized_message)
   target_link_libraries(test_serialized_message ${PROJECT_NAME} rcpputils::rcpputils ${test_msgs_TARGETS})
 endif()
 ament_add_gtest(test_service test_service.cpp)
+ament_add_test_label(test_service mimick)
 if(TARGET test_service)
   target_link_libraries(test_service ${PROJECT_NAME} mimick ${rcl_interfaces_TARGES} ${test_msgs_TARGETS})
 endif()
 ament_add_gmock(test_service_introspection test_service_introspection.cpp)
+ament_add_test_label(test_service_introspection mimick)
 if(TARGET test_service_introspection)
   target_link_libraries(test_service_introspection ${PROJECT_NAME} mimick ${service_msgs_TARGETS} ${test_msgs_TARGETS})
 endif()
 # Creating and destroying nodes is slow with Connext, so this needs larger timeout.
 ament_add_gtest(test_subscription test_subscription.cpp TIMEOUT 120)
+ament_add_test_label(test_subscription mimick)
 if(TARGET test_subscription)
   target_link_libraries(test_subscription ${PROJECT_NAME} mimick ${test_msgs_TARGETS})
 endif()
@@ -417,6 +443,7 @@ endif()
 
 ament_add_gtest(test_timer test_timer.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}")
+ament_add_test_label(test_timer mimick)
 if(TARGET test_timer)
   target_link_libraries(test_timer ${PROJECT_NAME} mimick rcl::rcl)
 endif()
@@ -435,6 +462,7 @@ endif()
 
 ament_add_gtest(test_utilities test_utilities.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}")
+ament_add_test_label(test_utilities mimick)
 if(TARGET test_utilities)
   target_link_libraries(test_utilities ${PROJECT_NAME} mimick rcl::rcl)
 endif()
@@ -473,6 +501,15 @@ if(TARGET test_executors)
   target_link_libraries(test_executors_timer_cancel_behavior ${PROJECT_NAME} ${rosgraph_msgs_TARGETS})
 endif()
 
+ament_add_gtest(
+  test_executors_callback_group_behavior
+  executors/test_executors_callback_group_behavior.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}"
+  TIMEOUT 180)
+if(TARGET test_executors)
+  target_link_libraries(test_executors_callback_group_behavior ${PROJECT_NAME})
+endif()
+
 ament_add_gtest(
   test_executors_intraprocess
   executors/test_executors_intraprocess.cpp
@@ -484,6 +521,7 @@ endif()
 
 ament_add_gtest(test_static_single_threaded_executor executors/test_static_single_threaded_executor.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}")
+ament_add_test_label(test_static_single_threaded_executor mimick)
 if(TARGET test_static_single_threaded_executor)
   target_link_libraries(test_static_single_threaded_executor ${PROJECT_NAME} mimick ${test_msgs_TARGETS})
 endif()
@@ -494,12 +532,6 @@ if(TARGET test_multi_threaded_executor)
   target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME})
 endif()
 
-ament_add_gtest(test_static_executor_entities_collector executors/test_static_executor_entities_collector.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120)
-if(TARGET test_static_executor_entities_collector)
-  target_link_libraries(test_static_executor_entities_collector ${PROJECT_NAME} mimick rcpputils::rcpputils ${test_msgs_TARGETS})
-endif()
-
 ament_add_gtest(test_entities_collector executors/test_entities_collector.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120)
 if(TARGET test_entities_collector)
@@ -508,6 +540,7 @@ endif()
 
 ament_add_gtest(test_executor_notify_waitable executors/test_executor_notify_waitable.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120)
+ament_add_test_label(test_executor_notify_waitable mimick)
 if(TARGET test_executor_notify_waitable)
   target_link_libraries(test_executor_notify_waitable ${PROJECT_NAME} mimick rcpputils::rcpputils)
 endif()
@@ -525,6 +558,7 @@ endif()
 
 ament_add_gtest(test_guard_condition test_guard_condition.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}")
+ament_add_test_label(test_guard_condition mimick)
 if(TARGET test_guard_condition)
   target_link_libraries(test_guard_condition ${PROJECT_NAME} mimick)
 endif()
@@ -558,6 +592,7 @@ if(TARGET test_dynamic_storage)
 endif()
 
 ament_add_gtest(test_storage_policy_common wait_set_policies/test_storage_policy_common.cpp)
+ament_add_test_label(test_storage_policy_common mimick)
 if(TARGET test_storage_policy_common)
   target_link_libraries(test_storage_policy_common ${PROJECT_NAME} mimick ${test_msgs_TARGETS})
 endif()
@@ -572,6 +607,11 @@ if(TARGET test_thread_safe_synchronization)
   target_link_libraries(test_thread_safe_synchronization ${PROJECT_NAME} ${test_msgs_TARGETS})
 endif()
 
+ament_add_gtest(test_intra_process_waitable waitables/test_intra_process_waitable.cpp)
+if(TARGET test_intra_process_waitable)
+  target_link_libraries(test_intra_process_waitable ${PROJECT_NAME} ${test_msgs_TARGETS})
+endif()
+
 ament_add_gtest(test_rosout_qos test_rosout_qos.cpp)
 if(TARGET test_rosout_qos)
   target_link_libraries(test_rosout_qos ${PROJECT_NAME} rcl::rcl rmw::rmw)
@@ -585,11 +625,13 @@ endif()
 ament_add_gtest(test_executor test_executor.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}"
   TIMEOUT 120)
+ament_add_test_label(test_executor mimick)
 if(TARGET test_executor)
   target_link_libraries(test_executor ${PROJECT_NAME} mimick)
 endif()
 
 ament_add_gtest(test_graph_listener test_graph_listener.cpp)
+ament_add_test_label(test_graph_listener mimick)
 if(TARGET test_graph_listener)
   target_link_libraries(test_graph_listener ${PROJECT_NAME} mimick)
 endif()
@@ -601,6 +643,7 @@ function(test_subscription_content_filter_for_rmw_implementation)
     ENV ${rmw_implementation_env_var}
     TIMEOUT 120
   )
+  ament_add_test_label(test_subscription_content_filter${target_suffix} mimick)
   if(TARGET test_subscription_content_filter${target_suffix})
     target_link_libraries(test_subscription_content_filter${target_suffix} ${PROJECT_NAME} mimick ${test_msgs_TARGETS})
   endif()

rclcpp/test/rclcpp/executors/test_events_queue.cpp

@@ -68,6 +68,7 @@ TEST(TestEventsQueue, SimpleQueueTest)
   // Lets push an event into the queue and get it back
   rclcpp::experimental::executors::ExecutorEvent push_event = {
     simple_queue.get(),
+    nullptr,
     99,
     rclcpp::experimental::executors::ExecutorEventType::SUBSCRIPTION_EVENT,
     1};

rclcpp/test/rclcpp/executors/test_executors.cpp

@@ -82,8 +82,6 @@ public:
   int callback_count;
 };
 
-// spin_all and spin_some are not implemented correctly in StaticSingleThreadedExecutor, see:
-// https://github.com/ros2/rclcpp/issues/1219 for tracking
 template<typename T>
 class TestExecutorsStable : public TestExecutors<T> {};
 
@@ -106,10 +104,7 @@ TYPED_TEST(TestExecutors, detachOnDestruction)
 }
 
 // Make sure that the executor can automatically remove expired nodes correctly
-// Currently fails for StaticSingleThreadedExecutor so it is being skipped, see:
-// https://github.com/ros2/rclcpp/issues/1231
-TYPED_TEST(TestExecutorsStable, addTemporaryNode)
-{
+TYPED_TEST(TestExecutors, addTemporaryNode) {
   using ExecutorType = TypeParam;
   ExecutorType executor;
 
@@ -177,16 +172,19 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning)
 {
   using ExecutorType = TypeParam;
   ExecutorType executor;
+
+  std::atomic_bool timer_completed = false;
+  auto timer = this->node->create_wall_timer(
+    1ms, [&]() {
+      timer_completed.store(true);
+    });
+
   executor.add_node(this->node);
-
-  bool timer_completed = false;
-  auto timer = this->node->create_wall_timer(1ms, [&]() {timer_completed = true;});
-
   std::thread spinner([&]() {executor.spin();});
-  // Sleep for a short time to verify executor.spin() is going, and didn't throw.
 
+  // Sleep for a short time to verify executor.spin() is going, and didn't throw.
   auto start = std::chrono::steady_clock::now();
-  while (!timer_completed && (std::chrono::steady_clock::now() - start) < 10s) {
+  while (!timer_completed.load() && (std::chrono::steady_clock::now() - start) < 10s) {
     std::this_thread::sleep_for(1ms);
   }
 
@@ -339,21 +337,29 @@ public:
   TestWaitable() = default;
 
   void
-  add_to_wait_set(rcl_wait_set_t * wait_set) override
+  add_to_wait_set(rcl_wait_set_t & wait_set) override
   {
-    rclcpp::detail::add_guard_condition_to_rcl_wait_set(*wait_set, gc_);
+    if (trigger_count_ > 0) {
+      // Keep the gc triggered until the trigger count is reduced back to zero.
+      // This is necessary if trigger() results in the wait set waking, but not
+      // executing this waitable, in which case it needs to be re-triggered.
+      gc_.trigger();
+    }
+    rclcpp::detail::add_guard_condition_to_rcl_wait_set(wait_set, gc_);
   }
 
   void trigger()
   {
+    trigger_count_++;
     gc_.trigger();
   }
 
   bool
-  is_ready(rcl_wait_set_t * wait_set) override
+  is_ready(const rcl_wait_set_t & wait_set) override
   {
-    for (size_t i = 0; i < wait_set->size_of_guard_conditions; ++i) {
-      auto rcl_guard_condition = wait_set->guard_conditions[i];
+    is_ready_count_++;
+    for (size_t i = 0; i < wait_set.size_of_guard_conditions; ++i) {
+      auto rcl_guard_condition = wait_set.guard_conditions[i];
       if (&gc_.get_rcl_guard_condition() == rcl_guard_condition) {
         return true;
       }
@@ -375,9 +381,9 @@ public:
   }
 
   void
-  execute(std::shared_ptr<void> & data) override
+  execute(const std::shared_ptr<void> &) override
   {
-    (void) data;
+    trigger_count_--;
     count_++;
     if (nullptr != on_execute_callback_) {
       on_execute_callback_();
@@ -414,13 +420,21 @@ public:
   get_number_of_ready_guard_conditions() override {return 1;}
 
   size_t
-  get_count()
+  get_count() const
   {
     return count_;
   }
 
+  size_t
+  get_is_ready_call_count() const
+  {
+    return is_ready_count_;
+  }
+
 private:
-  size_t count_ = 0;
+  std::atomic<size_t> trigger_count_ = 0;
+  std::atomic<size_t> is_ready_count_ = 0;
+  std::atomic<size_t> count_ = 0;
   rclcpp::GuardCondition gc_;
   std::function<void()> on_execute_callback_ = nullptr;
 };
@@ -833,3 +847,185 @@ TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr)
 
   rclcpp::shutdown();
 }
+
+// Check spin functions with non default context
+TEST(TestExecutors, testSpinWithNonDefaultContext)
+{
+  auto non_default_context = std::make_shared<rclcpp::Context>();
+  non_default_context->init(0, nullptr);
+
+  {
+    auto node =
+      std::make_unique<rclcpp::Node>("node", rclcpp::NodeOptions().context(non_default_context));
+
+    EXPECT_NO_THROW(rclcpp::spin_some(node->get_node_base_interface()));
+
+    EXPECT_NO_THROW(rclcpp::spin_all(node->get_node_base_interface(), 1s));
+
+    auto check_spin_until_future_complete = [&]() {
+        std::promise<bool> promise;
+        std::future<bool> future = promise.get_future();
+        promise.set_value(true);
+
+        auto shared_future = future.share();
+        auto ret = rclcpp::spin_until_future_complete(
+          node->get_node_base_interface(), shared_future, 1s);
+        EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret);
+      };
+    EXPECT_NO_THROW(check_spin_until_future_complete());
+  }
+
+  rclcpp::shutdown(non_default_context);
+}
+
+template<typename T>
+class TestBusyWaiting : public ::testing::Test
+{
+public:
+  void SetUp() override
+  {
+    rclcpp::init(0, nullptr);
+
+    const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info();
+    std::stringstream test_name;
+    test_name << test_info->test_case_name() << "_" << test_info->name();
+    node = std::make_shared<rclcpp::Node>("node", test_name.str());
+    callback_group = node->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive,
+      /* automatically_add_to_executor_with_node =*/ false);
+
+    auto waitable_interfaces = node->get_node_waitables_interface();
+    waitable = std::make_shared<TestWaitable>();
+    waitable_interfaces->add_waitable(waitable, callback_group);
+
+    executor = std::make_shared<T>();
+    executor->add_callback_group(callback_group, node->get_node_base_interface());
+  }
+
+  void TearDown() override
+  {
+    rclcpp::shutdown();
+  }
+
+  void
+  set_up_and_trigger_waitable(std::function<void()> extra_callback = nullptr)
+  {
+    this->has_executed = false;
+    this->waitable->set_on_execute_callback([this, extra_callback]() {
+        if (!this->has_executed) {
+        // trigger once to see if the second trigger is handled or not
+        // this follow up trigger simulates new entities becoming ready while
+        // the executor is executing something else, e.g. subscription got data
+        // or a timer expired, etc.
+        // spin_some would not handle this second trigger, since it collects
+        // work only once, whereas spin_all should handle it since it
+        // collects work multiple times
+          this->waitable->trigger();
+          this->has_executed = true;
+        }
+        if (nullptr != extra_callback) {
+          extra_callback();
+        }
+    });
+    this->waitable->trigger();
+  }
+
+  void
+  check_for_busy_waits(std::chrono::steady_clock::time_point start_time)
+  {
+    // rough time based check, since the work to be done was very small it
+    // should be safe to check that we didn't use more than half the
+    // max duration, which itself is much larger than necessary
+    // however, it could still produce a false-positive
+    EXPECT_LT(
+      std::chrono::steady_clock::now() - start_time,
+      max_duration / 2)
+      << "executor took a long time to execute when it should have done "
+      << "nothing and should not have blocked either, but this could be a "
+      << "false negative if the computer is really slow";
+
+    // this check is making some assumptions about the implementation of the
+    // executors, but it should be safe to say that a busy wait may result in
+    // hundreds or thousands of calls to is_ready(), but "normal" executor
+    // behavior should be within an order of magnitude of the number of
+    // times that the waitable was executed
+    ASSERT_LT(waitable->get_is_ready_call_count(), 10u * this->waitable->get_count());
+  }
+
+  static constexpr auto max_duration = 10s;
+
+  rclcpp::Node::SharedPtr node;
+  rclcpp::CallbackGroup::SharedPtr callback_group;
+  std::shared_ptr<TestWaitable> waitable;
+  std::chrono::steady_clock::time_point start_time;
+  std::shared_ptr<T> executor;
+  bool has_executed;
+};
+
+TYPED_TEST_SUITE(TestBusyWaiting, ExecutorTypes, ExecutorTypeNames);
+
+TYPED_TEST(TestBusyWaiting, test_spin_all)
+{
+  this->set_up_and_trigger_waitable();
+
+  auto start_time = std::chrono::steady_clock::now();
+  this->executor->spin_all(this->max_duration);
+  this->check_for_busy_waits(start_time);
+  // this should get the initial trigger, and the follow up from in the callback
+  ASSERT_EQ(this->waitable->get_count(), 2u);
+}
+
+TYPED_TEST(TestBusyWaiting, test_spin_some)
+{
+  this->set_up_and_trigger_waitable();
+
+  auto start_time = std::chrono::steady_clock::now();
+  this->executor->spin_some(this->max_duration);
+  this->check_for_busy_waits(start_time);
+  // this should get the inital trigger, but not the follow up in the callback
+  ASSERT_EQ(this->waitable->get_count(), 1u);
+}
+
+TYPED_TEST(TestBusyWaiting, test_spin)
+{
+  std::condition_variable cv;
+  std::mutex cv_m;
+  bool first_check_passed = false;
+
+  this->set_up_and_trigger_waitable([&cv, &cv_m, &first_check_passed]() {
+      cv.notify_one();
+      if (!first_check_passed) {
+        std::unique_lock<std::mutex> lk(cv_m);
+        cv.wait_for(lk, 1s, [&]() {return first_check_passed;});
+      }
+  });
+
+  auto start_time = std::chrono::steady_clock::now();
+  std::thread t([this]() {
+      this->executor->spin();
+    });
+
+  // wait until thread has started (first execute of waitable)
+  {
+    std::unique_lock<std::mutex> lk(cv_m);
+    cv.wait_for(lk, 10s);
+  }
+  EXPECT_GT(this->waitable->get_count(), 0u);
+
+  first_check_passed = true;
+  cv.notify_one();
+
+  // wait until the executor has finished (second execute of waitable)
+  {
+    std::unique_lock<std::mutex> lk(cv_m);
+    cv.wait_for(lk, 10s);
+  }
+  EXPECT_EQ(this->waitable->get_count(), 2u);
+
+  this->executor->cancel();
+  t.join();
+
+  this->check_for_busy_waits(start_time);
+  // this should get the initial trigger, and the follow up from in the callback
+  ASSERT_EQ(this->waitable->get_count(), 2u);
+}

rclcpp/test/rclcpp/executors/test_executors_callback_group_behavior.cpp

@@ -0,0 +1,156 @@
+// Copyright 2024 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/**
+ * This test checks that when callback groups go out of scope, that their associated executable
+ * entities should not be returned as valid executables.
+ *
+ * The test makes use of a bit of executor internals, but is meant to prevent regressions of behavior.
+ * Ref: https://github.com/ros2/rclcpp/issues/2474
+ */
+
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <future>
+
+#include <rclcpp/callback_group.hpp>
+#include <rclcpp/executor.hpp>
+#include <rclcpp/node.hpp>
+
+std::chrono::milliseconds g_timer_period {1};
+
+class CustomExecutor : public rclcpp::Executor
+{
+public:
+  explicit CustomExecutor(const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions())
+  : rclcpp::Executor(options)
+  {}
+
+  ~CustomExecutor() override = default;
+
+  void spin() override {}
+
+  void collect()
+  {
+    this->collect_entities();
+  }
+
+  void wait()
+  {
+    this->wait_for_work(g_timer_period * 10);
+  }
+
+  size_t collected_timers() const
+  {
+    return this->current_collection_.timers.size();
+  }
+
+  rclcpp::AnyExecutable next()
+  {
+    rclcpp::AnyExecutable ret;
+    this->get_next_ready_executable(ret);
+    return ret;
+  }
+};
+
+
+TEST(TestCallbackGroup, valid_callback_group)
+{
+  rclcpp::init(0, nullptr);
+
+  // Create a timer associated with a callback group
+  auto node = std::make_shared<rclcpp::Node>("node");
+
+  std::promise<void> promise;
+  std::future<void> future = promise.get_future();
+  auto timer_callback = [&promise]() {
+      promise.set_value();
+    };
+
+  // Add the callback group to the executor
+  auto executor = CustomExecutor();
+  auto cbg = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, true);
+  auto timer = node->create_wall_timer(g_timer_period, timer_callback, cbg);
+  executor.add_callback_group(cbg, node->get_node_base_interface());
+
+  ASSERT_EQ(
+    rclcpp::FutureReturnCode::SUCCESS,
+    executor.spin_until_future_complete(future, std::chrono::seconds(10)));
+
+  // Collect the entities
+  executor.collect();
+  EXPECT_EQ(1u, executor.collected_timers());
+
+  executor.wait();
+  auto next_executable = executor.next();
+  EXPECT_EQ(timer, next_executable.timer);
+  EXPECT_EQ(cbg, next_executable.callback_group);
+  EXPECT_NE(nullptr, next_executable.data);
+
+  EXPECT_EQ(nullptr, next_executable.client);
+  EXPECT_EQ(nullptr, next_executable.service);
+  EXPECT_EQ(nullptr, next_executable.subscription);
+  EXPECT_EQ(nullptr, next_executable.waitable);
+
+  rclcpp::shutdown();
+}
+
+TEST(TestCallbackGroup, invalid_callback_group)
+{
+  rclcpp::init(0, nullptr);
+
+  // Create a timer associated with a callback group
+  auto node = std::make_shared<rclcpp::Node>("node");
+
+  std::promise<void> promise;
+  std::future<void> future = promise.get_future();
+  auto timer_callback = [&promise]() {
+      promise.set_value();
+    };
+
+  // Add the callback group to the executor
+  auto executor = CustomExecutor();
+  auto cbg = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
+  auto timer = node->create_wall_timer(g_timer_period, timer_callback, cbg);
+  executor.add_callback_group(cbg, node->get_node_base_interface());
+
+  ASSERT_EQ(
+    rclcpp::FutureReturnCode::SUCCESS,
+    executor.spin_until_future_complete(future, std::chrono::seconds(10)));
+
+  // Collect the entities
+  executor.collect();
+  EXPECT_EQ(1u, executor.collected_timers());
+
+  executor.wait();
+
+  cbg.reset();
+
+  // Since the callback group has been reset, this should not be allowed to
+  // be a valid executable (timer and cbg should both be nullptr).
+  // In the regression, timer == next_executable.timer whil
+  // next_executable.callback_group == nullptr, which was incorrect.
+  auto next_executable = executor.next();
+  EXPECT_EQ(nullptr, next_executable.timer);
+  EXPECT_EQ(nullptr, next_executable.callback_group);
+
+  EXPECT_EQ(nullptr, next_executable.client);
+  EXPECT_EQ(nullptr, next_executable.service);
+  EXPECT_EQ(nullptr, next_executable.subscription);
+  EXPECT_EQ(nullptr, next_executable.waitable);
+  EXPECT_EQ(nullptr, next_executable.data);
+
+  rclcpp::shutdown();
+}

rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp

@@ -54,8 +54,16 @@ public:
       std::bind(&TimerNode::Timer2Callback, this));
   }
 
-  int GetTimer1Cnt() {return cnt1_;}
-  int GetTimer2Cnt() {return cnt2_;}
+  int GetTimer1Cnt()
+  {
+    const std::lock_guard<std::mutex> lock(mutex_);
+    return cnt1_;
+  }
+  int GetTimer2Cnt()
+  {
+    const std::lock_guard<std::mutex> lock(mutex_);
+    return cnt2_;
+  }
 
   void ResetTimer1()
   {
@@ -82,16 +90,24 @@ public:
 private:
   void Timer1Callback()
   {
-    cnt1_++;
+    {
+      const std::lock_guard<std::mutex> lock(mutex_);
+      cnt1_++;
+    }
     RCLCPP_DEBUG(this->get_logger(), "Timer 1! (%d)", cnt1_);
   }
 
   void Timer2Callback()
   {
-    cnt2_++;
+    {
+      const std::lock_guard<std::mutex> lock(mutex_);
+      cnt2_++;
+    }
     RCLCPP_DEBUG(this->get_logger(), "Timer 2! (%d)", cnt2_);
   }
 
+  std::mutex mutex_;
+
   rclcpp::TimerBase::SharedPtr timer1_;
   rclcpp::TimerBase::SharedPtr timer2_;
   int cnt1_ = 0;
@@ -130,6 +146,18 @@ public:
     }
   }
 
+  bool wait_for_connection(std::chrono::nanoseconds timeout)
+  {
+    auto end_time = std::chrono::steady_clock::now() + timeout;
+    while (clock_publisher_->get_subscription_count() == 0 &&
+      (std::chrono::steady_clock::now() < end_time))
+    {
+      std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+
+    return clock_publisher_->get_subscription_count() != 0;
+  }
+
   void sleep_for(rclcpp::Duration duration)
   {
     rclcpp::Time start_time(0, 0, RCL_ROS_TIME);
@@ -148,7 +176,10 @@ public:
         return;
       }
       std::this_thread::sleep_for(realtime_clock_step_.to_chrono<std::chrono::milliseconds>());
-      rostime_ += ros_update_duration_;
+      {
+        const std::lock_guard<std::mutex> lock(mutex_);
+        rostime_ += ros_update_duration_;
+      }
     }
   }
 
@@ -163,9 +194,11 @@ private:
 
   void PublishClock()
   {
-    const std::lock_guard<std::mutex> lock(mutex_);
     auto message = rosgraph_msgs::msg::Clock();
-    message.clock = rostime_;
+    {
+      const std::lock_guard<std::mutex> lock(mutex_);
+      message.clock = rostime_;
+    }
     clock_publisher_->publish(message);
   }
 
@@ -227,6 +260,9 @@ public:
       [this]() {
         executor.spin();
       });
+
+    EXPECT_TRUE(this->sim_clock_node->wait_for_connection(50ms));
+    EXPECT_EQ(RCL_ROS_TIME, node->get_clock()->ros_time_is_active());
   }
 
   void TearDown()

rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp

@@ -230,9 +230,9 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) {
 class TestWaitable : public rclcpp::Waitable
 {
 public:
-  void add_to_wait_set(rcl_wait_set_t *) override {}
+  void add_to_wait_set(rcl_wait_set_t &) override {}
 
-  bool is_ready(rcl_wait_set_t *) override {return true;}
+  bool is_ready(const rcl_wait_set_t &) override {return true;}
 
   std::shared_ptr<void>
   take_data() override
@@ -240,10 +240,7 @@ public:
     return nullptr;
   }
   void
-  execute(std::shared_ptr<void> & data) override
-  {
-    (void) data;
-  }
+  execute(const std::shared_ptr<void> &) override {}
 };
 
 TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) {
@@ -513,11 +510,6 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_to_wait_set_nullptr) {
   entities_collector_->init(&wait_set, memory_strategy);
   RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
 
-  EXPECT_THROW(
-    entities_collector_->add_to_wait_set(nullptr),
-    std::invalid_argument);
-  rcl_reset_error();
-
   EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
 }
 

rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp

@@ -56,7 +56,7 @@ TEST_F(TestStaticSingleThreadedExecutor, add_callback_group_trigger_guard_failed
       "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
     RCLCPP_EXPECT_THROW_EQ(
       executor.add_callback_group(cb_group, node->get_node_base_interface(), true),
-      std::runtime_error("error not set"));
+      std::runtime_error("Failed to trigger guard condition on callback group add: error not set"));
   }
 }
 
@@ -69,7 +69,7 @@ TEST_F(TestStaticSingleThreadedExecutor, add_node_trigger_guard_failed) {
       "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
     RCLCPP_EXPECT_THROW_EQ(
       executor.add_node(node),
-      std::runtime_error("error not set"));
+      std::runtime_error("Failed to trigger guard condition on node add: error not set"));
   }
 }
 
@@ -86,7 +86,8 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_callback_group_trigger_guard_fai
       "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
     RCLCPP_EXPECT_THROW_EQ(
       executor.remove_callback_group(cb_group, true),
-      std::runtime_error("error not set"));
+      std::runtime_error(
+        "Failed to trigger guard condition on callback group remove: error not set"));
   }
 }
 
@@ -99,7 +100,7 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_node_failed) {
       "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
     RCLCPP_EXPECT_THROW_EQ(
       executor.remove_node(node, true),
-      std::runtime_error("Node needs to be associated with this executor."));
+      std::runtime_error("Node '/ns/node' needs to be associated with an executor."));
   }
 }
 
@@ -114,7 +115,7 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_node_trigger_guard_failed) {
       "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
     RCLCPP_EXPECT_THROW_EQ(
       executor.remove_node(node, true),
-      std::runtime_error("error not set"));
+      std::runtime_error("Failed to trigger guard condition on node remove: error not set"));
   }
 }
 

rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp

@@ -32,8 +32,8 @@ public:
   : TimerBase(node->get_clock(), std::chrono::nanoseconds(1),
       node->get_node_base_interface()->get_context()) {}
 
-  bool call() override {return true;}
-  void execute_callback() override {}
+  std::shared_ptr<void> call() override {return nullptr;}
+  void execute_callback(const std::shared_ptr<void> &) override {}
   bool is_steady() override {return false;}
 };
 

rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp

@@ -28,8 +28,8 @@
 class TestWaitable : public rclcpp::Waitable
 {
 public:
-  void add_to_wait_set(rcl_wait_set_t *) override {}
-  bool is_ready(rcl_wait_set_t *) override {return false;}
+  void add_to_wait_set(rcl_wait_set_t &) override {}
+  bool is_ready(const rcl_wait_set_t &) override {return false;}
 
   std::shared_ptr<void>
   take_data() override
@@ -37,10 +37,7 @@ public:
     return nullptr;
   }
 
-  void execute(std::shared_ptr<void> & data) override
-  {
-    (void) data;
-  }
+  void execute(const std::shared_ptr<void> &) override {}
 };
 
 class TestNodeWaitables : public ::testing::Test

rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp

@@ -39,14 +39,14 @@ static bool test_waitable_result = false;
 class TestWaitable : public rclcpp::Waitable
 {
 public:
-  void add_to_wait_set(rcl_wait_set_t *) override
+  void add_to_wait_set(rcl_wait_set_t &) override
   {
     if (!test_waitable_result) {
       throw std::runtime_error("TestWaitable add_to_wait_set failed");
     }
   }
 
-  bool is_ready(rcl_wait_set_t *) override
+  bool is_ready(const rcl_wait_set_t &) override
   {
     return test_waitable_result;
   }
@@ -57,61 +57,7 @@ public:
     return nullptr;
   }
 
-  void execute(std::shared_ptr<void> & data) override
-  {
-    (void) data;
-  }
-};
-
-static bool test_waitable_result2 = false;
-
-class TestWaitable2 : public rclcpp::Waitable
-{
-public:
-  explicit TestWaitable2(rcl_publisher_t * pub_ptr)
-  : pub_ptr_(pub_ptr),
-    pub_event_(rcl_get_zero_initialized_event())
-  {
-    EXPECT_EQ(
-      rcl_publisher_event_init(&pub_event_, pub_ptr_, RCL_PUBLISHER_OFFERED_DEADLINE_MISSED),
-      RCL_RET_OK);
-  }
-
-  ~TestWaitable2()
-  {
-    EXPECT_EQ(rcl_event_fini(&pub_event_), RCL_RET_OK);
-  }
-
-  void add_to_wait_set(rcl_wait_set_t * wait_set) override
-  {
-    EXPECT_EQ(rcl_wait_set_add_event(wait_set, &pub_event_, &wait_set_event_index_), RCL_RET_OK);
-  }
-
-  bool is_ready(rcl_wait_set_t *) override
-  {
-    return test_waitable_result2;
-  }
-
-  std::shared_ptr<void>
-  take_data() override
-  {
-    return nullptr;
-  }
-
-  void execute(std::shared_ptr<void> & data) override
-  {
-    (void) data;
-  }
-
-  size_t get_number_of_ready_events() override
-  {
-    return 1;
-  }
-
-private:
-  rcl_publisher_t * pub_ptr_;
-  rcl_event_t pub_event_;
-  size_t wait_set_event_index_;
+  void execute(const std::shared_ptr<void> &) override {}
 };
 
 struct RclWaitSetSizes
@@ -708,129 +654,20 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_timer) {
 }
 
 TEST_F(TestAllocatorMemoryStrategy, get_next_waitable) {
+  auto node1 = std::make_shared<rclcpp::Node>("waitable_node", "ns");
+  auto node2 = std::make_shared<rclcpp::Node>("waitable_node2", "ns");
+  rclcpp::Waitable::SharedPtr waitable1 = std::make_shared<TestWaitable>();
+  rclcpp::Waitable::SharedPtr waitable2 = std::make_shared<TestWaitable>();
+  node1->get_node_waitables_interface()->add_waitable(waitable1, nullptr);
+  node2->get_node_waitables_interface()->add_waitable(waitable2, nullptr);
+
   auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) {
       rclcpp::AnyExecutable result;
       allocator_memory_strategy()->get_next_waitable(result, weak_groups_to_nodes);
       return result;
     };
 
-  {
-    auto node1 = std::make_shared<rclcpp::Node>(
-      "waitable_node", "ns",
-      rclcpp::NodeOptions()
-      .start_parameter_event_publisher(false)
-      .start_parameter_services(false));
-
-    rclcpp::PublisherOptions pub_options;
-    pub_options.use_default_callbacks = false;
-
-    auto pub1 = node1->create_publisher<test_msgs::msg::Empty>(
-      "test_topic_1", rclcpp::QoS(10), pub_options);
-
-    auto waitable1 =
-      std::make_shared<TestWaitable2>(pub1->get_publisher_handle().get());
-    node1->get_node_waitables_interface()->add_waitable(waitable1, nullptr);
-
-    auto basic_node = create_node_with_disabled_callback_groups("basic_node");
-    WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
-    basic_node->for_each_callback_group(
-      [basic_node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      {
-        weak_groups_to_nodes.insert(
-          std::pair<rclcpp::CallbackGroup::WeakPtr,
-          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            group_ptr,
-            basic_node->get_node_base_interface()));
-      });
-    node1->for_each_callback_group(
-      [node1, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      {
-        weak_groups_to_nodes.insert(
-          std::pair<rclcpp::CallbackGroup::WeakPtr,
-          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            group_ptr,
-            node1->get_node_base_interface()));
-      });
-    allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
-
-    rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-    ASSERT_EQ(
-      rcl_wait_set_init(
-        &wait_set,
-        allocator_memory_strategy()->number_of_ready_subscriptions(),
-        allocator_memory_strategy()->number_of_guard_conditions(),
-        allocator_memory_strategy()->number_of_ready_timers(),
-        allocator_memory_strategy()->number_of_ready_clients(),
-        allocator_memory_strategy()->number_of_ready_services(),
-        allocator_memory_strategy()->number_of_ready_events(),
-        rclcpp::contexts::get_global_default_context()->get_rcl_context().get(),
-        allocator_memory_strategy()->get_allocator()),
-      RCL_RET_OK);
-
-    ASSERT_TRUE(allocator_memory_strategy()->add_handles_to_wait_set(&wait_set));
-
-    ASSERT_EQ(
-      rcl_wait(
-        &wait_set,
-        std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::milliseconds(100))
-        .count()),
-      RCL_RET_OK);
-    test_waitable_result2 = true;
-    allocator_memory_strategy()->remove_null_handles(&wait_set);
-
-    rclcpp::AnyExecutable result = get_next_entity(weak_groups_to_nodes);
-    EXPECT_EQ(result.node_base, node1->get_node_base_interface());
-    test_waitable_result2 = false;
-
-    EXPECT_EQ(rcl_wait_set_fini(&wait_set), RCL_RET_OK);
-  }
-
-  {
-    auto node2 = std::make_shared<rclcpp::Node>(
-      "waitable_node2", "ns",
-      rclcpp::NodeOptions()
-      .start_parameter_services(false)
-      .start_parameter_event_publisher(false));
-
-    rclcpp::PublisherOptions pub_options;
-    pub_options.use_default_callbacks = false;
-
-    auto pub2 = node2->create_publisher<test_msgs::msg::Empty>(
-      "test_topic_2", rclcpp::QoS(10), pub_options);
-
-    auto waitable2 =
-      std::make_shared<TestWaitable2>(pub2->get_publisher_handle().get());
-    node2->get_node_waitables_interface()->add_waitable(waitable2, nullptr);
-
-    auto basic_node2 = std::make_shared<rclcpp::Node>(
-      "basic_node2", "ns",
-      rclcpp::NodeOptions()
-      .start_parameter_services(false)
-      .start_parameter_event_publisher(false));
-    WeakCallbackGroupsToNodesMap weak_groups_to_uncollected_nodes;
-    basic_node2->for_each_callback_group(
-      [basic_node2, &weak_groups_to_uncollected_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      {
-        weak_groups_to_uncollected_nodes.insert(
-          std::pair<rclcpp::CallbackGroup::WeakPtr,
-          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            group_ptr,
-            basic_node2->get_node_base_interface()));
-      });
-    node2->for_each_callback_group(
-      [node2,
-      &weak_groups_to_uncollected_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      {
-        weak_groups_to_uncollected_nodes.insert(
-          std::pair<rclcpp::CallbackGroup::WeakPtr,
-          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            group_ptr,
-            node2->get_node_base_interface()));
-      });
-
-    rclcpp::AnyExecutable failed_result = get_next_entity(weak_groups_to_uncollected_nodes);
-    EXPECT_EQ(failed_result.node_base, nullptr);
-  }
+  EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity));
 }
 
 TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_mutually_exclusive) {

rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp

@@ -200,22 +200,31 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_t
 {
   using ExecutorType = TypeParam;
 
+  auto count_callback_groups_in_node = [](auto node) {
+      size_t num = 0;
+      node->get_node_base_interface()->for_each_callback_group(
+        [&num](auto) {
+          num++;
+        });
+      return num;
+    };
+
   ExecutorType executor;
   auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
   executor.add_node(node->get_node_base_interface());
-  ASSERT_EQ(executor.get_all_callback_groups().size(), 1u);
-  std::atomic_int timer_count {0};
+  ASSERT_EQ(executor.get_all_callback_groups().size(), count_callback_groups_in_node(node));
+  std::atomic_size_t timer_count {0};
   auto timer_callback = [&executor, &timer_count]() {
-      if (timer_count > 0) {
-        ASSERT_EQ(executor.get_all_callback_groups().size(), 3u);
+      auto cur_timer_count = timer_count++;
+      printf("in timer_callback(%zu)\n", cur_timer_count);
+      if (cur_timer_count > 0) {
         executor.cancel();
       }
-      timer_count++;
     };
   rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group(
     rclcpp::CallbackGroupType::MutuallyExclusive);
   rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer(
-    2s, timer_callback, cb_grp);
+    1s, timer_callback, cb_grp);
   rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group(
     rclcpp::CallbackGroupType::MutuallyExclusive, false);
   auto timer2_callback = []() {};
@@ -227,6 +236,7 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_t
   rclcpp::TimerBase::SharedPtr timer3_ = node->create_wall_timer(
     2s, timer3_callback, cb_grp3);
   executor.spin();
+  ASSERT_GT(timer_count.load(), 0u);
 }
 
 /*

rclcpp/test/rclcpp/test_clock.cpp

@@ -0,0 +1,229 @@
+// Copyright 2024 Cellumation GmbH
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <chrono>
+
+#include "rcl/error_handling.h"
+#include "rcl/time.h"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/time_source.hpp"
+
+#include "../utils/rclcpp_gtest_macros.hpp"
+
+using namespace std::chrono_literals;
+
+class TestClockWakeup : public ::testing::TestWithParam<rcl_clock_type_e>
+{
+public:
+  void test_wakeup_before_sleep(const rclcpp::Clock::SharedPtr & clock)
+  {
+    std::atomic_bool thread_finished = false;
+
+    std::thread wait_thread = std::thread(
+      [&clock, &thread_finished]()
+      {
+        // make sure the thread starts sleeping late
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        clock->sleep_until(clock->now() + std::chrono::seconds(3));
+        thread_finished = true;
+      });
+
+    // notify the clock, that the sleep shall be interrupted
+    clock->cancel_sleep_or_wait();
+
+    auto start_time = std::chrono::steady_clock::now();
+    auto cur_time = start_time;
+    while (!thread_finished && start_time + std::chrono::seconds(1) > cur_time) {
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+      cur_time = std::chrono::steady_clock::now();
+    }
+
+    wait_thread.join();
+
+    EXPECT_TRUE(thread_finished);
+    EXPECT_LT(cur_time, start_time + std::chrono::seconds(1));
+  }
+
+  void test_wakeup_after_sleep(const rclcpp::Clock::SharedPtr & clock)
+  {
+    std::atomic_bool thread_finished = false;
+
+    std::thread wait_thread = std::thread(
+      [&clock, &thread_finished]()
+      {
+        clock->sleep_until(clock->now() + std::chrono::seconds(3));
+        thread_finished = true;
+      });
+
+    // make sure the thread is already sleeping before we send the cancel
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
+    // notify the clock, that the sleep shall be interrupted
+    clock->cancel_sleep_or_wait();
+
+    auto start_time = std::chrono::steady_clock::now();
+    auto cur_time = start_time;
+    while (!thread_finished && start_time + std::chrono::seconds(1) > cur_time) {
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+      cur_time = std::chrono::steady_clock::now();
+    }
+
+    wait_thread.join();
+
+    EXPECT_TRUE(thread_finished);
+    EXPECT_LT(cur_time, start_time + std::chrono::seconds(1));
+  }
+
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("my_node");
+  }
+
+  void TearDown()
+  {
+    node.reset();
+  }
+
+  rclcpp::Node::SharedPtr node;
+};
+
+INSTANTIATE_TEST_SUITE_P(
+  Clocks,
+  TestClockWakeup,
+  ::testing::Values(
+    RCL_SYSTEM_TIME, RCL_ROS_TIME, RCL_STEADY_TIME
+));
+
+TEST_P(TestClockWakeup, wakeup_sleep) {
+  auto clock = std::make_shared<rclcpp::Clock>(GetParam());
+  test_wakeup_after_sleep(clock);
+  test_wakeup_before_sleep(clock);
+}
+
+TEST_F(TestClockWakeup, wakeup_sleep_ros_time_active) {
+  node->set_parameter({"use_sim_time", true});
+  auto clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+  rclcpp::TimeSource time_source(node);
+  time_source.attachClock(clock);
+
+  EXPECT_TRUE(clock->ros_time_is_active());
+
+  test_wakeup_after_sleep(clock);
+  test_wakeup_before_sleep(clock);
+}
+
+TEST_F(TestClockWakeup, no_wakeup_on_sim_time) {
+  node->set_parameter({"use_sim_time", true});
+  auto clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+  rclcpp::TimeSource time_source(node);
+  time_source.attachClock(clock);
+
+  EXPECT_TRUE(clock->ros_time_is_active());
+
+  std::atomic_bool thread_finished = false;
+
+  std::thread wait_thread = std::thread(
+    [&clock, &thread_finished]()
+    {
+      // make sure the thread starts sleeping late
+      clock->sleep_until(clock->now() + std::chrono::milliseconds(10));
+      thread_finished = true;
+    });
+
+  // make sure, that the sim time clock does not wakeup, as no clock is provided
+  std::this_thread::sleep_for(std::chrono::milliseconds(500));
+  EXPECT_FALSE(thread_finished);
+
+  // notify the clock, that the sleep shall be interrupted
+  clock->cancel_sleep_or_wait();
+
+  auto start_time = std::chrono::steady_clock::now();
+  auto cur_time = start_time;
+  while (!thread_finished && start_time + std::chrono::seconds(1) > cur_time) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    cur_time = std::chrono::steady_clock::now();
+  }
+
+  wait_thread.join();
+
+  EXPECT_TRUE(thread_finished);
+  EXPECT_LT(cur_time, start_time + std::chrono::seconds(1));
+}
+
+TEST_F(TestClockWakeup, multiple_threads_wait_on_one_clock) {
+  auto clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+
+  std::vector<bool> thread_finished(10, false);
+
+  std::vector<std::thread> threads;
+
+  for (size_t nr = 0; nr < thread_finished.size(); nr++) {
+    threads.push_back(
+      std::thread(
+        [&clock, &thread_finished, nr]()
+        {
+          // make sure the thread starts sleeping late
+          clock->sleep_until(clock->now() + std::chrono::seconds(10));
+          thread_finished[nr] = true;
+        }));
+  }
+
+  // wait a bit so all threads can execute the sleep_until
+  std::this_thread::sleep_for(std::chrono::milliseconds(500));
+
+  for (const bool & finished : thread_finished) {
+    EXPECT_FALSE(finished);
+  }
+
+  rclcpp::shutdown();
+
+  auto start_time = std::chrono::steady_clock::now();
+  auto cur_time = start_time;
+  bool threads_finished = false;
+  while (!threads_finished && start_time + std::chrono::seconds(1) > cur_time) {
+    threads_finished = true;
+    for (const bool finished : thread_finished) {
+      if (!finished) {
+        threads_finished = false;
+      }
+    }
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    cur_time = std::chrono::steady_clock::now();
+  }
+
+  for (const bool finished : thread_finished) {
+    EXPECT_TRUE(finished);
+  }
+
+  for (auto & thread : threads) {
+    thread.join();
+  }
+
+  EXPECT_LT(cur_time, start_time + std::chrono::seconds(1));
+}

rclcpp/test/rclcpp/test_executor.cpp

@@ -46,23 +46,6 @@ public:
   {
     spin_node_once_nanoseconds(node, std::chrono::milliseconds(100));
   }
-
-  rclcpp::memory_strategy::MemoryStrategy * memory_strategy_ptr()
-  {
-    return memory_strategy_.get();
-  }
-
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr local_get_node_by_group(
-    rclcpp::CallbackGroup::SharedPtr group)
-  {
-    std::lock_guard<std::mutex> guard_{mutex_};  // only to make the TSA happy
-    return get_node_by_group(weak_groups_to_nodes_, group);
-  }
-
-  rclcpp::CallbackGroup::SharedPtr local_get_group_by_timer(rclcpp::TimerBase::SharedPtr timer)
-  {
-    return get_group_by_timer(timer);
-  }
 };
 
 class TestExecutor : public ::testing::Test
@@ -130,7 +113,7 @@ TEST_F(TestExecutor, constructor_bad_wait_set_init) {
   auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_init, RCL_RET_ERROR);
   RCLCPP_EXPECT_THROW_EQ(
     static_cast<void>(std::make_unique<DummyExecutor>()),
-    std::runtime_error("Failed to create wait set in Executor constructor: error not set"));
+    std::runtime_error("Failed to create wait set: error not set"));
 }
 
 TEST_F(TestExecutor, add_callback_group_twice) {
@@ -142,7 +125,7 @@ TEST_F(TestExecutor, add_callback_group_twice) {
   cb_group->get_associated_with_executor_atomic().exchange(false);
   RCLCPP_EXPECT_THROW_EQ(
     dummy.add_callback_group(cb_group, node->get_node_base_interface(), false),
-    std::runtime_error("Callback group was already added to executor."));
+    std::runtime_error("Callback group has already been added to this executor."));
 }
 
 TEST_F(TestExecutor, add_callback_group_failed_trigger_guard_condition) {
@@ -168,9 +151,15 @@ TEST_F(TestExecutor, remove_callback_group_null_node) {
 
   node.reset();
 
+
+  /**
+   * TODO(mjcarroll): Assert this when we are enforcing that nodes must be destroyed
+   * after their created callback groups.
   RCLCPP_EXPECT_THROW_EQ(
     dummy.remove_callback_group(cb_group, false),
     std::runtime_error("Node must not be deleted before its callback group(s)."));
+   */
+  EXPECT_NO_THROW(dummy.remove_callback_group(cb_group, false));
 }
 
 TEST_F(TestExecutor, remove_callback_group_failed_trigger_guard_condition) {
@@ -197,7 +186,7 @@ TEST_F(TestExecutor, remove_node_not_associated) {
 
   RCLCPP_EXPECT_THROW_EQ(
     dummy.remove_node(node->get_node_base_interface(), false),
-    std::runtime_error("Node needs to be associated with an executor."));
+    std::runtime_error("Node '/ns/node' needs to be associated with an executor."));
 }
 
 TEST_F(TestExecutor, remove_node_associated_with_different_executor) {
@@ -211,7 +200,7 @@ TEST_F(TestExecutor, remove_node_associated_with_different_executor) {
 
   RCLCPP_EXPECT_THROW_EQ(
     dummy2.remove_node(node1->get_node_base_interface(), false),
-    std::runtime_error("Node needs to be associated with this executor."));
+    std::runtime_error("Node '/ns/node1' needs to be associated with this executor."));
 }
 
 TEST_F(TestExecutor, spin_node_once_nanoseconds) {
@@ -328,42 +317,14 @@ TEST_F(TestExecutor, cancel_failed_trigger_guard_condition) {
     std::runtime_error("Failed to trigger guard condition in cancel: error not set"));
 }
 
-TEST_F(TestExecutor, set_memory_strategy_nullptr) {
-  DummyExecutor dummy;
-
+TEST_F(TestExecutor, create_executor_fail_wait_set_clear) {
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR);
   RCLCPP_EXPECT_THROW_EQ(
-    dummy.set_memory_strategy(nullptr),
-    std::runtime_error("Received NULL memory strategy in executor."));
+    DummyExecutor dummy,
+    std::runtime_error("Couldn't clear the wait set: error not set"));
 }
 
-TEST_F(TestExecutor, set_memory_strategy) {
-  DummyExecutor dummy;
-  rclcpp::memory_strategy::MemoryStrategy::SharedPtr strategy =
-    std::make_shared<
-    rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>();
-
-  dummy.set_memory_strategy(strategy);
-  EXPECT_EQ(dummy.memory_strategy_ptr(), strategy.get());
-}
-
-TEST_F(TestExecutor, spin_once_failed_trigger_guard_condition) {
-  DummyExecutor dummy;
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  auto timer =
-    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {});
-
-  dummy.add_node(node);
-  // Wait for the wall timer to have expired.
-  std::this_thread::sleep_for(std::chrono::milliseconds(50));
-  auto mock = mocking_utils::patch_and_return(
-    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
-  RCLCPP_EXPECT_THROW_EQ(
-    dummy.spin_once(std::chrono::milliseconds(1)),
-    std::runtime_error(
-      "Failed to trigger guard condition from execute_any_executable: error not set"));
-}
-
-TEST_F(TestExecutor, spin_some_fail_wait_set_clear) {
+TEST_F(TestExecutor, spin_all_fail_wait_set_clear) {
   DummyExecutor dummy;
   auto node = std::make_shared<rclcpp::Node>("node", "ns");
   auto timer =
@@ -371,9 +332,10 @@ TEST_F(TestExecutor, spin_some_fail_wait_set_clear) {
 
   dummy.add_node(node);
   auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR);
+
   RCLCPP_EXPECT_THROW_EQ(
-    dummy.spin_some(std::chrono::milliseconds(1)),
-    std::runtime_error("Couldn't clear wait set: error not set"));
+    dummy.spin_all(std::chrono::milliseconds(1)),
+    std::runtime_error("Couldn't clear the wait set: error not set"));
 }
 
 TEST_F(TestExecutor, spin_some_fail_wait_set_resize) {
@@ -401,7 +363,7 @@ TEST_F(TestExecutor, spin_some_fail_add_handles_to_wait_set) {
     RCL_RET_ERROR);
   RCLCPP_EXPECT_THROW_EQ(
     dummy.spin_some(std::chrono::milliseconds(1)),
-    std::runtime_error("Couldn't fill wait set"));
+    std::runtime_error("Couldn't fill wait set: error not set"));
 }
 
 TEST_F(TestExecutor, spin_some_fail_wait) {
@@ -417,71 +379,6 @@ TEST_F(TestExecutor, spin_some_fail_wait) {
     std::runtime_error("rcl_wait() failed: error not set"));
 }
 
-TEST_F(TestExecutor, get_node_by_group_null_group) {
-  DummyExecutor dummy;
-  ASSERT_EQ(nullptr, dummy.local_get_node_by_group(nullptr));
-}
-
-TEST_F(TestExecutor, get_node_by_group) {
-  DummyExecutor dummy;
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
-    rclcpp::CallbackGroupType::MutuallyExclusive);
-  dummy.add_callback_group(cb_group, node->get_node_base_interface(), false);
-  ASSERT_EQ(node->get_node_base_interface().get(), dummy.local_get_node_by_group(cb_group).get());
-}
-
-TEST_F(TestExecutor, get_node_by_group_not_found) {
-  DummyExecutor dummy;
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
-    rclcpp::CallbackGroupType::MutuallyExclusive);
-  ASSERT_EQ(nullptr, dummy.local_get_node_by_group(cb_group).get());
-}
-
-TEST_F(TestExecutor, get_group_by_timer_nullptr) {
-  DummyExecutor dummy;
-  ASSERT_EQ(nullptr, dummy.local_get_group_by_timer(nullptr));
-}
-
-TEST_F(TestExecutor, get_group_by_timer) {
-  DummyExecutor dummy;
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
-    rclcpp::CallbackGroupType::MutuallyExclusive);
-  auto timer =
-    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group);
-  dummy.add_node(node);
-
-  ASSERT_EQ(cb_group.get(), dummy.local_get_group_by_timer(timer).get());
-}
-
-TEST_F(TestExecutor, get_group_by_timer_with_deleted_group) {
-  DummyExecutor dummy;
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
-    rclcpp::CallbackGroupType::MutuallyExclusive);
-  auto timer =
-    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group);
-  dummy.add_node(node);
-
-  cb_group.reset();
-
-  ASSERT_EQ(nullptr, dummy.local_get_group_by_timer(timer).get());
-}
-
-TEST_F(TestExecutor, get_group_by_timer_add_callback_group) {
-  DummyExecutor dummy;
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
-    rclcpp::CallbackGroupType::MutuallyExclusive);
-  auto timer =
-    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group);
-  dummy.add_callback_group(cb_group, node->get_node_base_interface(), false);
-
-  ASSERT_EQ(cb_group.get(), dummy.local_get_group_by_timer(timer).get());
-}
-
 TEST_F(TestExecutor, spin_until_future_complete_in_spin_until_future_complete) {
   DummyExecutor dummy;
   auto node = std::make_shared<rclcpp::Node>("node", "ns");

rclcpp/test/rclcpp/test_guard_condition.cpp

@@ -105,19 +105,11 @@ TEST_F(TestGuardCondition, add_to_wait_set) {
         "lib:rclcpp", rcl_wait_set_add_guard_condition, RCL_RET_OK);
 
       rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-      EXPECT_NO_THROW(gc->add_to_wait_set(&wait_set));
-      EXPECT_NO_THROW(gc->add_to_wait_set(&wait_set));
+      EXPECT_NO_THROW(gc->add_to_wait_set(wait_set));
+      EXPECT_NO_THROW(gc->add_to_wait_set(wait_set));
 
       rcl_wait_set_t wait_set_2 = rcl_get_zero_initialized_wait_set();
-      EXPECT_THROW(gc->add_to_wait_set(&wait_set_2), std::runtime_error);
-    }
-
-    {
-      auto mock = mocking_utils::patch_and_return(
-        "lib:rclcpp", rcl_wait_set_add_guard_condition, RCL_RET_ERROR);
-
-      auto gd = std::make_shared<rclcpp::GuardCondition>();
-      EXPECT_THROW(gd->add_to_wait_set(nullptr), rclcpp::exceptions::RCLError);
+      EXPECT_THROW(gc->add_to_wait_set(wait_set_2), std::runtime_error);
     }
   }
 }

rclcpp/test/rclcpp/test_memory_strategy.cpp

@@ -35,10 +35,11 @@ typedef std::map<rclcpp::CallbackGroup::WeakPtr,
 class TestWaitable : public rclcpp::Waitable
 {
 public:
-  void add_to_wait_set(rcl_wait_set_t *) override {}
-  bool is_ready(rcl_wait_set_t *) override {return true;}
+  void add_to_wait_set(rcl_wait_set_t &) override {}
+  bool is_ready(const rcl_wait_set_t &) override {return true;}
+
   std::shared_ptr<void> take_data() override {return nullptr;}
-  void execute(std::shared_ptr<void> & data) override {(void)data;}
+  void execute(const std::shared_ptr<void> &) override {}
 };
 
 class TestMemoryStrategy : public ::testing::Test

rclcpp/test/rclcpp/test_publisher.cpp

@@ -526,7 +526,7 @@ TEST_F(TestPublisher, run_event_handlers) {
 
   for (const auto & key_event_pair : publisher->get_event_handlers()) {
     auto handler = key_event_pair.second;
-    std::shared_ptr<void> data = handler->take_data();
+    const std::shared_ptr<void> data = handler->take_data();
     handler->execute(data);
   }
 }

rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp

@@ -379,9 +379,7 @@ TEST_F(TestPublisher, test_large_message_unique)
   auto pub = node->create_publisher<StringTypeAdapter>(topic_name, 1);
 
   static constexpr size_t length = 10 * 1024 * 1024;
-  auto message_data = std::make_unique<std::string>();
-  message_data->reserve(length);
-  std::fill(message_data->begin(), message_data->begin() + length, '#');
+  auto message_data = std::make_unique<std::string>(length, '#');
   pub->publish(std::move(message_data));
 }
 
@@ -400,8 +398,6 @@ TEST_F(TestPublisher, test_large_message_constref)
   auto pub = node->create_publisher<StringTypeAdapter>(topic_name, 1);
 
   static constexpr size_t length = 10 * 1024 * 1024;
-  std::string message_data;
-  message_data.reserve(length);
-  std::fill(message_data.begin(), message_data.begin() + length, '#');
+  std::string message_data(length, '#');
   pub->publish(message_data);
 }

rclcpp/test/rclcpp/test_qos_event.cpp

@@ -301,14 +301,14 @@ TEST_F(TestQosEvent, add_to_wait_set) {
   {
     rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
     auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_add_event, RCL_RET_OK);
-    EXPECT_NO_THROW(handler.add_to_wait_set(&wait_set));
+    EXPECT_NO_THROW(handler.add_to_wait_set(wait_set));
   }
 
   {
     rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
     auto mock = mocking_utils::patch_and_return(
       "lib:rclcpp", rcl_wait_set_add_event, RCL_RET_ERROR);
-    EXPECT_THROW(handler.add_to_wait_set(&wait_set), rclcpp::exceptions::RCLError);
+    EXPECT_THROW(handler.add_to_wait_set(wait_set), rclcpp::exceptions::RCLError);
   }
 }
 

rclcpp/test/rclcpp/test_subscription_content_filter.cpp

@@ -310,3 +310,25 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_fil
     }
   }
 }
+
+TEST_F(
+  CLASSNAME(
+    TestContentFilterSubscription,
+    RMW_IMPLEMENTATION), create_two_content_filters_with_same_topic_name_and_destroy) {
+
+  // Create another content filter
+  auto options = rclcpp::SubscriptionOptions();
+
+  std::string filter_expression = "int32_value > %0";
+  std::vector<std::string> expression_parameters = {"4"};
+
+  options.content_filter_options.filter_expression = filter_expression;
+  options.content_filter_options.expression_parameters = expression_parameters;
+
+  auto callback = [](std::shared_ptr<const test_msgs::msg::BasicTypes>) {};
+  auto sub_2 = node->create_subscription<test_msgs::msg::BasicTypes>(
+    "content_filter_topic", qos, callback, options);
+
+  EXPECT_NE(nullptr, sub_2);
+  sub_2.reset();
+}

rclcpp/test/rclcpp/test_time.cpp

@@ -138,6 +138,8 @@ TEST_F(TestTime, conversions) {
 
     EXPECT_ANY_THROW(rclcpp::Time(-1, 1));
 
+    EXPECT_ANY_THROW(rclcpp::Time(-1));
+
     EXPECT_ANY_THROW(
     {
       rclcpp::Time assignment(1, 2);
@@ -168,48 +170,6 @@ TEST_F(TestTime, conversions) {
     EXPECT_EQ(time_msg.nanosec, HALF_SEC_IN_NS);
     EXPECT_EQ(rclcpp::Time(time_msg).nanoseconds(), ONE_AND_HALF_SEC_IN_NS);
   }
-
-  {
-    // Can rclcpp::Time be negative or not? The following constructor works:
-    rclcpp::Time time(-HALF_SEC_IN_NS);
-    auto time_msg = static_cast<builtin_interfaces::msg::Time>(time);
-    EXPECT_EQ(time_msg.sec, -1);
-    EXPECT_EQ(time_msg.nanosec, HALF_SEC_IN_NS);
-
-    // The opposite conversion throws...
-    EXPECT_ANY_THROW(
-    {
-      rclcpp::Time negative_time(time_msg);
-    });
-  }
-
-  {
-    // Can rclcpp::Time be negative or not? The following constructor works:
-    rclcpp::Time time(-ONE_SEC_IN_NS);
-    auto time_msg = static_cast<builtin_interfaces::msg::Time>(time);
-    EXPECT_EQ(time_msg.sec, -1);
-    EXPECT_EQ(time_msg.nanosec, 0u);
-
-    // The opposite conversion throws...
-    EXPECT_ANY_THROW(
-    {
-      rclcpp::Time negative_time(time_msg);
-    });
-  }
-
-  {
-    // Can rclcpp::Time be negative or not? The following constructor works:
-    rclcpp::Time time(-ONE_AND_HALF_SEC_IN_NS);
-    auto time_msg = static_cast<builtin_interfaces::msg::Time>(time);
-    EXPECT_EQ(time_msg.sec, -2);
-    EXPECT_EQ(time_msg.nanosec, HALF_SEC_IN_NS);
-
-    // The opposite conversion throws...
-    EXPECT_ANY_THROW(
-    {
-      rclcpp::Time negative_time(time_msg);
-    });
-  }
 }
 
 TEST_F(TestTime, operators) {
@@ -326,31 +286,18 @@ TEST_F(TestTime, overflow_detectors) {
 
 TEST_F(TestTime, overflows) {
   rclcpp::Time max_time(std::numeric_limits<rcl_time_point_value_t>::max());
-  rclcpp::Time min_time(std::numeric_limits<rcl_time_point_value_t>::min());
   rclcpp::Duration one(1ns);
   rclcpp::Duration two(2ns);
 
-  // Cross min/max
+  // Cross max
   EXPECT_THROW(max_time + one, std::overflow_error);
-  EXPECT_THROW(min_time - one, std::underflow_error);
-  EXPECT_THROW(max_time - min_time, std::overflow_error);
-  EXPECT_THROW(min_time - max_time, std::underflow_error);
   EXPECT_THROW(rclcpp::Time(max_time) += one, std::overflow_error);
-  EXPECT_THROW(rclcpp::Time(min_time) -= one, std::underflow_error);
   EXPECT_NO_THROW(max_time - max_time);
-  EXPECT_NO_THROW(min_time - min_time);
 
-  // Cross zero in both directions
+  // Cross zero
   rclcpp::Time one_time(1);
-  EXPECT_NO_THROW(one_time - two);
-  EXPECT_NO_THROW(rclcpp::Time(one_time) -= two);
-
-  rclcpp::Time minus_one_time(-1);
-  EXPECT_NO_THROW(minus_one_time + two);
-  EXPECT_NO_THROW(rclcpp::Time(minus_one_time) += two);
-
-  EXPECT_NO_THROW(one_time - minus_one_time);
-  EXPECT_NO_THROW(minus_one_time - one_time);
+  EXPECT_THROW(one_time - two, std::runtime_error);
+  EXPECT_THROW(rclcpp::Time(one_time) -= two, std::runtime_error);
 
   rclcpp::Time two_time(2);
   EXPECT_NO_THROW(one_time - two_time);
@@ -432,41 +379,24 @@ TEST_F(TestTime, test_overflow_underflow_throws) {
   RCLCPP_EXPECT_THROW_EQ(
     test_time = rclcpp::Time(INT64_MAX) + rclcpp::Duration(1ns),
     std::overflow_error("addition leads to int64_t overflow"));
-  RCLCPP_EXPECT_THROW_EQ(
-    test_time = rclcpp::Time(INT64_MIN) + rclcpp::Duration(-1ns),
-    std::underflow_error("addition leads to int64_t underflow"));
 
   RCLCPP_EXPECT_THROW_EQ(
     test_time = rclcpp::Time(INT64_MAX) - rclcpp::Duration(-1ns),
     std::overflow_error("time subtraction leads to int64_t overflow"));
-  RCLCPP_EXPECT_THROW_EQ(
-    test_time = rclcpp::Time(INT64_MIN) - rclcpp::Duration(1ns),
-    std::underflow_error("time subtraction leads to int64_t underflow"));
 
   test_time = rclcpp::Time(INT64_MAX);
   RCLCPP_EXPECT_THROW_EQ(
     test_time += rclcpp::Duration(1ns),
     std::overflow_error("addition leads to int64_t overflow"));
-  test_time = rclcpp::Time(INT64_MIN);
-  RCLCPP_EXPECT_THROW_EQ(
-    test_time += rclcpp::Duration(-1ns),
-    std::underflow_error("addition leads to int64_t underflow"));
 
   test_time = rclcpp::Time(INT64_MAX);
   RCLCPP_EXPECT_THROW_EQ(
     test_time -= rclcpp::Duration(-1ns),
     std::overflow_error("time subtraction leads to int64_t overflow"));
-  test_time = rclcpp::Time(INT64_MIN);
-  RCLCPP_EXPECT_THROW_EQ(
-    test_time -= rclcpp::Duration(1ns),
-    std::underflow_error("time subtraction leads to int64_t underflow"));
 
   RCLCPP_EXPECT_THROW_EQ(
     test_time = rclcpp::Duration::from_nanoseconds(INT64_MAX) + rclcpp::Time(1),
     std::overflow_error("addition leads to int64_t overflow"));
-  RCLCPP_EXPECT_THROW_EQ(
-    test_time = rclcpp::Duration::from_nanoseconds(INT64_MIN) + rclcpp::Time(-1),
-    std::underflow_error("addition leads to int64_t underflow"));
 }
 
 class TestClockSleep : public ::testing::Test

rclcpp/test/rclcpp/test_timer.cpp

@@ -252,6 +252,28 @@ TEST_P(TestTimer, callback_with_timer) {
   EXPECT_FALSE(timer_ptr->is_ready());
 }
 
+TEST_P(TestTimer, callback_with_timer_info) {
+  rclcpp::TimerInfo info;
+  auto timer_callback = [&info](const rclcpp::TimerInfo & timer_info) {
+      info = timer_info;
+    };
+  switch (timer_type) {
+    case TimerType::WALL_TIMER:
+      timer = test_node->create_wall_timer(1ms, timer_callback);
+      break;
+    case TimerType::GENERIC_TIMER:
+      timer = test_node->create_timer(1ms, timer_callback);
+      break;
+  }
+  auto start = std::chrono::steady_clock::now();
+  while (info.actual_call_time.nanoseconds() == 0 &&
+    (std::chrono::steady_clock::now() - start) < std::chrono::milliseconds(100))
+  {
+    executor->spin_once(std::chrono::milliseconds(10));
+  }
+  EXPECT_GE(info.actual_call_time, info.expected_call_time);
+}
+
 TEST_P(TestTimer, callback_with_period_zero) {
   rclcpp::TimerBase * timer_ptr = nullptr;
   auto timer_callback = [&timer_ptr](rclcpp::TimerBase & timer) {

rclcpp/test/rclcpp/test_timers_manager.cpp

@@ -367,22 +367,23 @@ TEST_F(TestTimersManager, check_one_timer_cancel_doesnt_affect_other_timers)
   auto timers_manager = std::make_shared<TimersManager>(
     rclcpp::contexts::get_global_default_context());
 
-  size_t t1_runs = 0;
+  std::atomic<size_t> t1_runs = 0;
+  const size_t cancel_iter = 5;
   std::shared_ptr<TimerT> t1;
   // After a while cancel t1. Don't remove it though.
   // Simulates typical usage in a Node where a timer is cancelled but not removed,
   // since typical users aren't going to mess around with the timer manager.
   t1 = TimerT::make_shared(
     1ms,
-    [&t1_runs, &t1]() {
+    [ =, &t1_runs, &t1]() {
       t1_runs++;
-      if (t1_runs == 5) {
+      if (t1_runs == cancel_iter) {
         t1->cancel();
       }
     },
     rclcpp::contexts::get_global_default_context());
 
-  size_t t2_runs = 0;
+  std::atomic<size_t> t2_runs = 0;
   auto t2 = TimerT::make_shared(
     1ms,
     [&t2_runs]() {
@@ -397,11 +398,42 @@ TEST_F(TestTimersManager, check_one_timer_cancel_doesnt_affect_other_timers)
   // Start timers thread
   timers_manager->start();
 
-  std::this_thread::sleep_for(15ms);
+  // Wait for t1 to be canceled
+  auto loop_start_time = std::chrono::high_resolution_clock::now();
+  while (!t1->is_canceled()) {
+    auto now = std::chrono::high_resolution_clock::now();
+    if (now - loop_start_time >= std::chrono::seconds(30)) {
+      FAIL() << "timeout waiting for t1 to be canceled";
+      break;
+    }
+    std::this_thread::sleep_for(3ms);
+  }
+
+  EXPECT_TRUE(t1->is_canceled());
+  EXPECT_FALSE(t2->is_canceled());
+  EXPECT_EQ(t1_runs, cancel_iter);
+
+  // Verify that t2 is still being invoked
+  const size_t start_t2_runs = t2_runs;
+  const size_t num_t2_extra_runs = 6;
+  loop_start_time = std::chrono::high_resolution_clock::now();
+  while (t2_runs < start_t2_runs + num_t2_extra_runs) {
+    auto now = std::chrono::high_resolution_clock::now();
+    if (now - loop_start_time >= std::chrono::seconds(30)) {
+      FAIL() << "timeout waiting for t2 to do some runs";
+      break;
+    }
+    std::this_thread::sleep_for(3ms);
+  }
+
+  EXPECT_TRUE(t1->is_canceled());
+  EXPECT_FALSE(t2->is_canceled());
+  // t1 hasn't run since before
+  EXPECT_EQ(t1_runs, cancel_iter);
+  // t2 has run the expected additional number of times
+  EXPECT_GE(t2_runs, start_t2_runs + num_t2_extra_runs);
+  // the t2 runs are strictly more than the t1 runs
+  EXPECT_GT(t2_runs, t1_runs);
 
-  // t1 has stopped running
-  EXPECT_NE(t1_runs, t2_runs);
-  // Check that t2 has significantly more calls
-  EXPECT_LT(t1_runs + 5, t2_runs);
   timers_manager->stop();
 }

rclcpp/test/rclcpp/test_utilities.cpp

@@ -25,7 +25,20 @@
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/utilities.hpp"
 
+#ifdef __clang__
+#pragma clang diagnostic push
+#pragma clang diagnostic ignored "-Wordered-compare-function-pointers"
+#endif
+// TODO(ahcorde): the function mocking_utils::patch_and_return called with
+// rcl_logging_configure_with_output_handler is returning: "Comparison between pointer and integer"
+// Disabling this warning is fine for now.
+// Related issue https://github.com/ros2/rclcpp/issues/2488
 #include "../mocking_utils/patch.hpp"
+
+#ifdef __clang__
+#pragma clang diagnostic pop
+#endif
+
 #include "../utils/rclcpp_gtest_macros.hpp"
 
 TEST(TestUtilities, remove_ros_arguments) {

rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp

@@ -51,14 +51,14 @@ public:
   TestWaitable()
   : is_ready_(false) {}
 
-  void add_to_wait_set(rcl_wait_set_t *) override {}
+  void add_to_wait_set(rcl_wait_set_t &) override {}
 
-  bool is_ready(rcl_wait_set_t *) override {return is_ready_;}
+  bool is_ready(const rcl_wait_set_t &) override {return is_ready_;}
 
   std::shared_ptr<void> take_data() override {return nullptr;}
 
   void
-  execute(std::shared_ptr<void> & data) override {(void)data;}
+  execute(const std::shared_ptr<void> &) override {}
 
   void set_is_ready(bool value) {is_ready_ = value;}
 

rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp

@@ -50,14 +50,15 @@ class TestWaitable : public rclcpp::Waitable
 public:
   TestWaitable()
   : is_ready_(false) {}
-  void add_to_wait_set(rcl_wait_set_t *) override {}
 
-  bool is_ready(rcl_wait_set_t *) override {return is_ready_;}
+  void add_to_wait_set(rcl_wait_set_t &) override {}
+
+  bool is_ready(const rcl_wait_set_t &) override {return is_ready_;}
 
   std::shared_ptr<void> take_data() override {return nullptr;}
 
   void
-  execute(std::shared_ptr<void> & data) override {(void)data;}
+  execute(const std::shared_ptr<void> &) override {}
 
   void set_is_ready(bool value) {is_ready_ = value;}
 

rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp

@@ -50,19 +50,20 @@ class TestWaitable : public rclcpp::Waitable
 public:
   TestWaitable()
   : is_ready_(false), add_to_wait_set_(false) {}
-  void add_to_wait_set(rcl_wait_set_t *) override
+
+  void add_to_wait_set(rcl_wait_set_t &) override
   {
     if (!add_to_wait_set_) {
       throw std::runtime_error("waitable unexpectedly failed to be added to wait set");
     }
   }
 
-  bool is_ready(rcl_wait_set_t *) override {return is_ready_;}
+  bool is_ready(const rcl_wait_set_t &) override {return is_ready_;}
 
   std::shared_ptr<void> take_data() override {return nullptr;}
 
   void
-  execute(std::shared_ptr<void> & data) override {(void)data;}
+  execute(const std::shared_ptr<void> & data) override {(void)data;}
 
   void set_is_ready(bool value) {is_ready_ = value;}
 

rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp

@@ -50,14 +50,15 @@ class TestWaitable : public rclcpp::Waitable
 public:
   TestWaitable()
   : is_ready_(false) {}
-  void add_to_wait_set(rcl_wait_set_t *) override {}
 
-  bool is_ready(rcl_wait_set_t *) override {return is_ready_;}
+  void add_to_wait_set(rcl_wait_set_t &) override {}
+
+  bool is_ready(const rcl_wait_set_t &) override {return is_ready_;}
 
   std::shared_ptr<void> take_data() override {return nullptr;}
 
   void
-  execute(std::shared_ptr<void> & data) override {(void)data;}
+  execute(const std::shared_ptr<void> &) override {}
 
   void set_is_ready(bool value) {is_ready_ = value;}
 

rclcpp/test/rclcpp/waitables/test_intra_process_waitable.cpp

@@ -0,0 +1,46 @@
+// Copyright 2024 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "rclcpp/rclcpp.hpp"
+#include "test_msgs/msg/empty.hpp"
+
+#include "./waitable_test_helpers.hpp"
+
+class TestIntraProcessWaitable : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase() {rclcpp::init(0, nullptr);}
+  static void TearDownTestCase() {rclcpp::shutdown();}
+};
+
+TEST_F(TestIntraProcessWaitable, test_that_waitable_stays_ready_after_second_wait) {
+  auto node = std::make_shared<rclcpp::Node>(
+    "test_node",
+    rclcpp::NodeOptions().use_intra_process_comms(true));
+
+  using test_msgs::msg::Empty;
+  auto sub = node->create_subscription<Empty>("test_topic", 10, [](const Empty &) {});
+  auto pub = node->create_publisher<Empty>("test_topic", 10);
+
+  auto make_sub_intra_process_waitable_ready = [pub]() {
+      pub->publish(Empty());
+    };
+
+  rclcpp::test::waitables::do_test_that_waitable_stays_ready_after_second_wait(
+    sub->get_intra_process_waitable(),
+    make_sub_intra_process_waitable_ready,
+    true /* expected_to_stay_ready */);
+}

rclcpp/test/rclcpp/waitables/waitable_test_helpers.hpp

@@ -0,0 +1,117 @@
+// Copyright 2024 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAITABLES__WAITABLE_TEST_HELPERS_HPP_
+#define RCLCPP__WAITABLES__WAITABLE_TEST_HELPERS_HPP_
+
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <functional>
+#include <memory>
+
+#include <rclcpp/wait_set.hpp>
+
+namespace rclcpp
+{
+namespace test
+{
+namespace waitables
+{
+
+/// Test that a given waitable is ready after a second wait.
+/**
+ * The purpose of this test is to check that a waitable will remain ready
+ * on subsequent wait calls, if that is the expected behavior.
+ * Not all waitables should remain ready after a wait call, which can be
+ * expressed in the expected_to_stay_ready argument which defaults to true.
+ * If set to false, it will check that it is not ready after a second wait, as
+ * well as some other parts of the test.
+ *
+ * The given waitable should:
+ *
+ *   - not be ready initially
+ *   - not be ready after being waited on (and timing out)
+ *   - should become ready after the make_waitable_ready method is called
+ *   - may or may not be ready at this point
+ *   - should be ready after waiting on it, within the wait_timeout
+ *   - should be ready still after a second wait (unless expected_to_stay_ready = false)
+ *   - if expected_to_stay_ready, should become not ready after a take_data/execute
+ */
+template<typename WaitableT>
+void
+do_test_that_waitable_stays_ready_after_second_wait(
+  const std::shared_ptr<WaitableT> & waitable,
+  std::function<void()> make_waitable_ready,
+  bool expected_to_stay_ready = true,
+  std::chrono::nanoseconds wait_timeout = std::chrono::seconds(5))
+{
+  rclcpp::WaitSet wait_set;
+  wait_set.add_waitable(waitable);
+
+  // not ready initially
+  EXPECT_FALSE(waitable->is_ready(wait_set.get_rcl_wait_set()))
+    << "waitable is unexpectedly ready before waiting";
+
+  // not ready after a wait that timesout
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(0));
+    EXPECT_EQ(wait_result.kind(), rclcpp::WaitResultKind::Timeout)
+      << "wait set did not timeout as expected";
+    EXPECT_FALSE(waitable->is_ready(wait_set.get_rcl_wait_set()))
+      << "waitable is unexpectedly ready after waiting, but before making ready";
+  }
+
+  // make it ready and wait on it
+  make_waitable_ready();
+  {
+    auto wait_result = wait_set.wait(wait_timeout);
+    EXPECT_EQ(wait_result.kind(), rclcpp::WaitResultKind::Ready)
+      << "wait set was not ready after the waitable should have been made ready";
+    EXPECT_TRUE(waitable->is_ready(wait_set.get_rcl_wait_set()))
+      << "waitable is unexpectedly not ready after making it ready and waiting";
+  }
+
+  // wait again, and see that it is ready as expected or not expected
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(0));
+    if (expected_to_stay_ready) {
+      EXPECT_EQ(wait_result.kind(), rclcpp::WaitResultKind::Ready)
+        << "wait set was not ready on a second wait on the waitable";
+      EXPECT_TRUE(waitable->is_ready(wait_set.get_rcl_wait_set()))
+        << "waitable unexpectedly not ready after second wait";
+    } else {
+      EXPECT_EQ(wait_result.kind(), rclcpp::WaitResultKind::Timeout)
+        << "wait set did not time out after the waitable should have no longer been ready";
+      EXPECT_FALSE(waitable->is_ready(wait_set.get_rcl_wait_set()))
+        << "waitable was ready after waiting a second time, which was not expected";
+    }
+  }
+
+  // if expected_to_stay_ready, check that take_data/execute makes it not ready
+  if (expected_to_stay_ready) {
+    waitable->execute(waitable->take_data());
+    auto wait_result = wait_set.wait(std::chrono::seconds(0));
+    EXPECT_EQ(wait_result.kind(), rclcpp::WaitResultKind::Timeout)
+      << "wait set did not time out after the waitable should have no longer been ready";
+    EXPECT_FALSE(waitable->is_ready(wait_set.get_rcl_wait_set()))
+      << "waitable was unexpectedly ready after a take_data and execute";
+  }
+}
+
+}  // namespace waitables
+}  // namespace test
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAITABLES__WAITABLE_TEST_HELPERS_HPP_

rclcpp_action/CHANGELOG.rst

@@ -3,6 +3,46 @@ Changelog for package rclcpp_action
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+28.3.0 (2024-06-17)
+-------------------
+* Add 'mimick' label to tests which use Mimick (`#2516 <https://github.com/ros2/rclcpp/issues/2516>`_)
+* Contributors: Scott K Logan
+
+28.2.0 (2024-04-26)
+-------------------
+
+28.1.0 (2024-04-16)
+-------------------
+* Remove references to index.ros.org. (`#2504 <https://github.com/ros2/rclcpp/issues/2504>`_)
+* Contributors: Chris Lalancette
+
+28.0.1 (2024-04-16)
+-------------------
+* Callback after cancel (`#2281 <https://github.com/ros2/rclcpp/issues/2281>`_)
+  * feat(Client): Added function to stop callbacks of a goal handle
+  This function allows us to drop the handle in a locked context.
+  If we do not do this within a lock, there will be a race condition between
+  the deletion of the shared_ptr of the handle and the result / feedback
+  callbacks.
+  * fix: make Client goal handle recursive
+  This fixes deadlocks due to release of goal handles in callbacks etc.
+  * fix(ActionGoalClient): Fixed memory leak for nominal case
+  This fixes a memory leak due to a self reference in the ClientGoalHandle.
+  Note, this fix will only work, if the ClientGoalHandle ever receives
+  a result callback.
+  * doc: Updated documentation of rclcpp_action::Client::async_send_goal
+  * docs: Made the async_send_goal documentation more explicit
+  Co-authored-by: Janosch Machowinski <J.Machowinski@cellumation.com>
+* Remake of "fix: Fixed race condition in action server between is_ready and take" (`#2495 <https://github.com/ros2/rclcpp/issues/2495>`_)
+  Some background information: is_ready, take_data and execute data
+  may be called from different threads in any order. The code in the old
+  state expected them to be called in series, without interruption.
+  This lead to multiple race conditions, as the state of the pimpl objects
+  was altered by the three functions in a non thread safe way.
+  Co-authored-by: Janosch Machowinski <j.machowinski@nospam.org>
+* update rclcpp::Waitable API to use references and const (`#2467 <https://github.com/ros2/rclcpp/issues/2467>`_)
+* Contributors: William Woodall, jmachowinski
+
 28.0.0 (2024-03-28)
 -------------------
 * Do not generate the exception when action service response timeout. (`#2464 <https://github.com/ros2/rclcpp/issues/2464>`_)

rclcpp_action/CMakeLists.txt

@@ -80,6 +80,7 @@ if(BUILD_TESTING)
   add_subdirectory(test/benchmark)
 
   ament_add_gtest(test_client test/test_client.cpp TIMEOUT 180)
+  ament_add_test_label(test_client mimick)
   if(TARGET test_client)
     target_link_libraries(test_client
       ${PROJECT_NAME}
@@ -93,6 +94,7 @@ if(BUILD_TESTING)
   endif()
 
   ament_add_gtest(test_server test/test_server.cpp TIMEOUT 180)
+  ament_add_test_label(test_server mimick)
   if(TARGET test_server)
     target_link_libraries(test_server
       ${PROJECT_NAME}
@@ -104,6 +106,7 @@ if(BUILD_TESTING)
   endif()
 
   ament_add_gtest(test_server_goal_handle test/test_server_goal_handle.cpp)
+  ament_add_test_label(test_server_goal_handle mimick)
   if(TARGET test_server_goal_handle)
     target_link_libraries(test_server_goal_handle
       ${PROJECT_NAME}

rclcpp_action/README.md

@@ -2,7 +2,7 @@
 
 Adds action APIs for C++.
 
-The link to the latest rclcpp_action API documentation, which includes a complete list of its main components and features, can be found on the rclcpp_action package info page, at the [ROS Index](https://index.ros.org/p/rclcpp_action/).
+The link to the latest rclcpp_action API documentation, which includes a complete list of its main components and features, can be found on the [rclcpp_action package info page](https://docs.ros.org/en/rolling/p/rclcpp_action).
 For more information about Actions in ROS 2, see the [design document](http://design.ros2.org/articles/actions.html).
 
 ## Quality Declaration

rclcpp_action/include/rclcpp_action/client.hpp

@@ -122,12 +122,12 @@ public:
   /// \internal
   RCLCPP_ACTION_PUBLIC
   void
-  add_to_wait_set(rcl_wait_set_t * wait_set) override;
+  add_to_wait_set(rcl_wait_set_t & wait_set) override;
 
   /// \internal
   RCLCPP_ACTION_PUBLIC
   bool
-  is_ready(rcl_wait_set_t * wait_set) override;
+  is_ready(const rcl_wait_set_t & wait_set) override;
 
   /// \internal
   RCLCPP_ACTION_PUBLIC
@@ -142,7 +142,7 @@ public:
   /// \internal
   RCLCPP_ACTION_PUBLIC
   void
-  execute(std::shared_ptr<void> & data) override;
+  execute(const std::shared_ptr<void> & data) override;
 
   /// \internal
   /// Set a callback to be called when action client entities have an event
@@ -405,12 +405,22 @@ public:
 
   /// Send an action goal and asynchronously get the result.
   /**
-   * If the goal is accepted by an action server, the returned future is set to a `ClientGoalHandle`.
+   * If the goal is accepted by an action server, the returned future is set to a `GoalHandle::SharedPtr`.
    * If the goal is rejected by an action server, then the future is set to a `nullptr`.
    *
-   * The returned goal handle is used to monitor the status of the goal and get the final result.
-   * It is valid as long as you hold a reference to the shared pointer or until the
-   * rclcpp_action::Client is destroyed at which point the goal status will become UNKNOWN.
+   * The goal handle in the future is used to monitor the status of the goal and get the final result.
+   *
+   * If callbacks were set in @param options, you will receive callbacks, as long as you hold a reference
+   * to the shared pointer contained in the returned future, or rclcpp_action::Client is destroyed. Dropping
+   * the shared pointer to the goal handle will not cancel the goal. In order to cancel it, you must explicitly
+   * call async_cancel_goal.
+   *
+   * WARNING this method has inconsistent behaviour and a memory leak bug.
+   * If you set the result callback in @param options, the handle will be self referencing, and you will receive
+   * callbacks even though you do not hold a reference to the shared pointer. In this case, the self reference will
+   * be deleted if the result callback was received. If there is no result callback, there will be a memory leak.
+   *
+   * To prevent the memory leak, you may call stop_callbacks() explicit. This will delete the self reference.
    *
    * \param[in] goal The goal request.
    * \param[in] options Options for sending the goal request. Contains references to callbacks for
@@ -448,7 +458,7 @@ public:
         std::shared_ptr<GoalHandle> goal_handle(
           new GoalHandle(goal_info, options.feedback_callback, options.result_callback));
         {
-          std::lock_guard<std::mutex> guard(goal_handles_mutex_);
+          std::lock_guard<std::recursive_mutex> guard(goal_handles_mutex_);
           goal_handles_[goal_handle->get_goal_id()] = goal_handle;
         }
         promise->set_value(goal_handle);
@@ -466,7 +476,7 @@ public:
     // To prevent the list from growing out of control, forget about any goals
     // with no more user references
     {
-      std::lock_guard<std::mutex> guard(goal_handles_mutex_);
+      std::lock_guard<std::recursive_mutex> guard(goal_handles_mutex_);
       auto goal_handle_it = goal_handles_.begin();
       while (goal_handle_it != goal_handles_.end()) {
         if (!goal_handle_it->second.lock()) {
@@ -496,7 +506,7 @@ public:
     typename GoalHandle::SharedPtr goal_handle,
     ResultCallback result_callback = nullptr)
   {
-    std::lock_guard<std::mutex> lock(goal_handles_mutex_);
+    std::lock_guard<std::recursive_mutex> lock(goal_handles_mutex_);
     if (goal_handles_.count(goal_handle->get_goal_id()) == 0) {
       throw exceptions::UnknownGoalHandleError();
     }
@@ -531,7 +541,7 @@ public:
     typename GoalHandle::SharedPtr goal_handle,
     CancelCallback cancel_callback = nullptr)
   {
-    std::lock_guard<std::mutex> lock(goal_handles_mutex_);
+    std::lock_guard<std::recursive_mutex> lock(goal_handles_mutex_);
     if (goal_handles_.count(goal_handle->get_goal_id()) == 0) {
       throw exceptions::UnknownGoalHandleError();
     }
@@ -562,6 +572,63 @@ public:
     return async_cancel(cancel_request, cancel_callback);
   }
 
+  /// Stops the callbacks for the goal in a thread safe way
+  /**
+   * This will NOT cancel the goal, it will only stop the callbacks.
+   *
+   * After the call to this function, it is guaranteed that there
+   * will be no more callbacks from the goal. This is not guaranteed
+   * if multiple threads are involved, and the goal_handle is just
+   * dropped.
+   *
+   * \param[in] goal_handle The goal were the callbacks shall be stopped
+   */
+  void stop_callbacks(typename GoalHandle::SharedPtr goal_handle)
+  {
+    goal_handle->set_feedback_callback(typename GoalHandle::FeedbackCallback());
+    goal_handle->set_result_callback(typename GoalHandle::ResultCallback());
+
+    std::lock_guard<std::recursive_mutex> guard(goal_handles_mutex_);
+    const GoalUUID & goal_id = goal_handle->get_goal_id();
+    auto it = goal_handles_.find(goal_id);
+    if (goal_handles_.end() == it) {
+      // someone else already deleted the entry
+      // e.g. the result callback
+      RCLCPP_DEBUG(
+        this->get_logger(),
+        "Given goal is unknown. Ignoring...");
+      return;
+    }
+    goal_handles_.erase(it);
+  }
+
+  /// Stops the callbacks for the goal in a thread safe way
+  /**
+   * For futher information see stop_callbacks(typename GoalHandle::SharedPtr goal_handle)
+   */
+  void stop_callbacks(const GoalUUID & goal_id)
+  {
+    typename GoalHandle::SharedPtr goal_handle;
+    {
+      std::lock_guard<std::recursive_mutex> guard(goal_handles_mutex_);
+      auto it = goal_handles_.find(goal_id);
+      if (goal_handles_.end() == it) {
+        // someone else already deleted the entry
+        // e.g. the result callback
+        RCLCPP_DEBUG(
+          this->get_logger(),
+          "Given goal is unknown. Ignoring...");
+        return;
+      }
+
+      goal_handle = it->lock();
+    }
+
+    if (goal_handle) {
+      stop_callbacks(goal_handle);
+    }
+  }
+
   /// Asynchronously request all goals at or before a specified time be canceled.
   /**
    * \param[in] stamp The timestamp for the cancel goal request.
@@ -590,7 +657,7 @@ public:
   virtual
   ~Client()
   {
-    std::lock_guard<std::mutex> guard(goal_handles_mutex_);
+    std::lock_guard<std::recursive_mutex> guard(goal_handles_mutex_);
     auto it = goal_handles_.begin();
     while (it != goal_handles_.end()) {
       typename GoalHandle::SharedPtr goal_handle = it->second.lock();
@@ -637,7 +704,7 @@ private:
   void
   handle_feedback_message(std::shared_ptr<void> message) override
   {
-    std::lock_guard<std::mutex> guard(goal_handles_mutex_);
+    std::lock_guard<std::recursive_mutex> guard(goal_handles_mutex_);
     using FeedbackMessage = typename ActionT::Impl::FeedbackMessage;
     typename FeedbackMessage::SharedPtr feedback_message =
       std::static_pointer_cast<FeedbackMessage>(message);
@@ -674,7 +741,7 @@ private:
   void
   handle_status_message(std::shared_ptr<void> message) override
   {
-    std::lock_guard<std::mutex> guard(goal_handles_mutex_);
+    std::lock_guard<std::recursive_mutex> guard(goal_handles_mutex_);
     using GoalStatusMessage = typename ActionT::Impl::GoalStatusMessage;
     auto status_message = std::static_pointer_cast<GoalStatusMessage>(message);
     for (const GoalStatus & status : status_message->status_list) {
@@ -723,7 +790,7 @@ private:
           wrapped_result.goal_id = goal_handle->get_goal_id();
           wrapped_result.code = static_cast<ResultCode>(result_response->status);
           goal_handle->set_result(wrapped_result);
-          std::lock_guard<std::mutex> lock(goal_handles_mutex_);
+          std::lock_guard<std::recursive_mutex> lock(goal_handles_mutex_);
           goal_handles_.erase(goal_handle->get_goal_id());
         });
     } catch (rclcpp::exceptions::RCLError & ex) {
@@ -755,7 +822,7 @@ private:
   }
 
   std::map<GoalUUID, typename GoalHandle::WeakPtr> goal_handles_;
-  std::mutex goal_handles_mutex_;
+  std::recursive_mutex goal_handles_mutex_;
 };
 }  // namespace rclcpp_action
 

rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp

@@ -75,6 +75,7 @@ ClientGoalHandle<ActionT>::set_result(const WrappedResult & wrapped_result)
   result_promise_.set_value(wrapped_result);
   if (result_callback_) {
     result_callback_(wrapped_result);
+    result_callback_ = ResultCallback();
   }
 }
 

rclcpp_action/include/rclcpp_action/server.hpp

@@ -22,6 +22,7 @@
 #include <unordered_map>
 #include <utility>
 
+#include "action_msgs/srv/cancel_goal.hpp"
 #include "rcl/event_callback.h"
 #include "rcl_action/action_server.h"
 #include "rosidl_runtime_c/action_type_support_struct.h"
@@ -77,6 +78,7 @@ public:
     GoalService,
     ResultService,
     CancelService,
+    Expired,
   };
 
   RCLCPP_ACTION_PUBLIC
@@ -119,13 +121,13 @@ public:
   /// \internal
   RCLCPP_ACTION_PUBLIC
   void
-  add_to_wait_set(rcl_wait_set_t * wait_set) override;
+  add_to_wait_set(rcl_wait_set_t & wait_set) override;
 
   /// Return true if any entity belonging to the action server is ready to be executed.
   /// \internal
   RCLCPP_ACTION_PUBLIC
   bool
-  is_ready(rcl_wait_set_t *) override;
+  is_ready(const rcl_wait_set_t & wait_set) override;
 
   RCLCPP_ACTION_PUBLIC
   std::shared_ptr<void>
@@ -139,7 +141,7 @@ public:
   /// \internal
   RCLCPP_ACTION_PUBLIC
   void
-  execute(std::shared_ptr<void> & data) override;
+  execute(const std::shared_ptr<void> & data) override;
 
   /// \internal
   /// Set a callback to be called when action server entities have an event
@@ -279,19 +281,29 @@ private:
   /// \internal
   RCLCPP_ACTION_PUBLIC
   void
-  execute_goal_request_received(std::shared_ptr<void> & data);
+  execute_goal_request_received(
+    rcl_ret_t ret,
+    rcl_action_goal_info_t goal_info,
+    rmw_request_id_t request_header,
+    std::shared_ptr<void> message);
 
   /// Handle a request to cancel goals on the server
   /// \internal
   RCLCPP_ACTION_PUBLIC
   void
-  execute_cancel_request_received(std::shared_ptr<void> & data);
+  execute_cancel_request_received(
+    rcl_ret_t ret,
+    std::shared_ptr<action_msgs::srv::CancelGoal::Request> request,
+    rmw_request_id_t request_header);
 
   /// Handle a request to get the result of an action
   /// \internal
   RCLCPP_ACTION_PUBLIC
   void
-  execute_result_request_received(std::shared_ptr<void> & data);
+  execute_result_request_received(
+    rcl_ret_t ret,
+    std::shared_ptr<void> result_request,
+    rmw_request_id_t request_header);
 
   /// Handle a timeout indicating a completed goal should be forgotten by the server
   /// \internal
@@ -345,7 +357,8 @@ public:
 
   /// Signature of a callback that accepts or rejects goal requests.
   using GoalCallback = std::function<GoalResponse(
-        const GoalUUID &, std::shared_ptr<const typename ActionT::Goal>)>;
+        const GoalUUID &,
+        std::shared_ptr<const typename ActionT::Goal>)>;
   /// Signature of a callback that accepts or rejects requests to cancel a goal.
   using CancelCallback = std::function<CancelResponse(std::shared_ptr<ServerGoalHandle<ActionT>>)>;
   /// Signature of a callback that is used to notify when the goal has been accepted.
@@ -455,7 +468,8 @@ protected:
   void
   call_goal_accepted_callback(
     std::shared_ptr<rcl_action_goal_handle_t> rcl_goal_handle,
-    GoalUUID uuid, std::shared_ptr<void> goal_request_message) override
+    GoalUUID uuid,
+    std::shared_ptr<void> goal_request_message) override
   {
     std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle;
     std::weak_ptr<Server<ActionT>> weak_this = this->shared_from_this();

rclcpp_action/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp_action</name>
-  <version>28.0.0</version>
+  <version>28.3.0</version>
   <description>Adds action APIs for C++.</description>
 
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>

rclcpp_action/src/client.cpp

@@ -19,6 +19,7 @@
 #include <string>
 #include <tuple>
 #include <utility>
+#include <variant>
 
 #include "rcl_action/action_client.h"
 #include "rcl_action/wait.h"
@@ -31,6 +32,67 @@
 namespace rclcpp_action
 {
 
+struct ClientBaseData
+{
+  struct FeedbackReadyData
+  {
+    FeedbackReadyData(rcl_ret_t retIn, std::shared_ptr<void> msg)
+    : ret(retIn), feedback_message(msg) {}
+    rcl_ret_t ret;
+    std::shared_ptr<void> feedback_message;
+  };
+  struct StatusReadyData
+  {
+    StatusReadyData(rcl_ret_t retIn, std::shared_ptr<void> msg)
+    : ret(retIn), status_message(msg) {}
+    rcl_ret_t ret;
+    std::shared_ptr<void> status_message;
+  };
+  struct GoalResponseData
+  {
+    GoalResponseData(rcl_ret_t retIn, rmw_request_id_t header, std::shared_ptr<void> response)
+    : ret(retIn), response_header(header), goal_response(response) {}
+    rcl_ret_t ret;
+    rmw_request_id_t response_header;
+    std::shared_ptr<void> goal_response;
+  };
+  struct CancelResponseData
+  {
+    CancelResponseData(rcl_ret_t retIn, rmw_request_id_t header, std::shared_ptr<void> response)
+    : ret(retIn), response_header(header), cancel_response(response) {}
+    rcl_ret_t ret;
+    rmw_request_id_t response_header;
+    std::shared_ptr<void> cancel_response;
+  };
+  struct ResultResponseData
+  {
+    ResultResponseData(rcl_ret_t retIn, rmw_request_id_t header, std::shared_ptr<void> response)
+    : ret(retIn), response_header(header), result_response(response) {}
+    rcl_ret_t ret;
+    rmw_request_id_t response_header;
+    std::shared_ptr<void> result_response;
+  };
+
+  std::variant<
+    FeedbackReadyData,
+    StatusReadyData,
+    GoalResponseData,
+    CancelResponseData,
+    ResultResponseData
+  > data;
+
+  explicit ClientBaseData(FeedbackReadyData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ClientBaseData(StatusReadyData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ClientBaseData(GoalResponseData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ClientBaseData(CancelResponseData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ClientBaseData(ResultResponseData && data_in)
+  : data(std::move(data_in)) {}
+};
+
 class ClientBaseImpl
 {
 public:
@@ -94,11 +156,11 @@ public:
   size_t num_clients{0u};
   size_t num_services{0u};
 
-  bool is_feedback_ready{false};
-  bool is_status_ready{false};
-  bool is_goal_response_ready{false};
-  bool is_cancel_response_ready{false};
-  bool is_result_response_ready{false};
+  // Lock for action_client_
+  std::recursive_mutex action_client_mutex_;
+
+  // next ready event for taking, will be set by is_ready and will be processed by take_data
+  std::atomic<size_t> next_ready_event;
 
   rclcpp::Context::SharedPtr context_;
   rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
@@ -142,6 +204,7 @@ bool
 ClientBase::action_server_is_ready() const
 {
   bool is_ready;
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
   rcl_ret_t ret = rcl_action_server_is_available(
     this->pimpl_->node_handle.get(),
     this->pimpl_->client_handle.get(),
@@ -253,35 +316,69 @@ ClientBase::get_number_of_ready_services()
 }
 
 void
-ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set)
+ClientBase::add_to_wait_set(rcl_wait_set_t & wait_set)
 {
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
   rcl_ret_t ret = rcl_action_wait_set_add_action_client(
-    wait_set, pimpl_->client_handle.get(), nullptr, nullptr);
+    &wait_set, pimpl_->client_handle.get(), nullptr, nullptr);
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret, "ClientBase::add_to_wait_set() failed");
   }
 }
 
 bool
-ClientBase::is_ready(rcl_wait_set_t * wait_set)
+ClientBase::is_ready(const rcl_wait_set_t & wait_set)
 {
-  rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready(
-    wait_set, pimpl_->client_handle.get(),
-    &pimpl_->is_feedback_ready,
-    &pimpl_->is_status_ready,
-    &pimpl_->is_goal_response_ready,
-    &pimpl_->is_cancel_response_ready,
-    &pimpl_->is_result_response_ready);
-  if (RCL_RET_OK != ret) {
-    rclcpp::exceptions::throw_from_rcl_error(
-      ret, "failed to check for any ready entities");
+  bool is_feedback_ready{false};
+  bool is_status_ready{false};
+  bool is_goal_response_ready{false};
+  bool is_cancel_response_ready{false};
+  bool is_result_response_ready{false};
+
+  rcl_ret_t ret;
+  {
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+    ret = rcl_action_client_wait_set_get_entities_ready(
+      &wait_set, pimpl_->client_handle.get(),
+      &is_feedback_ready,
+      &is_status_ready,
+      &is_goal_response_ready,
+      &is_cancel_response_ready,
+      &is_result_response_ready);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(
+        ret, "failed to check for any ready entities");
+    }
   }
-  return
-    pimpl_->is_feedback_ready ||
-    pimpl_->is_status_ready ||
-    pimpl_->is_goal_response_ready ||
-    pimpl_->is_cancel_response_ready ||
-    pimpl_->is_result_response_ready;
+
+  pimpl_->next_ready_event = std::numeric_limits<size_t>::max();
+
+  if (is_feedback_ready) {
+    pimpl_->next_ready_event = static_cast<size_t>(EntityType::FeedbackSubscription);
+    return true;
+  }
+
+  if (is_status_ready) {
+    pimpl_->next_ready_event = static_cast<size_t>(EntityType::StatusSubscription);
+    return true;
+  }
+
+  if (is_goal_response_ready) {
+    pimpl_->next_ready_event = static_cast<size_t>(EntityType::GoalClient);
+    return true;
+  }
+
+  if (is_result_response_ready) {
+    pimpl_->next_ready_event = static_cast<size_t>(EntityType::ResultClient);
+    return true;
+  }
+
+  if (is_cancel_response_ready) {
+    pimpl_->next_ready_event = static_cast<size_t>(EntityType::CancelClient);
+    return true;
+  }
+
+  return false;
 }
 
 void
@@ -432,7 +529,6 @@ ClientBase::set_callback_to_entity(
       }
     };
 
-
   // 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.
@@ -550,140 +646,155 @@ ClientBase::clear_on_ready_callback()
 std::shared_ptr<void>
 ClientBase::take_data()
 {
-  if (pimpl_->is_feedback_ready) {
-    std::shared_ptr<void> feedback_message = this->create_feedback_message();
-    rcl_ret_t ret = rcl_action_take_feedback(
-      pimpl_->client_handle.get(), feedback_message.get());
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(
-        ret, feedback_message));
-  } else if (pimpl_->is_status_ready) {
-    std::shared_ptr<void> status_message = this->create_status_message();
-    rcl_ret_t ret = rcl_action_take_status(
-      pimpl_->client_handle.get(), status_message.get());
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(
-        ret, status_message));
-  } else if (pimpl_->is_goal_response_ready) {
-    rmw_request_id_t response_header;
-    std::shared_ptr<void> goal_response = this->create_goal_response();
-    rcl_ret_t ret = rcl_action_take_goal_response(
-      pimpl_->client_handle.get(), &response_header, goal_response.get());
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(
-        ret, response_header, goal_response));
-  } else if (pimpl_->is_result_response_ready) {
-    rmw_request_id_t response_header;
-    std::shared_ptr<void> result_response = this->create_result_response();
-    rcl_ret_t ret = rcl_action_take_result_response(
-      pimpl_->client_handle.get(), &response_header, result_response.get());
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(
-        ret, response_header, result_response));
-  } else if (pimpl_->is_cancel_response_ready) {
-    rmw_request_id_t response_header;
-    std::shared_ptr<void> cancel_response = this->create_cancel_response();
-    rcl_ret_t ret = rcl_action_take_cancel_response(
-      pimpl_->client_handle.get(), &response_header, cancel_response.get());
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(
-        ret, response_header, cancel_response));
-  } else {
+  // next_ready_event is an atomic, caching localy
+  size_t next_ready_event = pimpl_->next_ready_event.exchange(std::numeric_limits<uint32_t>::max());
+
+  if (next_ready_event == std::numeric_limits<uint32_t>::max()) {
     throw std::runtime_error("Taking data from action client but nothing is ready");
   }
+
+  return take_data_by_entity_id(next_ready_event);
 }
 
 std::shared_ptr<void>
 ClientBase::take_data_by_entity_id(size_t id)
 {
+  std::shared_ptr<ClientBaseData> data_ptr;
+  rcl_ret_t ret;
+
   // Mark as ready the entity from which we want to take data
   switch (static_cast<EntityType>(id)) {
     case EntityType::GoalClient:
-      pimpl_->is_goal_response_ready = true;
+      {
+        rmw_request_id_t response_header;
+        std::shared_ptr<void> goal_response;
+        {
+          std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+
+          goal_response = this->create_goal_response();
+          ret = rcl_action_take_goal_response(
+            pimpl_->client_handle.get(), &response_header, goal_response.get());
+        }
+        data_ptr = std::make_shared<ClientBaseData>(
+          ClientBaseData::GoalResponseData(
+            ret, response_header, goal_response));
+      }
       break;
     case EntityType::ResultClient:
-      pimpl_->is_result_response_ready = true;
+      {
+        rmw_request_id_t response_header;
+        std::shared_ptr<void> result_response;
+        {
+          std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+          result_response = this->create_result_response();
+          ret = rcl_action_take_result_response(
+            pimpl_->client_handle.get(), &response_header, result_response.get());
+        }
+        data_ptr =
+          std::make_shared<ClientBaseData>(
+          ClientBaseData::ResultResponseData(
+            ret, response_header, result_response));
+      }
       break;
     case EntityType::CancelClient:
-      pimpl_->is_cancel_response_ready = true;
+      {
+        rmw_request_id_t response_header;
+        std::shared_ptr<void> cancel_response;
+        {
+          std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+          cancel_response = this->create_cancel_response();
+          ret = rcl_action_take_cancel_response(
+            pimpl_->client_handle.get(), &response_header, cancel_response.get());
+        }
+        data_ptr =
+          std::make_shared<ClientBaseData>(
+          ClientBaseData::CancelResponseData(
+            ret, response_header, cancel_response));
+      }
       break;
     case EntityType::FeedbackSubscription:
-      pimpl_->is_feedback_ready = true;
+      {
+        std::shared_ptr<void> feedback_message;
+        {
+          std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+          feedback_message = this->create_feedback_message();
+          ret = rcl_action_take_feedback(
+            pimpl_->client_handle.get(), feedback_message.get());
+        }
+        data_ptr =
+          std::make_shared<ClientBaseData>(
+          ClientBaseData::FeedbackReadyData(
+            ret, feedback_message));
+      }
       break;
     case EntityType::StatusSubscription:
-      pimpl_->is_status_ready = true;
+      {
+        std::shared_ptr<void> status_message;
+        {
+          std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+          status_message = this->create_status_message();
+          ret = rcl_action_take_status(
+            pimpl_->client_handle.get(), status_message.get());
+        }
+        data_ptr =
+          std::make_shared<ClientBaseData>(
+          ClientBaseData::StatusReadyData(
+            ret, status_message));
+      }
       break;
   }
 
-  return take_data();
+  return std::static_pointer_cast<void>(data_ptr);
 }
 
 void
-ClientBase::execute(std::shared_ptr<void> & data)
+ClientBase::execute(const std::shared_ptr<void> & data_in)
 {
-  if (!data) {
-    throw std::runtime_error("'data' is empty");
+  if (!data_in) {
+    throw std::invalid_argument("'data_in' is unexpectedly empty");
   }
 
-  if (pimpl_->is_feedback_ready) {
-    auto shared_ptr = std::static_pointer_cast<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(data);
-    auto ret = std::get<0>(*shared_ptr);
-    pimpl_->is_feedback_ready = false;
-    if (RCL_RET_OK == ret) {
-      auto feedback_message = std::get<1>(*shared_ptr);
-      this->handle_feedback_message(feedback_message);
-    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking feedback");
-    }
-  } else if (pimpl_->is_status_ready) {
-    auto shared_ptr = std::static_pointer_cast<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(data);
-    auto ret = std::get<0>(*shared_ptr);
-    pimpl_->is_status_ready = false;
-    if (RCL_RET_OK == ret) {
-      auto status_message = std::get<1>(*shared_ptr);
-      this->handle_status_message(status_message);
-    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking status");
-    }
-  } else if (pimpl_->is_goal_response_ready) {
-    auto shared_ptr = std::static_pointer_cast<
-      std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(data);
-    auto ret = std::get<0>(*shared_ptr);
-    pimpl_->is_goal_response_ready = false;
-    if (RCL_RET_OK == ret) {
-      auto response_header = std::get<1>(*shared_ptr);
-      auto goal_response = std::get<2>(*shared_ptr);
-      this->handle_goal_response(response_header, goal_response);
-    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking goal response");
-    }
-  } else if (pimpl_->is_result_response_ready) {
-    auto shared_ptr = std::static_pointer_cast<
-      std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(data);
-    auto ret = std::get<0>(*shared_ptr);
-    pimpl_->is_result_response_ready = false;
-    if (RCL_RET_OK == ret) {
-      auto response_header = std::get<1>(*shared_ptr);
-      auto result_response = std::get<2>(*shared_ptr);
-      this->handle_result_response(response_header, result_response);
-    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking result response");
-    }
-  } else if (pimpl_->is_cancel_response_ready) {
-    auto shared_ptr = std::static_pointer_cast<
-      std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(data);
-    auto ret = std::get<0>(*shared_ptr);
-    pimpl_->is_cancel_response_ready = false;
-    if (RCL_RET_OK == ret) {
-      auto response_header = std::get<1>(*shared_ptr);
-      auto cancel_response = std::get<2>(*shared_ptr);
-      this->handle_cancel_response(response_header, cancel_response);
-    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking cancel response");
-    }
-  } else {
-    throw std::runtime_error("Executing action client but nothing is ready");
-  }
+  std::shared_ptr<ClientBaseData> data_ptr = std::static_pointer_cast<ClientBaseData>(data_in);
+
+  std::visit(
+    [&](auto && data) -> void {
+      using T = std::decay_t<decltype(data)>;
+      if constexpr (std::is_same_v<T, ClientBaseData::FeedbackReadyData>) {
+        if (RCL_RET_OK == data.ret) {
+          this->handle_feedback_message(data.feedback_message);
+        } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) {
+          rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking feedback");
+        }
+      }
+      if constexpr (std::is_same_v<T, ClientBaseData::StatusReadyData>) {
+        if (RCL_RET_OK == data.ret) {
+          this->handle_status_message(data.status_message);
+        } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) {
+          rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking status");
+        }
+      }
+      if constexpr (std::is_same_v<T, ClientBaseData::GoalResponseData>) {
+        if (RCL_RET_OK == data.ret) {
+          this->handle_goal_response(data.response_header, data.goal_response);
+        } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) {
+          rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking goal response");
+        }
+      }
+      if constexpr (std::is_same_v<T, ClientBaseData::ResultResponseData>) {
+        if (RCL_RET_OK == data.ret) {
+          this->handle_result_response(data.response_header, data.result_response);
+        } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) {
+          rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking result response");
+        }
+      }
+      if constexpr (std::is_same_v<T, ClientBaseData::CancelResponseData>) {
+        if (RCL_RET_OK == data.ret) {
+          this->handle_cancel_response(data.response_header, data.cancel_response);
+        } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) {
+          rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking cancel response");
+        }
+      }
+    }, data_ptr->data);
 }
 
 }  // namespace rclcpp_action

rclcpp_action/src/server.cpp

@@ -18,7 +18,9 @@
 #include <tuple>
 #include <unordered_map>
 #include <utility>
+#include <variant>
 #include <vector>
+#include <deque>
 
 #include "rcl_action/action_server.h"
 #include "rcl_action/wait.h"
@@ -26,15 +28,48 @@
 #include "rcpputils/scope_exit.hpp"
 
 #include "action_msgs/msg/goal_status_array.hpp"
-#include "action_msgs/srv/cancel_goal.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp_action/server.hpp"
 
 using rclcpp_action::ServerBase;
 using rclcpp_action::GoalUUID;
 
+struct ServerBaseData;
+
 namespace rclcpp_action
 {
+
+struct ServerBaseData
+{
+  using GoalRequestData = std::tuple<
+    rcl_ret_t,
+    const rcl_action_goal_info_t,
+    rmw_request_id_t,
+    std::shared_ptr<void>
+  >;
+
+  using CancelRequestData = std::tuple<
+    rcl_ret_t,
+    std::shared_ptr<action_msgs::srv::CancelGoal::Request>,
+    rmw_request_id_t
+  >;
+
+  using ResultRequestData = std::tuple<rcl_ret_t, std::shared_ptr<void>, rmw_request_id_t>;
+
+  using GoalExpiredData = struct Empty {};
+
+  std::variant<GoalRequestData, CancelRequestData, ResultRequestData, GoalExpiredData> data;
+
+  explicit ServerBaseData(GoalRequestData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ServerBaseData(CancelRequestData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ServerBaseData(ResultRequestData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ServerBaseData(GoalExpiredData && data_in)
+  : data(std::move(data_in)) {}
+};
+
 class ServerBaseImpl
 {
 public:
@@ -60,11 +95,6 @@ public:
   size_t num_services_ = 0;
   size_t num_guard_conditions_ = 0;
 
-  std::atomic<bool> goal_request_ready_{false};
-  std::atomic<bool> cancel_request_ready_{false};
-  std::atomic<bool> result_request_ready_{false};
-  std::atomic<bool> goal_expired_{false};
-
   // Lock for unordered_maps
   std::recursive_mutex unordered_map_mutex_;
 
@@ -75,8 +105,12 @@ public:
   // rcl goal handles are kept so api to send result doesn't try to access freed memory
   std::unordered_map<GoalUUID, std::shared_ptr<rcl_action_goal_handle_t>> goal_handles_;
 
+  // next ready event for taking, will be set by is_ready and will be processed by take_data
+  std::atomic<size_t> next_ready_event;
+
   rclcpp::Logger logger_;
 };
+
 }  // namespace rclcpp_action
 
 ServerBase::ServerBase(
@@ -165,18 +199,18 @@ ServerBase::get_number_of_ready_guard_conditions()
 }
 
 void
-ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set)
+ServerBase::add_to_wait_set(rcl_wait_set_t & wait_set)
 {
   std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
   rcl_ret_t ret = rcl_action_wait_set_add_action_server(
-    wait_set, pimpl_->action_server_.get(), NULL);
+    &wait_set, pimpl_->action_server_.get(), NULL);
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret, "ServerBase::add_to_wait_set() failed");
   }
 }
 
 bool
-ServerBase::is_ready(rcl_wait_set_t * wait_set)
+ServerBase::is_ready(const rcl_wait_set_t & wait_set)
 {
   bool goal_request_ready;
   bool cancel_request_ready;
@@ -186,7 +220,7 @@ ServerBase::is_ready(rcl_wait_set_t * wait_set)
   {
     std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
     ret = rcl_action_server_wait_set_get_entities_ready(
-      wait_set,
+      &wait_set,
       pimpl_->action_server_.get(),
       &goal_request_ready,
       &cancel_request_ready,
@@ -194,124 +228,153 @@ ServerBase::is_ready(rcl_wait_set_t * wait_set)
       &goal_expired);
   }
 
-  pimpl_->goal_request_ready_ = goal_request_ready;
-  pimpl_->cancel_request_ready_ = cancel_request_ready;
-  pimpl_->result_request_ready_ = result_request_ready;
-  pimpl_->goal_expired_ = goal_expired;
-
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
 
-  return pimpl_->goal_request_ready_.load() ||
-         pimpl_->cancel_request_ready_.load() ||
-         pimpl_->result_request_ready_.load() ||
-         pimpl_->goal_expired_.load();
+  pimpl_->next_ready_event = std::numeric_limits<uint32_t>::max();
+
+  if (goal_request_ready) {
+    pimpl_->next_ready_event = static_cast<uint32_t>(EntityType::GoalService);
+    return true;
+  }
+
+  if (cancel_request_ready) {
+    pimpl_->next_ready_event = static_cast<uint32_t>(EntityType::CancelService);
+    return true;
+  }
+
+  if (result_request_ready) {
+    pimpl_->next_ready_event = static_cast<uint32_t>(EntityType::ResultService);
+    return true;
+  }
+
+  if (goal_expired) {
+    pimpl_->next_ready_event = static_cast<uint32_t>(EntityType::Expired);
+    return true;
+  }
+
+  return false;
 }
 
 std::shared_ptr<void>
 ServerBase::take_data()
 {
-  if (pimpl_->goal_request_ready_.load()) {
-    rcl_ret_t ret;
-    rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
-    rmw_request_id_t request_header;
+  size_t next_ready_event = pimpl_->next_ready_event.exchange(std::numeric_limits<uint32_t>::max());
 
-    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
-
-    std::shared_ptr<void> message = create_goal_request();
-    ret = rcl_action_take_goal_request(
-      pimpl_->action_server_.get(),
-      &request_header,
-      message.get());
-
-    return std::static_pointer_cast<void>(
-      std::make_shared
-      <std::tuple<rcl_ret_t, rcl_action_goal_info_t, rmw_request_id_t, std::shared_ptr<void>>>(
-        ret,
-        goal_info,
-        request_header, message));
-  } else if (pimpl_->cancel_request_ready_.load()) {
-    rcl_ret_t ret;
-    rmw_request_id_t request_header;
-
-    // Initialize cancel request
-    auto request = std::make_shared<action_msgs::srv::CancelGoal::Request>();
-
-    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
-    ret = rcl_action_take_cancel_request(
-      pimpl_->action_server_.get(),
-      &request_header,
-      request.get());
-
-    return std::static_pointer_cast<void>(
-      std::make_shared
-      <std::tuple<rcl_ret_t, std::shared_ptr<action_msgs::srv::CancelGoal::Request>,
-      rmw_request_id_t>>(ret, request, request_header));
-  } else if (pimpl_->result_request_ready_.load()) {
-    rcl_ret_t ret;
-    // Get the result request message
-    rmw_request_id_t request_header;
-    std::shared_ptr<void> result_request = create_result_request();
-    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
-    ret = rcl_action_take_result_request(
-      pimpl_->action_server_.get(), &request_header, result_request.get());
-
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, std::shared_ptr<void>, rmw_request_id_t>>(
-        ret, result_request, request_header));
-  } else if (pimpl_->goal_expired_.load()) {
-    return nullptr;
-  } else {
-    throw std::runtime_error("Taking data from action server but nothing is ready");
+  if (next_ready_event == std::numeric_limits<uint32_t>::max()) {
+    throw std::runtime_error("ServerBase::take_data() called but no data is ready");
   }
+
+  return take_data_by_entity_id(next_ready_event);
 }
 
 std::shared_ptr<void>
 ServerBase::take_data_by_entity_id(size_t id)
 {
+  std::shared_ptr<ServerBaseData> data_ptr;
   // Mark as ready the entity from which we want to take data
   switch (static_cast<EntityType>(id)) {
     case EntityType::GoalService:
-      pimpl_->goal_request_ready_ = true;
+      {
+        rcl_ret_t ret;
+        rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
+        rmw_request_id_t request_header;
+
+        std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+
+        std::shared_ptr<void> message = create_goal_request();
+        ret = rcl_action_take_goal_request(
+          pimpl_->action_server_.get(),
+          &request_header,
+          message.get());
+
+        data_ptr = std::make_shared<ServerBaseData>(
+          ServerBaseData::GoalRequestData(ret, goal_info, request_header, message));
+      }
       break;
     case EntityType::ResultService:
-      pimpl_->result_request_ready_ = true;
+      {
+        rcl_ret_t ret;
+        // Get the result request message
+        rmw_request_id_t request_header;
+        std::shared_ptr<void> result_request = create_result_request();
+        std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+        ret = rcl_action_take_result_request(
+          pimpl_->action_server_.get(), &request_header, result_request.get());
+
+        data_ptr =
+          std::make_shared<ServerBaseData>(
+          ServerBaseData::ResultRequestData(ret, result_request, request_header));
+      }
       break;
     case EntityType::CancelService:
-      pimpl_->cancel_request_ready_ = true;
+      {
+        rcl_ret_t ret;
+        rmw_request_id_t request_header;
+
+        // Initialize cancel request
+        auto request = std::make_shared<action_msgs::srv::CancelGoal::Request>();
+
+        std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+        ret = rcl_action_take_cancel_request(
+          pimpl_->action_server_.get(),
+          &request_header,
+          request.get());
+
+        data_ptr =
+          std::make_shared<ServerBaseData>(
+          ServerBaseData::CancelRequestData(ret, request, request_header));
+      }
+      break;
+    case EntityType::Expired:
+      {
+        data_ptr =
+          std::make_shared<ServerBaseData>(ServerBaseData::GoalExpiredData());
+      }
       break;
   }
 
-  return take_data();
+  return std::static_pointer_cast<void>(data_ptr);
 }
 
 void
-ServerBase::execute(std::shared_ptr<void> & data)
+ServerBase::execute(const std::shared_ptr<void> & data_in)
 {
-  if (!data && !pimpl_->goal_expired_.load()) {
-    throw std::runtime_error("'data' is empty");
+  if (!data_in) {
+    throw std::runtime_error("ServerBase::execute: give data pointer was null");
   }
 
-  if (pimpl_->goal_request_ready_.load()) {
-    execute_goal_request_received(data);
-  } else if (pimpl_->cancel_request_ready_.load()) {
-    execute_cancel_request_received(data);
-  } else if (pimpl_->result_request_ready_.load()) {
-    execute_result_request_received(data);
-  } else if (pimpl_->goal_expired_.load()) {
-    execute_check_expired_goals();
-  } else {
-    throw std::runtime_error("Executing action server but nothing is ready");
-  }
+  std::shared_ptr<ServerBaseData> data_ptr = std::static_pointer_cast<ServerBaseData>(data_in);
+
+  std::visit(
+    [&](auto && data) -> void {
+      using T = std::decay_t<decltype(data)>;
+      if constexpr (std::is_same_v<T, ServerBaseData::GoalRequestData>) {
+        execute_goal_request_received(
+          std::get<0>(data), std::get<1>(data), std::get<2>(data),
+          std::get<3>(data));
+      }
+      if constexpr (std::is_same_v<T, ServerBaseData::CancelRequestData>) {
+        execute_cancel_request_received(std::get<0>(data), std::get<1>(data), std::get<2>(data));
+      }
+      if constexpr (std::is_same_v<T, ServerBaseData::ResultRequestData>) {
+        execute_result_request_received(std::get<0>(data), std::get<1>(data), std::get<2>(data));
+      }
+      if constexpr (std::is_same_v<T, ServerBaseData::GoalExpiredData>) {
+        execute_check_expired_goals();
+      }
+    },
+    data_ptr->data);
 }
 
 void
-ServerBase::execute_goal_request_received(std::shared_ptr<void> & data)
+ServerBase::execute_goal_request_received(
+  rcl_ret_t ret,
+  rcl_action_goal_info_t goal_info,
+  rmw_request_id_t request_header,
+  const std::shared_ptr<void> message)
 {
-  auto shared_ptr = std::static_pointer_cast
-    <std::tuple<rcl_ret_t, rcl_action_goal_info_t, rmw_request_id_t, std::shared_ptr<void>>>(data);
-  rcl_ret_t ret = std::get<0>(*shared_ptr);
   if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) {
     // Ignore take failure because connext fails if it receives a sample without valid data.
     // This happens when a client shuts down and connext receives a sample saying the client is
@@ -320,14 +383,6 @@ ServerBase::execute_goal_request_received(std::shared_ptr<void> & data)
   } else if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
-  rcl_action_goal_info_t goal_info = std::get<1>(*shared_ptr);
-  rmw_request_id_t request_header = std::get<2>(*shared_ptr);
-  std::shared_ptr<void> message = std::get<3>(*shared_ptr);
-
-  bool expected = true;
-  if (!pimpl_->goal_request_ready_.compare_exchange_strong(expected, false)) {
-    return;
-  }
 
   GoalUUID uuid = get_goal_id_from_goal_request(message.get());
   convert(uuid, &goal_info);
@@ -405,16 +460,14 @@ ServerBase::execute_goal_request_received(std::shared_ptr<void> & data)
     // Tell user to start executing action
     call_goal_accepted_callback(handle, uuid, message);
   }
-  data.reset();
 }
 
 void
-ServerBase::execute_cancel_request_received(std::shared_ptr<void> & data)
+ServerBase::execute_cancel_request_received(
+  rcl_ret_t ret,
+  std::shared_ptr<action_msgs::srv::CancelGoal::Request> request,
+  rmw_request_id_t request_header)
 {
-  auto shared_ptr = std::static_pointer_cast
-    <std::tuple<rcl_ret_t, std::shared_ptr<action_msgs::srv::CancelGoal::Request>,
-      rmw_request_id_t>>(data);
-  auto ret = std::get<0>(*shared_ptr);
   if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) {
     // Ignore take failure because connext fails if it receives a sample without valid data.
     // This happens when a client shuts down and connext receives a sample saying the client is
@@ -423,9 +476,6 @@ ServerBase::execute_cancel_request_received(std::shared_ptr<void> & data)
   } else if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
-  auto request = std::get<1>(*shared_ptr);
-  auto request_header = std::get<2>(*shared_ptr);
-  pimpl_->cancel_request_ready_ = false;
 
   // Convert c++ message to C message
   rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request();
@@ -504,15 +554,14 @@ ServerBase::execute_cancel_request_received(std::shared_ptr<void> & data)
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
-  data.reset();
 }
 
 void
-ServerBase::execute_result_request_received(std::shared_ptr<void> & data)
+ServerBase::execute_result_request_received(
+  rcl_ret_t ret,
+  std::shared_ptr<void> result_request,
+  rmw_request_id_t request_header)
 {
-  auto shared_ptr = std::static_pointer_cast
-    <std::tuple<rcl_ret_t, std::shared_ptr<void>, rmw_request_id_t>>(data);
-  auto ret = std::get<0>(*shared_ptr);
   if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) {
     // Ignore take failure because connext fails if it receives a sample without valid data.
     // This happens when a client shuts down and connext receives a sample saying the client is
@@ -521,10 +570,7 @@ ServerBase::execute_result_request_received(std::shared_ptr<void> & data)
   } else if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
-  auto result_request = std::get<1>(*shared_ptr);
-  auto request_header = std::get<2>(*shared_ptr);
 
-  pimpl_->result_request_ready_ = false;
   std::shared_ptr<void> result_response;
 
   // check if the goal exists
@@ -568,7 +614,6 @@ ServerBase::execute_result_request_received(std::shared_ptr<void> & data)
       rclcpp::exceptions::throw_from_rcl_error(rcl_ret);
     }
   }
-  data.reset();
 }
 
 void