def run(self):
_rclpy.rclpy_wait(self.wait_set, -1)
response = _rclpy.rclpy_take_response(self.client.client_handle,
self.client.srv_type.Response,
self.client.sequence_number)
if response:
self.client.response = response
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 run(self):
[sigint_gc, sigint_gc_handle] = _rclpy.rclpy_get_sigint_guard_condition()
_rclpy.rclpy_wait_set_add_entity('guard_condition', self.wait_set, sigint_gc)
_rclpy.rclpy_wait(self.wait_set, -1)
guard_condition_ready_list = \
_rclpy.rclpy_get_ready_entities('guard_condition', self.wait_set)
# destroying here to make sure we dont call shutdown before cleaning up
_rclpy.rclpy_destroy_entity(sigint_gc)
if sigint_gc_handle in guard_condition_ready_list:
rclpy.utilities.shutdown()
return
seq, response = _rclpy.rclpy_take_response(
self.client.client_handle,
self.client.srv_type.Response)
if seq is not None and seq == self.client.sequence_number:
self.client.response = response
def _wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
"""
Yield callbacks that are ready to be performed.
Raises :class:`TimeoutException` on timeout.
:param timeout_sec: Seconds to wait. Block forever if None or negative. Don't wait if 0
:type timeout_sec: float or None
:param nodes: A list of nodes to wait on. Wait on all nodes if None.
:type nodes: list or None
:rtype: Generator[(callable, entity, :class:`rclpy.node.Node`)]
"""
timeout_timer = None
timeout_nsec = timeout_sec_to_nsec(timeout_sec)
if timeout_nsec > 0:
timeout_timer = WallTimer(None, None, timeout_nsec)
if nodes is None:
nodes = self.get_nodes()
# Yield tasks in-progress before waiting for new work
tasks = None
with self._tasks_lock:
tasks = list(self._tasks)
if tasks:
for task, entity, node in reversed(tasks):
if not task.executing() and not task.done() and (
node is None or node in nodes):
yield task, entity, node
with self._tasks_lock:
# Get rid of any tasks that are done
self._tasks = list(
filter(lambda t_e_n: not t_e_n[0].done(), self._tasks))
yielded_work = False
while not yielded_work and not self._is_shutdown:
# Gather entities that can be waited on
subscriptions = []
guards = []
timers = []
clients = []
services = []
for node in nodes:
subscriptions.extend(
filter(self.can_execute, node.subscriptions))
timers.extend(filter(self.can_execute, node.timers))
clients.extend(filter(self.can_execute, node.clients))
services.extend(filter(self.can_execute, node.services))
node_guards = filter(self.can_execute, node.guards)
# retrigger a guard condition that was triggered but not handled
for gc in node_guards:
if gc._executor_triggered:
gc.trigger()
guards.append(gc)
(sigint_gc,
sigint_gc_handle) = _rclpy.rclpy_get_sigint_guard_condition()
if timeout_timer is not None:
timers.append(timeout_timer)
# Construct a wait set
with _WaitSet() as wait_set:
_rclpy.rclpy_wait_set_init(wait_set, len(subscriptions),
len(guards) + 2, len(timers),
len(clients), len(services))
entities = {
'subscription': (subscriptions, 'subscription_handle'),
'guard_condition': (guards, 'guard_handle'),
'client': (clients, 'client_handle'),
'service': (services, 'service_handle'),
'timer': (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_add_entity('guard_condition', wait_set,
sigint_gc)
_rclpy.rclpy_wait_set_add_entity('guard_condition', wait_set,
self._guard_condition)
# Wait for something to become ready
_rclpy.rclpy_wait(wait_set, timeout_nsec)
# get ready entities
subs_ready = _rclpy.rclpy_get_ready_entities(
'subscription', wait_set)
guards_ready = _rclpy.rclpy_get_ready_entities(
'guard_condition', wait_set)
timers_ready = _rclpy.rclpy_get_ready_entities(
'timer', wait_set)
clients_ready = _rclpy.rclpy_get_ready_entities(
'client', wait_set)
services_ready = _rclpy.rclpy_get_ready_entities(
'service', wait_set)
_rclpy.rclpy_destroy_entity(sigint_gc)
# Mark all guards as triggered before yielding any handlers since they're auto-taken
for gc in guards:
if gc.guard_pointer in guards_ready:
gc._executor_triggered = True
# Process ready entities one node at a time
for node in nodes:
for tmr in node.timers:
if tmr.timer_pointer in timers_ready:
# Check that a timer is ready to workaround rcl issue with cancelled timers
if _rclpy.rclpy_is_timer_ready(tmr.timer_handle):
if tmr.callback_group.can_execute(tmr):
handler = self._make_handler(
tmr, node, self._take_timer,
self._execute_timer)
yielded_work = True
yield handler, tmr, node
for sub in node.subscriptions:
if sub.subscription_pointer in subs_ready:
if sub.callback_group.can_execute(sub):
handler = self._make_handler(
sub, node, self._take_subscription,
self._execute_subscription)
yielded_work = True
yield handler, sub, node
for gc in node.guards:
if gc._executor_triggered:
if gc.callback_group.can_execute(gc):
handler = self._make_handler(
gc, node, self._take_guard_condition,
self._execute_guard_condition)
yielded_work = True
yield handler, gc, node
for client in node.clients:
if client.client_pointer in clients_ready:
if client.callback_group.can_execute(client):
handler = self._make_handler(
client, node, self._take_client,
self._execute_client)
yielded_work = True
yield handler, client, node
for srv in node.services:
if srv.service_pointer in services_ready:
if srv.callback_group.can_execute(srv):
handler = self._make_handler(
srv, node, self._take_service,
self._execute_service)
yielded_work = True
yield handler, srv, node
# Check timeout timer
if (timeout_nsec == 0
or (timeout_timer is not None
and timeout_timer.timer_pointer in timers_ready)):
raise TimeoutException()
def _wait_for_ready_callbacks(
self,
timeout_sec: float = None,
nodes: List['Node'] = None
) -> Generator[Tuple[Task, WaitableEntityType, 'Node'], None, None]:
"""
Yield callbacks that are ready to be executed.
:raise TimeoutException: on timeout.
:raise ShutdownException: on if executor was shut down.
:param timeout_sec: Seconds to wait. Block forever if ``None`` or negative.
Don't wait if 0.
:param nodes: A list of nodes to wait on. Wait on all nodes if ``None``.
"""
timeout_timer = None
timeout_nsec = timeout_sec_to_nsec(timeout_sec)
if timeout_nsec > 0:
timeout_timer = Timer(None,
None,
timeout_nsec,
self._clock,
context=self._context)
yielded_work = False
while not yielded_work and not self._is_shutdown:
# Refresh "all" nodes in case executor was woken by a node being added or removed
nodes_to_use = nodes
if nodes is None:
nodes_to_use = self.get_nodes()
# Yield tasks in-progress before waiting for new work
tasks = None
with self._tasks_lock:
tasks = list(self._tasks)
if tasks:
for task, entity, node in reversed(tasks):
if (not task.executing() and not task.done()
and (node is None or node in nodes_to_use)):
yielded_work = True
yield task, entity, node
with self._tasks_lock:
# Get rid of any tasks that are done
self._tasks = list(
filter(lambda t_e_n: not t_e_n[0].done(), self._tasks))
# Gather entities that can be waited on
subscriptions: List[Subscription] = []
guards: List[GuardCondition] = []
timers: List[Timer] = []
clients: List[Client] = []
services: List[Service] = []
waitables: List[Waitable] = []
for node in nodes_to_use:
subscriptions.extend(
filter(self.can_execute, node.subscriptions))
timers.extend(filter(self.can_execute, node.timers))
clients.extend(filter(self.can_execute, node.clients))
services.extend(filter(self.can_execute, node.services))
node_guards = filter(self.can_execute, node.guards)
waitables.extend(filter(self.can_execute, node.waitables))
# retrigger a guard condition that was triggered but not handled
for gc in node_guards:
if gc._executor_triggered:
gc.trigger()
guards.append(gc)
if timeout_timer is not None:
timers.append(timeout_timer)
guards.append(self._guard)
guards.append(self._sigint_gc)
entity_count = NumberOfEntities(len(subscriptions), len(guards),
len(timers), len(clients),
len(services))
for waitable in waitables:
entity_count += waitable.get_num_entities()
# Construct a wait set
with _WaitSet() as wait_set, ExitStack() as context_stack:
sub_capsules = []
for sub in subscriptions:
try:
sub_capsules.append(
context_stack.enter_context(sub.handle))
except InvalidHandle:
entity_count.num_subscriptions -= 1
client_capsules = []
for cli in clients:
try:
client_capsules.append(
context_stack.enter_context(cli.handle))
except InvalidHandle:
entity_count.num_clients -= 1
service_capsules = []
for srv in services:
try:
service_capsules.append(
context_stack.enter_context(srv.handle))
except InvalidHandle:
entity_count.num_services -= 1
timer_capsules = []
for tmr in timers:
try:
timer_capsules.append(
context_stack.enter_context(tmr.handle))
except InvalidHandle:
entity_count.num_timers -= 1
guard_capsules = []
for gc in guards:
try:
guard_capsules.append(
context_stack.enter_context(gc.handle))
except InvalidHandle:
entity_count.num_guard_conditions -= 1
context_capsule = context_stack.enter_context(
self._context.handle)
_rclpy.rclpy_wait_set_init(
wait_set, entity_count.num_subscriptions,
entity_count.num_guard_conditions, entity_count.num_timers,
entity_count.num_clients, entity_count.num_services,
entity_count.num_events, context_capsule)
_rclpy.rclpy_wait_set_clear_entities(wait_set)
for sub_capsule in sub_capsules:
_rclpy.rclpy_wait_set_add_entity('subscription', wait_set,
sub_capsule)
for cli_capsule in client_capsules:
_rclpy.rclpy_wait_set_add_entity('client', wait_set,
cli_capsule)
for srv_capsule in service_capsules:
_rclpy.rclpy_wait_set_add_entity('service', wait_set,
srv_capsule)
for tmr_capsule in timer_capsules:
_rclpy.rclpy_wait_set_add_entity('timer', wait_set,
tmr_capsule)
for gc_capsule in guard_capsules:
_rclpy.rclpy_wait_set_add_entity('guard_condition',
wait_set, gc_capsule)
for waitable in waitables:
waitable.add_to_wait_set(wait_set)
# Wait for something to become ready
_rclpy.rclpy_wait(wait_set, timeout_nsec)
if self._is_shutdown:
raise ShutdownException()
if not self._context.ok():
raise ExternalShutdownException()
# get ready entities
subs_ready = _rclpy.rclpy_get_ready_entities(
'subscription', wait_set)
guards_ready = _rclpy.rclpy_get_ready_entities(
'guard_condition', wait_set)
timers_ready = _rclpy.rclpy_get_ready_entities(
'timer', wait_set)
clients_ready = _rclpy.rclpy_get_ready_entities(
'client', wait_set)
services_ready = _rclpy.rclpy_get_ready_entities(
'service', wait_set)
# Mark all guards as triggered before yielding since they're auto-taken
for gc in guards:
if gc.handle.pointer in guards_ready:
gc._executor_triggered = True
# Check waitables before wait set is destroyed
for node in nodes_to_use:
for wt in node.waitables:
# Only check waitables that were added to the wait set
if wt in waitables and wt.is_ready(wait_set):
handler = self._make_handler(
wt, node, lambda e: e.take_data(),
self._execute_waitable)
yielded_work = True
yield handler, wt, node
# Process ready entities one node at a time
for node in nodes_to_use:
for tmr in node.timers:
if tmr.handle.pointer in timers_ready:
with tmr.handle as capsule:
# Check timer is ready to workaround rcl issue with cancelled timers
if _rclpy.rclpy_is_timer_ready(capsule):
if tmr.callback_group.can_execute(tmr):
handler = self._make_handler(
tmr, node, self._take_timer,
self._execute_timer)
yielded_work = True
yield handler, tmr, node
for sub in node.subscriptions:
if sub.handle.pointer in subs_ready:
if sub.callback_group.can_execute(sub):
handler = self._make_handler(
sub, node, self._take_subscription,
self._execute_subscription)
yielded_work = True
yield handler, sub, node
for gc in node.guards:
if gc._executor_triggered:
if gc.callback_group.can_execute(gc):
handler = self._make_handler(
gc, node, self._take_guard_condition,
self._execute_guard_condition)
yielded_work = True
yield handler, gc, node
for client in node.clients:
if client.handle.pointer in clients_ready:
if client.callback_group.can_execute(client):
handler = self._make_handler(
client, node, self._take_client,
self._execute_client)
yielded_work = True
yield handler, client, node
for srv in node.services:
if srv.handle.pointer in services_ready:
if srv.callback_group.can_execute(srv):
handler = self._make_handler(
srv, node, self._take_service,
self._execute_service)
yielded_work = True
yield handler, srv, node
# Check timeout timer
if (timeout_nsec == 0
or (timeout_timer is not None
and timeout_timer.handle.pointer in timers_ready)):
raise TimeoutException()
if self._is_shutdown:
raise ShutdownException()
def run(self):
_rclpy.rclpy_wait(self.wait_set, -1)
response = _rclpy.rclpy_take_response(
self.client.client_handle, self.client.srv_type.Response, self.client.sequence_number)
if response:
self.client.response = response
def _wait_for_ready_callbacks_priority(
self,
timeout_sec: float = None,
nodes: List['Node'] = None,
condition: Callable[[], bool] = lambda: False,
) -> Generator[Tuple[Task, WaitableEntityType, 'Node'], None, None]:
"""
Yield callbacks that are ready to be executed.
:raise TimeoutException: on timeout.
:raise ShutdownException: on if executor was shut down.
:param timeout_sec: Seconds to wait. Block forever if ``None`` or negative.
Don't wait if 0.
:param nodes: A list of nodes to wait on. Wait on all nodes if ``None``.
:param condition: A callable that makes the function return immediately when it evaluates
to True.
"""
timeout_timer = None
timeout_nsec = timeout_sec_to_nsec(timeout_sec)
# timeout_nsec = timeout_sec_to_nsec(1) #haya
# print("timeout_sec", timeout_sec)
# print("timeout_nsec", timeout_nsec)
if timeout_nsec > 0:
timeout_timer = Timer(None, None, timeout_nsec, self._clock, context=self._context)
yielded_work = False
while not yielded_work and not self._is_shutdown and not condition():
# Refresh "all" nodes in case executor was woken by a node being added or removed
nodes_to_use = nodes
if nodes is None:
nodes_to_use = self.get_nodes()
nodes_to_use_priority = nodes
if nodes is None:
nodes_to_use_priority = self.get_nodes_priority()
print("nodes_to_use_priority", nodes_to_use_priority)
# Yield tasks in-progress before waiting for new work
tasks = None
with self._tasks_lock:
tasks = list(self._tasks)
if tasks:
for task, entity, node in reversed(tasks):
if (not task.executing() and not task.done() and
(node is None or node in nodes_to_use)):
yielded_work = True
yield task, entity, node
with self._tasks_lock:
# Get rid of any tasks that are done
self._tasks = list(filter(lambda t_e_n: not t_e_n[0].done(), self._tasks))
# Gather entities that can be waited on
subscriptions: List[Subscription] = []
guards: List[GuardCondition] = []
timers: List[Timer] = []
clients: List[Client] = []
services: List[Service] = []
waitables: List[Waitable] = []
for node in nodes_to_use:
subscriptions.extend(filter(self.can_execute, node.subscriptions))
# print(subscriptions.extend(filter(self.can_execute, node.subscriptions)))
# print(node.subscriptions)
timers.extend(filter(self.can_execute, node.timers))
# print(node.timers)
clients.extend(filter(self.can_execute, node.clients))
# print(node.clients)
services.extend(filter(self.can_execute, node.services))
node_guards = filter(self.can_execute, node.guards)
waitables.extend(filter(self.can_execute, node.waitables))
# retrigger a guard condition that was triggered but not handled
for gc in node_guards:
if gc._executor_triggered:
gc.trigger()
guards.append(gc)
if timeout_timer is not None:
timers.append(timeout_timer)
guards.append(self._guard)
guards.append(self._sigint_gc)
entity_count = NumberOfEntities(
len(subscriptions), len(guards), len(timers), len(clients), len(services))
print(entity_count)
for waitable in waitables:
entity_count += waitable.get_num_entities()
# Construct a wait set
with _WaitSet() as wait_set, ExitStack() as context_stack:
sub_capsules = []
for sub in subscriptions:
try:
sub_capsules.append(context_stack.enter_context(sub.handle))
except InvalidHandle:
entity_count.num_subscriptions -= 1
client_capsules = []
for cli in clients:
try:
client_capsules.append(context_stack.enter_context(cli.handle))
except InvalidHandle:
entity_count.num_clients -= 1
service_capsules = []
for srv in services:
try:
service_capsules.append(context_stack.enter_context(srv.handle))
except InvalidHandle:
entity_count.num_services -= 1
timer_capsules = []
for tmr in timers:
try:
timer_capsules.append(context_stack.enter_context(tmr.handle))
except InvalidHandle:
entity_count.num_timers -= 1
guard_capsules = []
for gc in guards:
try:
guard_capsules.append(context_stack.enter_context(gc.handle))
except InvalidHandle:
entity_count.num_guard_conditions -= 1
context_capsule = context_stack.enter_context(self._context.handle)
_rclpy.rclpy_wait_set_init(
wait_set,
entity_count.num_subscriptions,
entity_count.num_guard_conditions,
entity_count.num_timers,
entity_count.num_clients,
entity_count.num_services,
entity_count.num_events,
context_capsule)
# print("haya")
# print(entity_count.num_timers)
# print(entity_count.num_subscriptions)
# print(entity_count.num_clients)
# print(wait_set)
# print(context_capsule)
# print("haya_tlqkf")
_rclpy.rclpy_wait_set_clear_entities(wait_set)
for sub_capsule in sub_capsules:
# print('tlqkf!!!!!!')
_rclpy.rclpy_wait_set_add_entity('subscription', wait_set, sub_capsule)
for cli_capsule in client_capsules:
# print('tlqkf!!!!!!')
_rclpy.rclpy_wait_set_add_entity('client', wait_set, cli_capsule)
for srv_capsule in service_capsules:
_rclpy.rclpy_wait_set_add_entity('service', wait_set, srv_capsule)
for tmr_capsule in timer_capsules:
_rclpy.rclpy_wait_set_add_entity('timer', wait_set, tmr_capsule)
for gc_capsule in guard_capsules:
_rclpy.rclpy_wait_set_add_entity('guard_condition', wait_set, gc_capsule)
for waitable in waitables:
waitable.add_to_wait_set(wait_set)
# Wait for something to become ready
_rclpy.rclpy_wait(wait_set, timeout_nsec)
if self._is_shutdown:handle.pointer
# raise ShutdownException()
if not self._context.ok():
raise ExternalShutdownException()
# get ready entities
subs_ready = _rclpy.rclpy_get_ready_entities('subscription', wait_set)
guards_ready = _rclpy.rclpy_get_ready_entities('guard_condition', wait_set)
timers_ready = _rclpy.rclpy_get_ready_entities('timer', wait_set)
clients_ready = _rclpy.rclpy_get_ready_entities('client', wait_set)
services_ready = _rclpy.rclpy_get_ready_entities('service', wait_set)
# Mark all guards as triggered before yielding since they're auto-taken
for gc in guards:
if gc.handle.pointer in guards_ready:
gc._executor_triggered = True
# Check waitables before wait set is destroyed
for node in nodes_to_use:
for wt in node.waitables:
# Only check waitables that were added to the wait set
if wt in waitables and wt.is_ready(wait_set):
handler = self._make_handler(
wt, node, lambda e: e.take_data(), self._execute_waitable)
yielded_work = True
print('tlqkf!!!!!!')
# print(node)
yield handler, wt, node
# # haya
# # priority based scheduling
# ####################### 1 #########################
# # 0. initialization
# # print('nodes: ', nodes)
# # print('nodes_to_use: ', nodes_to_use)
# # print('nodes: ', list(nodes))
# # print('before: ', subs_ready)
# # 1. node interface
# interface_set = 0
# # interface_set = []
# for node in nodes_to_use:
# for sub in node.subscriptions:
# if sub.handle.pointer in subs_ready:
# if "SourceLinearX" in str(node): # haya hard cording to be chaged
# print('digh!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!')
# print('pointer of SourceLinearX: ', sub.handle.pointer)
# interface_set = sub.handle.pointer
# # print('pointer2: ', interface_set)
# # 2. get priority_set
# print('before: ', subs_ready)
# priority_set = []
# remain_set = []
# for idx in range(len(subs_ready)):
# # print('idx: ', idx)
# if interface_set == subs_ready[idx]:
# # print('A')
# priority_set.append(subs_ready[idx])
# else:
# # print('B')
# remain_set.append(subs_ready[idx])
# # 3. re-sort subs_ready based on priority_set
# # print('priority_set: ', priority_set)
# # print('remain_set: ',remain_set)
# subs_ready = priority_set + remain_set
# print('after: ', subs_ready)
# if len(subs_ready) > 0:
# print('subs_ready[0]: ', subs_ready[0])
############################### 2 ##########################
# print('before: ', nodes_to_use)
# print('before: ', subs_ready)
# priority_set = []
# remain_set = []
# for idx in range(len(nodes_to_use)):
# if "SourceLinearX" in str(nodes_to_use[idx]): # haya hard cording to be chaged
# priority_set.append(nodes_to_use[idx])
# else:
# remain_set.append(nodes_to_use[idx])
# nodes_to_use = priority_set + remain_set
# # print('after: ', nodes_to_use)
# # print('after: ', subs_ready)
# wooyoung 1st attempt start
# temp1 = []
# temp2 = []
# temp3 = []
# number = 0
# for node in nodes_to_use:
# print('!!!!!!!!!!!!!!!!!!!!!')
# print('number ', number)
# number=number+1
# print("BEFORE: ", node.woo())
# for sub in node.subscriptions:
# # if sub.handle.pointer in subs_ready:
# if "source_d" in str(sub.topic):
# temp1.append(sub)
# print("?????????FINDFINDFINDFIND?????????", sub)
# else:
# temp2.append(sub)
# temp3.extend(temp1+temp2)
# print("TEMP3: ", temp3)
# node.woo2(temp3)
# temp1.clear()
# temp2.clear()
# temp3.clear()
# print("AFTER: ", node.woo())
# wooyoung 1st attempt end
# wooyoung 2nd attempt start
temp1 = []
temp2 = []
number = 0
for node in nodes_to_use:
print('!!!!!!!!!!!!!!!!!!!!!')
print('number ', number)
number=number+1
print("BEFORE: ", node.woo())
for sub in node.subscriptions:
# if sub.handle.pointer in subs_ready:
if "source_d" in str(sub.topic):
temp1.append(sub)
else:
temp2.append(sub)
# wooyoung 2nd attempt end
for node in nodes_to_use:
# haya_sub = haya_sub + 1
# print("2: ", haya_sub)
# print(node)
# print(nodes_to_use)
haya_sub_sub = 0
for tmr in node.timers:
# haya_sub_sub = haya_sub_sub + 1
# print("3: ", haya_sub_sub)
# print(node)
# print(node.timers)
# print(tmr)
if tmr.handle.pointer in timers_ready:
# print('tmr')
# print(timers_ready)
# print(tmr)
# print(tmr.handle.pointer)
with tmr.handle as capsule:
# Check timer is ready to workaround rcl issue with cancelled timers
if _rclpy.rclpy_is_timer_ready(capsule):
if tmr.callback_group.can_execute(tmr):
handler = self._make_handler(
tmr, node, self._take_timer, self._execute_timer)
yielded_work = True
# print('tlqkf1')
# print(handler)
# print(tmr)
# print(node)
yield handler, tmr, node
for node in nodes_to_use:
for sub in node.subscriptions:
if sub in temp1:
if sub.handle.pointer in subs_ready:
if sub.callback_group.can_execute(sub):
handler = self._make_handler(sub, node, self._take_subscription, self._execute_subscription)
yielded_work = True
yield handler, sub, node
start = time.time()
for node in nodes_to_use:
for sub in node.subscriptions:
if sub in temp2:
if sub.handle.pointer in subs_ready:
if sub.callback_group.can_execute(sub):
handler = self._make_handler(sub, node, self._take_subscription, self._execute_subscription)
yielded_work = True
yield handler, sub, node
print("!!!!!!!!TIME!!!!!!!!!!: ", (time.time()-start)*1000)
for node in nodes_to_use:
for gc in node.guards:
# print(node)
if gc._executor_triggered:
if gc.callback_group.can_execute(gc):
handler = self._make_handler(
gc, node, self._take_guard_condition,
self._execute_guard_condition)
yielded_work = True
# print('tlqkf3')
# print(node)
yield handler, gc, node
for node in nodes_to_use:
for client in node.clients:
# print(node)
if client.handle.pointer in clients_ready:
# print("&&&&&&&&&&&&&&&&&&&&&&&&&&&")
# print("client")
# print(clients_ready)
# print(client.handle.pointer)
if client.callback_group.can_execute(client):
handler = self._make_handler(
client, node, self._take_client, self._execute_client)
yielded_work = True
# print('tlqkf4')
# print(node)
# print('----------------------------------')
yield handler, client, node
for node in nodes_to_use:
for srv in node.services:
# print(node)
if srv.handle.pointer in services_ready:
# print("&&&&&&&&&&&&&&&&&&&&&&&&&&&")
# print("services")
# print(services_ready)
# print(srv.handle.pointer)
if srv.callback_group.can_execute(srv):
handler = self._make_handler(
srv, node, self._take_service, self._execute_service)
yielded_work = True
# print('tlqkf5')
# print(node)
# print('----------------------------------')
yield handler, srv, node
# Check timeout timer
if (
timeout_nsec == 0 or
(timeout_timer is not None and timeout_timer.handle.pointer in timers_ready)
):
raise TimeoutException()
if self._is_shutdown:
raise ShutdownException()
if condition():
raise ConditionReachedException()
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)
