def take_data(self):
"""Take stuff from lower level so the wait set doesn't immediately wake again."""
if self.subscription_is_ready:
self.subscription_is_ready = False
msg_info = _rclpy.rclpy_take(self.subscription, EmptyMsg, False)
if msg_info is not None:
return msg_info[0]
return None
def spin_once(node):
wait_set = _rclpy.rclpy_get_zero_initialized_wait_set()
_rclpy.rclpy_wait_set_init(wait_set, len(node.subscriptions), 0, 0)
_rclpy.rclpy_wait_set_clear_subscriptions(wait_set)
for subscription in node.subscriptions:
_rclpy.rclpy_wait_set_add_subscription(wait_set, subscription.subscription_handle)
_rclpy.rclpy_wait(wait_set)
# TODO(wjwwood): properly implement this by checking the contents of the wait_set.
for subscription in node.subscriptions:
msg = _rclpy.rclpy_take(subscription.subscription_handle, subscription.msg_type)
if msg:
subscription.callback(msg)
def wait_for_message(node, topic):
wait_set = _rclpy.rclpy_get_zero_initialized_wait_set()
_rclpy.rclpy_wait_set_init(wait_set, 1, 0, 0)
_rclpy.rclpy_wait_set_clear_subscriptions(wait_set)
for subscription in node.subscriptions:
if subscription.topic == topic:
_rclpy.rclpy_wait_set_add_subscription(wait_set, subscription.subscription_handle)
_rclpy.rclpy_wait(wait_set)
# TODO(wjwwood): properly implement this by checking the contents of the wait_set.
for subscription in node.subscriptions:
if subscription.topic == topic:
msg = _rclpy.rclpy_take(subscription.subscription_handle, subscription.msg_type)
if msg:
return msg
def take_data(self):
"""Take stuff from lower level so the wait set doesn't immediately wake again."""
if self.subscription_is_ready:
self.subscription_is_ready = False
return _rclpy.rclpy_take(self.subscription, EmptyMsg, False)
return None
def _take_subscription(self, sub):
msg = _rclpy.rclpy_take(sub.subscription_handle, sub.msg_type)
return msg
def _take_subscription(self, sub):
with sub.handle as capsule:
msg_info = _rclpy.rclpy_take(capsule, sub.msg_type, sub.raw)
if msg_info is not None:
return msg_info[0]
return None
def _take_subscription(self, sub):
with sub.handle as capsule:
msg = _rclpy.rclpy_take(capsule, sub.msg_type, sub.raw)
return msg
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)
