rework spin_some & spin_all

Author: Mauro Passerino

rclcpp/include/rclcpp/executors/events_executor.hpp

@@ -82,8 +82,10 @@ class EventsExecutor : public rclcpp::Executor
 
   /// Events executor implementation of spin some
   /**
-   * This function will execute ready timers and events
-   * until timeout or no more work available.
+   * This non-blocking function will execute the timers and events
+   * that were ready when this API was called, until timeout or no
+   * more work available. New ready-timers/events arrived while
+   * executing work, won't be taken into account here.
    *
    * Example:
    *   while(condition) {
@@ -96,6 +98,20 @@ class EventsExecutor : public rclcpp::Executor
   void
   spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)) override;
 
+  /// Events executor implementation of spin all
+  /**
+   * This non-blocking function will execute timers and events
+   * until timeout or no more work available. If new ready-timers/events
+   * arrive while executing work available, they will be executed
+   * as long as the timeout hasn't expired.
+   *
+   * Example:
+   *   while(condition) {
+   *     spin_all();
+   *     sleep(); // User should have some sync work or
+   *              // sleep to avoid a 100% CPU usage
+   *   }
+   */
   RCLCPP_PUBLIC
   void
   spin_all(std::chrono::nanoseconds max_duration) override;

rclcpp/include/rclcpp/executors/timers_manager.hpp

@@ -127,6 +127,14 @@ class TimersManager
    */
   void remove_timer(rclcpp::TimerBase::SharedPtr timer);
 
+  /**
+   * @brief Executes head timer if ready at time point.
+   * @param tp the timepoint to check for
+   * @return true if head timer was ready at tp
+   */
+  bool execute_head_timer_if_ready_at_tp(
+    std::chrono::time_point<std::chrono::steady_clock> tp);
+
   // This is what the TimersManager uses to denote a duration forever.
   // We don't use std::chrono::nanoseconds::max because it will overflow.
   // See https://en.cppreference.com/w/cpp/thread/condition_variable/wait_for

rclcpp/include/rclcpp/experimental/buffers/events_queue.hpp

@@ -83,6 +83,15 @@ class EventsQueue
   bool
   empty() const = 0;
 
+  /**
+   * @brief Returns the number of elements in the queue.
+   * @return the number of elements in the queue.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  size() const = 0;
+
   /**
    * @brief Initializes the queue
    */

rclcpp/include/rclcpp/experimental/buffers/simple_events_queue.hpp

@@ -89,6 +89,18 @@ class SimpleEventsQueue : public EventsQueue
     return event_queue_.empty();
   }
 
+  /**
+   * @brief Returns the number of elements in the queue.
+   * @return the number of elements in the queue.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  size() const
+  {
+    return event_queue_.size();
+  }
+
   /**
    * @brief Initializes the queue
    */

rclcpp/src/rclcpp/executors/events_executor.cpp

@@ -96,90 +96,95 @@ EventsExecutor::spin_some(std::chrono::nanoseconds max_duration)
       return elapsed_time < max_duration;
     };
 
