diff --git a/rclcpp/include/rclcpp/wait_result.hpp b/rclcpp/include/rclcpp/wait_result.hpp index 74d3d2183c..6a4c7ea8b7 100644 --- a/rclcpp/include/rclcpp/wait_result.hpp +++ b/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, 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 + next_ready_timer() { check_wait_result_dirty(); auto ret = std::shared_ptr{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. diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 49f9e33891..332cd29738 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/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) { diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 2d837103e5..08c4841d00 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/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