From 5d99e3b594dd9330540327aa74c7b28212f35466 Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Mon, 8 Apr 2024 21:04:16 +0200 Subject: [PATCH] refactor(wait_Result): Remove clear_timer_with_index construct Signed-off-by: Janosch Machowinski --- rclcpp/include/rclcpp/wait_result.hpp | 49 ++++--------------- rclcpp/src/rclcpp/executor.cpp | 16 +----- .../static_single_threaded_executor.cpp | 9 +--- 3 files changed, 12 insertions(+), 62 deletions(-) diff --git a/rclcpp/include/rclcpp/wait_result.hpp b/rclcpp/include/rclcpp/wait_result.hpp index 74d3d2183c..9ac9346047 100644 --- a/rclcpp/include/rclcpp/wait_result.hpp +++ b/rclcpp/include/rclcpp/wait_result.hpp @@ -142,18 +142,8 @@ class WaitResult final } } - /// Get the next ready timer and its index in the wait result, but do not clear it. + /// Get the next ready timer and its index in the wait result /** - * 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. * @@ -164,44 +154,24 @@ class WaitResult final * \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) + 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) { - if (rcl_wait_set.timers[ii] != nullptr) { - ret = wait_set.timers(ii); + for (; next_timer_index_ < wait_set.size_of_timers(); ++next_timer_index_) { + if (rcl_wait_set.timers[next_timer_index_] != nullptr) { + ret = wait_set.timers(next_timer_index_); + rcl_wait_set.timers[next_timer_index_] = nullptr; + ++next_timer_index_; 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. @@ -322,6 +292,7 @@ class WaitResult final WaitSetT * wait_set_pointer_ = nullptr; + size_t next_timer_index_ = 0; size_t next_waitable_index_ = 0; }; diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 87b71e29ce..382ccb6de7 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -676,28 +676,15 @@ 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()) { - current_timer_index++; 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. if (!timer->call()) { - current_timer_index++; continue; } any_executable.timer = timer; @@ -705,7 +692,6 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) valid_executable = true; break; } - current_timer_index++; } } diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index b8fe92cb09..4fb40c206d 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); if (timer->call()) { execute_timer(timer); any_ready_executable = true;