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: parameter = Parameter() Parameter.name = args.name parameter.value = get_parameter_value(string_value=args.value) response = call_set_parameters( node=node, node_name=args.node_name, parameters=[parameter]) # output response assert len(response.results) == 1 result = response.results[0] if result.successful: msg = 'Set parameter successful' if result.reason: msg += ': ' + result.reason print(msg) else: msg = 'Setting parameter failed' if result.reason: msg += ': ' + result.reason print(msg, file=sys.stderr)
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: if args.all: transitions = call_get_transition_graph( node=node, states={node_name: None}) else: transitions = call_get_available_transitions( node=node, states={node_name: None}) transitions = transitions[node_name] if isinstance(transitions, Exception): return 'Exception while calling service of node ' \ "'{args.node_name}': {transitions}" \ .format_map(locals()) for t in transitions: print('- {t.transition.label} [{t.transition.id}]\n' '\tStart: {t.start_state.label}\n' '\tGoal: {t.goal_state.label}'.format_map(locals()))
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: parameter = Parameter() Parameter.name = args.name value = ParameterValue() value.type = ParameterType.PARAMETER_NOT_SET parameter.value = value response = call_set_parameters(node=node, node_name=args.node_name, parameters=[parameter]) # output response assert len(response.results) == 1 result = response.results[0] if result.successful: msg = 'Deleted parameter successfully' if result.reason: msg += ': ' + result.reason print(msg) else: msg = 'Deleting parameter failed' if result.reason: msg += ': ' + result.reason print(msg, file=sys.stderr)
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_names = [n.full_name for n in node_names] node_name = get_absolute_node_name(args.node_name) if node_name: if node_name not in node_names: return 'Node not found' with DirectNode(args) as node: states = call_get_states(node=node, node_names=node_names) # output exceptions for node_name in sorted(states.keys()): state = states[node_name] if isinstance(state, Exception): print('Exception while calling service of node ' "'{node_name}': {state}".format_map(locals()), file=sys.stderr) del states[node_name] if args.node_name: return 1 # output current states for node_name in sorted(states.keys()): state = states[node_name] prefix = '' if not args.node_name: prefix = '{node_name}: '.format_map(locals()) print(prefix + '{state.label} [{state.id}]'.format_map(locals()))
def get_param(node_name, name, default, params_glob): """ Gets a parameter from a given node """ if params_glob and not any( fnmatch.fnmatch(str(name), glob) for glob in params_glob): # If the glob list is not empty and there are no glob matches, # stop the attempt to get the parameter. return # If the glob list is empty (i.e. false) or the parameter matches # one of the glob strings, continue to get the parameter. if default is not "": try: default = loads(default) except ValueError: pass # Keep default without modifications. node_name = get_absolute_node_name(node_name) with param_server_lock: try: # call_get_parameters will fail if node does not exist. response = call_get_parameters(node=_node, node_name=node_name, parameter_names=[name]) pvalue = response.values[0] # if type is 0 (parameter not set), the next line will raise an exception # and return value shall go to default. value = getattr(pvalue, _parameter_type_mapping[pvalue.type]) except: # If either the node or the parameter does not exist, return default. value = default return dumps(value)
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] regex_filter = getattr(args, 'filter') if regex_filter is not None: regex_filter = re.compile(regex_filter[0]) with DirectNode(args) as node: responses = {} for node_name in node_names: responses[node_name] = call_list_parameters( node=node, node_name=node_name.full_name, prefixes=args.param_prefixes) # print responses for node_name in sorted(responses.keys()): response = responses[node_name] if response is None: print( 'Wait for service timed out waiting for ' f'parameter services for node {node_name}') continue elif response.result() is None: e = response.exception() print( 'Exception while calling service of node ' f"'{node_name.full_name}': {e}", file=sys.stderr) continue response = response.result().result.names sorted_names = sorted(response) if regex_filter is not None: sorted_names = [name for name in sorted_names if regex_filter.match(name)] if not args.node_name and sorted_names: print(f'{node_name.full_name}:') # get descriptors for the node if needs to print parameter type name_to_type_map = {} if args.param_type is True: resp = call_describe_parameters( node=node, node_name=node_name.full_name, parameter_names=sorted_names) for descriptor in resp.descriptors: name_to_type_map[descriptor.name] = get_parameter_type_string( descriptor.type) for name in sorted_names: if args.param_type is True: param_type_str = name_to_type_map[name] print(f' {name} (type: {param_type_str})') else: print(f' {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: yaml_output = {node_name.full_name: {'ros__parameters': {}}} # retrieve values response = call_list_parameters(node=node, node_name=absolute_node_name) if response is None: raise RuntimeError( 'Wait for service timed out waiting for ' f'parameter services for node {node_name.full_name}') elif response.result() is None: e = response.exception() raise RuntimeError( 'Exception while calling service of node ' f"'{node_name.full_name}': {e}") response = response.result().result.names response = sorted(response) parameter_values = self.get_parameter_values(node, absolute_node_name, response) for param_name, pval in zip(response, parameter_values): self.insert_dict( yaml_output[node_name.full_name]['ros__parameters'], param_name, pval) 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: 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: clients = {} futures = {} # create clients for node_name in node_names: client = node.create_client( ListParameters, '{node_name.full_name}/list_parameters'.format_map( locals())) clients[node_name] = client # 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 = 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 node_name in node_names: rclpy.spin_until_future_complete(node, futures[node_name]) # 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('{node_name.full_name}:'.format_map(locals())) response = future.result() for name in sorted(response.result.names): print(' {name}'.format_map(locals())) else: e = future.exception() print('Exception while calling service of node ' "'{node_name.full_name}': {e}".format_map(locals()), file=sys.stderr)
def init(parent_node_name): """ Initializes params module with a rclpy.node.Node for further use. This function has to be called before any other for the module to work. """ global _node, _parent_node_name # TODO(@jubeira): remove this node; use rosapi node with MultiThreadedExecutor or # async / await to prevent the service calls from blocking. _node = rclpy.create_node('rosapi_params') _parent_node_name = get_absolute_node_name(parent_node_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) 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: load_parameter_file(node=node, node_name=node_name, parameter_file=args.parameter_file, use_wildcard=not args.no_use_wildcard)
def delete_param(node_name, name, params_glob): """ Deletes a parameter in a given node """ if params_glob and not any( fnmatch.fnmatch(str(name), glob) for glob in params_glob): # If the glob list is not empty and there are no glob matches, # stop the attempt to delete the parameter. return # If the glob list is empty (i.e. false) or the parameter matches # one of the glob strings, continue to delete the parameter. node_name = get_absolute_node_name(node_name) if has_param(node_name, name, params_glob): with param_server_lock: _set_param(node_name, name, None, ParameterType.PARAMETER_NOT_SET)
def get_node_param_names(node_name, params_glob): """ Gets list of parameter names for a given node """ node_name = get_absolute_node_name(node_name) with param_server_lock: if params_glob: # If there is a parameter glob, filter by it. return list( filter( lambda x: any( fnmatch.fnmatch(str(x), glob) for glob in params_glob), _get_param_names(node_name))) else: # If there is no parameter glob, don't filter. return _get_param_names(node_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) 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: transitions = call_get_available_transitions( node=node, states={args.node_name: None}) transitions = transitions[args.node_name] if isinstance(transitions, Exception): return 'Exception while calling service of node ' \ "'{args.node_name}': {transitions}" \ .format_map(locals()) # identify requested transition for transition in [t.transition for t in transitions]: if transition.label == args.transition: break else: for transition in [t.transition for t in transitions]: if str(transition.id) == args.transition: break else: return \ 'Unknown transition requested, available ones are:' + \ ''.join( '\n- {t.transition.label} [{t.transition.id}]' .format_map(locals()) for t in transitions) results = call_change_states( node=node, transitions={args.node_name: transition}) result = results[args.node_name] # output response if isinstance(result, Exception): print('Exception while calling service of node ' "'{args.node_name}': {result}".format_map(locals()), file=sys.stderr) elif result: print('Transitioning successful') else: print('Transitioning failed', file=sys.stderr)
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_describe_parameters( node=node, node_name=args.node_name, parameter_names=args.parameter_names or None) # output response for descriptor in response.descriptors: self._print_descriptor(descriptor)
def set_param(node_name, name, value, params_glob): """ Sets a parameter in a given node """ if params_glob and not any( fnmatch.fnmatch(str(name), glob) for glob in params_glob): # If the glob list is not empty and there are no glob matches, # stop the attempt to set the parameter. return # If the glob list is empty (i.e. false) or the parameter matches # one of the glob strings, continue to set the parameter. d = None try: d = loads(value) except ValueError: raise Exception( "Due to the type flexibility of the ROS parameter server, the value argument to set_param must be a JSON-formatted string." ) node_name = get_absolute_node_name(node_name) with param_server_lock: _set_param(node_name, name, value)
def has_param(node_name, name, params_glob): """ Checks whether a given node has a parameter or not """ if params_glob and not any( fnmatch.fnmatch(str(name), glob) for glob in params_glob): # If the glob list is not empty and there are no glob matches, # stop the attempt to set the parameter. return False # If the glob list is empty (i.e. false) or the parameter matches # one of the glob strings, check whether the parameter exists. node_name = get_absolute_node_name(node_name) with param_server_lock: try: response = call_get_parameters(node=_node, node_name=node_name, parameter_names=[name]) except: return False return response.values[0].type > 0 and response.values[0].type < len( _parameter_type_mapping)
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.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.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(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: 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] regex_filter = getattr(args, 'filter') if regex_filter is not None: regex_filter = re.compile(regex_filter[0]) 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, timeout_sec=1.0) # print responses for node_name in sorted(futures.keys()): future = futures[node_name] if future.result() is None: e = future.exception() print( 'Exception while calling service of node ' f"'{node_name.full_name}': {e}", file=sys.stderr) response = future.result() sorted_names = sorted(response.result.names) if regex_filter is not None: sorted_names = [ name for name in sorted_names if regex_filter.match(name) ] if not args.node_name and sorted_names: print(f'{node_name.full_name}:') # get descriptors for the node if needs to print parameter type name_to_type_map = {} if args.param_type is True: resp = call_describe_parameters( node=node, node_name=node_name.full_name, parameter_names=sorted_names) for descriptor in resp.descriptors: name_to_type_map[ descriptor.name] = get_parameter_type_string( descriptor.type) for name in sorted_names: if args.param_type is True: param_type_str = name_to_type_map[name] print(f' {name} (type: {param_type_str})') else: print(f' {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) 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)