Skip to content

Commit

Permalink
[events executor] - Fix Behavior with Timer Cancel (#2375)
Browse files Browse the repository at this point in the history
* fix

Signed-off-by: Matt Condino <mwcondino@gmail.com>

* add timer cancel tests

Signed-off-by: Matt Condino <mwcondino@gmail.com>

* cleanup header include

Signed-off-by: Matt Condino <mwcondino@gmail.com>

* reverting change to timer_greater function

Signed-off-by: Gus Brigantino <gusbrig97@gmail.com>

* use std::optional, and handle edgecase of 1 cancelled timer

Signed-off-by: Matt Condino <mwcondino@gmail.com>

* clean up run_timers func

Signed-off-by: Gus Brigantino <gusbrig97@gmail.com>

* some fixes and added tests for cancel then reset of timers.

Signed-off-by: Matt Condino <mwcondino@gmail.com>

* refactor and clean up. remove cancelled timer tracking.

Signed-off-by: Matt Condino <mwcondino@gmail.com>

* remove unused method for size()

Signed-off-by: Matt Condino <mwcondino@gmail.com>

* linting

Signed-off-by: Matt Condino <mwcondino@gmail.com>

* relax timing constraints in tests

Signed-off-by: Matt Condino <mwcondino@gmail.com>

* further relax timing constraints to ensure windows tests are not flaky.

Signed-off-by: Matt Condino <mwcondino@gmail.com>

* use sim clock for tests, pub clock at .25 realtime rate.

Signed-off-by: Matt Condino <mwcondino@gmail.com>

---------

Signed-off-by: Matt Condino <mwcondino@gmail.com>
Signed-off-by: Gus Brigantino <gusbrig97@gmail.com>
Co-authored-by: Gus Brigantino <gusbrig97@gmail.com>
  • Loading branch information
mwcondino and gusbrigantino authored Jan 30, 2024
1 parent 265f5ec commit 99f0fc9
Show file tree
Hide file tree
Showing 5 changed files with 468 additions and 21 deletions.
12 changes: 7 additions & 5 deletions rclcpp/include/rclcpp/experimental/timers_manager.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -22,10 +22,10 @@
#include <functional>
#include <memory>
#include <mutex>
#include <optional>
#include <thread>
#include <utility>
#include <vector>

#include "rclcpp/context.hpp"
#include "rclcpp/timer.hpp"

Expand Down Expand Up @@ -172,13 +172,14 @@ class TimersManager
* @brief Get the amount of time before the next timer triggers.
* This function is thread safe.
*
* @return std::chrono::nanoseconds to wait,
* @return std::optional<std::chrono::nanoseconds> to wait,
* the returned value could be negative if the timer is already expired
* or std::chrono::nanoseconds::max() if there are no timers stored in the object.
* If the head timer was cancelled, then this will return a nullopt.
* @throws std::runtime_error if the timers thread was already running.
*/
RCLCPP_PUBLIC
std::chrono::nanoseconds get_head_timeout();
std::optional<std::chrono::nanoseconds> get_head_timeout();

private:
RCLCPP_DISABLE_COPY(TimersManager)
Expand Down Expand Up @@ -512,12 +513,13 @@ class TimersManager
* @brief Get the amount of time before the next timer triggers.
* This function is not thread safe, acquire a mutex before calling it.
*
* @return std::chrono::nanoseconds to wait,
* @return std::optional<std::chrono::nanoseconds> to wait,
* the returned value could be negative if the timer is already expired
* or std::chrono::nanoseconds::max() if the heap is empty.
* If the head timer was cancelled, then this will return a nullopt.
* This function is not thread safe, acquire the timers_mutex_ before calling it.
*/
std::chrono::nanoseconds get_head_timeout_unsafe();
std::optional<std::chrono::nanoseconds> get_head_timeout_unsafe();

/**
* @brief Executes all the timers currently ready when the function is invoked
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -206,11 +206,12 @@ EventsExecutor::spin_once_impl(std::chrono::nanoseconds timeout)
timeout = std::chrono::nanoseconds::max();
}

// Select the smallest between input timeout and timer timeout
// Select the smallest between input timeout and timer timeout.
// Cancelled timers are not considered.
bool is_timer_timeout = false;
auto next_timer_timeout = timers_manager_->get_head_timeout();
if (next_timer_timeout < timeout) {
timeout = next_timer_timeout;
if (next_timer_timeout.has_value() && next_timer_timeout.value() < timeout) {
timeout = next_timer_timeout.value();
is_timer_timeout = true;
}

Expand Down
45 changes: 32 additions & 13 deletions rclcpp/src/rclcpp/experimental/timers_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -100,7 +100,7 @@ void TimersManager::stop()
}
}

std::chrono::nanoseconds TimersManager::get_head_timeout()
std::optional<std::chrono::nanoseconds> TimersManager::get_head_timeout()
{
// Do not allow to interfere with the thread running
if (running_) {
Expand Down Expand Up @@ -169,7 +169,7 @@ void TimersManager::execute_ready_timer(const rclcpp::TimerBase * timer_id)
}
}

std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe()
std::optional<std::chrono::nanoseconds> TimersManager::get_head_timeout_unsafe()
{
// If we don't have any weak pointer, then we just return maximum timeout
if (weak_timers_heap_.empty()) {
Expand All @@ -191,7 +191,9 @@ std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe()
}
head_timer = locked_heap.front();
}

if (head_timer->is_canceled()) {
return std::nullopt;
}
return head_timer->time_until_trigger();
}

Expand Down Expand Up @@ -242,17 +244,34 @@ void TimersManager::run_timers()
// Lock mutex
std::unique_lock<std::mutex> lock(timers_mutex_);

std::chrono::nanoseconds time_to_sleep = get_head_timeout_unsafe();
std::optional<std::chrono::nanoseconds> time_to_sleep = get_head_timeout_unsafe();

// If head timer was cancelled, try to reheap and get a new head.
// This avoids an edge condition where head timer is cancelled, but other
// valid timers remain in the heap.
if (!time_to_sleep.has_value()) {
// Re-heap to (possibly) move cancelled timer from head of heap. If
// entire heap is cancelled, this will still result in a nullopt.
TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
locked_heap.heapify();
weak_timers_heap_.store(locked_heap);
time_to_sleep = get_head_timeout_unsafe();
}

// 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 {
// Wait until notification that timers have been updated
timers_cv_.wait(lock, [this]() {return timers_updated_;});
}
// If no timers, or all timers cancelled, wait for an update.
if (!time_to_sleep.has_value() || (time_to_sleep.value() == std::chrono::nanoseconds::max()) ) {
// Wait until notification that timers have been updated
timers_cv_.wait(lock, [this]() {return timers_updated_;});

// Re-heap in case ordering changed due to a cancelled timer
// re-activating.
TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
locked_heap.heapify();
weak_timers_heap_.store(locked_heap);
} else if (time_to_sleep.value() != std::chrono::nanoseconds::zero()) {
// If time_to_sleep is zero, we immediately execute. Otherwise, wait
// until timeout or notification that timers have been updated
timers_cv_.wait_for(lock, time_to_sleep.value(), [this]() {return timers_updated_;});
}

// Reset timers updated flag
Expand Down
Loading

0 comments on commit 99f0fc9

Please sign in to comment.