From fc02ffe4c9b242da9b50e4c9c8a8c94fdd58f0c7 Mon Sep 17 00:00:00 2001 From: Brian Date: Fri, 22 Jul 2022 14:55:47 -0700 Subject: [PATCH] refactor: make ros2param use rclpy.parameter_client (#716) Refactors ros2param to use rclpy.parameter_client (https://github.com/ros2/rclpy/commit/3053a8ad9cc266300c7c8e3070bb6552652cf88d). Co-authored-by: Jacob Perron Signed-off-by: Brian Chen --- ros2param/ros2param/api/__init__.py | 225 +++++++--------------------- ros2param/ros2param/verb/dump.py | 51 +++---- ros2param/ros2param/verb/list.py | 60 +++----- ros2param/ros2param/verb/set.py | 3 +- ros2param/test/test_api.py | 4 +- ros2param/test/test_verb_load.py | 5 +- 6 files changed, 96 insertions(+), 252 deletions(-) diff --git a/ros2param/ros2param/api/__init__.py b/ros2param/ros2param/api/__init__.py index 9ae41f03e..ef03a2456 100644 --- a/ros2param/ros2param/api/__init__.py +++ b/ros2param/ros2param/api/__init__.py @@ -13,113 +13,44 @@ # limitations under the License. import sys +import warnings -from rcl_interfaces.msg import Parameter from rcl_interfaces.msg import ParameterType -from rcl_interfaces.msg import ParameterValue -from rcl_interfaces.srv import DescribeParameters -from rcl_interfaces.srv import GetParameters -from rcl_interfaces.srv import ListParameters -from rcl_interfaces.srv import SetParameters import rclpy -from rclpy.parameter import PARAMETER_SEPARATOR_STRING +from rclpy.parameter import get_parameter_value as rclpy_get_parameter_value +from rclpy.parameter import parameter_dict_from_yaml_file +from rclpy.parameter import parameter_value_to_python +from rclpy.parameter_client import AsyncParameterClient from ros2cli.node.direct import DirectNode -import yaml + +def get_parameter_value(*, string_value): + warnings.warn('get_parameter_value() is deprecated. ' + 'Use rclpy.parameter.get_parameter_value instead') + return rclpy_get_parameter_value(string_value) def get_value(*, parameter_value): """Get the value from a ParameterValue.""" - if parameter_value.type == ParameterType.PARAMETER_BOOL: - value = parameter_value.bool_value - elif parameter_value.type == ParameterType.PARAMETER_INTEGER: - value = parameter_value.integer_value - elif parameter_value.type == ParameterType.PARAMETER_DOUBLE: - value = parameter_value.double_value - elif parameter_value.type == ParameterType.PARAMETER_STRING: - value = parameter_value.string_value - elif parameter_value.type == ParameterType.PARAMETER_BYTE_ARRAY: - value = list(parameter_value.byte_array_value) - elif parameter_value.type == ParameterType.PARAMETER_BOOL_ARRAY: - value = list(parameter_value.bool_array_value) - elif parameter_value.type == ParameterType.PARAMETER_INTEGER_ARRAY: - value = list(parameter_value.integer_array_value) - elif parameter_value.type == ParameterType.PARAMETER_DOUBLE_ARRAY: - value = list(parameter_value.double_array_value) - elif parameter_value.type == ParameterType.PARAMETER_STRING_ARRAY: - value = list(parameter_value.string_array_value) - elif parameter_value.type == ParameterType.PARAMETER_NOT_SET: - value = None - else: - value = None - - return value - - -def get_parameter_value(*, string_value): - """Guess the desired type of the parameter based on the string value.""" - value = ParameterValue() + value = None try: - yaml_value = yaml.safe_load(string_value) - except yaml.parser.ParserError: - yaml_value = string_value - - if isinstance(yaml_value, bool): - value.type = ParameterType.PARAMETER_BOOL - value.bool_value = yaml_value - elif isinstance(yaml_value, int): - value.type = ParameterType.PARAMETER_INTEGER - value.integer_value = yaml_value - elif isinstance(yaml_value, float): - value.type = ParameterType.PARAMETER_DOUBLE - value.double_value = yaml_value - elif isinstance(yaml_value, list): - if all((isinstance(v, bool) for v in yaml_value)): - value.type = ParameterType.PARAMETER_BOOL_ARRAY - value.bool_array_value = yaml_value - elif all((isinstance(v, int) for v in yaml_value)): - value.type = ParameterType.PARAMETER_INTEGER_ARRAY - value.integer_array_value = yaml_value - elif all((isinstance(v, float) for v in yaml_value)): - value.type = ParameterType.PARAMETER_DOUBLE_ARRAY - value.double_array_value = yaml_value - elif all((isinstance(v, str) for v in yaml_value)): - value.type = ParameterType.PARAMETER_STRING_ARRAY - value.string_array_value = yaml_value - else: - value.type = ParameterType.PARAMETER_STRING - value.string_value = string_value - else: - value.type = ParameterType.PARAMETER_STRING - value.string_value = yaml_value + value = parameter_value_to_python(parameter_value) + except RuntimeError as e: + print(f'Runtime error {e} raised') return value -def parse_parameter_dict(*, namespace, parameter_dict): - parameters = [] - for param_name, param_value in parameter_dict.items(): - full_param_name = namespace + param_name - # Unroll nested parameters - if type(param_value) == dict: - parameters += parse_parameter_dict( - namespace=full_param_name + PARAMETER_SEPARATOR_STRING, - parameter_dict=param_value) - else: - parameter = Parameter() - parameter.name = full_param_name - parameter.value = get_parameter_value(string_value=str(param_value)) - parameters.append(parameter) - return parameters - - -def load_parameter_dict(*, node, node_name, parameter_dict): - - parameters = parse_parameter_dict(namespace='', parameter_dict=parameter_dict) - response = call_set_parameters( - node=node, node_name=node_name, parameters=parameters) - - # output response - assert len(response.results) == len(parameters) +def load_parameter_file(*, node, node_name, parameter_file, use_wildcard): + client = AsyncParameterClient(node, node_name) + ready = client.wait_for_services(timeout_sec=5.0) + if not ready: + raise RuntimeError('Wait for service timed out waiting for ' + f'parameter services for node {node_name}') + future = client.load_parameter_file(parameter_file, use_wildcard) + parameters = list(parameter_dict_from_yaml_file(parameter_file, use_wildcard).values()) + rclpy.spin_until_future_complete(node, future) + response = future.result() + assert len(response.results) == len(parameters), 'Not all parameters set' for i in range(0, len(response.results)): result = response.results[i] param_name = parameters[i].name @@ -135,105 +66,59 @@ def load_parameter_dict(*, node, node_name, parameter_dict): print(msg, file=sys.stderr) -def load_parameter_file(*, node, node_name, parameter_file, use_wildcard): - # Remove leading slash and namespaces - with open(parameter_file, 'r') as f: - param_file = yaml.safe_load(f) - param_keys = [] - if use_wildcard and '/**' in param_file: - param_keys.append('/**') - if node_name in param_file: - param_keys.append(node_name) - - if param_keys == []: - raise RuntimeError('Param file does not contain parameters for {}, ' - ' only for nodes: {}' .format(node_name, param_file.keys())) - param_dict = {} - for k in param_keys: - value = param_file[k] - if type(value) != dict or 'ros__parameters' not in value: - raise RuntimeError('Invalid structure of parameter file for node {}' - 'expected same format as provided by ros2 param dump' - .format(k)) - param_dict.update(value['ros__parameters']) - load_parameter_dict(node=node, node_name=node_name, parameter_dict=param_dict) - - def call_describe_parameters(*, node, node_name, parameter_names=None): - # create client - client = node.create_client( - DescribeParameters, f'{node_name}/describe_parameters') - - # call as soon as ready - ready = client.wait_for_service(timeout_sec=5.0) + client = AsyncParameterClient(node, node_name) + ready = client.wait_for_services(timeout_sec=5.0) if not ready: - raise RuntimeError('Wait for service timed out') - - request = DescribeParameters.Request() - if parameter_names: - request.names = parameter_names - future = client.call_async(request) + raise RuntimeError('Wait for service timed out waiting for ' + f'parameter services for node {node_name}') + future = client.describe_parameters(parameter_names) rclpy.spin_until_future_complete(node, future) - - # handle response response = future.result() + if response is None: + raise RuntimeError('Exception while calling service of node ' + f'{node_name}: {future.exception()}') return response def call_get_parameters(*, node, node_name, parameter_names): - # create client - client = node.create_client(GetParameters, f'{node_name}/get_parameters') - - # call as soon as ready - ready = client.wait_for_service(timeout_sec=5.0) + client = AsyncParameterClient(node, node_name) + ready = client.wait_for_services(timeout_sec=5.0) if not ready: - raise RuntimeError('Wait for service timed out') - - request = GetParameters.Request() - request.names = parameter_names - future = client.call_async(request) + raise RuntimeError('Wait for service timed out waiting for ' + f'parameter services for node {node_name}') + future = client.get_parameters(parameter_names) rclpy.spin_until_future_complete(node, future) - - # handle response response = future.result() + if response is None: + raise RuntimeError('Exception while calling service of node ' + f'{node_name}: {future.exception()}') return response def call_set_parameters(*, node, node_name, parameters): - # create client - client = node.create_client(SetParameters, f'{node_name}/set_parameters') - - # call as soon as ready - ready = client.wait_for_service(timeout_sec=5.0) + client = AsyncParameterClient(node, node_name) + ready = client.wait_for_services(timeout_sec=5.0) if not ready: - raise RuntimeError('Wait for service timed out') - - request = SetParameters.Request() - request.parameters = parameters - future = client.call_async(request) + raise RuntimeError('Wait for service timed out waiting for ' + f'parameter services for node {node_name}') + future = client.set_parameters(parameters) rclpy.spin_until_future_complete(node, future) - - # handle response response = future.result() + if response is None: + raise RuntimeError('Exception while calling service of node ' + f'{node_name}: {future.exception()}') return response -def call_list_parameters(*, node, node_name, prefix=None): - # create client - client = node.create_client(ListParameters, f'{node_name}/list_parameters') - - # call as soon as ready - ready = client.wait_for_service(timeout_sec=5.0) +def call_list_parameters(*, node, node_name, prefixes=None): + client = AsyncParameterClient(node, node_name) + ready = client.wait_for_services(timeout_sec=5.0) if not ready: - raise RuntimeError('Wait for service timed out') - - request = ListParameters.Request() - future = client.call_async(request) + return None + future = client.list_parameters(prefixes=prefixes) rclpy.spin_until_future_complete(node, future) - - # handle response - response = future.result() - return response.result.names + return future def get_parameter_type_string(parameter_type): diff --git a/ros2param/ros2param/verb/dump.py b/ros2param/ros2param/verb/dump.py index 98c5b01fe..09f812226 100644 --- a/ros2param/ros2param/verb/dump.py +++ b/ros2param/ros2param/verb/dump.py @@ -15,24 +15,19 @@ import os import sys -from rcl_interfaces.srv import ListParameters - -import rclpy from rclpy.parameter import PARAMETER_SEPARATOR_STRING - from ros2cli.node.direct import DirectNode from ros2cli.node.strategy import add_arguments from ros2cli.node.strategy import NodeStrategy - from ros2node.api import get_absolute_node_name from ros2node.api import get_node_names from ros2node.api import NodeNameCompleter from ros2node.api import parse_node_name from ros2param.api import call_get_parameters +from ros2param.api import call_list_parameters from ros2param.api import get_value from ros2param.verb import VerbExtension - import yaml @@ -57,17 +52,17 @@ def add_arguments(self, parser, cli_name): # noqa: D102 help='DEPRECATED: Does nothing.') @staticmethod - def get_parameter_value(node, node_name, param): + def get_parameter_values(node, node_name, params): response = call_get_parameters( node=node, node_name=node_name, - parameter_names=[param]) + parameter_names=params) # requested parameter not set if not response.values: return '# Parameter not set' # extract type specific value - return get_value(parameter_value=response.values[0]) + return [get_value(parameter_value=i) for i in response.values] def insert_dict(self, dictionary, key, value): split = key.split(PARAMETER_SEPARATOR_STRING, 1) @@ -94,36 +89,28 @@ def main(self, *, args): # noqa: D102 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() + 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", diff --git a/ros2param/ros2param/verb/list.py b/ros2param/ros2param/verb/list.py index 1ba6ee151..8a3f4abf6 100644 --- a/ros2param/ros2param/verb/list.py +++ b/ros2param/ros2param/verb/list.py @@ -15,8 +15,6 @@ import re import sys -from rcl_interfaces.srv import ListParameters -import rclpy from ros2cli.node.direct import DirectNode from ros2cli.node.strategy import add_arguments from ros2cli.node.strategy import NodeStrategy @@ -24,9 +22,9 @@ from ros2node.api import get_node_names from ros2node.api import NodeNameCompleter from ros2param.api import call_describe_parameters +from ros2param.api import call_list_parameters from ros2param.api import get_parameter_type_string from ros2param.verb import VerbExtension -from ros2service.api import get_service_names class ListVerb(VerbExtension): @@ -69,53 +67,29 @@ def main(self, *, args): # noqa: D102 if regex_filter is not None: regex_filter = re.compile(regex_filter[0]) - with NodeStrategy(args) as node: - service_names = get_service_names( - node=node, include_hidden_services=args.include_hidden_nodes) - with DirectNode(args) as node: - clients = {} - futures = {} - # create clients for nodes which have the service + responses = {} 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) - + 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(futures.keys()): - future = futures[node_name] - if future.result() is None: - e = future.exception() + 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 = future.result() - sorted_names = sorted(response.result.names) + 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: diff --git a/ros2param/ros2param/verb/set.py b/ros2param/ros2param/verb/set.py index 347c93aa1..86c29fa06 100644 --- a/ros2param/ros2param/verb/set.py +++ b/ros2param/ros2param/verb/set.py @@ -15,14 +15,15 @@ import sys from rcl_interfaces.msg import Parameter +from rclpy.parameter import get_parameter_value from ros2cli.node.direct import DirectNode from ros2cli.node.strategy import add_arguments from ros2cli.node.strategy import NodeStrategy from ros2node.api import get_absolute_node_name from ros2node.api import get_node_names from ros2node.api import NodeNameCompleter + from ros2param.api import call_set_parameters -from ros2param.api import get_parameter_value from ros2param.api import ParameterNameCompleter from ros2param.verb import VerbExtension diff --git a/ros2param/test/test_api.py b/ros2param/test/test_api.py index 8144539ca..9c42c8a9e 100644 --- a/ros2param/test/test_api.py +++ b/ros2param/test/test_api.py @@ -90,9 +90,7 @@ ), ], ) -def test_get_parameter_value( - string_value, expected_type, value_attribute, expected_value -): +def test_get_parameter_value(string_value, expected_type, value_attribute, expected_value): value = get_parameter_value(string_value=string_value) assert value.type == expected_type assert getattr(value, value_attribute) == expected_value diff --git a/ros2param/test/test_verb_load.py b/ros2param/test/test_verb_load.py index 2f8911311..e5619399a 100644 --- a/ros2param/test/test_verb_load.py +++ b/ros2param/test/test_verb_load.py @@ -129,7 +129,7 @@ def generate_test_description(rmw_implementation): ]) -class TestVerbDump(unittest.TestCase): +class TestVerbLoad(unittest.TestCase): @classmethod def setUpClass( @@ -305,8 +305,7 @@ def test_verb_load_wildcard(self): assert param_load_command.wait_for_shutdown(timeout=TEST_TIMEOUT) assert param_load_command.exit_code != launch_testing.asserts.EXIT_OK assert launch_testing.tools.expect_output( - expected_lines=['Param file does not contain parameters for ' - f'{TEST_NAMESPACE}/{TEST_NODE}'], + expected_lines=['Param file does not contain selected parameters'], text=param_load_command.output, strict=False )