def destroy_timer(self, timer: WallTimer) -> bool:
"""
Destroy a timer created by the node.
:return: ``True`` if successful, ``False`` otherwise.
"""
if timer in self.__timers:
self.__timers.remove(timer)
try:
timer.destroy()
except InvalidHandle:
return False
return True
return False
def create_timer(self,
timer_period_sec: float,
callback: Callable,
callback_group: CallbackGroup = None) -> WallTimer:
"""
Create a new timer.
The timer will be started and every ``timer_period_sec`` number of seconds the provided
callback function will be called.
:param timer_period_sec: The period (s) of the 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.
"""
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)
timer.handle.requires(self.handle)
self.__timers.append(timer)
callback_group.add_entity(timer)
return timer
def create_timer(self, timer_period_sec, callback):
timer_period_nsec = int(float(timer_period_sec) * S_TO_NS)
[timer_handle, timer_pointer] = _rclpy.rclpy_create_timer(timer_period_nsec)
timer = WallTimer(timer_handle, timer_pointer, callback, timer_period_nsec)
self.timers.append(timer)
return timer
def create_timer(self, timer_period_sec, callback, callback_group=None):
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)
self.timers.append(timer)
callback_group.add_entity(timer)
return timer
def _wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
"""
Yield callbacks that are ready to be performed.
Raises :class:`TimeoutException` on timeout.
:param timeout_sec: Seconds to wait. Block forever if None or negative. Don't wait if 0
:type timeout_sec: float or None
:param nodes: A list of nodes to wait on. Wait on all nodes if None.
:type nodes: list or None
:rtype: Generator[(callable, entity, :class:`rclpy.node.Node`)]
"""
timeout_timer = None
timeout_nsec = timeout_sec_to_nsec(timeout_sec)
if timeout_nsec > 0:
timeout_timer = WallTimer(None, None, timeout_nsec)
if nodes is None:
nodes = self.get_nodes()
# Yield tasks in-progress before waiting for new work
tasks = None
with self._tasks_lock:
tasks = list(self._tasks)
if tasks:
for task, entity, node in reversed(tasks):
if not task.executing() and not task.done() and (
node is None or node in nodes):
yield task, entity, node
with self._tasks_lock:
# Get rid of any tasks that are done
self._tasks = list(
filter(lambda t_e_n: not t_e_n[0].done(), self._tasks))
yielded_work = False
while not yielded_work and not self._is_shutdown:
# Gather entities that can be waited on
subscriptions = []
guards = []
timers = []
clients = []
services = []
for node in nodes:
subscriptions.extend(
filter(self.can_execute, node.subscriptions))
timers.extend(filter(self.can_execute, node.timers))
clients.extend(filter(self.can_execute, node.clients))
services.extend(filter(self.can_execute, node.services))
node_guards = filter(self.can_execute, node.guards)
# retrigger a guard condition that was triggered but not handled
for gc in node_guards:
if gc._executor_triggered:
gc.trigger()
guards.append(gc)
(sigint_gc,
sigint_gc_handle) = _rclpy.rclpy_get_sigint_guard_condition()
if timeout_timer is not None:
timers.append(timeout_timer)
# Construct a wait set
with _WaitSet() as wait_set:
_rclpy.rclpy_wait_set_init(wait_set, len(subscriptions),
len(guards) + 2, len(timers),
len(clients), len(services))
entities = {
'subscription': (subscriptions, 'subscription_handle'),
'guard_condition': (guards, 'guard_handle'),
'client': (clients, 'client_handle'),
'service': (services, 'service_handle'),
'timer': (timers, 'timer_handle'),
}
for entity, (handles, handle_name) in entities.items():
_rclpy.rclpy_wait_set_clear_entities(entity, wait_set)
for h in handles:
_rclpy.rclpy_wait_set_add_entity(
entity, wait_set, h.__getattribute__(handle_name))
_rclpy.rclpy_wait_set_add_entity('guard_condition', wait_set,
sigint_gc)
_rclpy.rclpy_wait_set_add_entity('guard_condition', wait_set,
self._guard_condition)
# Wait for something to become ready
_rclpy.rclpy_wait(wait_set, timeout_nsec)
# get ready entities
subs_ready = _rclpy.rclpy_get_ready_entities(
'subscription', wait_set)
guards_ready = _rclpy.rclpy_get_ready_entities(
'guard_condition', wait_set)
timers_ready = _rclpy.rclpy_get_ready_entities(
'timer', wait_set)
clients_ready = _rclpy.rclpy_get_ready_entities(
'client', wait_set)
services_ready = _rclpy.rclpy_get_ready_entities(
'service', wait_set)
_rclpy.rclpy_destroy_entity(sigint_gc)
# Mark all guards as triggered before yielding any handlers since they're auto-taken
for gc in guards:
if gc.guard_pointer in guards_ready:
gc._executor_triggered = True
# Process ready entities one node at a time
for node in nodes:
for tmr in node.timers:
if tmr.timer_pointer in timers_ready:
# Check that a timer is ready to workaround rcl issue with cancelled timers
if _rclpy.rclpy_is_timer_ready(tmr.timer_handle):
if tmr.callback_group.can_execute(tmr):
handler = self._make_handler(
tmr, node, self._take_timer,
self._execute_timer)
yielded_work = True
yield handler, tmr, node
for sub in node.subscriptions:
if sub.subscription_pointer in subs_ready:
if sub.callback_group.can_execute(sub):
handler = self._make_handler(
sub, node, self._take_subscription,
self._execute_subscription)
yielded_work = True
yield handler, sub, node
for gc in node.guards:
if gc._executor_triggered:
if gc.callback_group.can_execute(gc):
handler = self._make_handler(
gc, node, self._take_guard_condition,
self._execute_guard_condition)
yielded_work = True
yield handler, gc, node
for client in node.clients:
if client.client_pointer in clients_ready:
if client.callback_group.can_execute(client):
handler = self._make_handler(
client, node, self._take_client,
self._execute_client)
yielded_work = True
yield handler, client, node
for srv in node.services:
if srv.service_pointer in services_ready:
if srv.callback_group.can_execute(srv):
handler = self._make_handler(
srv, node, self._take_service,
self._execute_service)
yielded_work = True
yield handler, srv, node
# Check timeout timer
if (timeout_nsec == 0
or (timeout_timer is not None
and timeout_timer.timer_pointer in timers_ready)):
raise TimeoutException()
def _wait_for_ready_callbacks(
self,
timeout_sec: float = None,
nodes: List['Node'] = None
) -> Generator[Tuple[Task, WaitableEntityType, 'Node'], None, None]:
"""
Yield callbacks that are ready to be executed.
:raise TimeoutException: on timeout.
:raise ShutdownException: on if executor was shut down.
:param timeout_sec: Seconds to wait. Block forever if ``None`` or negative.
Don't wait if 0.
:param nodes: A list of nodes to wait on. Wait on all nodes if ``None``.
"""
timeout_timer = None
timeout_nsec = timeout_sec_to_nsec(timeout_sec)
if timeout_nsec > 0:
timeout_timer = WallTimer(None, None, timeout_nsec)
yielded_work = False
while not yielded_work and not self._is_shutdown:
# Refresh "all" nodes in case executor was woken by a node being added or removed
nodes_to_use = nodes
if nodes is None:
nodes_to_use = self.get_nodes()
# Yield tasks in-progress before waiting for new work
tasks = None
with self._tasks_lock:
tasks = list(self._tasks)
if tasks:
for task, entity, node in reversed(tasks):
if (not task.executing() and not task.done() and
(node is None or node in nodes_to_use)):
yielded_work = True
yield task, entity, node
with self._tasks_lock:
# Get rid of any tasks that are done
self._tasks = list(filter(lambda t_e_n: not t_e_n[0].done(), self._tasks))
# Gather entities that can be waited on
subscriptions: List[Subscription] = []
guards: List[GuardCondition] = []
timers: List[WallTimer] = []
clients: List[Client] = []
services: List[Service] = []
waitables: List[Waitable] = []
for node in nodes_to_use:
subscriptions.extend(filter(self.can_execute, node.subscriptions))
timers.extend(filter(self.can_execute, node.timers))
clients.extend(filter(self.can_execute, node.clients))
services.extend(filter(self.can_execute, node.services))
node_guards = filter(self.can_execute, node.guards)
waitables.extend(filter(self.can_execute, node.waitables))
# retrigger a guard condition that was triggered but not handled
for gc in node_guards:
if gc._executor_triggered:
gc.trigger()
guards.append(gc)
if timeout_timer is not None:
timers.append(timeout_timer)
guards.append(self._guard)
guards.append(self._sigint_gc)
entity_count = NumberOfEntities(
len(subscriptions), len(guards), len(timers), len(clients), len(services))
for waitable in waitables:
entity_count += waitable.get_num_entities()
# Construct a wait set
with _WaitSet() as wait_set, ExitStack() as context_stack:
sub_capsules = []
for sub in subscriptions:
try:
sub_capsules.append(context_stack.enter_context(sub.handle))
except InvalidHandle:
entity_count.num_subscriptions -= 1
client_capsules = []
for cli in clients:
try:
client_capsules.append(context_stack.enter_context(cli.handle))
except InvalidHandle:
entity_count.num_clients -= 1
service_capsules = []
for srv in services:
try:
service_capsules.append(context_stack.enter_context(srv.handle))
except InvalidHandle:
entity_count.num_services -= 1
timer_capsules = []
for tmr in timers:
try:
timer_capsules.append(context_stack.enter_context(tmr.handle))
except InvalidHandle:
entity_count.num_timers -= 1
guard_capsules = []
for gc in guards:
try:
guard_capsules.append(context_stack.enter_context(gc.handle))
except InvalidHandle:
entity_count.num_guard_conditions -= 1
_rclpy.rclpy_wait_set_init(
wait_set,
entity_count.num_subscriptions,
entity_count.num_guard_conditions,
entity_count.num_timers,
entity_count.num_clients,
entity_count.num_services,
entity_count.num_events,
self._context.handle)
_rclpy.rclpy_wait_set_clear_entities(wait_set)
for sub_capsule in sub_capsules:
_rclpy.rclpy_wait_set_add_entity('subscription', wait_set, sub_capsule)
for cli_capsule in client_capsules:
_rclpy.rclpy_wait_set_add_entity('client', wait_set, cli_capsule)
for srv_capsule in service_capsules:
_rclpy.rclpy_wait_set_add_entity('service', wait_set, srv_capsule)
for tmr_capsule in timer_capsules:
_rclpy.rclpy_wait_set_add_entity('timer', wait_set, tmr_capsule)
for gc_capsule in guard_capsules:
_rclpy.rclpy_wait_set_add_entity('guard_condition', wait_set, gc_capsule)
for waitable in waitables:
waitable.add_to_wait_set(wait_set)
# Wait for something to become ready
_rclpy.rclpy_wait(wait_set, timeout_nsec)
if self._is_shutdown:
raise ShutdownException()
# get ready entities
subs_ready = _rclpy.rclpy_get_ready_entities('subscription', wait_set)
guards_ready = _rclpy.rclpy_get_ready_entities('guard_condition', wait_set)
timers_ready = _rclpy.rclpy_get_ready_entities('timer', wait_set)
clients_ready = _rclpy.rclpy_get_ready_entities('client', wait_set)
services_ready = _rclpy.rclpy_get_ready_entities('service', wait_set)
# Mark all guards as triggered before yielding since they're auto-taken
for gc in guards:
if gc.handle.pointer in guards_ready:
gc._executor_triggered = True
# Check waitables before wait set is destroyed
for node in nodes_to_use:
for wt in node.waitables:
# Only check waitables that were added to the wait set
if wt in waitables and wt.is_ready(wait_set):
handler = self._make_handler(
wt, node, lambda e: e.take_data(), self._execute_waitable)
yielded_work = True
yield handler, wt, node
# Process ready entities one node at a time
for node in nodes_to_use:
for tmr in node.timers:
if tmr.handle.pointer in timers_ready:
with tmr.handle as capsule:
# Check timer is ready to workaround rcl issue with cancelled timers
if _rclpy.rclpy_is_timer_ready(capsule):
if tmr.callback_group.can_execute(tmr):
handler = self._make_handler(
tmr, node, self._take_timer, self._execute_timer)
yielded_work = True
yield handler, tmr, node
for sub in node.subscriptions:
if sub.handle.pointer in subs_ready:
if sub.callback_group.can_execute(sub):
handler = self._make_handler(
sub, node, self._take_subscription, self._execute_subscription)
yielded_work = True
yield handler, sub, node
for gc in node.guards:
if gc._executor_triggered:
if gc.callback_group.can_execute(gc):
handler = self._make_handler(
gc, node, self._take_guard_condition,
self._execute_guard_condition)
yielded_work = True
yield handler, gc, node
for client in node.clients:
if client.handle.pointer in clients_ready:
if client.callback_group.can_execute(client):
handler = self._make_handler(
client, node, self._take_client, self._execute_client)
yielded_work = True
yield handler, client, node
for srv in node.services:
if srv.handle.pointer in services_ready:
if srv.callback_group.can_execute(srv):
handler = self._make_handler(
srv, node, self._take_service, self._execute_service)
yielded_work = True
yield handler, srv, node
# Check timeout timer
if (
timeout_nsec == 0 or
(timeout_timer is not None and timeout_timer.handle.pointer in timers_ready)
):
raise TimeoutException()