Add thread-safe methods to trigger notifications and prepare work in executor

Signed-off-by: HarunTeper <harun.teper@tu-dortmund.de>

Author: HarunTeper

rclcpp/include/rclcpp/executor.hpp

@@ -455,6 +455,14 @@ class Executor
   void
   execute_any_executable(AnyExecutable & any_exec);
 
+  /// Trigger the notify guard condition to wake up the executor.
+  /**
+   * This function is thread safe.
+   */
+  RCLCPP_PUBLIC
+  void
+  trigger_executor_notify();
+
   /// Run subscription executable.
   /**
    * Do necessary setup and tear-down as well as executing the subscription.
@@ -508,6 +516,27 @@ class Executor
   void
   wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
+  /// Prepare the wait set to bee waited on.
+  /**
+   * Builds a set of waitable entities, which are passed to the middleware.
+   * After building wait set, waits on middleware to notify.
+   * \throws std::runtime_error if the wait set can be cleared
+   */
+  RCLCPP_PUBLIC
+  void
+  prepare_work();
+
+  /// Block until more work becomes avilable or timeout is reached.
+  /**
+   * Builds a set of waitable entities, which are passed to the middleware.
+   * After building wait set, waits on middleware to notify.
+   * \param[in] timeout duration to wait for new work to become available.
+   * \throws std::runtime_error if the wait set can be cleared
+   */
+  RCLCPP_PUBLIC
+  void
+  wait_for_work_simple(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+
   /// Check for executable in ready state and populate union structure.
   /**
    * \param[out] any_executable populated union structure of ready executable
@@ -556,6 +585,10 @@ class Executor
 
   mutable std::mutex mutex_;
 
+  mutable std::mutex notify_mutex_;
+
+  std::vector<rclcpp::CallbackGroup::SharedPtr> wait_for_work_cbgs_;
+
   /// The context associated with this executor.
   std::shared_ptr<rclcpp::Context> context_;
 
@@ -583,6 +616,10 @@ class Executor
   rclcpp::WaitSet wait_set_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
   std::optional<rclcpp::WaitResult<rclcpp::WaitSet>> wait_result_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
+  /// Previous WaitSet to be waited on.
+  rclcpp::WaitSet previous_wait_set_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+  std::optional<rclcpp::WaitResult<rclcpp::WaitSet>> previous_wait_result_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
   /// Hold the current state of the collection being waited on by the waitset
   rclcpp::executors::ExecutorEntitiesCollection current_collection_ RCPPUTILS_TSA_GUARDED_BY(
     mutex_);

rclcpp/src/rclcpp/executor.cpp

@@ -26,6 +26,7 @@
 #include "rcl/allocator.h"
 #include "rcl/error_handling.h"
 #include "rclcpp/executors/executor_notify_waitable.hpp"
+#include "rclcpp/logging.hpp"
 #include "rclcpp/subscription_wait_set_mask.hpp"
 #include "rcpputils/scope_exit.hpp"
 
@@ -480,9 +481,18 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
     const std::shared_ptr<void> & const_data = any_exec.data;
     any_exec.waitable->execute(const_data);
   }
+}
 
-  // Reset the callback_group, regardless of type
-  any_exec.callback_group->can_be_taken_from().store(true);
+void
+Executor::trigger_executor_notify()
+{
+  try {
+    interrupt_guard_condition_->trigger();
+  } catch (const rclcpp::exceptions::RCLError & ex) {
+    throw std::runtime_error(
+            std::string(
+              "Failed to trigger guard condition on callback group change: ") + ex.what());
+  }
 }
 
 template<typename Taker, typename Handler>
@@ -770,6 +780,53 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
   }
 }
 
+void Executor::prepare_work()
+{
+  {
+    std::lock_guard<std::mutex> guard(mutex_);
+    if (this->entities_need_rebuild_.exchange(false) || current_collection_.empty()) {
+      RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Rebuilding executor entities");
+      this->collect_entities();
+    }
+
+    auto callback_groups = this->collector_.get_all_callback_groups();
+    wait_for_work_cbgs_.resize(callback_groups.size());
+    for(const auto & w_ptr : callback_groups) {
+      auto shr_ptr = w_ptr.lock();
+      if(shr_ptr) {
+        wait_for_work_cbgs_.push_back(std::move(shr_ptr));
+      }
+    }
+  }
+}
+
+void
+Executor::wait_for_work_simple(std::chrono::nanoseconds timeout)
+{
+  TRACETOOLS_TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count());
+
+  this->wait_result_.reset();
+  this->wait_result_.emplace(wait_set_.wait(timeout));
+
+  // drop references to the callback groups, before trying to execute anything
+  wait_for_work_cbgs_.clear();
+
+  if (!this->wait_result_ || this->wait_result_->kind() == WaitResultKind::Empty) {
+    RCUTILS_LOG_WARN_NAMED(
+      "rclcpp",
+      "empty wait set received in wait(). This should never happen.");
+  } else {
+    if (this->wait_result_->kind() == WaitResultKind::Ready && current_notify_waitable_) {
+      RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Getting wait set");
+      auto & rcl_wait_set = this->wait_result_->get_wait_set().get_rcl_wait_set();
+      if (current_notify_waitable_->is_ready(rcl_wait_set)) {
+        RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Executing notify waitable");
+        current_notify_waitable_->execute(current_notify_waitable_->take_data());
+      }
+    }
+  }
+}
+
 bool
 Executor::get_next_ready_executable(AnyExecutable & any_executable)
 {
@@ -896,20 +953,26 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable)
 bool
 Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanoseconds timeout)
 {
+  notify_mutex_.lock();
   bool success = false;
   // Check to see if there are any subscriptions or timers needing service
   // TODO(wjwwood): improve run to run efficiency of this function
   success = get_next_ready_executable(any_executable);
   // If there are none
   if (!success) {
     // Wait for subscriptions or timers to work on
+    prepare_work();
+    notify_mutex_.unlock();
     wait_for_work(timeout);
     if (!spinning.load()) {
       return false;
     }
     // Try again
     success = get_next_ready_executable(any_executable);
   }
+  else {
+    notify_mutex_.unlock();
+  }
   return success;
 }
 

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -99,15 +99,15 @@ 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());
+      std::lock_guard wait_lock{notify_mutex_};
+
+      any_exec.callback_group->can_be_taken_from().store(true);
+
+      if (any_exec.callback_group &&
+        any_exec.callback_group->type() == CallbackGroupType::MutuallyExclusive)
+      {
+        trigger_executor_notify();
       }
     }