std::shared_ptr<rclcpp::Context> context,

std::function<void(const rclcpp::TimerBase *, const std::shared_ptr<void> &)> on_ready_callback)

: on_ready_callback_(on_ready_callback),

context_(std::move(context))

{

{

if (!timer) {

throw std::invalid_argument("TimersManager::add_timer() trying to add nullptr timer");

}

bool added = false;

{

std::unique_lock<std::mutex> lock(timers_mutex_);

added = weak_timers_heap_.add_timer(timer);

timers_updated_ = timers_updated_ || added;

}

timer->set_on_reset_callback(

[this](size_t arg) {

{

(void)arg;

std::unique_lock<std::mutex> lock(timers_mutex_);

timers_updated_ = true;

}

timers_cv_.notify_one();

});

if (added) {

// Notify that a timer has been added

timers_cv_.notify_one();

}

{

// Make sure that the thread is not already running

if (running_.exchange(true)) {

throw std::runtime_error("TimersManager::start() can't start timers thread as already running");

}

timers_thread_ = std::thread(&TimersManager::run_timers, this);

{

// Lock stop() function to prevent race condition in destructor

std::unique_lock<std::mutex> lock(stop_mutex_);

running_ = false;

// Notify the timers manager thread to wake up

{

std::unique_lock<std::mutex> lock(timers_mutex_);

timers_updated_ = true;

}

timers_cv_.notify_one();

// Join timers thread if it's running

if (timers_thread_.joinable()) {

timers_thread_.join();

}

{

// Do not allow to interfere with the thread running

if (running_) {

throw std::runtime_error(

"get_head_timeout() can't be used while timers thread is running");

}

std::unique_lock<std::mutex> lock(timers_mutex_);

return this->get_head_timeout_unsafe();

{

// Do not allow to interfere with the thread running

if (running_) {

throw std::runtime_error(

"get_number_ready_timers() can't be used while timers thread is running");

}

std::unique_lock<std::mutex> lock(timers_mutex_);

TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();

return locked_heap.get_number_ready_timers();

{

// Do not allow to interfere with the thread running

if (running_) {

throw std::runtime_error(

"execute_head_timer() can't be used while timers thread is running");

}

std::unique_lock<std::mutex> lock(timers_mutex_);

TimersHeap timers_heap = weak_timers_heap_.validate_and_lock();

// Nothing to do if we don't have any timer

if (timers_heap.empty()) {

return false;

}

TimerPtr head_timer = timers_heap.front();

const bool timer_ready = head_timer->is_ready();

if (timer_ready) {

// NOTE: here we always execute the timer, regardless of whether the

// on_ready_callback is set or not.

auto data = head_timer->call();

if (!data) {

// someone canceled the timer between is_ready and call

return false;

}

head_timer->execute_callback(data);

timers_heap.heapify_root();

weak_timers_heap_.store(timers_heap);

}

return timer_ready;

const rclcpp::TimerBase * timer_id,

const std::shared_ptr<void> & data)

{

TimerPtr ready_timer;

{

std::unique_lock<std::mutex> lock(timers_mutex_);

ready_timer = weak_timers_heap_.get_timer(timer_id);

}

if (ready_timer) {

ready_timer->execute_callback(data);

}

{

// If we don't have any weak pointer, then we just return maximum timeout

if (weak_timers_heap_.empty()) {

return std::chrono::nanoseconds::max();

}

// Weak heap is not empty, so try to lock the first element.

// If it is still a valid pointer, it is guaranteed to be the correct head

TimerPtr head_timer = weak_timers_heap_.front().lock();

if (!head_timer) {

// The first element has expired, we can't make other assumptions on the heap

// and we need to entirely validate it.

TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();

// NOTE: the following operations will not modify any element in the heap, so we

// don't have to call `weak_timers_heap_.store(locked_heap)` at the end.

if (locked_heap.empty()) {

return std::chrono::nanoseconds::max();

}

head_timer = locked_heap.front();

}

if (head_timer->is_canceled()) {

return std::nullopt;

}

return head_timer->time_until_trigger();

{

// We start by locking the timers

TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();

// Nothing to do if we don't have any timer

if (locked_heap.empty()) {

return;

}

// Keep executing timers until they are ready and they were already ready when we started.

// The two checks prevent this function from blocking indefinitely if the

// time required for executing the timers is longer than their period.

TimerPtr head_timer = locked_heap.front();

const size_t number_ready_timers = locked_heap.get_number_ready_timers();

size_t executed_timers = 0;

while (executed_timers < number_ready_timers && head_timer->is_ready()) {

auto data = head_timer->call();

if (data) {

if (on_ready_callback_) {

on_ready_callback_(head_timer.get(), data);

} else {

head_timer->execute_callback(data);

}

} else {

// someone canceled the timer between is_ready and call

// we don't do anything, as the timer is now 'processed'

}

executed_timers++;

// Executing a timer will result in updating its time_until_trigger, so re-heapify

locked_heap.heapify_root();

// Get new head timer

head_timer = locked_heap.front();

}

// After having performed work on the locked heap we reflect the changes to weak one.

// Timers will be already sorted the next time we need them if none went out of scope.

weak_timers_heap_.store(locked_heap);

{

// Make sure the running flag is set to false when we exit from this function

// to allow restarting the timers thread.

RCPPUTILS_SCOPE_EXIT(this->running_.store(false); );

while (rclcpp::ok(context_) && running_) {

// Lock mutex

std::unique_lock<std::mutex> lock(timers_mutex_);

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();

}

// 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

timers_updated_ = false;

// Execute timers

this->execute_ready_timers_unsafe();

}

{

{

// Lock mutex and then clear all data structures

std::unique_lock<std::mutex> lock(timers_mutex_);

TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();

locked_heap.clear_timers_on_reset_callbacks();

weak_timers_heap_.clear();

timers_updated_ = true;

}

// Notify timers thread such that it can re-compute its timeout

timers_cv_.notify_one();

{

bool removed = false;

{

std::unique_lock<std::mutex> lock(timers_mutex_);

removed = weak_timers_heap_.remove_timer(timer);

timers_updated_ = timers_updated_ || removed;

}

if (removed) {

// Notify timers thread such that it can re-compute its timeout

timers_cv_.notify_one();

timer->clear_on_reset_callback();

}