Skip to content

Commit

Permalink
Add in the ability to start timers paused.
Browse files Browse the repository at this point in the history
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 ros2/rclcpp#2005

Signed-off-by: Chris Lalancette <clalancette@gmail.com>
  • Loading branch information
clalancette committed Jul 2, 2023
1 parent 739e05b commit 21d0a5d
Show file tree
Hide file tree
Showing 6 changed files with 49 additions and 8 deletions.
5 changes: 4 additions & 1 deletion rclpy/rclpy/node.py
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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)
Expand Down
19 changes: 17 additions & 2 deletions rclpy/rclpy/timer.py
Original file line number Diff line number Diff line change
Expand Up @@ -14,19 +14,34 @@

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


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
Expand Down
6 changes: 3 additions & 3 deletions rclpy/src/rclpy/timer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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");
Expand Down Expand Up @@ -159,7 +159,7 @@ void
define_timer(py::object module)
{
py::class_<Timer, Destroyable, std::shared_ptr<Timer>>(module, "Timer")
.def(py::init<Clock &, Context &, int64_t>())
.def(py::init<Clock &, Context &, int64_t, bool>())
.def_property_readonly(
"pointer", [](const Timer & timer) {
return reinterpret_cast<size_t>(timer.rcl_ptr());
Expand Down
3 changes: 2 additions & 1 deletion rclpy/src/rclpy/timer.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -46,9 +46,10 @@ class Timer : public Destroyable, public std::enable_shared_from_this<Timer>
* \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;

Expand Down
22 changes: 22 additions & 0 deletions rclpy/test/test_timer.py
Original file line number Diff line number Diff line change
Expand Up @@ -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()
2 changes: 1 addition & 1 deletion rclpy/test/test_waitable.py
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down

0 comments on commit 21d0a5d

Please sign in to comment.