-  // Execute events until timeout or no more work available
-  while (max_duration_not_elapsed()) {
-    bool work_available = false;
+  // Get the number of events ready at this time point
+  std::unique_lock<std::mutex> lock(push_mutex_);
+  size_t available_events_at_tp = events_queue_->size();
+  lock.unlock();
 
-    // Check for ready timers
-    auto next_timer_timeout = timers_manager_->get_head_timeout();
+  size_t executed_events = 0;
 
-    bool timer_ready = next_timer_timeout < 0ns;
+  // Checks if all events that were ready when spin_some was called, were executed.
+  auto executed_ready_events_at_tp = [executed_events, available_events_at_tp]() {
+      return executed_events >= available_events_at_tp;
+    };
 
-    if (timer_ready) {
-      timers_manager_->execute_head_timer();
-      work_available = true;
-    } else {
-      // Execute first event from queue if it exists
+  // Execute events and timers ready when spin_some was called,
+  // until timeout or no more work available.
+  while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) {
+    // Execute first event from queue if it exists
+    if (!executed_ready_events_at_tp()) {
       std::unique_lock<std::mutex> lock(push_mutex_);
 
       bool has_event = !events_queue_->empty();
 
       if (has_event) {
         rmw_listener_event_t event = events_queue_->front();
         events_queue_->pop();
+        // std::cout << "Execute event" << std::endl;
         this->execute_event(event);
-        work_available = true;
+        executed_events++;
+        continue;
       }
     }
 
-    // If there's no more work available, exit
-    if (!work_available) {
-      break;
+    // Execute timer, if was ready at start
+    if (timers_manager_->execute_head_timer_if_ready_at_tp(start)) {
+      continue;
     }
+
+    // If there's no more work available, exit
+    break;
   }
 }
 
 void
 EventsExecutor::spin_all(std::chrono::nanoseconds max_duration)
 {
-  if (max_duration <= 0ns) {
-    throw std::invalid_argument("max_duration must be positive");
+  if (spinning.exchange(true)) {
+    throw std::runtime_error("spin_all() called while already spinning");
   }
 
-  if (spinning.exchange(true)) {
-    throw std::runtime_error("spin_some() called while already spinning");
+  if (max_duration < 0ns) {
+    throw std::invalid_argument("max_duration must be positive");
   }
+
   RCLCPP_SCOPE_EXIT(this->spinning.store(false););
 
-  // When condition variable is notified, check this predicate to proceed
-  auto has_event_predicate = [this]() {return !events_queue_->empty();};
+  // In this context a 0 input max_duration means no duration limit
+  if (std::chrono::nanoseconds(0) == max_duration) {
+    max_duration = timers_manager_->MAX_TIME;
+  }
 
   auto start = std::chrono::steady_clock::now();
+
   auto max_duration_not_elapsed = [max_duration, start]() {
       auto elapsed_time = std::chrono::steady_clock::now() - start;
       return elapsed_time < max_duration;
     };
 
-  // Select the smallest between input max duration and timer timeout
-  auto next_timer_timeout = timers_manager_->get_head_timeout();
-  if (next_timer_timeout < max_duration) {
-    max_duration = next_timer_timeout;
-  }
-
-  {
-    // Wait once until timeout or event
-    std::unique_lock<std::mutex> push_lock(push_mutex_);
-    events_queue_cv_.wait_for(push_lock, max_duration, has_event_predicate);
-  }
+  // Execute timer and events until timeout or no more work available
+  while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) {
+    // Execute first event from queue if it exists
+    {
+      std::unique_lock<std::mutex> lock(push_mutex_);
 
-  auto timeout = timers_manager_->get_head_timeout();
+      bool has_event = !events_queue_->empty();
 
-  // Keep executing until no more work to do or timeout expired
-  while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) {
-    std::unique_lock<std::mutex> push_lock(push_mutex_);
-    EventQueue execution_event_queue = events_queue_->get_all_events();
-    push_lock.unlock();
+      if (has_event) {
+        rmw_listener_event_t event = events_queue_->front();
+        events_queue_->pop();
+        this->execute_event(event);
+        continue;
+      }
+    }
 
-    // Exit if there is no more work to do
-    const bool ready_timer = timeout < 0ns;
-    const bool has_events = !execution_event_queue.empty();
-    if (!ready_timer && !has_events) {
-      break;
+    // Execute timer, if was ready
+    if (timers_manager_->execute_head_timer()) {
+      continue;
     }
 
-    // Execute all ready work
-    timeout = timers_manager_->execute_ready_timers();
-    this->consume_all_events(execution_event_queue);
+    // If there's no more work available, exit
+    break;
   }
 }
 

rclcpp/src/rclcpp/executors/timers_manager.cpp

@@ -230,3 +230,40 @@ void TimersManager::remove_timer(TimerPtr timer)
     timers_cv_.notify_one();
   }
 }
+
+bool TimersManager::execute_head_timer_if_ready_at_tp(
+  std::chrono::time_point<std::chrono::steady_clock> timepoint)
+{
+  // Do not allow to interfere with the thread running
+  if (running_) {
+    throw std::runtime_error(
+            "TimersManager::execute_head_timer_if_ready_at_tp() can't"
+            "be used while timers thread is running");
+  }
+
+  std::unique_lock<std::mutex> lock(timers_mutex_);
+
+  TimersHeap timers_heap = weak_timers_heap_.validate_and_lock();
+
+  // Nothing to do if we don't have any timer
+  if (timers_heap.empty()) {
+    return false;
+  }
+
+  TimerPtr head = timers_heap.front();
+
+  // A ready timer will return a negative duration when calling time_until_trigger
+  auto timepoint_timer_ready = std::chrono::steady_clock::now() + head->time_until_trigger();
+
+  auto timer_was_ready_at_tp = timepoint_timer_ready < timepoint;
+
+  if (timer_was_ready_at_tp) {
+    // Head timer is ready, execute
+    head->execute_callback();
+    timers_heap.heapify_root();
+    weak_timers_heap_.store(timers_heap);
+    return true;
+  } else {
+    return false;
+  }
+}

rclcpp/test/rclcpp/executors/test_events_executor.cpp

@@ -254,6 +254,9 @@ TEST_F(TestEventsExecutor, spin_all_max_duration)
         t_runs++;
       });
 
+    // Sleep some time for the timer to be ready when spin
+    std::this_thread::sleep_for(10ms);
+
     EventsExecutor executor;
     executor.add_node(node);
 
@@ -265,7 +268,6 @@ TEST_F(TestEventsExecutor, spin_all_max_duration)
   }
 
   EventsExecutor executor;
-  EXPECT_THROW(executor.spin_all(0ms), std::invalid_argument);
   EXPECT_THROW(executor.spin_all(-5ms), std::invalid_argument);
 }