fix: Removed peek_next_ready_timer and replaced it with next_ready_timer

This fixes multiple bugs with not advancing the index correctly, and
clearing the timer from the waitset in certain conditions.

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

rclcpp/include/rclcpp/wait_result.hpp

@@ -142,66 +142,23 @@ class WaitResult final
     }
   }
 
-  /// 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)
+  /// Get the next ready timer, clearing it from the wait result.
+  std::shared_ptr<rclcpp::TimerBase>
+  next_ready_timer()
   {
     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) {
+      for (size_t ii = 0; ii < wait_set.size_of_timers(); ++ii) {
         if (rcl_wait_set.timers[ii] != nullptr) {
-          ret = wait_set.timers(ii);
+          wait_set.timers(ii).swap(ret);
           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;
+    return ret;
   }
 
   /// Get the next ready subscription, clearing it from the wait result.

rclcpp/src/rclcpp/executor.cpp

@@ -679,24 +679,13 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable)
   }
 
   if (!valid_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;
+    while (auto timer = wait_result_->next_ready_timer()) {
       auto entity_iter = current_collection_.timers.find(timer->get_timer_handle().get());
       if (entity_iter != current_collection_.timers.end()) {
         auto callback_group = entity_iter->second.callback_group.lock();
         if (callback_group && !callback_group->can_be_taken_from()) {
           continue;
         }
-        // At this point the timer is either ready for execution or was perhaps
-        // it was canceled, based on the result of call(), but either way it
-        // should not be checked again from peek_next_ready_timer(), so clear
-        // it from the wait result.
-        wait_result_->clear_timer_with_index(current_timer_index);
         // Check that the timer should be called still, i.e. it wasn't canceled.
         any_executable.data = timer->call();
         if (!any_executable.data) {

rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp

@@ -144,16 +144,9 @@ bool StaticSingleThreadedExecutor::execute_ready_executables(
     }
   }
 
-  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;
+  while (auto timer = wait_result.next_ready_timer()) {
     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