diff --git a/.gitignore b/.gitignore index 203501ea402..bb346f49ba9 100644 --- a/.gitignore +++ b/.gitignore @@ -45,6 +45,7 @@ install __pycache__/ *.py[cod] .ipynb_checkpoints +.pytest_cache/ sphinx_doc/_build diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 03e7fdb70dd..7914f990582 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -51,11 +51,17 @@ def generate_launch_description() -> LaunchDescription: # Map fully qualified names to relative ones so the node's namespace can be prepended. remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + yaml_substitutions = { + 'KEEPOUT_ZONE_ENABLED': use_keepout_zones, + 'SPEED_ZONE_ENABLED': use_speed_zones, + } + configured_params = ParameterFile( RewrittenYaml( source_file=params_file, root_key=namespace, param_rewrites={}, + value_rewrites=yaml_substitutions, convert_types=True, ), allow_substs=True, diff --git a/nav2_bringup/launch/keepout_zone_launch.py b/nav2_bringup/launch/keepout_zone_launch.py index 502b6de4548..2a8a1925bd6 100644 --- a/nav2_bringup/launch/keepout_zone_launch.py +++ b/nav2_bringup/launch/keepout_zone_launch.py @@ -45,11 +45,16 @@ def generate_launch_description() -> LaunchDescription: # Map fully qualified names to relative ones so the node's namespace can be prepended. remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + yaml_substitutions = { + 'KEEPOUT_ZONE_ENABLED': use_keepout_zones, + } + configured_params = ParameterFile( RewrittenYaml( source_file=params_file, root_key=namespace, param_rewrites={}, + value_rewrites=yaml_substitutions, convert_types=True, ), allow_substs=True, diff --git a/nav2_bringup/launch/speed_zone_launch.py b/nav2_bringup/launch/speed_zone_launch.py index 240cc55a3b1..02f8254231b 100644 --- a/nav2_bringup/launch/speed_zone_launch.py +++ b/nav2_bringup/launch/speed_zone_launch.py @@ -45,11 +45,16 @@ def generate_launch_description() -> LaunchDescription: # Map fully qualified names to relative ones so the node's namespace can be prepended. remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + yaml_substitutions = { + 'SPEED_ZONE_ENABLED': use_speed_zones, + } + configured_params = ParameterFile( RewrittenYaml( source_file=params_file, root_key=namespace, param_rewrites={}, + value_rewrites=yaml_substitutions, convert_types=True, ), allow_substs=True, diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 19c1edbf591..7e9261e1564 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -227,7 +227,7 @@ local_costmap: filters: ["keepout_filter"] keepout_filter: plugin: "nav2_costmap_2d::KeepoutFilter" - enabled: True + enabled: KEEPOUT_ZONE_ENABLED filter_info_topic: "keepout_costmap_filter_info" inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" @@ -279,11 +279,11 @@ global_costmap: filters: ["keepout_filter", "speed_filter"] keepout_filter: plugin: "nav2_costmap_2d::KeepoutFilter" - enabled: True + enabled: KEEPOUT_ZONE_ENABLED filter_info_topic: "keepout_costmap_filter_info" speed_filter: plugin: "nav2_costmap_2d::SpeedFilter" - enabled: True + enabled: SPEED_ZONE_ENABLED filter_info_topic: "speed_costmap_filter_info" speed_limit_topic: "speed_limit" obstacle_layer: diff --git a/nav2_common/CMakeLists.txt b/nav2_common/CMakeLists.txt index 139835a896b..1f79afd2c96 100644 --- a/nav2_common/CMakeLists.txt +++ b/nav2_common/CMakeLists.txt @@ -15,3 +15,16 @@ install( DIRECTORY cmake DESTINATION share/${PROJECT_NAME} ) + +install( + DIRECTORY test + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_cmake_test REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + + ament_add_pytest_test(test_rewritten_yaml test/test_rewritten_yaml.py + ) +endif() diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py index 49ec39b5377..e475dc410fc 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/nav2_common/nav2_common/launch/rewritten_yaml.py @@ -48,6 +48,7 @@ def __init__( param_rewrites: dict[str, launch.SomeSubstitutionsType], root_key: Optional[launch.SomeSubstitutionsType] = None, key_rewrites: Optional[dict[str, launch.SomeSubstitutionsType]] = None, + value_rewrites: Optional[dict[str, launch.SomeSubstitutionsType]] = None, convert_types: bool = False, ) -> None: super().__init__() @@ -58,6 +59,7 @@ def __init__( :param: param_rewrites mappings to replace :param: root_key if provided, the contents are placed under this key :param: key_rewrites keys of mappings to replace + :param: value_rewrites values to replace :param: convert_types whether to attempt converting the string to a number or boolean """ @@ -68,6 +70,7 @@ def __init__( normalize_to_list_of_substitutions(source_file) self.__param_rewrites = {} self.__key_rewrites = {} + self.__value_rewrites = {} self.__convert_types = convert_types self.__root_key = None for key in param_rewrites: @@ -79,6 +82,11 @@ def __init__( self.__key_rewrites[key] = normalize_to_list_of_substitutions( key_rewrites[key] ) + if value_rewrites is not None: + for value in value_rewrites: + self.__value_rewrites[value] = normalize_to_list_of_substitutions( + value_rewrites[value] + ) if root_key is not None: self.__root_key = normalize_to_list_of_substitutions(root_key) @@ -94,11 +102,15 @@ def describe(self) -> str: def perform(self, context: launch.LaunchContext) -> str: yaml_filename = launch.utilities.perform_substitutions(context, self.name) rewritten_yaml = tempfile.NamedTemporaryFile(mode='w', delete=False) - param_rewrites, keys_rewrites = self.resolve_rewrites(context) - data = yaml.safe_load(open(yaml_filename)) + param_rewrites, keys_rewrites, value_rewrites = self.resolve_rewrites(context) + + with open(yaml_filename, 'r') as yaml_file: + data = yaml.safe_load(yaml_file) + self.substitute_params(data, param_rewrites) self.add_params(data, param_rewrites) self.substitute_keys(data, keys_rewrites) + self.substitute_values(data, value_rewrites) if self.__root_key is not None: root_key = launch.utilities.perform_substitutions(context, self.__root_key) if root_key: @@ -108,7 +120,7 @@ def perform(self, context: launch.LaunchContext) -> str: return rewritten_yaml.name def resolve_rewrites(self, context: launch.LaunchContext) -> \ - tuple[dict[str, str], dict[str, str]]: + tuple[dict[str, str], dict[str, str], dict[str, str]]: resolved_params = {} for key in self.__param_rewrites: resolved_params[key] = launch.utilities.perform_substitutions( @@ -119,7 +131,12 @@ def resolve_rewrites(self, context: launch.LaunchContext) -> \ resolved_keys[key] = launch.utilities.perform_substitutions( context, self.__key_rewrites[key] ) - return resolved_params, resolved_keys + resolved_values = {} + for value in self.__value_rewrites: + resolved_values[value] = launch.utilities.perform_substitutions( + context, self.__value_rewrites[value] + ) + return resolved_params, resolved_keys, resolved_values def substitute_params(self, yaml: dict[str, YamlValue], param_rewrites: dict[str, str]) -> None: @@ -149,6 +166,24 @@ def add_params(self, yaml: dict[str, YamlValue], if 'ros__parameters' in yaml_keys: yaml = self.updateYamlPathVals(yaml, yaml_keys, new_val) + def substitute_values( + self, yaml: dict[str, YamlValue], + value_rewrites: dict[str, str]) -> None: + + def process_value(value: YamlValue) -> YamlValue: + if isinstance(value, dict): + for k, v in list(value.items()): + value[k] = process_value(v) + return value + elif isinstance(value, list): + return [process_value(v) for v in value] + elif str(value) in value_rewrites: + return self.convert(value_rewrites[str(value)]) + return value + + for key in list(yaml.keys()): + yaml[key] = process_value(yaml[key]) + def updateYamlPathVals( self, yaml: dict[str, YamlValue], yaml_key_list: list[str], rewrite_val: YamlValue) -> dict[str, YamlValue]: diff --git a/nav2_common/package.xml b/nav2_common/package.xml index f2cebe1c8f7..675de1fcc2f 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -20,6 +20,10 @@ ament_cmake_core + ament_cmake_test + ament_cmake_pytest + python3-pytest + ament_cmake diff --git a/nav2_common/test/test_rewritten_yaml.py b/nav2_common/test/test_rewritten_yaml.py new file mode 100644 index 00000000000..2bf69b95f35 --- /dev/null +++ b/nav2_common/test/test_rewritten_yaml.py @@ -0,0 +1,133 @@ +# Copyright (c) 2025 Leander Stephen Desouza +# +# 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 os +import tempfile +from typing import Generator + +import launch +from launch.substitutions import LaunchConfiguration +from nav2_common.launch import RewrittenYaml +import pytest +import yaml + + +class TestRewrittenYamlValueRewrites: + """Test that value rewrites work correctly in RewrittenYaml.""" + + @pytest.fixture(autouse=True) + def setup_teardown(self) -> Generator[None, None, None]: + # Create a temporary YAML file for testing + self.test_yaml = tempfile.NamedTemporaryFile(mode='w', delete=False) + self.test_yaml.write("""\ + param0: placeholder_bool + param1: placeholder_string + param2: placeholder_float + param3: placeholder_int + param4: placeholder_list + param5: placeholder_dict + nested: + param6: placeholder_bool + param7: placeholder_string + param8: placeholder_float + param9: placeholder_int + param10: placeholder_list + param11: placeholder_dict + other_value: 42 + list_values: + - placeholder_bool + - placeholder_string + - placeholder_float + - placeholder_int + - placeholder_list + - placeholder_dict + - normal_value + """) + self.test_yaml.close() + yield + os.unlink(self.test_yaml.name) + + def test_value_rewrites(self) -> None: + """Test that value rewrites work for various types.""" + # Set up launch configurations for our test values + launch_configurations = { + 'test_bool': 'true', + 'test_string': 'replaced_string', + 'test_float': '3.14', + 'test_int': '100', + 'test_list': '["string", 42, 2.718, ["sublist_item1", "sublist_item2"]]', + 'test_dict': ('{"key1": "value1", "key2": 2, "key3": 3.14, ' + '"key4": ["list_item1", "list_item2"]}') + } + context = launch.LaunchContext() + for key, value in launch_configurations.items(): + context.launch_configurations[key] = value + + value_rewrites = { + 'placeholder_bool': LaunchConfiguration('test_bool'), + 'placeholder_string': LaunchConfiguration('test_string'), + 'placeholder_float': LaunchConfiguration('test_float'), + 'placeholder_int': LaunchConfiguration('test_int'), + 'placeholder_list': LaunchConfiguration('test_list'), + 'placeholder_dict': LaunchConfiguration('test_dict'), + } + + rewriter = RewrittenYaml( + source_file=self.test_yaml.name, + param_rewrites={}, + value_rewrites=value_rewrites, + convert_types=True, + ) + output_file = rewriter.perform(context) + + try: + with open(output_file) as f: + result = yaml.safe_load(f) + + assert result['param0'] is True + assert result['param1'] == 'replaced_string' + assert result['param2'] == 3.14 + assert result['param3'] == 100 + assert result['param4'] == \ + '["string", 42, 2.718, ["sublist_item1", "sublist_item2"]]' + assert result['param5'] == \ + '{"key1": "value1", "key2": 2, "key3": 3.14, "key4": ["list_item1", "list_item2"]}' + + # Check nested values + assert result['nested']['param6'] is True + assert result['nested']['param7'] == 'replaced_string' + assert result['nested']['param8'] == 3.14 + assert result['nested']['param9'] == 100 + assert result['nested']['param10'] == \ + '["string", 42, 2.718, ["sublist_item1", "sublist_item2"]]' + assert result['nested']['param11'] == \ + '{"key1": "value1", "key2": 2, "key3": 3.14, "key4": ["list_item1", "list_item2"]}' + + # Check list values + assert result['list_values'][0] is True + assert result['list_values'][1] == 'replaced_string' + assert result['list_values'][2] == 3.14 + assert result['list_values'][3] == 100 + assert result['list_values'][4] == \ + '["string", 42, 2.718, ["sublist_item1", "sublist_item2"]]' + assert result['list_values'][5] == \ + '{"key1": "value1", "key2": 2, "key3": 3.14, "key4": ["list_item1", "list_item2"]}' + + # Check other values remain unchanged + assert result['nested']['other_value'] == 42 + assert result['list_values'][6] == 'normal_value' + + finally: + if os.path.exists(output_file): + os.unlink(output_file) diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py index 3a934d0b6eb..01b71676695 100755 --- a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py @@ -50,7 +50,12 @@ def generate_launch_description() -> LaunchDescription: # Replace the `use_astar` setting on the params file configured_params = RewrittenYaml( - source_file=params_file, root_key='', param_rewrites={}, convert_types=True + source_file=params_file, root_key='', param_rewrites={}, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, + convert_types=True ) return LaunchDescription( diff --git a/nav2_system_tests/src/behaviors/backup/test_backup_behavior.launch.py b/nav2_system_tests/src/behaviors/backup/test_backup_behavior.launch.py index 291dbb4b836..f543296447d 100755 --- a/nav2_system_tests/src/behaviors/backup/test_backup_behavior.launch.py +++ b/nav2_system_tests/src/behaviors/backup/test_backup_behavior.launch.py @@ -44,6 +44,10 @@ def generate_launch_description() -> LaunchDescription: source_file=params_file, root_key='', param_rewrites=param_substitutions, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, convert_types=True, ) diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior.launch.py b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior.launch.py index 016d9ef163f..6e267e21f25 100755 --- a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior.launch.py +++ b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior.launch.py @@ -44,6 +44,10 @@ def generate_launch_description() -> LaunchDescription: source_file=params_file, root_key='', param_rewrites=param_substitutions, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, convert_types=True, ) diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py index cadef8c367a..007291cd4a0 100755 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py @@ -44,6 +44,10 @@ def generate_launch_description() -> LaunchDescription: source_file=params_file, root_key='', param_rewrites=param_substitutions, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, convert_types=True, ) diff --git a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py index 2efe32f7662..f4230ac00bd 100755 --- a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py @@ -53,7 +53,12 @@ def generate_launch_description() -> LaunchDescription: # Replace the `use_astar` setting on the params file configured_params = RewrittenYaml( - source_file=params_file, root_key='', param_rewrites={}, convert_types=True + source_file=params_file, root_key='', param_rewrites={}, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, + convert_types=True ) return LaunchDescription( diff --git a/nav2_system_tests/src/route/test_route_launch.py b/nav2_system_tests/src/route/test_route_launch.py index 81848d5c6ce..7b8e4e97949 100755 --- a/nav2_system_tests/src/route/test_route_launch.py +++ b/nav2_system_tests/src/route/test_route_launch.py @@ -73,6 +73,10 @@ def generate_launch_description() -> LaunchDescription: source_file=params_file, root_key='', param_rewrites=param_substitutions, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, convert_types=True, ) diff --git a/nav2_system_tests/src/system_failure/test_system_failure_launch.py b/nav2_system_tests/src/system_failure/test_system_failure_launch.py index 8b8e2542c6d..b8735d3e515 100755 --- a/nav2_system_tests/src/system_failure/test_system_failure_launch.py +++ b/nav2_system_tests/src/system_failure/test_system_failure_launch.py @@ -59,6 +59,10 @@ def generate_launch_description() -> LaunchDescription: source_file=params_file, root_key='', param_rewrites=param_substitutions, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, convert_types=True, ) diff --git a/nav2_system_tests/src/waypoint_follower/test_case_launch.py b/nav2_system_tests/src/waypoint_follower/test_case_launch.py index 0c92849d806..b3ed12cddb6 100755 --- a/nav2_system_tests/src/waypoint_follower/test_case_launch.py +++ b/nav2_system_tests/src/waypoint_follower/test_case_launch.py @@ -57,6 +57,10 @@ def generate_launch_description() -> LaunchDescription: source_file=params_file, root_key='', param_rewrites=param_substitutions, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, convert_types=True) context = LaunchContext()