diff --git a/rclpy/src/rclpy/timer.cpp b/rclpy/src/rclpy/timer.cpp index c058e8af0..19c4497d1 100644 --- a/rclpy/src/rclpy/timer.cpp +++ b/rclpy/src/rclpy/timer.cpp @@ -105,11 +105,13 @@ void Timer::change_timer_period(int64_t period_nsec) } } -int64_t Timer::time_until_next_call() +std::optional Timer::time_until_next_call() { int64_t remaining_time; rcl_ret_t ret = rcl_timer_get_time_until_next_call(rcl_timer_.get(), &remaining_time); - if (ret != RCL_RET_OK) { + if (ret == RCL_RET_TIMER_CANCELED) { + return std::nullopt; + } else if (ret != RCL_RET_OK) { throw RCLError("failed to get time until next timer call"); } diff --git a/rclpy/src/rclpy/timer.hpp b/rclpy/src/rclpy/timer.hpp index 68195289c..971006fe8 100644 --- a/rclpy/src/rclpy/timer.hpp +++ b/rclpy/src/rclpy/timer.hpp @@ -16,6 +16,7 @@ #define RCLPY__TIMER_HPP_ #include +#include #include @@ -87,9 +88,10 @@ class Timer : public Destroyable, public std::enable_shared_from_this * * Raises RCLError there is an rcl error * - * \return the time until next call in nanoseconds + * \return the time until next call in nanoseconds. + * std::nullopt if the timer is canceled. */ - int64_t time_until_next_call(); + std::optional time_until_next_call(); /// Get the time since the timer has been called /** diff --git a/rclpy/test/test_timer.py b/rclpy/test/test_timer.py index 6480f6f1a..e52ee7d7a 100644 --- a/rclpy/test/test_timer.py +++ b/rclpy/test/test_timer.py @@ -18,6 +18,7 @@ import pytest import rclpy +from rclpy.constants import S_TO_NS from rclpy.executors import SingleThreadedExecutor @@ -49,7 +50,7 @@ def test_zero_callback(period): executor = SingleThreadedExecutor(context=context) try: executor.add_node(node) - # The first spin_once() takes long enough for 1ms timer tests to fail + # The first spin_once() takes long enough for 1ms timer tests to fail executor.spin_once(timeout_sec=0) callbacks = [] @@ -78,7 +79,7 @@ def test_number_callbacks(period): executor = SingleThreadedExecutor(context=context) try: executor.add_node(node) - # The first spin_once() takes long enough for 1ms timer tests to fail + # The first spin_once() takes long enough for 1ms timer tests to fail executor.spin_once(timeout_sec=0) callbacks = [] @@ -110,7 +111,7 @@ def test_cancel_reset(period): executor = SingleThreadedExecutor(context=context) try: executor.add_node(node) - # The first spin_once() takes long enough for 1ms timer tests to fail + # The first spin_once() takes long enough for 1ms timer tests to fail executor.spin_once(timeout_sec=0) callbacks = [] @@ -148,3 +149,34 @@ def test_cancel_reset(period): node.destroy_node() finally: rclpy.shutdown(context=context) + + +def test_time_until_next_call(): + node = None + executor = None + timer = None + context = rclpy.context.Context() + rclpy.init(context=context) + try: + node = rclpy.create_node('test_time_until_next_call', context=context) + executor = SingleThreadedExecutor(context=context) + executor.add_node(node) + executor.spin_once(timeout_sec=0) + timer = node.create_timer(1, lambda: None) + assert not timer.is_canceled() + executor.spin_once(0.1) + assert timer.time_until_next_call() <= (1 * S_TO_NS) + timer.reset() + assert not timer.is_canceled() + assert timer.time_until_next_call() <= (1 * S_TO_NS) + timer.cancel() + assert timer.is_canceled() + assert timer.time_until_next_call() is None + finally: + if timer is not None: + node.destroy_timer(timer) + if executor is not None: + executor.shutdown() + if node is not None: + node.destroy_node() + rclpy.shutdown(context=context)