From 21d0a5ddc6a249775c7437394191b89dc1aa5415 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Sat, 1 Jul 2023 21:41:31 +0000 Subject: [PATCH] Add in the ability to start timers paused. This was possible in the past by creating a timer and then immediately canceling it. But that could lead to a potential race where the timer fired once before it was canceled. By allowing for the user to create it paused, we avoid that race. This is the rclpy version of https://github.com/ros2/rclcpp/pull/2005 Signed-off-by: Chris Lalancette --- rclpy/rclpy/node.py | 5 ++++- rclpy/rclpy/timer.py | 19 +++++++++++++++++-- rclpy/src/rclpy/timer.cpp | 6 +++--- rclpy/src/rclpy/timer.hpp | 3 ++- rclpy/test/test_timer.py | 22 ++++++++++++++++++++++ rclpy/test/test_waitable.py | 2 +- 6 files changed, 49 insertions(+), 8 deletions(-) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 45698c226..00617f00b 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -1676,6 +1676,7 @@ def create_timer( callback: Callable, callback_group: Optional[CallbackGroup] = None, clock: Optional[Clock] = None, + autostart: Optional[bool] = True, ) -> Timer: """ Create a new timer. @@ -1694,7 +1695,9 @@ def create_timer( callback_group = self.default_callback_group if clock is None: clock = self._clock - timer = Timer(callback, callback_group, timer_period_nsec, clock, context=self.context) + timer = Timer( + callback, callback_group, timer_period_nsec, clock, context=self.context, + autostart=autostart) callback_group.add_entity(timer) self._timers.append(timer) diff --git a/rclpy/rclpy/timer.py b/rclpy/rclpy/timer.py index 0c3be6d8e..b55995607 100644 --- a/rclpy/rclpy/timer.py +++ b/rclpy/rclpy/timer.py @@ -14,6 +14,12 @@ import threading +from typing import Callable +from typing import Optional + +from rclpy.callback_groups import CallbackGroup +from rclpy.clock import Clock +from rclpy.context import Context from rclpy.exceptions import InvalidHandle, ROSInterruptException from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.utilities import get_default_context @@ -21,12 +27,21 @@ class Timer: - def __init__(self, callback, callback_group, timer_period_ns, clock, *, context=None): + def __init__( + self, + callback: Callable, + callback_group: CallbackGroup, + timer_period_ns: int, + clock: Clock, + *, + context: Optional[Context] = None, + autostart: Optional[bool] = True + ): self._context = get_default_context() if context is None else context self._clock = clock with self._clock.handle, self._context.handle: self.__timer = _rclpy.Timer( - self._clock.handle, self._context.handle, timer_period_ns) + self._clock.handle, self._context.handle, timer_period_ns, autostart) self.timer_period_ns = timer_period_ns self.callback = callback self.callback_group = callback_group diff --git a/rclpy/src/rclpy/timer.cpp b/rclpy/src/rclpy/timer.cpp index 13de155f7..c3e744f09 100644 --- a/rclpy/src/rclpy/timer.cpp +++ b/rclpy/src/rclpy/timer.cpp @@ -36,7 +36,7 @@ Timer::destroy() } Timer::Timer( - Clock & clock, Context & context, int64_t period_nsec) + Clock & clock, Context & context, int64_t period_nsec, bool autostart) : context_(context), clock_(clock) { // Create a client @@ -61,7 +61,7 @@ Timer::Timer( rcl_ret_t ret = rcl_timer_init2( rcl_timer_.get(), clock_.rcl_ptr(), context.rcl_ptr(), - period_nsec, NULL, allocator, true); + period_nsec, NULL, allocator, autostart); if (RCL_RET_OK != ret) { throw RCLError("failed to create timer"); @@ -159,7 +159,7 @@ void define_timer(py::object module) { py::class_>(module, "Timer") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Timer & timer) { return reinterpret_cast(timer.rcl_ptr()); diff --git a/rclpy/src/rclpy/timer.hpp b/rclpy/src/rclpy/timer.hpp index 929a8df6a..0f3393128 100644 --- a/rclpy/src/rclpy/timer.hpp +++ b/rclpy/src/rclpy/timer.hpp @@ -46,9 +46,10 @@ class Timer : public Destroyable, public std::enable_shared_from_this * \param[in] clock pycapsule containing an rcl_clock_t * \param[in] context Capsule for an rcl_timer_t * \param[in] period_nsec The period of the timer in nanoseconds + * \param[in] autostart Whether to automatically start the timer * \return a timer capsule */ - Timer(Clock & clock, Context & context, int64_t period_nsec); + Timer(Clock & clock, Context & context, int64_t period_nsec, bool autostart); ~Timer() = default; diff --git a/rclpy/test/test_timer.py b/rclpy/test/test_timer.py index e52ee7d7a..394f75036 100644 --- a/rclpy/test/test_timer.py +++ b/rclpy/test/test_timer.py @@ -180,3 +180,25 @@ def test_time_until_next_call(): if node is not None: node.destroy_node() rclpy.shutdown(context=context) + + +def test_timer_without_autostart(): + node = None + timer = None + rclpy.init() + try: + node = rclpy.create_node('test_timer_without_autostart') + timer = node.create_timer(1, lambda: None, autostart=False) + assert timer.is_canceled() + + timer.reset() + assert not timer.is_canceled() + + timer.cancel() + assert timer.is_canceled() + finally: + if timer is not None: + node.destroy_timer(timer) + if node is not None: + node.destroy_node() + rclpy.shutdown() diff --git a/rclpy/test/test_waitable.py b/rclpy/test/test_waitable.py index f79841c51..5455906dd 100644 --- a/rclpy/test/test_waitable.py +++ b/rclpy/test/test_waitable.py @@ -131,7 +131,7 @@ def __init__(self, node): period_nanoseconds = 10000 with self._clock.handle, node.context.handle: self.timer = _rclpy.Timer( - self._clock.handle, node.context.handle, period_nanoseconds) + self._clock.handle, node.context.handle, period_nanoseconds, True) self.timer_index = None self.timer_is_ready = False