From aeac8493a894d32d50f2221759d68f844b0973e7 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Tue, 31 Oct 2023 22:51:48 -0700 Subject: [PATCH] fix loading parameter behavior from yaml file. Ref: https://github.com/ros2/ros2cli/issues/863 Signed-off-by: Tomoya Fujita --- rclpy/rclpy/parameter.py | 68 ++++++++++++++++++--------- rclpy/rclpy/parameter_client.py | 7 ++- rclpy/test/test_parameter.py | 72 +++++++++++++++++++++++++---- rclpy/test/test_parameter_client.py | 4 +- 4 files changed, 116 insertions(+), 35 deletions(-) diff --git a/rclpy/rclpy/parameter.py b/rclpy/rclpy/parameter.py index 767290343..d1e9b91a2 100644 --- a/rclpy/rclpy/parameter.py +++ b/rclpy/rclpy/parameter.py @@ -284,36 +284,60 @@ def parameter_dict_from_yaml_file( """ with open(parameter_file, 'r') as f: param_file = yaml.safe_load(f) - param_keys = [] param_dict = {} - if use_wildcard and '/**' in param_file: - param_keys.append('/**') + # check and add if wildcard is available in 1st keys + # wildcard key must go to the front of param_keys so that + # node-namespaced parameters will override the wildcard parameters + if use_wildcard and '/**' in param_file.keys(): + value = param_file['/**'] + if type(value) != dict and 'ros__parameters' not in value: + raise RuntimeError( + 'YAML file is not a valid ROS parameter file for wildcard(/**)') + param_dict.update(value['ros__parameters']) + # parse parameter yaml file based on target node namespace and name if target_nodes: for n in target_nodes: - if n not in param_file.keys(): - raise RuntimeError(f'Param file does not contain parameters for {n},' - f'only for nodes: {list(param_file.keys())} ') - param_keys.append(n) - else: - # wildcard key must go to the front of param_keys so that - # node-namespaced parameters will override the wildcard parameters - keys = set(param_file.keys()) - keys.discard('/**') - param_keys.extend(keys) - - if len(param_keys) == 0: - raise RuntimeError('Param file does not contain selected parameters') - - for n in param_keys: - value = param_file[n] - if type(value) != dict or 'ros__parameters' not in value: - raise RuntimeError(f'YAML file is not a valid ROS parameter file for node {n}') - param_dict.update(value['ros__parameters']) + abs_name = _get_absolute_node_name(n) + ns, node_basename = abs_name.rsplit('/', 1) + if abs_name in param_file.keys(): + # found absolute node name w or w/o namespace + value = param_file[abs_name] + if type(value) != dict and 'ros__parameters' not in value: + raise RuntimeError( + f'YAML file is not a valid ROS parameter file for node {abs_name}') + param_dict.update(value['ros__parameters']) + if ns == '' and node_basename in param_file.keys(): + # found non-absolute node name without namespace + value = param_file[node_basename] + if type(value) != dict and 'ros__parameters' not in value: + raise RuntimeError('YAML file is not a valid ROS parameter ' + f'file for node {node_basename}') + param_dict.update(value['ros__parameters']) + elif ns in param_file.keys(): + # found namespace + if node_basename in param_file[ns].keys(): + value = param_file[ns][node_basename] + if type(value) != dict and 'ros__parameters' not in value: + raise RuntimeError('YAML file is not a valid ROS parameter ' + f'file for namespace {ns} node {node_basename}') + param_dict.update(value['ros__parameters']) + + if not param_dict: + raise RuntimeError('Param file does not contain any valid parameters') + return _unpack_parameter_dict(namespace, param_dict) +def _get_absolute_node_name(node_name): + if not node_name: + return None + if node_name[0] != '/': + node_name = '/' + node_name + return node_name + + def _unpack_parameter_dict(namespace, parameter_dict): """ Flatten a parameter dictionary recursively. diff --git a/rclpy/rclpy/parameter_client.py b/rclpy/rclpy/parameter_client.py index e549a8698..6414c105a 100644 --- a/rclpy/rclpy/parameter_client.py +++ b/rclpy/rclpy/parameter_client.py @@ -309,7 +309,9 @@ def load_parameter_file( :param use_wildcard: Whether to use wildcard expansion. :return: Future with the result from the set_parameters call. """ - param_dict = parameter_dict_from_yaml_file(parameter_file, use_wildcard) + print(self.remote_node_name) + param_dict = parameter_dict_from_yaml_file( + parameter_file, use_wildcard, target_nodes=[self.remote_node_name]) future = self.set_parameters(list(param_dict.values()), callback=callback) return future @@ -331,7 +333,8 @@ def load_parameter_file_atomically( :param use_wildcard: Whether to use wildcard expansion. :return: Future with the result from the set_parameters_atomically call. """ - param_dict = parameter_dict_from_yaml_file(parameter_file, use_wildcard) + param_dict = parameter_dict_from_yaml_file( + parameter_file, use_wildcard, target_nodes=[self.remote_node_name]) future = self.set_parameters_atomically(list(param_dict.values()), callback=callback) return future diff --git a/rclpy/test/test_parameter.py b/rclpy/test/test_parameter.py index bc474333b..2c6a5928c 100644 --- a/rclpy/test/test_parameter.py +++ b/rclpy/test/test_parameter.py @@ -259,14 +259,59 @@ def test_parameter_value_to_python(self): parameter_value_to_python(parameter_value) def test_parameter_dict_from_yaml_file(self): - yaml_string = """/param_test_target: - ros__parameters: - param_1: 1 - param_str: string + yaml_string = """ + /param_test_target: + ros__parameters: + abs-nodename: true + param_test_target: + ros__parameters: + base-nodename: true + /foo/param_test_target: + ros__parameters: + abs-ns-nodename: true + /foo: + param_test_target: + ros__parameters: + abs-ns-base-nodename: true + /bar/param_test_target: + ros__parameters: + abs-ns-nodename: false + /bar: + param_test_target: + ros__parameters: + abs-ns-base-nodename: false + /**: + ros__parameters: + wildcard: true """ - expected = { - 'param_1': Parameter('param_1', Parameter.Type.INTEGER, 1).to_parameter_msg(), - 'param_str': Parameter('param_str', Parameter.Type.STRING, 'string').to_parameter_msg() + + # target nodes is specified, so it should only parse wildcard + expected_no_target_node = { + 'wildcard': Parameter('wildcard', Parameter.Type.BOOL, True).to_parameter_msg(), + } + # target nodes is specified with wildcard enabled + expected_target_node_wildcard = { + 'wildcard': Parameter( + 'wildcard', Parameter.Type.BOOL, True).to_parameter_msg(), + 'abs-nodename': Parameter( + 'abs-nodename', Parameter.Type.BOOL, True).to_parameter_msg(), + 'base-nodename': Parameter( + 'base-nodename', Parameter.Type.BOOL, True).to_parameter_msg(), + } + # target nodes is specified with wildcard disabled + expected_target_node = { + 'abs-nodename': Parameter( + 'abs-nodename', Parameter.Type.BOOL, True).to_parameter_msg(), + 'base-nodename': Parameter( + 'base-nodename', Parameter.Type.BOOL, True).to_parameter_msg(), + } + # target nodes is specified with wildcard and namespace + expected_target_node_ns = { + 'wildcard': Parameter('wildcard', Parameter.Type.BOOL, True).to_parameter_msg(), + 'abs-ns-nodename': Parameter( + 'abs-ns-nodename', Parameter.Type.BOOL, True).to_parameter_msg(), + 'abs-ns-base-nodename': Parameter( + 'abs-ns-base-nodename', Parameter.Type.BOOL, True).to_parameter_msg(), } try: @@ -274,8 +319,17 @@ def test_parameter_dict_from_yaml_file(self): f.write(yaml_string) f.flush() f.close() - parameter_dict = parameter_dict_from_yaml_file(f.name) - assert parameter_dict == expected + parameter_dict = parameter_dict_from_yaml_file(f.name, True) + assert parameter_dict == expected_no_target_node + parameter_dict = parameter_dict_from_yaml_file( + f.name, True, target_nodes=['param_test_target']) + assert parameter_dict == expected_target_node_wildcard + parameter_dict = parameter_dict_from_yaml_file( + f.name, False, target_nodes=['/param_test_target']) + assert parameter_dict == expected_target_node + parameter_dict = parameter_dict_from_yaml_file( + f.name, True, target_nodes=['/foo/param_test_target']) + assert parameter_dict == expected_target_node_ns finally: if os.path.exists(f.name): os.unlink(f.name) diff --git a/rclpy/test/test_parameter_client.py b/rclpy/test/test_parameter_client.py index 6797a0f4a..d33f48e79 100644 --- a/rclpy/test/test_parameter_client.py +++ b/rclpy/test/test_parameter_client.py @@ -139,7 +139,7 @@ def test_delete_parameters(self): assert result.results[0].successful def test_load_parameter_file(self): - yaml_string = """/param_test_target: + yaml_string = """/test_parameter_client_target: ros__parameters: param_1: 1 param_str: "string" @@ -160,7 +160,7 @@ def test_load_parameter_file(self): os.unlink(f.name) def test_load_parameter_file_atomically(self): - yaml_string = """/param_test_target: + yaml_string = """/test_parameter_client_target: ros__parameters: param_1: 1 param_str: "string"