From 26163d09ebeb4678381ff2bd0a30ce6e179537bc Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Mon, 28 Feb 2022 11:08:15 +0000 Subject: [PATCH 1/2] Use new rcl_timer_get_next_call_time Signed-off-by: Mauro Passerino --- rclcpp/include/rclcpp/timer.hpp | 9 +++++++ rclcpp/include/rclcpp/timers_manager.hpp | 3 +-- rclcpp/src/rclcpp/timer.cpp | 32 +++++++++++++++++++++++- 3 files changed, 41 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 52df423d83..1f92937f42 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -120,6 +120,15 @@ class TimerBase std::chrono::nanoseconds time_until_trigger(); + /// Returns a time object indicating when the timer has its next scheduled callback. + /** + * \return A rclcpp::Time representing when the next callback should be executed. + * \throws std::runtime_error if the rcl_timer_get_next_call_time returns a failure + */ + RCLCPP_PUBLIC + rclcpp::Time + next_call_time(); + /// Is the clock steady (i.e. is the time between ticks constant?) /** \return True if the clock used by this timer is steady. */ virtual bool is_steady() = 0; diff --git a/rclcpp/include/rclcpp/timers_manager.hpp b/rclcpp/include/rclcpp/timers_manager.hpp index 37228a7fc0..97b05e2603 100644 --- a/rclcpp/include/rclcpp/timers_manager.hpp +++ b/rclcpp/include/rclcpp/timers_manager.hpp @@ -460,8 +460,7 @@ class TimersManager */ static bool timer_greater(TimerPtr a, TimerPtr b) { - // FIXME! - return a->time_until_trigger() > b->time_until_trigger(); + return a->next_call_time() >= b->next_call_time(); } std::vector owned_heap_; diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 70414f38c6..4d5b76b336 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -116,8 +116,17 @@ TimerBase::is_ready() std::chrono::nanoseconds TimerBase::time_until_trigger() { + bool is_canceled = false; + rcl_ret_t ret = rcl_timer_is_canceled(timer_handle_.get(), &is_canceled); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't get timer cancelled state"); + } + if (is_canceled) { + return std::chrono::nanoseconds::max(); + } + int64_t time_until_next_call = 0; - rcl_ret_t ret = rcl_timer_get_time_until_next_call( + ret = rcl_timer_get_time_until_next_call( timer_handle_.get(), &time_until_next_call); if (ret != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error(ret, "Timer could not get time until next call"); @@ -125,6 +134,27 @@ TimerBase::time_until_trigger() return std::chrono::nanoseconds(time_until_next_call); } +rclcpp::Time +TimerBase::next_call_time() +{ + bool is_canceled = false; + rcl_ret_t ret = rcl_timer_is_canceled(timer_handle_.get(), &is_canceled); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't get timer cancelled state"); + } + if (is_canceled) { + // We don't use rclcpp::Time::max() because we want to specify the clock type + return rclcpp::Time(std::numeric_limits::max(), 999999999, clock_->get_clock_type()); + } + + rcl_time_point_value_t time_point = 0; + ret = rcl_timer_get_next_call_time(timer_handle_.get(), &time_point); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Timer could not get next call time"); + } + return rclcpp::Time(time_point, clock_->get_clock_type()); +} + std::shared_ptr TimerBase::get_timer_handle() { From aa6db4bd69416334577830ab0fb7337b4645a7c6 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Mon, 28 Feb 2022 11:08:48 +0000 Subject: [PATCH 2/2] Call timer call() to reset next trigger time Signed-off-by: Mauro Passerino --- rclcpp/src/rclcpp/timers_manager.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp/src/rclcpp/timers_manager.cpp b/rclcpp/src/rclcpp/timers_manager.cpp index a25fe66b87..1038ce92e9 100644 --- a/rclcpp/src/rclcpp/timers_manager.cpp +++ b/rclcpp/src/rclcpp/timers_manager.cpp @@ -165,6 +165,7 @@ void TimersManager::execute_ready_timer(const void * timer_id) ready_timer = weak_timers_heap_.get_timer(timer_id); } if (ready_timer) { + ready_timer->call(); ready_timer->execute_callback(); } }