diff --git a/README.md b/README.md index 29601f0d..79de4741 100644 --- a/README.md +++ b/README.md @@ -384,6 +384,28 @@ params.gain.joints_map.at("joint1").interfaces_map.at("position").value params.gain.get_entry("joint1").get_entry("position").value ``` +#### Key array scope resolution + +The `key` used by a `__map_` segment does not need to be defined at the root namespace level. It can also be a **sibling** within the same struct, or defined anywhere in a parent scope. +This allows you to co-locate the key array alongside the map it controls: + +```yaml +cpp_name_space: + # key array defined as a sibling of the map that uses it + nested_map: + entries: + type: string_array + default_value: ["entry1", "entry2"] + description: "Keys for the nested map" + __map_entries: # resolved to nested_map.entries (sibling scope) + value: + type: double + default_value: 1.0 + description: "A value keyed by entries" +``` + +> **Note:** Scope resolution searches the current struct first, then walks up to parent scopes. If the key array is not found in any scope, the bare name is used as a fallback. + ### Use generated struct in Cpp The generated header file is named based on the target library name you passed as the first argument to the cmake function. If you specified it to be `turtlesim_parameters` you can then include the generated code with `#include `. diff --git a/example/config/implementation.yaml b/example/config/implementation.yaml index 4cd1ba91..a582c32a 100644 --- a/example/config/implementation.yaml +++ b/example/config/implementation.yaml @@ -115,3 +115,9 @@ admittance_controller: # general settings enable_parameter_update_without_reactivation: true use_feedforward_commanded_input: true + + nested_map: + entry1: + value: 3.14 + entry2: + value: 2.71 diff --git a/example/src/minimal_publisher.cpp b/example/src/minimal_publisher.cpp index be9f571c..4d0bb042 100644 --- a/example/src/minimal_publisher.cpp +++ b/example/src/minimal_publisher.cpp @@ -56,6 +56,11 @@ MinimalPublisher::MinimalPublisher(const rclcpp::NodeOptions& options) for (auto d : fixed_array) { RCLCPP_INFO(get_logger(), "value: '%s'", std::to_string(d).c_str()); } + + RCLCPP_INFO(get_logger(), "self.params.nested_map.entry1.value = '%f'", + params_.nested_map.entries_map["entry1"].value); + RCLCPP_INFO(get_logger(), "self.params.nested_map.entry2.value = '%f'", + params_.nested_map.entries_map["entry2"].value); } void MinimalPublisher::timer_callback() { diff --git a/example/src/parameters.yaml b/example/src/parameters.yaml index 7836c8e6..5f46ad89 100644 --- a/example/src/parameters.yaml +++ b/example/src/parameters.yaml @@ -76,6 +76,17 @@ admittance_controller: default_value: 2.0 description: "general gain" + nested_map: + entries: + type: string_array + default_value: ["entry1", "entry2"] + description: "Keys for nested mapped parameters" + __map_entries: + value: + type: double + default_value: 1.0 + description: "A value in the nested map with sibling keys" + fixed_string: type: string_fixed_25 default_value: "string_value" diff --git a/example_python/config/implementation.yaml b/example_python/config/implementation.yaml index 4cd1ba91..a582c32a 100644 --- a/example_python/config/implementation.yaml +++ b/example_python/config/implementation.yaml @@ -115,3 +115,9 @@ admittance_controller: # general settings enable_parameter_update_without_reactivation: true use_feedforward_commanded_input: true + + nested_map: + entry1: + value: 3.14 + entry2: + value: 2.71 diff --git a/example_python/generate_parameter_module_example/minimal_publisher.py b/example_python/generate_parameter_module_example/minimal_publisher.py index ef2ffade..668502f4 100644 --- a/example_python/generate_parameter_module_example/minimal_publisher.py +++ b/example_python/generate_parameter_module_example/minimal_publisher.py @@ -53,6 +53,15 @@ def __init__(self): for d in self.params.fixed_array: self.get_logger().info("value: '%s'" % str(d)) + self.get_logger().info( + "self.params.nested_map.entry1.value = '%s'" + % self.params.nested_map.get_entry('entry1').value + ) + self.get_logger().info( + "self.params.nested_map.entry2.value = '%s'" + % self.params.nested_map.get_entry('entry2').value + ) + def timer_callback(self): if self.param_listener.is_old(self.params): self.param_listener.refresh_dynamic_parameters() diff --git a/example_python/generate_parameter_module_example/parameters.yaml b/example_python/generate_parameter_module_example/parameters.yaml index d6d8f1b6..acd64638 100644 --- a/example_python/generate_parameter_module_example/parameters.yaml +++ b/example_python/generate_parameter_module_example/parameters.yaml @@ -80,6 +80,17 @@ admittance_controller: default_value: 2.0 description: "general gain" + nested_map: + entries: + type: string_array + default_value: ["entry1", "entry2"] + description: "Keys for nested mapped parameters" + __map_entries: + value: + type: double + default_value: 1.0 + description: "A value in the nested map with sibling keys" + fixed_string: type: string_fixed_25 default_value: "string_value" diff --git a/generate_parameter_library_py/generate_parameter_library_py/parse_yaml.py b/generate_parameter_library_py/generate_parameter_library_py/parse_yaml.py index 59b9dfe2..ed00d297 100644 --- a/generate_parameter_library_py/generate_parameter_library_py/parse_yaml.py +++ b/generate_parameter_library_py/generate_parameter_library_py/parse_yaml.py @@ -105,12 +105,37 @@ def get_dynamic_parameter_field(yaml_parameter_name: str): return '.'.join(field) -def get_dynamic_mapped_parameter(yaml_parameter_name: str): - tmp = yaml_parameter_name.split('.') - mapped_params = [ - val.replace('__map_', '') for val in tmp[:-1] if is_mapped_parameter(val) +def get_dynamic_mapped_parameter( + yaml_parameter_name: str, declared_names: set = None +) -> list: + """Get the resolved paths to the key arrays for each __map_ segment.""" + keys = get_dynamic_mapped_parameter_keys(yaml_parameter_name) + if not declared_names: + return keys + + struct_name = get_dynamic_struct_name(yaml_parameter_name) + return [_resolve_key_path(k, struct_name, declared_names) for k in keys] + + +def _resolve_key_path(key_name: str, struct_name: str, declared_names: set) -> str: + """Resolve the full path to a mapped parameter key array by searching scopes.""" + prefix = struct_name + while prefix: + candidate = f'{prefix}.{key_name}' + if candidate in declared_names: + return candidate + # Move up one level (e.g., "a.b.c" -> "a.b") + prefix, _, _ = prefix.rpartition('.') + # Fallback to the bare key name if not found in any scope + return key_name + + +def get_dynamic_mapped_parameter_keys(yaml_parameter_name: str) -> list: + """Get the base key names (without __map_ prefix) for all mapped segments.""" + segments = yaml_parameter_name.split('.') + return [ + seg.replace('__map_', '') for seg in segments[:-1] if is_mapped_parameter(seg) ] - return mapped_params def get_dynamic_struct_name(yaml_parameter_name: str): @@ -129,7 +154,7 @@ def get_dynamic_parameter_name(yaml_parameter_name: str): def get_dynamic_parameter_map(yaml_parameter_name: str): - mapped_params = get_dynamic_mapped_parameter(yaml_parameter_name) + mapped_params = get_dynamic_mapped_parameter_keys(yaml_parameter_name) parameter_map = [val + '_map' for val in mapped_params] parameter_map = '.'.join(parameter_map) return parameter_map @@ -411,9 +436,18 @@ def __str__(self): class UpdateRuntimeParameter(UpdateParameterBase): + def __init__( + self, + parameter_name: str, + code_gen_variable: CodeGenVariableBase, + mapped_params: list = None, + ): + super().__init__(parameter_name, code_gen_variable) + self.mapped_params = mapped_params or [] + def __str__(self): parameter_validations_str = ''.join(str(x) for x in self.parameter_validations) - mapped_params = get_dynamic_mapped_parameter(self.parameter_name) + mapped_params = self.mapped_params parameter_map = get_dynamic_parameter_map(self.parameter_name) parameter_map = parameter_map.split('.') struct_name = get_dynamic_struct_name(self.parameter_name) @@ -542,6 +576,7 @@ def __init__( parameter_read_only: bool, parameter_validations: list, parameter_additional_constraints: str, + mapped_params: list = None, ): super().__init__( code_gen_variable, @@ -550,6 +585,7 @@ def __init__( parameter_validations, parameter_additional_constraints, ) + self.mapped_params = mapped_params or [] self.set_runtime_parameter = None self.param_struct_instance = 'updated_params' @@ -570,7 +606,7 @@ def __str__(self): bool_to_str = self.code_gen_variable.conversion.bool_to_str parameter_field = get_dynamic_parameter_field(self.parameter_name) - mapped_params = get_dynamic_mapped_parameter(self.parameter_name) + mapped_params = self.mapped_params parameter_map = get_dynamic_parameter_map(self.parameter_name) struct_name = get_dynamic_struct_name(self.parameter_name) parameter_map = parameter_map.split('.') @@ -619,9 +655,7 @@ def __str__(self): parameter_field = get_dynamic_parameter_field( self.dynamic_declare_parameter.parameter_name ) - mapped_params = get_dynamic_mapped_parameter( - self.dynamic_declare_parameter.parameter_name - ) + mapped_params = self.dynamic_declare_parameter.mapped_params data = { 'parameter_map': parameter_map, @@ -744,6 +778,7 @@ def __init__(self, language: str): 'Invalid language, only cpp, markdown, rst, and python are currently supported.' ) GenerateCode.templates = get_all_templates(language) + self.declared_param_names = set() self.language = language self.namespace = '' self.struct_tree = DeclareStruct('Params', []) @@ -808,6 +843,15 @@ def parse_params(self, name, value, nested_name_list): # check if runtime parameter is_runtime_parameter = is_mapped_parameter(param_name) + # Resolve mapped paths for dynamic params; track paths for standard params + mapped_params = [] + if is_runtime_parameter: + mapped_params = get_dynamic_mapped_parameter( + param_name, self.declared_param_names + ) + else: + self.declared_param_names.add(param_name) + if is_runtime_parameter: declare_parameter_set = SetRuntimeParameter(param_name, code_gen_variable) declare_parameter = DeclareRuntimeParameter( @@ -816,9 +860,12 @@ def parse_params(self, name, value, nested_name_list): read_only, validations, additional_constraints, + mapped_params, ) declare_parameter.add_set_runtime_parameter(declare_parameter_set) - update_parameter = UpdateRuntimeParameter(param_name, code_gen_variable) + update_parameter = UpdateRuntimeParameter( + param_name, code_gen_variable, mapped_params + ) else: declare_parameter = DeclareParameter( code_gen_variable, diff --git a/generate_parameter_library_py/generate_parameter_library_py/test/YAML_parse_error_test.py b/generate_parameter_library_py/generate_parameter_library_py/test/YAML_parse_error_test.py index 956a41dd..eb524b75 100644 --- a/generate_parameter_library_py/generate_parameter_library_py/test/YAML_parse_error_test.py +++ b/generate_parameter_library_py/generate_parameter_library_py/test/YAML_parse_error_test.py @@ -84,17 +84,17 @@ def test_expected(test_input, expected): print(e.value) -def test_parse_valid_parameter_file(): - try: - yaml_test_file = 'valid_parameters.yaml' - set_up(yaml_test_file) - except Exception as e: - assert False, f'failed to parse valid file, reason:{e}' - - -def test_parse_valid_parameter_file_including_none_type(): +@pytest.mark.parametrize( + 'yaml_test_file', + [ + ('valid_parameters.yaml'), + ('valid_parameters_with_none_type.yaml'), + ('nested_map_test.yaml'), + ('nested_map_keys.yaml'), + ], +) +def test_parse_valid_parameter_files(yaml_test_file): try: - yaml_test_file = 'valid_parameters_with_none_type.yaml' set_up(yaml_test_file) except Exception as e: assert False, f'failed to parse valid file, reason:{e}' diff --git a/generate_parameter_library_py/generate_parameter_library_py/test/mapped_parameter_test.py b/generate_parameter_library_py/generate_parameter_library_py/test/mapped_parameter_test.py new file mode 100644 index 00000000..703845cb --- /dev/null +++ b/generate_parameter_library_py/generate_parameter_library_py/test/mapped_parameter_test.py @@ -0,0 +1,52 @@ +# Copyright 2026 Visionick +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import pytest + +from generate_parameter_library_py.parse_yaml import get_dynamic_mapped_parameter + + +@pytest.mark.parametrize( + 'param_name,declared_params,expected', + [ + # entries is inside nested -> resolve to nested.entries + ('nested.__map_entries.value', {'nested.entries'}, ['nested.entries']), + # items is inside level1.level2 -> resolve to level1.level2.items + ( + 'level1.level2.__map_items.param', + {'level1.level2.items'}, + ['level1.level2.items'], + ), + # keys is at root -> resolve to bare name + ('__map_keys.value', {'keys'}, ['keys']), + # multi-level maps with keys at root -> all bare names + ( + '__map_level1.__map_level2.__map_level3.value', + {'level1', 'level2', 'level3'}, + ['level1', 'level2', 'level3'], + ), + # keys at root but __map_ inside a struct (like pid.__map_joints) + ('pid.__map_joints.p', {'joints'}, ['joints']), + # nested struct with keys at root (like nested_dynamic.__map_joints) + ( + 'nested_dynamic.__map_joints.__map_dof_names.nested', + {'joints', 'dof_names'}, + ['joints', 'dof_names'], + ), + ], +) +def test_get_dynamic_mapped_parameter_nested(param_name, declared_params, expected): + """Test that get_dynamic_mapped_parameter returns correct paths for nested maps.""" + result = get_dynamic_mapped_parameter(param_name, declared_params) + assert result == expected diff --git a/generate_parameter_library_py/generate_parameter_library_py/test/nested_map_keys.yaml b/generate_parameter_library_py/generate_parameter_library_py/test/nested_map_keys.yaml new file mode 100644 index 00000000..eb710063 --- /dev/null +++ b/generate_parameter_library_py/generate_parameter_library_py/test/nested_map_keys.yaml @@ -0,0 +1,12 @@ +nested_map_test: + nested: + entries: + type: string_array + default_value: ["entry1", "entry2"] + description: "Keys for the nested mapped parameters" + + __map_entries: + value: + type: double + default_value: 1.0 + description: "A value in the nested map" diff --git a/generate_parameter_library_py/generate_parameter_library_py/test/nested_map_test.yaml b/generate_parameter_library_py/generate_parameter_library_py/test/nested_map_test.yaml new file mode 100644 index 00000000..b1da3050 --- /dev/null +++ b/generate_parameter_library_py/generate_parameter_library_py/test/nested_map_test.yaml @@ -0,0 +1,8 @@ +# Test nested mapped parameters for get_dynamic_mapped_parameter +nested_map_test: + __map_outer: + __map_inner: + value: + type: double + default_value: 1.23 + description: "A deeply nested mapped parameter." diff --git a/generate_parameter_library_py/setup.py b/generate_parameter_library_py/setup.py index 1d7628b3..2f2c6cb9 100644 --- a/generate_parameter_library_py/setup.py +++ b/generate_parameter_library_py/setup.py @@ -63,6 +63,14 @@ 'share/' + package_name + '/test', ['generate_parameter_library_py/test/valid_parameters_with_none_type.yaml'], ), + ( + 'share/' + package_name + '/test', + ['generate_parameter_library_py/test/nested_map_test.yaml'], + ), + ( + 'share/' + package_name + '/test', + ['generate_parameter_library_py/test/nested_map_keys.yaml'], + ), ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ], install_requires=['setuptools', 'typeguard', 'jinja2', 'pyyaml'],