def test_service_timestamps(self): cli = self.node.create_client(GetParameters, 'get/parameters') srv = self.node.create_service( GetParameters, 'get/parameters', lambda request, response: response) try: self.assertTrue(cli.wait_for_service(timeout_sec=20)) cli.call_async(GetParameters.Request()) cycle_count = 0 while cycle_count < 5: with srv.handle as capsule: result = _rclpy.rclpy_take_request(capsule, srv.srv_type.Request) if result is not None: request, header = result source_timestamp = _rclpy.rclpy_service_info_get_source_timestamp(header) self.assertNotEqual(0, source_timestamp) return else: time.sleep(0.1) self.fail('Did not get a request in time') finally: self.node.destroy_client(cli) self.node.destroy_service(srv)
def take_data(self): """Take stuff from lower level so the wait set doesn't immediately wake again.""" if self.server_is_ready: self.server_is_ready = False return _rclpy.rclpy_take_request(self.server, EmptySrv.Request) return None
def _take_service(self, srv): request_and_header = _rclpy.rclpy_take_request(srv.service_handle, srv.srv_type.Request) return request_and_header
def _take_service(self, srv): with srv.handle as capsule: request_and_header = _rclpy.rclpy_take_request( capsule, srv.srv_type.Request) return request_and_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)
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)