fix: Expose timers used by rclcpp::Waitables (#2699)

Signed-off-by: Janosch Machowinski <[email protected]>

Author: jmachowinski

rclcpp/include/rclcpp/event_handler.hpp

@@ -20,6 +20,7 @@
 #include <mutex>
 #include <stdexcept>
 #include <string>
+#include <vector>
 
 #include "rcl/error_handling.h"
 #include "rcl/event_callback.h"
@@ -221,6 +222,13 @@ class EventHandlerBase : public Waitable
     }
   }
 
+  RCLCPP_PUBLIC
+  std::vector<std::shared_ptr<rclcpp::TimerBase>>
+  get_timers() const override
+  {
+    return {};
+  }
+
 protected:
   RCLCPP_PUBLIC
   void

rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp

@@ -19,6 +19,7 @@
 #include <memory>
 #include <mutex>
 #include <set>
+#include <vector>
 
 #include "rclcpp/guard_condition.hpp"
 #include "rclcpp/waitable.hpp"
@@ -146,6 +147,14 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable
   size_t
   get_number_of_ready_guard_conditions() override;
 
+  /// Returns the number of used Timers
+  /**
+   * Will always return an empty vector.
+   */
+  RCLCPP_PUBLIC
+  std::vector<std::shared_ptr<rclcpp::TimerBase>>
+  get_timers() const override;
+
 private:
   /// Callback to run when waitable executes
   std::function<void(void)> execute_callback_;

rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp

@@ -19,6 +19,7 @@
 #include <memory>
 #include <mutex>
 #include <string>
+#include <vector>
 
 #include "rcl/wait.h"
 #include "rmw/impl/cpp/demangle.hpp"
@@ -180,6 +181,13 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable
     on_new_message_callback_ = nullptr;
   }
 
+  RCLCPP_PUBLIC
+  std::vector<std::shared_ptr<rclcpp::TimerBase>>
+  get_timers() const override
+  {
+    return {};
+  }
+
 protected:
   std::recursive_mutex callback_mutex_;
   std::function<void(size_t)> on_new_message_callback_ {nullptr};

rclcpp/include/rclcpp/subscription_base.hpp

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

rclcpp/include/rclcpp/waitable.hpp

@@ -18,6 +18,7 @@
 #include <atomic>
 #include <functional>
 #include <memory>
