diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index 0aa6d1824..e61986e50 100644 --- a/rclpy/rclpy/executors.py +++ b/rclpy/rclpy/executors.py @@ -34,6 +34,8 @@ from rclpy.client import Client +from rclpy.clock import Clock +from rclpy.clock import ClockType from rclpy.context import Context from rclpy.guard_condition import GuardCondition from rclpy.handle import InvalidHandle @@ -43,7 +45,7 @@ from rclpy.subscription import Subscription from rclpy.task import Future from rclpy.task import Task -from rclpy.timer import WallTimer +from rclpy.timer import Timer from rclpy.utilities import get_default_context from rclpy.utilities import timeout_sec_to_nsec from rclpy.waitable import NumberOfEntities @@ -168,6 +170,8 @@ def __init__(self, *, context: Context = None) -> None: self._cb_iter = None self._last_args = None self._last_kwargs = None + # Executor cannot use ROS clock because that requires a node + self._clock = Clock(clock_type=ClockType.STEADY_TIME) self._sigint_gc = SignalHandlerGuardCondition(context) self._context.on_shutdown(self.wake) @@ -437,7 +441,7 @@ def _wait_for_ready_callbacks( timeout_timer = None timeout_nsec = timeout_sec_to_nsec(timeout_sec) if timeout_nsec > 0: - timeout_timer = WallTimer(None, None, timeout_nsec) + timeout_timer = Timer(None, None, timeout_nsec, self._clock) yielded_work = False while not yielded_work and not self._is_shutdown: @@ -463,7 +467,7 @@ def _wait_for_ready_callbacks( # Gather entities that can be waited on subscriptions: List[Subscription] = [] guards: List[GuardCondition] = [] - timers: List[WallTimer] = [] + timers: List[Timer] = [] clients: List[Client] = [] services: List[Service] = [] waitables: List[Waitable] = [] diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index fa6f5601e..c95115e0c 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -65,7 +65,7 @@ from rclpy.service import Service from rclpy.subscription import Subscription from rclpy.time_source import TimeSource -from rclpy.timer import WallTimer +from rclpy.timer import Timer from rclpy.type_support import check_for_type_support from rclpy.utilities import get_default_context from rclpy.validate_full_topic_name import validate_full_topic_name @@ -137,7 +137,7 @@ def __init__( self.__subscriptions: List[Subscription] = [] self.__clients: List[Client] = [] self.__services: List[Service] = [] - self.__timers: List[WallTimer] = [] + self.__timers: List[Timer] = [] self.__guards: List[GuardCondition] = [] self.__waitables: List[Waitable] = [] self._default_callback_group = MutuallyExclusiveCallbackGroup() @@ -214,7 +214,7 @@ def services(self) -> Iterator[Service]: yield from self.__services @property - def timers(self) -> Iterator[WallTimer]: + def timers(self) -> Iterator[Timer]: """Get timers that have been created on this node.""" yield from self.__timers @@ -1281,8 +1281,9 @@ def create_timer( self, timer_period_sec: float, callback: Callable, - callback_group: CallbackGroup = None - ) -> WallTimer: + callback_group: CallbackGroup = None, + clock: Clock = None, + ) -> Timer: """ Create a new timer. @@ -1293,11 +1294,14 @@ def create_timer( :param callback: A user-defined callback function that is called when the timer expires. :param callback_group: The callback group for the timer. If ``None``, then the nodes default callback group is used. + :param clock: The clock which the timer gets time from. """ timer_period_nsec = int(float(timer_period_sec) * S_TO_NS) if callback_group is None: callback_group = self.default_callback_group - timer = WallTimer(callback, callback_group, timer_period_nsec, context=self.context) + if clock is None: + clock = self._clock + timer = Timer(callback, callback_group, timer_period_nsec, clock, context=self.context) timer.handle.requires(self.handle) self.__timers.append(timer) @@ -1385,7 +1389,7 @@ def destroy_service(self, service: Service) -> bool: return True return False - def destroy_timer(self, timer: WallTimer) -> bool: + def destroy_timer(self, timer: Timer) -> bool: """ Destroy a timer created by the node. diff --git a/rclpy/rclpy/timer.py b/rclpy/rclpy/timer.py index c96ea3798..f69f66a5c 100644 --- a/rclpy/rclpy/timer.py +++ b/rclpy/rclpy/timer.py @@ -12,20 +12,16 @@ # See the License for the specific language governing permissions and # limitations under the License. -from rclpy.clock import Clock -from rclpy.clock import ClockType from rclpy.handle import Handle from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.utilities import get_default_context -# TODO(mikaelarguedas) create a Timer or ROSTimer once we can specify custom time sources -class WallTimer: +class Timer: - def __init__(self, callback, callback_group, timer_period_ns, *, context=None): + def __init__(self, callback, callback_group, timer_period_ns, clock, *, context=None): self._context = get_default_context() if context is None else context - # TODO(sloretz) Allow passing clocks in via timer constructor - self._clock = Clock(clock_type=ClockType.STEADY_TIME) + self._clock = clock with self._clock.handle as clock_capsule: self.__handle = Handle(_rclpy.rclpy_create_timer( clock_capsule, self._context.handle, timer_period_ns))