From 811af4ba057927e89b042f35d2c41df50218e5f3 Mon Sep 17 00:00:00 2001 From: Matt Condino Date: Tue, 28 Nov 2023 14:35:05 -0800 Subject: [PATCH] fix Signed-off-by: Matt Condino --- .../rclcpp/experimental/timers_manager.hpp | 12 +++++++++--- .../src/rclcpp/experimental/timers_manager.cpp | 18 ++++++++++++++---- 2 files changed, 23 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/timers_manager.hpp b/rclcpp/include/rclcpp/experimental/timers_manager.hpp index 688684376a..f89801ca00 100644 --- a/rclcpp/include/rclcpp/experimental/timers_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/timers_manager.hpp @@ -25,7 +25,6 @@ #include #include #include - #include "rclcpp/context.hpp" #include "rclcpp/timer.hpp" @@ -496,7 +495,14 @@ class TimersManager static bool timer_greater(TimerPtr a, TimerPtr b) { // TODO(alsora): this can cause an error if timers are using different clocks - return a->time_until_trigger() > b->time_until_trigger(); + bool both_canceled_or_active = a->is_canceled() == b->is_canceled(); + if (both_canceled_or_active) { + return a->time_until_trigger() > b->time_until_trigger(); + } + else if (a->is_canceled()) { + return true; + } + return false; } std::vector owned_heap_; @@ -517,7 +523,7 @@ class TimersManager * or std::chrono::nanoseconds::max() if the heap is empty. * This function is not thread safe, acquire the timers_mutex_ before calling it. */ - std::chrono::nanoseconds get_head_timeout_unsafe(); + std::chrono::nanoseconds get_head_timeout_unsafe(bool& head_was_canceled); /** * @brief Executes all the timers currently ready when the function is invoked diff --git a/rclcpp/src/rclcpp/experimental/timers_manager.cpp b/rclcpp/src/rclcpp/experimental/timers_manager.cpp index e179787b36..b6f562c03b 100644 --- a/rclcpp/src/rclcpp/experimental/timers_manager.cpp +++ b/rclcpp/src/rclcpp/experimental/timers_manager.cpp @@ -109,7 +109,8 @@ std::chrono::nanoseconds TimersManager::get_head_timeout() } std::unique_lock lock(timers_mutex_); - return this->get_head_timeout_unsafe(); + bool head_was_cancelled; + return this->get_head_timeout_unsafe(head_was_cancelled); } size_t TimersManager::get_number_ready_timers() @@ -169,7 +170,7 @@ void TimersManager::execute_ready_timer(const rclcpp::TimerBase * timer_id) } } -std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe() +std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe(bool& head_was_cancelled) { // If we don't have any weak pointer, then we just return maximum timeout if (weak_timers_heap_.empty()) { @@ -191,6 +192,7 @@ std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe() } head_timer = locked_heap.front(); } + head_was_cancelled = head_timer->is_canceled(); return head_timer->time_until_trigger(); } @@ -242,14 +244,22 @@ void TimersManager::run_timers() // Lock mutex std::unique_lock lock(timers_mutex_); - std::chrono::nanoseconds time_to_sleep = get_head_timeout_unsafe(); + bool head_was_cancelled = false; + std::chrono::nanoseconds time_to_sleep = get_head_timeout_unsafe(head_was_cancelled); // No need to wait if a timer is already available if (time_to_sleep > std::chrono::nanoseconds::zero()) { if (time_to_sleep != std::chrono::nanoseconds::max()) { // Wait until timeout or notification that timers have been updated timers_cv_.wait_for(lock, time_to_sleep, [this]() {return timers_updated_;}); - } else { + } + else if (head_was_cancelled) { + // Wait until notification that timers have been updated + TimersHeap locked_heap = weak_timers_heap_.validate_and_lock(); + locked_heap.heapify(); + weak_timers_heap_.store(locked_heap); + } + else { // Wait until notification that timers have been updated timers_cv_.wait(lock, [this]() {return timers_updated_;}); }