示例#1
0
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
示例#2
0
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))
示例#4
0
 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)
示例#5
0
 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)
示例#6
0
    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
示例#7
0
 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)
示例#8
0
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
示例#11
0
 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)
示例#12
0
 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