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)