+#include <vector>
 
 #include "rclcpp/macros.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -27,6 +28,8 @@
 namespace rclcpp
 {
 
+class TimerBase;
+
 class Waitable
 {
 public:
@@ -258,6 +261,17 @@ class Waitable
   void
   clear_on_ready_callback() = 0;
 
+  /// Returns all timers used by this waitable
+  /**
+   * Must return all timers used within the waitable.
+   * Note, it is not supported, that timers are added
+   * or removed over the lifetime of the waitable.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<std::shared_ptr<rclcpp::TimerBase>>
+  get_timers() const = 0;
+
 private:
   std::atomic<bool> in_use_by_wait_set_{false};
 };  // class Waitable

rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp

@@ -189,5 +189,12 @@ ExecutorNotifyWaitable::get_number_of_ready_guard_conditions()
   return notify_guard_conditions_.size();
 }
 
+std::vector<std::shared_ptr<rclcpp::TimerBase>>
+ExecutorNotifyWaitable::get_timers() const
+{
+  return {};
+}
+
+
 }  // namespace executors
 }  // namespace rclcpp

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

@@ -408,8 +408,16 @@ EventsExecutor::refresh_current_collection(
     [this](auto waitable) {
       waitable->set_on_ready_callback(
         this->create_waitable_callback(waitable.get()));
+      for (const auto & t : waitable->get_timers()) {
+        timers_manager_->add_timer(t);
+      }
     },
-    [](auto waitable) {waitable->clear_on_ready_callback();});
+    [this](auto waitable) {
+      waitable->clear_on_ready_callback();
+      for (const auto & t : waitable->get_timers()) {
+        timers_manager_->remove_timer(t);
+      }
+    });
 }
 
 std::function<void(size_t)>

rclcpp/test/rclcpp/executors/test_events_executor.cpp

@@ -497,3 +497,122 @@ TEST_F(TestEventsExecutor, test_default_incompatible_qos_callbacks)
 
   rcutils_logging_set_output_handler(original_output_handler);
 }
+
+class TestWaitableWithTimer : public rclcpp::Waitable
+{
+  static constexpr int TimerCallbackType = 0;
+
+public:
+  explicit TestWaitableWithTimer(const rclcpp::Context::SharedPtr & context)
+  {
+    auto timer_callback = [this] () {
+        timer_ready = true;
+        if (ready_callback) {
+        // inform executor that the waitable is ready to process a timer event
+          ready_callback(1, TimerCallbackType);
+        }
+      };
+    timer =
+      std::make_shared<rclcpp::WallTimer<decltype (timer_callback)>>(std::chrono::milliseconds(10),
+      std::move(timer_callback), context);
+  }
+
+  void
+  add_to_wait_set(rcl_wait_set_t & /*wait_set*/) override {}
+
+  bool
+  is_ready(const rcl_wait_set_t & /*wait_set*/) override
+  {
+    return false;
+  }
+
+  std::shared_ptr<void>
+  take_data() override
+  {
+    return std::shared_ptr<void>(nullptr);
+  }
+
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id) override
+  {
+    if (id != TimerCallbackType) {
+      throw std::runtime_error("Internal error, got wrong ID for take data");
+    }
+    return nullptr;
+  }
+
+  void
+  execute(const std::shared_ptr<void> &) override
+  {
+    if (timer_ready) {
+      timer_triggered_waitable_and_waitable_was_executed = true;
+    }
+  }
+
+  void
+  set_on_ready_callback(std::function<void(size_t, int)> callback) override
+  {
+    ready_callback = callback;
+  }
+
+  void
+  clear_on_ready_callback() override
+  {
+    ready_callback = std::function<void(size_t, int)>();
+  }
+
+  size_t
+  get_number_of_ready_guard_conditions() override
+  {
+    return 0;
+  }
+
+  std::vector<std::shared_ptr<rclcpp::TimerBase>> get_timers() const override
+  {
+    return {timer};
+  }
+
+  bool timerTriggeredWaitable() const
+  {
+    return timer_triggered_waitable_and_waitable_was_executed;
+  }
+
+private:
+  std::atomic_bool timer_triggered_waitable_and_waitable_was_executed = false;
+  std::atomic_bool timer_ready = false;
+  rclcpp::TimerBase::SharedPtr timer;
+  std::function<void(size_t, int)> ready_callback;
+};
+
+TEST_F(TestEventsExecutor, waitable_with_timer)
+{
+  auto node = std::make_shared<rclcpp::Node>("node");
+
+  auto waitable =
+    std::make_shared<TestWaitableWithTimer>(rclcpp::contexts::get_global_default_context());
+
+  EventsExecutor executor;
+  executor.add_node(node);
+
+  node->get_node_waitables_interface()->add_waitable(waitable,
+    node->get_node_base_interface()->get_default_callback_group());
+
+  std::thread spinner([&executor]() {executor.spin();});
+
+  size_t cnt = 0;
+  while (!waitable->timerTriggeredWaitable()) {
+    std::this_thread::sleep_for(10ms);
+    cnt++;
+
+    // This should terminate after ~20 ms, so a timeout of 500ms should be plenty
+    EXPECT_LT(cnt, 51);
+    if (cnt > 50) {
+      // timeout case
+      break;
+    }
+  }
+  executor.cancel();
+  spinner.join();
+
+  EXPECT_TRUE(waitable->timerTriggeredWaitable());
+}

rclcpp/test/rclcpp/executors/test_waitable.hpp

@@ -18,6 +18,7 @@
 #include <atomic>
 #include <functional>
 #include <memory>
+#include <vector>
 
 #include "rclcpp/waitable.hpp"
 #include "rclcpp/guard_condition.hpp"
@@ -64,6 +65,11 @@ class TestWaitable : public rclcpp::Waitable
   size_t
   get_is_ready_call_count() const;
 
+  std::vector<std::shared_ptr<rclcpp::TimerBase>> get_timers() const override
+  {
+    return {};
+  }
+
 private:
   std::atomic<size_t> trigger_count_ = 0;
   std::atomic<size_t> is_ready_count_ = 0;

rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp

@@ -38,6 +38,11 @@ class TestWaitable : public rclcpp::Waitable
   void clear_on_ready_callback() override {}
 
   std::shared_ptr<void> take_data_by_entity_id(size_t) override {return nullptr;}
+
+  std::vector<std::shared_ptr<rclcpp::TimerBase>> get_timers() const override
+  {
+    return {};
+  }
 };
 
 class TestNodeWaitables : public ::testing::Test