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