diff --git a/launch_ros/launch_ros/actions/node.py b/launch_ros/launch_ros/actions/node.py index 0b5c0180..8bbcb738 100644 --- a/launch_ros/launch_ros/actions/node.py +++ b/launch_ros/launch_ros/actions/node.py @@ -60,6 +60,7 @@ import yaml +from ..descriptions import CombinedParameterFiles from ..descriptions import Parameter from ..descriptions import ParameterFile @@ -296,7 +297,24 @@ def get_nested_dictionary_from_nested_key_value_pairs(params): normalized_params.append( get_nested_dictionary_from_nested_key_value_pairs([param])) continue - raise ValueError('param Entity should have name or from attribute') + + children = param.get_attr('file', data_type=List[Entity], optional=True) + if children: + param.assert_entity_completely_parsed() + file_paths = [] + for child in children: + file_path = child.get_attr('path') + file_paths.append(parser.parse_substitution(file_path)) + + if isinstance(allow_substs, str): + allow_substs = parser.parse_substitution(allow_substs) + else: + allow_substs = bool(allow_substs) + param.assert_entity_completely_parsed() + normalized_params.append( + CombinedParameterFiles(file_paths, allow_substs=allow_substs)) + continue + raise ValueError('param Entity should have name, from attribute, or file children') return normalized_params @classmethod diff --git a/launch_ros/launch_ros/descriptions/__init__.py b/launch_ros/launch_ros/descriptions/__init__.py index 1d693013..f82c18c5 100644 --- a/launch_ros/launch_ros/descriptions/__init__.py +++ b/launch_ros/launch_ros/descriptions/__init__.py @@ -16,6 +16,7 @@ from .composable_lifecycle_node import ComposableLifecycleNode from .composable_node import ComposableNode +from ..parameter_descriptions import CombinedParameterFiles from ..parameter_descriptions import Parameter from ..parameter_descriptions import ParameterFile from ..parameter_descriptions import ParameterValue @@ -27,4 +28,5 @@ 'Parameter', 'ParameterFile', 'ParameterValue', + 'CombinedParameterFiles', ] diff --git a/launch_ros/launch_ros/parameter_descriptions.py b/launch_ros/launch_ros/parameter_descriptions.py index 46f226d3..0401054b 100644 --- a/launch_ros/launch_ros/parameter_descriptions.py +++ b/launch_ros/launch_ros/parameter_descriptions.py @@ -164,7 +164,37 @@ def evaluate(self, context: LaunchContext) -> Tuple[Text, 'EvaluatedParameterVal return (name, value) -class ParameterFile: +class TempYamlFile: + """Common infrastructure for writing to a temporary yaml file.""" + + def __init__(self, prefix): + self.__temp_file_prefix = prefix + self.__created_tmp_file = False + self.evaluated_param_file: Optional[Path] = None + + def write_contents(self, s): + with NamedTemporaryFile(mode='w', prefix=self.__temp_file_prefix, delete=False) as f: + f.write(s) + self.evaluated_param_file = Path(f.name) + self.__created_tmp_file = True + + def write_yaml(self, o): + self.write_contents(yaml.dump(o)) + + def cleanup(self): + """Delete created temporary files.""" + if self.__created_tmp_file and self.evaluated_param_file is not None: + try: + os.unlink(self.evaluated_param_file) + except FileNotFoundError: + pass + self.evaluated_param_file = None + + def __del__(self): + self.cleanup() + + +class ParameterFile(TempYamlFile): """Describes a ROS parameter file.""" def __init__( @@ -179,12 +209,7 @@ def __init__( :param param_file: Path to a parameter file. :param allow_subst: Allow substitutions in the parameter file. """ - # In Python, __del__ is called even if the constructor throws an - # exception. It is possible for ensure_argument_type() below to - # throw an exception and try to access these member variables - # during cleanup, so make sure to initialize them here. - self.__evaluated_param_file: Optional[Path] = None - self.__created_tmp_file = False + TempYamlFile.__init__(self, 'launch_params_') ensure_argument_type( param_file, @@ -207,8 +232,8 @@ def __init__( @property def param_file(self) -> Union[FilePath, List[Substitution]]: """Getter for parameter file.""" - if self.__evaluated_param_file is not None: - return self.__evaluated_param_file + if self.evaluated_param_file is not None: + return self.evaluated_param_file return self.__param_file @property @@ -226,8 +251,8 @@ def __str__(self) -> Text: def evaluate(self, context: LaunchContext) -> Path: """Evaluate and return a parameter file path.""" - if self.__evaluated_param_file is not None: - return self.__evaluated_param_file + if self.evaluated_param_file is not None: + return self.evaluated_param_file param_file = self.__param_file if isinstance(param_file, list): @@ -237,29 +262,109 @@ def evaluate(self, context: LaunchContext) -> Path: allow_substs = perform_typed_substitution(context, self.__allow_substs, data_type=bool) param_file_path: Path = Path(param_file) if allow_substs: - with open(param_file_path, 'r') as f, NamedTemporaryFile( - mode='w', prefix='launch_params_', delete=False - ) as h: + with open(param_file_path, 'r') as f: parsed = perform_substitutions(context, parse_substitution(f.read())) try: yaml.safe_load(parsed) except Exception: raise SubstitutionFailure( 'The substituted parameter file is not a valid yaml file') - h.write(parsed) - param_file_path = Path(h.name) - self.__created_tmp_file = True - self.__evaluated_param_file = param_file_path + self.write_contents(parsed) + + param_file_path = self.evaluated_param_file + self.evaluated_param_file = param_file_path return param_file_path - def cleanup(self): - """Delete created temporary files.""" - if self.__created_tmp_file and self.__evaluated_param_file is not None: - try: - os.unlink(self.__evaluated_param_file) - except FileNotFoundError: - pass - self.__evaluated_param_file = None - def __del__(self): - self.cleanup() +def recursive_update(d, u): + for k, v in u.items(): + if isinstance(v, dict): + d[k] = recursive_update(d.get(k, {}), v) + else: + d[k] = v + return d + + +class CombinedParameterFiles(TempYamlFile): + """Describes a ROS parameter file made from multiple source files.""" + + def __init__( + self, + param_files: List[Union[FilePath, SomeSubstitutionsType]], + *, + allow_substs: [bool, SomeSubstitutionsType] = False + ) -> None: + """ + Construct a combined parameter file description. + + :param param_files: List of Paths to parameter files. + :param allow_subst: Allow substitutions in the parameter file. + """ + TempYamlFile.__init__(self, 'combined_launch_params_') + + # TODO: Fix typing + ensure_argument_type( + param_files, + SomeSubstitutionsType_types_tuple + (os.PathLike, bytes), + 'param_file', + 'ParameterFile()' + ) + ensure_argument_type( + allow_substs, + bool, + 'allow_subst', + 'ParameterFile()' + ) + self.__param_files: List[Union[List[Substitution], FilePath]] = [] + for param_file in param_files: + if isinstance(param_file, SomeSubstitutionsType_types_tuple): + param_file = normalize_to_list_of_substitutions(param_file) + self.__param_files.append(param_file) + + self.__allow_substs = normalize_typed_substitution(allow_substs, data_type=bool) + self.__evaluated_allow_substs: Optional[bool] = None + + @property + def param_files(self) -> List[Union[FilePath, List[Substitution]]]: + """Getter for parameter file.""" + if self.evaluated_param_file is not None: + return self.evaluated_param_file + return self.__param_files + + @property + def allow_substs(self) -> Union[bool, List[Substitution]]: + """Getter for allow substitutions argument.""" + if self.__evaluated_allow_substs is not None: + return self.__evaluated_allow_substs + return self.__allow_substs + + def __str__(self) -> Text: + return ( + 'launch_ros.description.CombinedParameterFiles' + f'(param_files={self.param_files}, allow_substs={self.allow_substs})' + ) + + def evaluate(self, context: LaunchContext) -> Path: + """Evaluate and return a parameter file path.""" + if self.evaluated_param_file is not None: + return self.evaluated_param_file + + allow_substs = perform_typed_substitution(context, self.__allow_substs, data_type=bool) + + d = {} + for param_file in self.__param_files: + if isinstance(param_file, list): + # list of substitutions + param_file = perform_substitutions(context, param_file) + + new_d = None + if allow_substs: + with open(param_file, 'r') as f: + subbed = perform_substitutions(context, parse_substitution(f.read())) + new_d = yaml.safe_load(subbed) + else: + new_d = yaml.safe_load(open(param_file)) + recursive_update(d, new_d) + + self.write_yaml(d) + return self.evaluated_param_file diff --git a/launch_ros/launch_ros/parameters_type.py b/launch_ros/launch_ros/parameters_type.py index f744d88d..e1f3478a 100644 --- a/launch_ros/launch_ros/parameters_type.py +++ b/launch_ros/launch_ros/parameters_type.py @@ -26,6 +26,7 @@ from launch.some_substitutions_type import SomeSubstitutionsType_types_tuple from launch.substitution import Substitution +from .parameter_descriptions import CombinedParameterFiles from .parameter_descriptions import Parameter as ParameterDescription from .parameter_descriptions import ParameterFile from .parameter_descriptions import ParameterValue as ParameterValueDescription @@ -37,7 +38,12 @@ _MultiValueType = Union[ Sequence[str], Sequence[int], Sequence[float], Sequence[bool], bytes] -SomeParameterFile = Union[SomeSubstitutionsType, pathlib.Path, ParameterFile] +SomeParameterFile = Union[ + SomeSubstitutionsType, + pathlib.Path, + ParameterFile, + CombinedParameterFiles +] SomeParameterName = Sequence[Union[Substitution, str]] SomeParameterValue = Union[ ParameterValueDescription, @@ -73,7 +79,9 @@ ParametersDict = Dict[ParameterName, ParameterValue] # Normalized parameters -Parameters = Sequence[Union[ParameterFile, ParametersDict, ParameterDescription]] +Parameters = Sequence[ + Union[ParameterFile, CombinedParameterFiles, ParametersDict, ParameterDescription] +] EvaluatedParameterValue = Union[_SingleValueType, _MultiValueType] # Evaluated parameters: filenames or dictionary after substitutions have been evaluated diff --git a/launch_ros/launch_ros/utilities/evaluate_parameters.py b/launch_ros/launch_ros/utilities/evaluate_parameters.py index ade9594a..e90dbfc7 100644 --- a/launch_ros/launch_ros/utilities/evaluate_parameters.py +++ b/launch_ros/launch_ros/utilities/evaluate_parameters.py @@ -31,6 +31,7 @@ import yaml +from ..parameter_descriptions import CombinedParameterFiles from ..parameter_descriptions import Parameter as ParameterDescription from ..parameter_descriptions import ParameterFile from ..parameter_descriptions import ParameterValue as ParameterValueDescription @@ -159,7 +160,7 @@ def evaluate_parameters(context: LaunchContext, parameters: Parameters) -> Evalu """ output_params: List[Union[pathlib.Path, Dict[str, EvaluatedParameterValue]]] = [] for param in parameters: - if isinstance(param, ParameterFile): + if isinstance(param, (ParameterFile, CombinedParameterFiles)): # Evaluate a list of Substitution to a file path output_params.append(param.evaluate(context)) elif isinstance(param, ParameterDescription): diff --git a/launch_ros/launch_ros/utilities/normalize_parameters.py b/launch_ros/launch_ros/utilities/normalize_parameters.py index f61957b9..a6948c0a 100644 --- a/launch_ros/launch_ros/utilities/normalize_parameters.py +++ b/launch_ros/launch_ros/utilities/normalize_parameters.py @@ -32,6 +32,7 @@ import yaml +from ..parameter_descriptions import CombinedParameterFiles from ..parameter_descriptions import Parameter as ParameterDescription from ..parameter_descriptions import ParameterFile from ..parameter_descriptions import ParameterValue as ParameterValueDescription @@ -178,13 +179,13 @@ def normalize_parameters(parameters: SomeParameters) -> Parameters: if isinstance(parameters, str) or not isinstance(parameters, Sequence): raise TypeError('Expecting list of parameters, got {}'.format(parameters)) - normalized_params: List[Union[ParameterFile, ParametersDict, ParameterDescription]] = [] + normalized_params: List[Union[ + ParameterFile, CombinedParameterFiles, ParametersDict, ParameterDescription + ]] = [] for param in parameters: if isinstance(param, Mapping): normalized_params.append(normalize_parameter_dict(param)) - elif isinstance(param, ParameterDescription): - normalized_params.append(param) - elif isinstance(param, ParameterFile): + elif isinstance(param, (ParameterDescription, ParameterFile, CombinedParameterFiles)): normalized_params.append(param) else: # It's a path diff --git a/test_launch_ros/test/test_launch_ros/descriptions/test_combined_param_files.py b/test_launch_ros/test/test_launch_ros/descriptions/test_combined_param_files.py new file mode 100644 index 00000000..81fc98ae --- /dev/null +++ b/test_launch_ros/test/test_launch_ros/descriptions/test_combined_param_files.py @@ -0,0 +1,66 @@ +# Copyright 2026 Metro Robots +# +# 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. + +"""Tests for launch_ros.descriptions.CombinedParameterFiles.""" + +from launch_ros.descriptions import CombinedParameterFiles +from test_parameter_file import get_parameter_file, MockContext +import yaml + + +def get_params(lc, desc): + evaluated_param_file = desc.evaluate(lc) + evaluated_values = yaml.safe_load(open(evaluated_param_file)) + return evaluated_values.get('/my_ns/my_node', {}).get('ros__parameters') + + +def test_combined_description(): + param_file_dict1 = { + '/my_ns/my_node': { + 'ros__parameters': { + 'indie_param1': 1, + 'repeated_param': 1, + } + } + } + param_file_dict2 = { + '/my_ns/my_node': { + 'ros__parameters': { + 'indie_param2': 2, + 'repeated_param': 2, + } + } + } + + lc = MockContext() + with get_parameter_file(yaml.dump(param_file_dict1)) as file_name1, \ + get_parameter_file(yaml.dump(param_file_dict2)) as file_name2: + + # Load parameters with dict1 first + desc_a = CombinedParameterFiles([file_name1, file_name2]) + params_a = get_params(lc, desc_a) + assert isinstance(params_a, dict) + assert params_a.get('indie_param1') == 1 + assert params_a.get('indie_param2') == 2 + assert params_a.get('indie_param3') is None + assert params_a.get('repeated_param') == 2 + + # Load parameters with dict2 first + desc_b = CombinedParameterFiles([file_name2, file_name1]) + params_b = get_params(lc, desc_b) + assert isinstance(params_b, dict) + assert params_b.get('indie_param1') == 1 + assert params_b.get('indie_param2') == 2 + assert params_b.get('indie_param3') is None + assert params_b.get('repeated_param') == 1 diff --git a/test_launch_ros/test/test_launch_ros/frontend/params2.yaml b/test_launch_ros/test/test_launch_ros/frontend/params2.yaml new file mode 100644 index 00000000..44257219 --- /dev/null +++ b/test_launch_ros/test/test_launch_ros/frontend/params2.yaml @@ -0,0 +1,5 @@ +my_ns: + my_node: + ros__parameters: + param_from_file_1: 5 + param_from_file_3: 3.14 diff --git a/test_launch_ros/test/test_launch_ros/frontend/test_node_frontend.py b/test_launch_ros/test/test_launch_ros/frontend/test_node_frontend.py index 0bebb6d5..75d448cf 100644 --- a/test_launch_ros/test/test_launch_ros/frontend/test_node_frontend.py +++ b/test_launch_ros/test/test_launch_ros/frontend/test_node_frontend.py @@ -23,6 +23,8 @@ from launch_ros.utilities import evaluate_parameters +import yaml + yaml_params = str(pathlib.Path(__file__).parent / 'params.yaml') # Escape backslashes if any to keep them after parsing takes place @@ -199,6 +201,12 @@ def check_launch_node(file): assert param_dict['param_group1.param14'] == ["'2'", "'5'", "'8'"] assert param_dict['param_group1.param15'] == ['2', '5', '8'] + param_path = evaluated_parameters[2] + file_params = yaml.safe_load(open(param_path)) + ros_params = file_params.get('my_ns', {}).get('my_node', {}).get('ros__parameters', {}) + assert ros_params['param_from_file_1'] == 1 + assert ros_params['param_from_file_2'] == 'asd' + # Check remappings exist remappings = ld.describe_sub_entities()[3]._Node__remappings assert remappings is not None diff --git a/test_launch_ros/test/test_launch_ros/frontend/test_node_with_combo_file_params.py b/test_launch_ros/test/test_launch_ros/frontend/test_node_with_combo_file_params.py new file mode 100644 index 00000000..e90d0360 --- /dev/null +++ b/test_launch_ros/test/test_launch_ros/frontend/test_node_with_combo_file_params.py @@ -0,0 +1,88 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# Copyright 2020 Open Avatar Inc. +# Copyright 2026 Metro Robots +# +# 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 io +import pathlib +import textwrap + +from launch import LaunchService +from launch.frontend import Parser + +from launch_ros.utilities import evaluate_parameters + +import yaml + +parent_folder = pathlib.Path(__file__).parent +# Escape backslashes if any to keep them after parsing takes place +yaml_params1 = str(parent_folder / 'params.yaml').replace('\\', '\\\\') +yaml_params2 = str(parent_folder / 'params2.yaml').replace('\\', '\\\\') + + +def test_launch_file_params_xml(): + xml_file = textwrap.dedent( + r""" + + + + + + + + + """.format(yaml_params1, yaml_params2)) # noqa: E501 + + with io.StringIO(xml_file) as f: + check_launch_node(f) + + +def test_launch_file_params_yaml(): + yaml_file = textwrap.dedent( + r""" + launch: + - node: + pkg: demo_nodes_py + exec: talker_qos + name: my_talker + args: '--number_of_cycles 1' + param: + - file: + - path: {} + - path: {} + """.format(yaml_params1, yaml_params2)) # noqa: E501 + + with io.StringIO(yaml_file) as f: + check_launch_node(f) + + +def check_launch_node(file): + root_entity, parser = Parser.load(file) + ld = parser.parse_description(root_entity) + ls = LaunchService() + ls.include_launch_description(ld) + assert 0 == ls.run() + evaluated_parameters = evaluate_parameters( + ls.context, + ld.describe_sub_entities()[0]._Node__parameters + ) + assert len(evaluated_parameters) == 1 + assert isinstance(evaluated_parameters[0], pathlib.Path) + + param_path = evaluated_parameters[0] + file_params = yaml.safe_load(open(param_path)) + ros_params = file_params.get('my_ns', {}).get('my_node', {}).get('ros__parameters', {}) + assert ros_params['param_from_file_1'] == 5 # Loaded from params1 overloaded by params2 + assert ros_params['param_from_file_2'] == 'asd' # Loaded from params1 + assert ros_params['param_from_file_3'] == 3.14 # Loaded from params2