def call_get_states(*, node, node_names):
clients = {}
futures = {}
# create clients
for node_name in node_names:
clients[node_name] = \
node.create_client(GetState, f'{node_name}/get_state')
# wait until all clients have been called
while True:
for node_name in [n for n in node_names if n not in futures]:
# call as soon as ready
client = clients[node_name]
if client.service_is_ready():
request = GetState.Request()
future = client.call_async(request)
futures[node_name] = future
if len(futures) == len(clients):
break
rclpy.spin_once(node, timeout_sec=1.0)
# wait for all responses
for future in futures.values():
rclpy.spin_until_future_complete(node, future)
# return current state or exception for each node
states = {}
for node_name, future in futures.items():
if future.result() is not None:
response = future.result()
states[node_name] = response.current_state
else:
states[node_name] = future.exception()
return states
def get_state(lifecycle_node):
node = rclpy.create_node('lc_client_py')
cli = node.create_client(GetState, lifecycle_node + '/get_state')
req = GetState.Request()
cli.call(req)
cli.wait_for_future()
print('%s is in state %s(%u)'
% (lifecycle_node, cli.response.current_state.label, cli.response.current_state.id))
def get_state(lifecycle_node):
node = rclpy.create_node('lc_client_py')
service_name = lifecycle_node + '/get_state'
cli = node.create_client(GetState, service_name)
if not cli.wait_for_service(timeout_sec=5.0):
node.get_logger().warn('Unable to call service %s' % service_name)
return
req = GetState.Request()
cli.call(req)
cli.wait_for_future()
node.get_logger().info('%s is in state %s(%u)' %
(lifecycle_node, cli.response.current_state.label,
cli.response.current_state.id))
def wait_for_node_active(self, node):
# wait for the bt_navigator to be in active state
node_service = '/' + node + '/get_state'
state_client = self.create_client(GetState, node_service)
while not state_client.wait_for_service(timeout_sec=1.0):
print(node_service + ' service not available, waiting...')
req = GetState.Request() # empty request
state = 'UNKNOWN'
while (state != 'active'):
self.get_logger().info('Getting ' + node + ' state...')
future = state_client.call_async(req)
rclpy.spin_until_future_complete(self, future)
if future.result() is not None:
state = future.result().current_state.label
self.get_logger().info('Result of get_state: %s' % state)
else:
self.get_logger().error('Exception while calling service: %r' % future.exception())
time.sleep(5)
def wait_for_node_active(self, node_name: str):
# Waits for the node within the tester namespace to become active
self.info_msg('Waiting for ' + node_name + ' to become active')
node_service = node_name + '/get_state'
state_client = self.create_client(GetState, node_service)
while not state_client.wait_for_service(timeout_sec=1.0):
self.info_msg(node_service + ' service not available, waiting...')
req = GetState.Request() # empty request
state = 'UNKNOWN'
while (state != 'active'):
self.info_msg('Getting ' + node_name + ' state...')
future = state_client.call_async(req)
rclpy.spin_until_future_complete(self, future)
if future.result() is not None:
state = future.result().current_state.label
self.info_msg('Result of get_state: %s' % state)
else:
self.error_msg('Exception while calling service: %r' % future.exception())
time.sleep(5)
def _waitForNodeToActivate(self, node_name):
# Waits for the node within the tester namespace to become active
self.debug(f'Waiting for {node_name} to become active..')
node_service = f'{node_name}/get_state'
state_client = self.create_client(GetState, node_service)
while not state_client.wait_for_service(timeout_sec=1.0):
self.info(f'{node_service} service not available, waiting...')
req = GetState.Request()
state = 'unknown'
while state != 'active':
self.debug(f'Getting {node_name} state...')
future = state_client.call_async(req)
rclpy.spin_until_future_complete(self, future)
if future.result() is not None:
state = future.result().current_state.label
self.debug(f'Result of get_state: {state}')
time.sleep(2)
return
def wait_for_node_active(self, node_name: str):
# Waits for the node within the tester namespace to become active
self.info_msg(f"Waiting for {node_name} to become active")
node_service = f"{node_name}/get_state"
state_client = self.create_client(GetState, node_service)
while not state_client.wait_for_service(timeout_sec=1.0):
self.info_msg(f"{node_service} service not available, waiting...")
req = GetState.Request() # empty request
state = "UNKNOWN"
while state != "active":
self.info_msg(f"Getting {node_name} state...")
future = state_client.call_async(req)
rclpy.spin_until_future_complete(self, future)
if future.result() is not None:
state = future.result().current_state.label
self.info_msg(f"Result of get_state: {state}")
else:
self.error_msg(
f"Exception while calling service: {future.exception()!r}")
time.sleep(5)
def get_state(lifecycle_node):
node = rclpy.create_node('lc_client_py')
service_name = lifecycle_node + '/get_state'
cli = node.create_client(GetState, service_name)
if not cli.wait_for_service(timeout_sec=5.0):
node.get_logger().warn('Unable to call service %s' % service_name)
return
req = GetState.Request()
future = cli.call_async(req)
rclpy.spin_until_future_complete(node, future)
if future.result() is not None:
resp = future.result()
node.get_logger().info(
'%s is in state %s(%u)' %
(lifecycle_node, resp.current_state.label, resp.current_state.id))
else:
node.get_logger.error('Exception %r during call %s' %
(future.exception(), lifecycle_node))
def wait_for_localization(self):
request = GetState.Request()
topic = f'/{self.args.robot_name}/amcl/get_state'
client = self.create_client(GetState, topic)
if not client.service_is_ready():
self.get_logger().info(f'waiting for service {topic}')
client.wait_for_service()
self.get_logger().info(f'connected to state service')
while True:
future = client.call_async(request)
rclpy.spin_until_future_complete(self, future)
if future.result() is not None:
print('response: %r' % future.result())
self.get_logger().info(f'{future.result()}')
if future.result().current_state.id == 3:
break
else:
raise RuntimeError(
'exception while calling service: %r' % future.exception())
self.send_initial_pose()
def get_lifecycle_state(self):
"""
Get lifecycle state.
Arguments
---------
None
Returns
-------
label : int
"""
future = self.client_get_state.call_async(GetState.Request())
rclpy.spin_until_future_complete(self,
future,
executor=self.executor,
timeout_sec=self.timeout_sec)
if future.result() is None:
raise RuntimeError(
"Interpreter tried to get current lifecycle state, but failed."
)
return future.result().current_state.label
def do_get_state_(self, label=""):
fut = self.get_state_client_.call_async(GetState.Request())
rclpy.spin_until_future_complete(self.node_, fut, executor=self.exe_)
resp = fut.result()
self.assertTrue(resp is not None)
self.assertEqual(resp.current_state.label, label)
def _get_mouse_lifecycle_state(self):
future = self._client_get_state.call_async(GetState.Request())
executor = rclpy.executors.SingleThreadedExecutor(context=self.context)
rclpy.spin_until_future_complete(self, future, executor=executor)
return future.result().current_state.label