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 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 __enter__(self):
self.wait_set = _rclpy.rclpy_get_zero_initialized_wait_set()
return self.wait_set
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)
