def __init__(self, client): threading.Thread.__init__(self) self.client = client self.wait_set = _rclpy.rclpy_get_zero_initialized_wait_set() _rclpy.rclpy_wait_set_init(self.wait_set, 0, 1, 0, 1, 0) _rclpy.rclpy_wait_set_clear_entities('client', self.wait_set) _rclpy.rclpy_wait_set_add_entity( 'client', self.wait_set, self.client.client_handle)
def __init__(self, client): threading.Thread.__init__(self) self.client = client self.wait_set = _rclpy.rclpy_get_zero_initialized_wait_set() _rclpy.rclpy_wait_set_init(self.wait_set, 0, 0, 0, 1, 0) _rclpy.rclpy_wait_set_clear_entities('client', self.wait_set) _rclpy.rclpy_wait_set_add_entity( 'client', self.wait_set, self.client.client_handle)
def test_call_publisher_rclpy_event_apis(self): # Go through the exposed apis and ensure that things don't explode when called # Make no assumptions about being able to actually receive the events publisher = self.node.create_publisher(EmptyMsg, 'test_topic', 10) wait_set = _rclpy.rclpy_get_zero_initialized_wait_set() _rclpy.rclpy_wait_set_init(wait_set, 0, 0, 0, 0, 0, 2, self.context.handle) deadline_event_handle = self._create_event_handle( publisher, QoSPublisherEventType.RCL_PUBLISHER_OFFERED_DEADLINE_MISSED) with deadline_event_handle as capsule: deadline_event_index = _rclpy.rclpy_wait_set_add_entity( 'event', wait_set, capsule) self.assertIsNotNone(deadline_event_index) liveliness_event_handle = self._create_event_handle( publisher, QoSPublisherEventType.RCL_PUBLISHER_LIVELINESS_LOST) with liveliness_event_handle as capsule: liveliness_event_index = _rclpy.rclpy_wait_set_add_entity( 'event', wait_set, capsule) self.assertIsNotNone(liveliness_event_index) # We live in our own namespace and have created no other participants, so # there can't be any of these events. _rclpy.rclpy_wait(wait_set, 0) self.assertFalse( _rclpy.rclpy_wait_set_is_ready('event', wait_set, deadline_event_index)) self.assertFalse( _rclpy.rclpy_wait_set_is_ready('event', wait_set, liveliness_event_index)) # Calling take data even though not ready should provide me an empty initialized message # Tests data conversion utilities in C side try: with deadline_event_handle as event_capsule, publisher.handle as publisher_capsule: event_data = _rclpy.rclpy_take_event( event_capsule, publisher_capsule, QoSPublisherEventType. RCL_PUBLISHER_OFFERED_DEADLINE_MISSED) self.assertIsInstance(event_data, QoSOfferedDeadlineMissedInfo) self.assertEqual(event_data.total_count, 0) self.assertEqual(event_data.total_count_change, 0) except NotImplementedError: pass try: with liveliness_event_handle as event_capsule, publisher.handle as publisher_capsule: event_data = _rclpy.rclpy_take_event( event_capsule, publisher_capsule, QoSPublisherEventType.RCL_PUBLISHER_LIVELINESS_LOST) self.assertIsInstance(event_data, QoSLivelinessLostInfo) self.assertEqual(event_data.total_count, 0) self.assertEqual(event_data.total_count_change, 0) except NotImplementedError: pass self.node.destroy_publisher(publisher)
def test_call_subscription_rclpy_event_apis(self): # Go through the exposed apis and ensure that things don't explode when called # Make no assumptions about being able to actually receive the events subscription = self.node.create_subscription(EmptyMsg, 'test_topic', Mock(), 10) wait_set = _rclpy.rclpy_get_zero_initialized_wait_set() with self.context.handle as context_handle: _rclpy.rclpy_wait_set_init(wait_set, 0, 0, 0, 0, 0, 2, context_handle) deadline_event_handle = self._create_event_handle( subscription, QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED) with deadline_event_handle as capsule: deadline_event_index = _rclpy.rclpy_wait_set_add_entity('event', wait_set, capsule) self.assertIsNotNone(deadline_event_index) liveliness_event_handle = self._create_event_handle( subscription, QoSSubscriptionEventType.RCL_SUBSCRIPTION_LIVELINESS_CHANGED) with liveliness_event_handle as capsule: liveliness_event_index = _rclpy.rclpy_wait_set_add_entity('event', wait_set, capsule) self.assertIsNotNone(liveliness_event_index) # We live in our own namespace and have created no other participants, so # there can't be any of these events. _rclpy.rclpy_wait(wait_set, 0) self.assertFalse(_rclpy.rclpy_wait_set_is_ready('event', wait_set, deadline_event_index)) self.assertFalse(_rclpy.rclpy_wait_set_is_ready('event', wait_set, liveliness_event_index)) # Calling take data even though not ready should provide me an empty initialized message # Tests data conversion utilities in C side try: with deadline_event_handle as event_capsule, subscription.handle as parent_capsule: event_data = _rclpy.rclpy_take_event( event_capsule, parent_capsule, QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED) self.assertIsInstance(event_data, QoSRequestedDeadlineMissedInfo) self.assertEqual(event_data.total_count, 0) self.assertEqual(event_data.total_count_change, 0) except NotImplementedError: pass try: with liveliness_event_handle as event_capsule, subscription.handle as parent_capsule: event_data = _rclpy.rclpy_take_event( event_capsule, parent_capsule, QoSSubscriptionEventType.RCL_SUBSCRIPTION_LIVELINESS_CHANGED) self.assertIsInstance(event_data, QoSLivelinessChangedInfo) self.assertEqual(event_data.alive_count, 0) self.assertEqual(event_data.alive_count_change, 0) self.assertEqual(event_data.not_alive_count, 0) self.assertEqual(event_data.not_alive_count_change, 0) except NotImplementedError: pass self.node.destroy_subscription(subscription)
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 add_to_wait_set(self, wait_set): """Add entities to wait set.""" self.client_index = _rclpy.rclpy_wait_set_add_entity('client', wait_set, self.client)
def add_to_wait_set(self, wait_set): """Add entities to wait set.""" self.guard_condition_index = _rclpy.rclpy_wait_set_add_entity( 'guard_condition', wait_set, self.guard_condition)
def add_to_wait_set(self, wait_set): """Add entities to wait set.""" self.subscription_index = _rclpy.rclpy_wait_set_add_entity( 'subscription', wait_set, self.subscription)
def add_to_wait_set(self, wait_set): """Add entities to wait set.""" self.timer_index = _rclpy.rclpy_wait_set_add_entity('timer', wait_set, self.timer)
def add_to_wait_set(self, wait_set): """Add entities to wait set.""" self.server_index = _rclpy.rclpy_wait_set_add_entity('service', wait_set, self.server)
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 = Timer(None, None, timeout_nsec, self._clock, context=self._context) 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[Timer] = [] 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 context_capsule = context_stack.enter_context( self._context.handle) _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, context_capsule) _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() if not self._context.ok(): raise ExternalShutdownException() # 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() if self._is_shutdown: raise ShutdownException()
def add_to_wait_set(self, wait_set): """Add entites to wait set.""" with self._event_handle as event_capsule: self._event_index = _rclpy.rclpy_wait_set_add_entity( 'event', wait_set, event_capsule)
def _wait_for_ready_callbacks_priority( self, timeout_sec: float = None, nodes: List['Node'] = None, condition: Callable[[], bool] = lambda: False, ) -> 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``. :param condition: A callable that makes the function return immediately when it evaluates to True. """ timeout_timer = None timeout_nsec = timeout_sec_to_nsec(timeout_sec) # timeout_nsec = timeout_sec_to_nsec(1) #haya # print("timeout_sec", timeout_sec) # print("timeout_nsec", timeout_nsec) if timeout_nsec > 0: timeout_timer = Timer(None, None, timeout_nsec, self._clock, context=self._context) yielded_work = False while not yielded_work and not self._is_shutdown and not condition(): # 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() nodes_to_use_priority = nodes if nodes is None: nodes_to_use_priority = self.get_nodes_priority() print("nodes_to_use_priority", nodes_to_use_priority) # 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[Timer] = [] clients: List[Client] = [] services: List[Service] = [] waitables: List[Waitable] = [] for node in nodes_to_use: subscriptions.extend(filter(self.can_execute, node.subscriptions)) # print(subscriptions.extend(filter(self.can_execute, node.subscriptions))) # print(node.subscriptions) timers.extend(filter(self.can_execute, node.timers)) # print(node.timers) clients.extend(filter(self.can_execute, node.clients)) # print(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)) print(entity_count) 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 context_capsule = context_stack.enter_context(self._context.handle) _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, context_capsule) # print("haya") # print(entity_count.num_timers) # print(entity_count.num_subscriptions) # print(entity_count.num_clients) # print(wait_set) # print(context_capsule) # print("haya_tlqkf") _rclpy.rclpy_wait_set_clear_entities(wait_set) for sub_capsule in sub_capsules: # print('tlqkf!!!!!!') _rclpy.rclpy_wait_set_add_entity('subscription', wait_set, sub_capsule) for cli_capsule in client_capsules: # print('tlqkf!!!!!!') _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:handle.pointer # raise ShutdownException() if not self._context.ok(): raise ExternalShutdownException() # 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 print('tlqkf!!!!!!') # print(node) yield handler, wt, node # # haya # # priority based scheduling # ####################### 1 ######################### # # 0. initialization # # print('nodes: ', nodes) # # print('nodes_to_use: ', nodes_to_use) # # print('nodes: ', list(nodes)) # # print('before: ', subs_ready) # # 1. node interface # interface_set = 0 # # interface_set = [] # for node in nodes_to_use: # for sub in node.subscriptions: # if sub.handle.pointer in subs_ready: # if "SourceLinearX" in str(node): # haya hard cording to be chaged # print('digh!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!') # print('pointer of SourceLinearX: ', sub.handle.pointer) # interface_set = sub.handle.pointer # # print('pointer2: ', interface_set) # # 2. get priority_set # print('before: ', subs_ready) # priority_set = [] # remain_set = [] # for idx in range(len(subs_ready)): # # print('idx: ', idx) # if interface_set == subs_ready[idx]: # # print('A') # priority_set.append(subs_ready[idx]) # else: # # print('B') # remain_set.append(subs_ready[idx]) # # 3. re-sort subs_ready based on priority_set # # print('priority_set: ', priority_set) # # print('remain_set: ',remain_set) # subs_ready = priority_set + remain_set # print('after: ', subs_ready) # if len(subs_ready) > 0: # print('subs_ready[0]: ', subs_ready[0]) ############################### 2 ########################## # print('before: ', nodes_to_use) # print('before: ', subs_ready) # priority_set = [] # remain_set = [] # for idx in range(len(nodes_to_use)): # if "SourceLinearX" in str(nodes_to_use[idx]): # haya hard cording to be chaged # priority_set.append(nodes_to_use[idx]) # else: # remain_set.append(nodes_to_use[idx]) # nodes_to_use = priority_set + remain_set # # print('after: ', nodes_to_use) # # print('after: ', subs_ready) # wooyoung 1st attempt start # temp1 = [] # temp2 = [] # temp3 = [] # number = 0 # for node in nodes_to_use: # print('!!!!!!!!!!!!!!!!!!!!!') # print('number ', number) # number=number+1 # print("BEFORE: ", node.woo()) # for sub in node.subscriptions: # # if sub.handle.pointer in subs_ready: # if "source_d" in str(sub.topic): # temp1.append(sub) # print("?????????FINDFINDFINDFIND?????????", sub) # else: # temp2.append(sub) # temp3.extend(temp1+temp2) # print("TEMP3: ", temp3) # node.woo2(temp3) # temp1.clear() # temp2.clear() # temp3.clear() # print("AFTER: ", node.woo()) # wooyoung 1st attempt end # wooyoung 2nd attempt start temp1 = [] temp2 = [] number = 0 for node in nodes_to_use: print('!!!!!!!!!!!!!!!!!!!!!') print('number ', number) number=number+1 print("BEFORE: ", node.woo()) for sub in node.subscriptions: # if sub.handle.pointer in subs_ready: if "source_d" in str(sub.topic): temp1.append(sub) else: temp2.append(sub) # wooyoung 2nd attempt end for node in nodes_to_use: # haya_sub = haya_sub + 1 # print("2: ", haya_sub) # print(node) # print(nodes_to_use) haya_sub_sub = 0 for tmr in node.timers: # haya_sub_sub = haya_sub_sub + 1 # print("3: ", haya_sub_sub) # print(node) # print(node.timers) # print(tmr) if tmr.handle.pointer in timers_ready: # print('tmr') # print(timers_ready) # print(tmr) # print(tmr.handle.pointer) 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 # print('tlqkf1') # print(handler) # print(tmr) # print(node) yield handler, tmr, node for node in nodes_to_use: for sub in node.subscriptions: if sub in temp1: 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 start = time.time() for node in nodes_to_use: for sub in node.subscriptions: if sub in temp2: 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 print("!!!!!!!!TIME!!!!!!!!!!: ", (time.time()-start)*1000) for node in nodes_to_use: for gc in node.guards: # print(node) 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 # print('tlqkf3') # print(node) yield handler, gc, node for node in nodes_to_use: for client in node.clients: # print(node) if client.handle.pointer in clients_ready: # print("&&&&&&&&&&&&&&&&&&&&&&&&&&&") # print("client") # print(clients_ready) # print(client.handle.pointer) if client.callback_group.can_execute(client): handler = self._make_handler( client, node, self._take_client, self._execute_client) yielded_work = True # print('tlqkf4') # print(node) # print('----------------------------------') yield handler, client, node for node in nodes_to_use: for srv in node.services: # print(node) if srv.handle.pointer in services_ready: # print("&&&&&&&&&&&&&&&&&&&&&&&&&&&") # print("services") # print(services_ready) # print(srv.handle.pointer) if srv.callback_group.can_execute(srv): handler = self._make_handler( srv, node, self._take_service, self._execute_service) yielded_work = True # print('tlqkf5') # print(node) # print('----------------------------------') 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() if self._is_shutdown: raise ShutdownException() if condition(): raise ConditionReachedException()
def spin_once(node, timeout_sec=None): wait_set = _rclpy.rclpy_get_zero_initialized_wait_set() _rclpy.rclpy_wait_set_init( wait_set, len(node.subscriptions), 1, len(node.timers), len(node.clients), len(node.services)) [sigint_gc, sigint_gc_handle] = _rclpy.rclpy_get_sigint_guard_condition() entities = { 'subscription': (node.subscriptions, 'subscription_handle'), 'client': (node.clients, 'client_handle'), 'service': (node.services, 'service_handle'), 'timer': (node.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_clear_entities('guard_condition', wait_set) _rclpy.rclpy_wait_set_add_entity('guard_condition', wait_set, sigint_gc) if timeout_sec is None: timeout = -1 else: timeout = int(float(timeout_sec) * S_TO_NS) _rclpy.rclpy_wait(wait_set, timeout) guard_condition_ready_list = _rclpy.rclpy_get_ready_entities('guard_condition', wait_set) if sigint_gc_handle in guard_condition_ready_list: raise KeyboardInterrupt timer_ready_list = _rclpy.rclpy_get_ready_entities('timer', wait_set) for tmr in [t for t in node.timers if t.timer_pointer in timer_ready_list]: if _rclpy.rclpy_is_timer_ready(tmr.timer_handle): _rclpy.rclpy_call_timer(tmr.timer_handle) tmr.callback() sub_ready_list = _rclpy.rclpy_get_ready_entities('subscription', wait_set) for sub in [s for s in node.subscriptions if s.subscription_pointer in sub_ready_list]: msg = _rclpy.rclpy_take(sub.subscription_handle, sub.msg_type) if msg: sub.callback(msg) client_ready_list = _rclpy.rclpy_get_ready_entities('client', wait_set) for client in [c for c in node.clients if c.client_pointer in client_ready_list]: response = _rclpy.rclpy_take_response( client.client_handle, client.srv_type.Response, client.sequence_number) if response: # clients spawn their own thread to wait for a response in the wait_for_future function # users can either use this mechanism or monitor the content of # client.response themselves to check if a response have been received client.response = response service_ready_list = _rclpy.rclpy_get_ready_entities('service', wait_set) for srv in [s for s in node.services if s.service_pointer in service_ready_list]: request_and_header = _rclpy.rclpy_take_request(srv.service_handle, srv.srv_type.Request) if request_and_header is None: continue [request, header] = request_and_header if request: response = srv.callback(request, srv.srv_type.Response()) srv.send_response(response, header)
def spin_once(node, timeout_sec=None): wait_set = _rclpy.rclpy_get_zero_initialized_wait_set() _rclpy.rclpy_wait_set_init(wait_set, len(node.subscriptions), 1, len(node.timers), len(node.clients), len(node.services)) [sigint_gc, sigint_gc_handle] = _rclpy.rclpy_get_sigint_guard_condition() entities = { 'subscription': (node.subscriptions, 'subscription_handle'), 'client': (node.clients, 'client_handle'), 'service': (node.services, 'service_handle'), 'timer': (node.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_clear_entities('guard_condition', wait_set) _rclpy.rclpy_wait_set_add_entity('guard_condition', wait_set, sigint_gc) if timeout_sec is None: timeout = -1 else: timeout = int(float(timeout_sec) * S_TO_NS) _rclpy.rclpy_wait(wait_set, timeout) guard_condition_ready_list = _rclpy.rclpy_get_ready_entities( 'guard_condition', wait_set) if sigint_gc_handle in guard_condition_ready_list: raise KeyboardInterrupt timer_ready_list = _rclpy.rclpy_get_ready_entities('timer', wait_set) for tmr in [t for t in node.timers if t.timer_pointer in timer_ready_list]: if _rclpy.rclpy_is_timer_ready(tmr.timer_handle): _rclpy.rclpy_call_timer(tmr.timer_handle) tmr.callback() sub_ready_list = _rclpy.rclpy_get_ready_entities('subscription', wait_set) for sub in [ s for s in node.subscriptions if s.subscription_pointer in sub_ready_list ]: msg = _rclpy.rclpy_take(sub.subscription_handle, sub.msg_type) if msg: sub.callback(msg) client_ready_list = _rclpy.rclpy_get_ready_entities('client', wait_set) for client in [ c for c in node.clients if c.client_pointer in client_ready_list ]: response = _rclpy.rclpy_take_response(client.client_handle, client.srv_type.Response, client.sequence_number) if response: # clients spawn their own thread to wait for a response in the wait_for_future function # users can either use this mechanism or monitor the content of # client.response themselves to check if a response have been received client.response = response service_ready_list = _rclpy.rclpy_get_ready_entities('service', wait_set) for srv in [ s for s in node.services if s.service_pointer in service_ready_list ]: request_and_header = _rclpy.rclpy_take_request(srv.service_handle, srv.srv_type.Request) if request_and_header is None: continue [request, header] = request_and_header if request: response = srv.callback(request, srv.srv_type.Response()) srv.send_response(response, header)