spin_some and spin_all call spin_some_impl

Author: Mauro Passerino

rclcpp/include/rclcpp/executors/events_executor.hpp

@@ -197,6 +197,10 @@ class EventsExecutor : public rclcpp::Executor
   void
   spin_once_impl(std::chrono::nanoseconds timeout) override;
 
+  RCLCPP_PUBLIC
+  void
+  spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive);
+
 private:
   RCLCPP_DISABLE_COPY(EventsExecutor)
 

rclcpp/include/rclcpp/executors/timers_manager.hpp

@@ -101,11 +101,12 @@ class TimersManager
   std::chrono::nanoseconds execute_ready_timers();
 
   /**
-   * @brief Executes one ready timer if available
-   *
-   * @return true if there was a timer ready
+   * @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();
+  bool execute_head_timer(
+    std::chrono::time_point<std::chrono::steady_clock> tp = TimePoint::max());
 
   /**
    * @brief Get the amount of time before the next timer expires.
@@ -127,19 +128,13 @@ 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
   static constexpr std::chrono::nanoseconds MAX_TIME = std::chrono::hours(90);
 
+  using TimePoint = std::chrono::time_point<std::chrono::steady_clock>;
+
 private:
   RCLCPP_DISABLE_COPY(TimersManager)
 

rclcpp/src/rclcpp/executors/events_executor.cpp

@@ -79,107 +79,75 @@ EventsExecutor::spin()
 void
 EventsExecutor::spin_some(std::chrono::nanoseconds max_duration)
 {
-  if (spinning.exchange(true)) {
-    throw std::runtime_error("spin_some() called while already spinning");
-  }
-  RCLCPP_SCOPE_EXIT(this->spinning.store(false););
-
   // 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;
-    };
-
-  // 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();
-
-  size_t executed_events = 0;
-
-  // 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;
-    };
-
-  // 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);
-        executed_events++;
-        continue;
-      }
-    }
-
-    // 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;
-  }
+  return this->spin_some_impl(max_duration, false);
 }
 
 void
 EventsExecutor::spin_all(std::chrono::nanoseconds max_duration)
 {
-  if (spinning.exchange(true)) {
-    throw std::runtime_error("spin_all() called while already spinning");
+  if (max_duration <= 0ns) {
+    throw std::invalid_argument("max_duration must be positive");
   }
+  return this->spin_some_impl(max_duration, true);
+}
 
-  if (max_duration < 0ns) {
-    throw std::invalid_argument("max_duration must be positive");
+void
+EventsExecutor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive)
+{
+  if (spinning.exchange(true)) {
+    throw std::runtime_error("spin_some() called while already spinning");
   }
 
   RCLCPP_SCOPE_EXIT(this->spinning.store(false););
 
-  // 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;
     };
 
-  // Execute timer and events until timeout or no more work available
+  size_t ready_events_at_start = 0;
+  size_t executed_events = 0;
+
+  if (!exhaustive) {
+    // Get the number of events ready at start
+    std::unique_lock<std::mutex> lock(push_mutex_);
+    ready_events_at_start = events_queue_->size();
+    lock.unlock();
+  }
+
   while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) {
-    // Execute first event from queue if it exists
-    {
+    // Execute first ready event from queue if exists
+    if (exhaustive || (executed_events < ready_events_at_start)) {
       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();
         this->execute_event(event);
+        executed_events++;
         continue;
       }
     }
 
-    // Execute timer, if was ready
-    if (timers_manager_->execute_head_timer()) {
+    bool timer_executed;
+
+    if (exhaustive) {
+      // Execute timer if is ready
+      timer_executed = timers_manager_->execute_head_timer();
+    } else {
+      // Execute timer if was ready at start
+      timer_executed = timers_manager_->execute_head_timer(start);
+    }
+
+    if (timer_executed) {
       continue;
     }
 

rclcpp/src/rclcpp/executors/timers_manager.cpp

@@ -116,7 +116,8 @@ std::chrono::nanoseconds TimersManager::execute_ready_timers()
   return this->get_head_timeout_unsafe(timers_heap);
 }
 
-bool TimersManager::execute_head_timer()
+bool TimersManager::execute_head_timer(
+  std::chrono::time_point<std::chrono::steady_clock> timepoint)
 {
   // Do not allow to interfere with the thread running
   if (running_) {
@@ -127,13 +128,27 @@ bool TimersManager::execute_head_timer()
   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();
-  if (head->is_ready()) {
+
+  bool timer_ready = false;
+
+  if (timepoint != TimePoint::max()) {
+    // 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();
+
+    // Here we check if the timer was already ready at timepoint
+    timer_ready = timepoint_timer_ready < timepoint;
+  } else {
+    timer_ready = head->is_ready();
+  }
+
+  if (timer_ready) {
     // Head timer is ready, execute
     head->execute_callback();
     timers_heap.heapify_root();
@@ -230,40 +245,3 @@ 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

@@ -268,6 +268,7 @@ 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);
 }
 

rclcpp/test/rclcpp/executors/test_executors.cpp

@@ -501,6 +501,7 @@ TYPED_TEST(TestExecutorsStable, spinAll) {
   // executor.
   bool spin_exited = false;
   std::thread spinner([&spin_exited, &executor, this]() {
+      std::this_thread::sleep_for(10ms);
       executor.spin_all(1s);
       executor.remove_node(this->node, true);
       spin_exited = true;