def destroy_timer(self, timer):
for tmr in self.timers:
if tmr.timer_handle == timer.timer_handle:
_rclpy.rclpy_destroy_entity(tmr.timer_handle)
self.timers.remove(tmr)
return True
return False
def destroy_node(self):
ret = True
if self.handle is None:
return ret
for sub in self.subscriptions:
ret &= _rclpy.rclpy_destroy_node_entity(
'subscription', sub.subscription_handle, self.handle)
self.subscriptions.remove(sub)
for pub in self.publishers:
ret &= _rclpy.rclpy_destroy_node_entity(
'publisher', pub.publisher_handle, self.handle)
self.publishers.remove(pub)
for cli in self.clients:
ret &= _rclpy.rclpy_destroy_node_entity(
'client', cli.client_handle, self.handle)
self.clients.remove(cli)
for srv in self.services:
ret &= _rclpy.rclpy_destroy_node_entity(
'service', srv.service_handle, self.handle)
self.services.remove(srv)
for tmr in self.timers:
ret &= _rclpy.rclpy_destroy_entity('timer', tmr.timer_handle)
self.timers.remove(tmr)
ret &= _rclpy.rclpy_destroy_entity('node', self.handle)
self._handle = None
return ret
def shutdown(self, timeout_sec: float = None) -> bool:
"""
Stop executing callbacks and wait for their completion.
:param timeout_sec: Seconds to wait. Block forever if ``None`` or negative.
Don't wait if 0.
:return: ``True`` if all outstanding callbacks finished executing, or ``False`` if the
timeot expires before all outstanding work is done.
"""
with self._shutdown_lock:
if not self._is_shutdown:
self._is_shutdown = True
# Tell executor it's been shut down
_rclpy.rclpy_trigger_guard_condition(self._guard_condition)
if not self._work_tracker.wait(timeout_sec):
return False
# Clean up stuff that won't be used anymore
with self._nodes_lock:
self._nodes = set()
with self._shutdown_lock:
if self._guard_condition:
_rclpy.rclpy_destroy_entity(self._guard_condition)
self._guard_condition = None
if self._sigint_gc:
self._sigint_gc.destroy()
self._sigint_gc = None
self._cb_iter = None
self._last_args = None
self._last_kwargs = None
return True
def destroy_guard_condition(self, guard):
for gc in self.guards:
if gc.guard_handle == guard.guard_handle:
_rclpy.rclpy_destroy_entity(gc.guard_handle)
self.guards.remove(gc)
return True
return False
def destroy_node(self):
ret = True
if self.handle is None:
return ret
for sub in self.subscriptions:
ret &= _rclpy.rclpy_destroy_node_entity('subscription',
sub.subscription_handle,
self.handle)
self.subscriptions.remove(sub)
for pub in self.publishers:
ret &= _rclpy.rclpy_destroy_node_entity('publisher',
pub.publisher_handle,
self.handle)
self.publishers.remove(pub)
for cli in self.clients:
ret &= _rclpy.rclpy_destroy_node_entity('client',
cli.client_handle,
self.handle)
self.clients.remove(cli)
for srv in self.services:
ret &= _rclpy.rclpy_destroy_node_entity('service',
srv.service_handle,
self.handle)
self.services.remove(srv)
for tmr in self.timers:
ret &= _rclpy.rclpy_destroy_entity('timer', tmr.timer_handle)
self.timers.remove(tmr)
ret &= _rclpy.rclpy_destroy_entity('node', self.handle)
self._handle = None
return ret
def destroy_timer(self, timer):
for tmr in self.timers:
if tmr.timer_handle == timer.timer_handle:
_rclpy.rclpy_destroy_entity(
'timer', tmr.timer_handle)
self.timers.remove(tmr)
return True
return False
def destroy_timer(self, timer):
for tmr in self.timers:
if tmr.timer_handle == timer.timer_handle:
_rclpy.rclpy_destroy_entity(tmr.timer_handle)
# TODO(sloretz) Store clocks on node and destroy them separately
_rclpy.rclpy_destroy_entity(tmr.clock._clock_handle)
self.timers.remove(tmr)
return True
return False
def destroy_guard_condition(self, guard: GuardCondition) -> bool:
"""
Destroy a guard condition created by the node.
:return: ``True`` if successful, ``False`` otherwise.
"""
for gc in self.guards:
if gc.guard_handle == guard.guard_handle:
_rclpy.rclpy_destroy_entity(gc.guard_handle)
self.guards.remove(gc)
return True
return False
def destroy_timer(self, timer: WallTimer) -> bool:
"""
Destroy a timer created by the node.
:return: ``True`` if successful, ``False`` otherwise.
"""
for tmr in self.timers:
if tmr.timer_handle == timer.timer_handle:
_rclpy.rclpy_destroy_entity(tmr.timer_handle)
# TODO(sloretz) Store clocks on node and destroy them separately
_rclpy.rclpy_destroy_entity(tmr.clock._clock_handle)
self.timers.remove(tmr)
return True
return False
def destroy_node(self):
ret = True
if self.handle is None:
return ret
while self.publishers:
pub = self.publishers.pop()
_rclpy.rclpy_destroy_node_entity(pub.publisher_handle, self.handle)
while self.subscriptions:
sub = self.subscriptions.pop()
_rclpy.rclpy_destroy_node_entity(sub.subscription_handle,
self.handle)
while self.clients:
cli = self.clients.pop()
_rclpy.rclpy_destroy_node_entity(cli.client_handle, self.handle)
while self.services:
srv = self.services.pop()
_rclpy.rclpy_destroy_node_entity(srv.service_handle, self.handle)
while self.timers:
tmr = self.timers.pop()
_rclpy.rclpy_destroy_entity(tmr.timer_handle)
# TODO(sloretz) Store clocks on node and destroy them separately
_rclpy.rclpy_destroy_entity(tmr.clock._clock_handle)
while self.guards:
gc = self.guards.pop()
_rclpy.rclpy_destroy_entity(gc.guard_handle)
_rclpy.rclpy_destroy_entity(self.handle)
self._handle = None
return ret
def destroy_node(self):
ret = True
if self.handle is None:
return ret
# Drop extra reference to parameter event publisher.
# It will be destroyed with other publishers below.
self._parameter_event_publisher = None
while self.publishers:
pub = self.publishers.pop()
_rclpy.rclpy_destroy_node_entity(pub.publisher_handle, self.handle)
while self.subscriptions:
sub = self.subscriptions.pop()
_rclpy.rclpy_destroy_node_entity(sub.subscription_handle,
self.handle)
while self.clients:
cli = self.clients.pop()
_rclpy.rclpy_destroy_node_entity(cli.client_handle, self.handle)
while self.services:
srv = self.services.pop()
_rclpy.rclpy_destroy_node_entity(srv.service_handle, self.handle)
while self.timers:
tmr = self.timers.pop()
_rclpy.rclpy_destroy_entity(tmr.timer_handle)
# TODO(sloretz) Store clocks on node and destroy them separately
_rclpy.rclpy_destroy_entity(tmr.clock._clock_handle)
while self.guards:
gc = self.guards.pop()
_rclpy.rclpy_destroy_entity(gc.guard_handle)
_rclpy.rclpy_destroy_entity(self.handle)
self._handle = None
return ret
def run(self):
[sigint_gc, sigint_gc_handle] = _rclpy.rclpy_get_sigint_guard_condition()
_rclpy.rclpy_wait_set_add_entity('guard_condition', self.wait_set, sigint_gc)
_rclpy.rclpy_wait(self.wait_set, -1)
guard_condition_ready_list = \
_rclpy.rclpy_get_ready_entities('guard_condition', self.wait_set)
# destroying here to make sure we dont call shutdown before cleaning up
_rclpy.rclpy_destroy_entity(sigint_gc)
if sigint_gc_handle in guard_condition_ready_list:
rclpy.utilities.shutdown()
return
seq, response = _rclpy.rclpy_take_response(
self.client.client_handle,
self.client.srv_type.Response)
if seq is not None and seq == self.client.sequence_number:
self.client.response = response
def destroy_node(self):
ret = True
if self.handle is None:
return ret
while self.publishers:
pub = self.publishers.pop()
_rclpy.rclpy_destroy_node_entity(pub.publisher_handle, self.handle)
while self.subscriptions:
sub = self.subscriptions.pop()
_rclpy.rclpy_destroy_node_entity(sub.subscription_handle,
self.handle)
while self.clients:
cli = self.clients.pop()
_rclpy.rclpy_destroy_node_entity(cli.client_handle, self.handle)
while self.services:
srv = self.services.pop()
_rclpy.rclpy_destroy_node_entity(srv.service_handle, self.handle)
while self.timers:
tmr = self.timers.pop()
_rclpy.rclpy_destroy_entity(tmr.timer_handle)
while self.guards:
gc = self.guards.pop()
_rclpy.rclpy_destroy_entity(gc.guard_handle)
_rclpy.rclpy_destroy_entity(self.handle)
self._handle = None
return ret
def destroy_node(self) -> bool:
"""
Destroy the node.
Frees resources used by the node, including any entities created by the following methods:
* :func:`create_publisher`
* :func:`create_subscription`
* :func:`create_client`
* :func:`create_service`
* :func:`create_timer`
* :func:`create_guard_condition`
:return: ``True`` if successful, ``False`` otherwise.
"""
ret = True
if self.handle is None:
return ret
# Drop extra reference to parameter event publisher.
# It will be destroyed with other publishers below.
self._parameter_event_publisher = None
while self.publishers:
pub = self.publishers.pop()
_rclpy.rclpy_destroy_node_entity(pub.publisher_handle, self.handle)
while self.subscriptions:
sub = self.subscriptions.pop()
_rclpy.rclpy_destroy_node_entity(sub.subscription_handle,
self.handle)
while self.clients:
cli = self.clients.pop()
_rclpy.rclpy_destroy_node_entity(cli.client_handle, self.handle)
while self.services:
srv = self.services.pop()
_rclpy.rclpy_destroy_node_entity(srv.service_handle, self.handle)
while self.timers:
tmr = self.timers.pop()
_rclpy.rclpy_destroy_entity(tmr.timer_handle)
# TODO(sloretz) Store clocks on node and destroy them separately
_rclpy.rclpy_destroy_entity(tmr.clock._clock_handle)
while self.guards:
gc = self.guards.pop()
_rclpy.rclpy_destroy_entity(gc.guard_handle)
_rclpy.rclpy_destroy_entity(self.handle)
self._handle = None
return ret
def shutdown(self, timeout_sec=None):
"""
Stop executing callbacks and wait for their completion.
Return true if all outstanding callbacks finished executing.
:param timeout_sec: Seconds to wait. Block forever if None or negative. Don't wait if 0
:type timeout_sec: float or None
:rtype: bool
"""
self._is_shutdown = True
if not self._work_tracker.wait(timeout_sec):
return False
# Clean up stuff that won't be used anymore
with self._nodes_lock:
self._nodes = set()
_rclpy.rclpy_destroy_entity(self._guard_condition)
self._guard_condition = None
self._cb_iter = None
self._last_args = None
self._last_kwargs = None
return True
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 __del__(self):
if self._guard_condition is not None:
_rclpy.rclpy_destroy_entity(self._guard_condition)
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)
node_entity_count = NumberOfEntities(len(subscriptions),
len(guards), len(timers),
len(clients), len(services))
executor_entity_count = NumberOfEntities(0, 2, 0, 0, 0)
entity_count = node_entity_count + executor_entity_count
for waitable in waitables:
entity_count += waitable.get_num_entities()
# Construct a wait set
with _WaitSet() as wait_set:
_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,
self._context.handle)
entities = {
'subscription': (subscriptions, 'subscription_handle'),
'guard_condition': (guards, 'guard_handle'),
'client': (clients, 'client_handle'),
'service': (services, 'service_handle'),
'timer': (timers, 'timer_handle'),
}
_rclpy.rclpy_wait_set_clear_entities(wait_set)
for entity, (handles, handle_name) in entities.items():
for h in handles:
_rclpy.rclpy_wait_set_add_entity(
entity, wait_set, h.__getattribute__(handle_name))
for waitable in waitables:
waitable.add_to_wait_set(wait_set)
(sigint_gc, sigint_gc_handle) = \
_rclpy.rclpy_get_sigint_guard_condition(self._context.handle)
try:
_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)
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)
finally:
_rclpy.rclpy_destroy_entity(sigint_gc)
# Mark all guards as triggered before yielding since they're auto-taken
for gc in guards:
if gc.guard_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.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 __del__(self):
if self._guard_condition is not None:
_rclpy.rclpy_destroy_entity(self._guard_condition)
if self._sigint_gc is not None:
self._sigint_gc.destroy()
def destroy(self):
if self.guard_handle is None:
return
_signals.rclpy_unregister_sigint_guard_condition(self.guard_handle)
_rclpy.rclpy_destroy_entity(self.guard_handle)
self.guard_handle = None
