def __call__(self, prefix, parsed_args, **kwargs): with NodeStrategy(parsed_args) as node: return get_topic_names(node=node, include_hidden_topics=getattr( parsed_args, self.include_hidden_topics_key))
def main(self, *, args): with NodeStrategy(args) as node: response = configure_controller(node, args.controller_manager, args.controller_name) if not response.ok: 'Error configuring controller, check controller_manager logs' return 'Successfully configured controller {}'.format(args.controller_name)
def main(self, *, args): # noqa: D102 with NodeStrategy(args) as node: node_names = get_node_names( node=node, include_hidden_nodes=args.include_hidden_nodes) absolute_node_name = get_absolute_node_name(args.node_name) node_name = parse_node_name(absolute_node_name) if absolute_node_name: if absolute_node_name not in [n.full_name for n in node_names]: return 'Node not found' if not os.path.isdir(args.output_dir): raise RuntimeError( f"'{args.output_dir}' is not a valid directory.") with DirectNode(args) as node: # create client service_name = f'{absolute_node_name}/list_parameters' client = node.create_client(ListParameters, service_name) client.wait_for_service() if not client.service_is_ready(): raise RuntimeError(f"Could not reach service '{service_name}'") request = ListParameters.Request() future = client.call_async(request) # wait for response rclpy.spin_until_future_complete(node, future) yaml_output = {node_name.full_name: {'ros__parameters': {}}} # retrieve values if future.result() is not None: response = future.result() for param_name in sorted(response.result.names): pval = self.get_parameter_value(node, absolute_node_name, param_name) self.insert_dict( yaml_output[node_name.full_name]['ros__parameters'], param_name, pval) else: e = future.exception() raise RuntimeError('Exception while calling service of node ' f"'{node_name.full_name}': {e}") if args.print: print( "WARNING: '--print' is deprecated; this utility prints to stdout by default", file=sys.stderr) if args.output_dir != '.': print( "WARNING: '--output-dir' is deprecated; use redirection to save to a file", file=sys.stderr) else: print(yaml.dump(yaml_output, default_flow_style=False)) return if absolute_node_name[0] == '/': file_name = absolute_node_name[1:].replace('/', '__') else: file_name = absolute_node_name.replace('/', '__') print('Saving to: ', os.path.join(args.output_dir, file_name + '.yaml')) with open(os.path.join(args.output_dir, file_name + '.yaml'), 'w') as yaml_file: yaml.dump(yaml_output, yaml_file, default_flow_style=False)
def main(self, *, args): # noqa: D102 with NodeStrategy(args) as node: node_names = get_node_names( node=node, include_hidden_nodes=args.include_hidden_nodes) node_name = get_absolute_node_name(args.node_name) if node_name not in {n.full_name for n in node_names}: return 'Node not found' with DirectNode(args) as node: response = call_get_parameters( node=node, node_name=args.node_name, parameter_names=[args.parameter_name]) assert len(response.values) <= 1 # requested parameter not set if not response.values: return 'Parameter not set' # extract type specific value pvalue = response.values[0] if pvalue.type == ParameterType.PARAMETER_BOOL: label = 'Boolean value is:' value = pvalue.bool_value elif pvalue.type == ParameterType.PARAMETER_INTEGER: label = 'Integer value is:' value = pvalue.integer_value elif pvalue.type == ParameterType.PARAMETER_DOUBLE: label = 'Double value is:' value = pvalue.double_value elif pvalue.type == ParameterType.PARAMETER_STRING: label = 'String value is:' value = pvalue.string_value elif pvalue.type == ParameterType.PARAMETER_BYTE_ARRAY: label = 'Byte values are:' value = pvalue.byte_array_value elif pvalue.type == ParameterType.PARAMETER_BOOL_ARRAY: label = 'Boolean values are:' value = pvalue.bool_array_value elif pvalue.type == ParameterType.PARAMETER_INTEGER_ARRAY: label = 'Integer values are:' value = pvalue.integer_array_value elif pvalue.type == ParameterType.PARAMETER_DOUBLE_ARRAY: label = 'Double values are:' value = pvalue.double_array_value elif pvalue.type == ParameterType.PARAMETER_STRING_ARRAY: label = 'String values are:' value = pvalue.string_array_value elif pvalue.type == ParameterType.PARAMETER_NOT_SET: label = 'Parameter not set.' value = None else: return f"Unknown parameter type '{pvalue.type}'" # output response if not args.hide_type: print(label, value) if value is not None else print(label) else: print(value)
def test_enforce_no_daemon(enforce_daemon_is_running): args = argparse.Namespace(no_daemon=True) with NodeStrategy(args=args) as node: assert node._daemon_node is None assert node._direct_node is not None
def test_with_no_daemon_running(enforce_no_daemon_is_running): with NodeStrategy(args=[]) as node: assert node._daemon_node is None assert node._direct_node is not None
def main(self, *, args): # noqa: D102 with NodeStrategy(args) as node: node_names = get_node_names( node=node, include_hidden_nodes=args.include_hidden_nodes) node_name = get_absolute_node_name(args.node_name) if node_name: if node_name not in [n.full_name for n in node_names]: return 'Node not found' node_names = [n for n in node_names if node_name == n.full_name] with DirectNode(args) as node: service_names = get_service_names( node=node, include_hidden_services=args.include_hidden_nodes) clients = {} futures = {} # create clients for nodes which have the service for node_name in node_names: service_name = f'{node_name.full_name}/list_parameters' if service_name in service_names: client = node.create_client(ListParameters, service_name) clients[node_name] = client # wait until all clients have been called while True: for node_name in [ n for n in clients.keys() if n not in futures ]: # call as soon as ready client = clients[node_name] if client.service_is_ready(): request = ListParameters.Request() for prefix in args.param_prefixes: request.prefixes.append(prefix) 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) # print responses for node_name in sorted(futures.keys()): future = futures[node_name] if future.result() is not None: if not args.node_name: print(f'{node_name.full_name}:') response = future.result() for name in sorted(response.result.names): print(f' {name}') else: e = future.exception() print( 'Exception while calling service of node ' f"'{node_name.full_name}': {e}", file=sys.stderr)