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)