Support both parameter file configurations for composable nodes (#259)

* Support both param file configurations in to_parameters_list.py

Signed-off-by: Rebecca Butler <[email protected]>

* Support another file config and add tests

Signed-off-by: Rebecca Butler <[email protected]>

* Update to work for multiple entries in yaml

Signed-off-by: Rebecca Butler <[email protected]>

* Fix deleted line

Signed-off-by: Rebecca Butler <[email protected]>

* Remove duplicated line

Signed-off-by: Rebecca Butler <[email protected]>

* Address reviewer comments

Signed-off-by: Jacob Perron <[email protected]>

* Emit warning iff no node name and ros__parameters foramt is used

Signed-off-by: Jacob Perron <[email protected]>

* Refactor after review

- Rename private function for dealing with ros__parameters entries
- Keep recursive parameters internal to function
- Skip evaluating parameters if dictionary is empty
- Use isinstance
- Strip trailing and leading '/' from node namespace

Signed-off-by: Jacob Perron <[email protected]>

* Remove unnecessary line

Signed-off-by: Jacob Perron <[email protected]>

* Reset keys list in case of dictionary value

We actually do this for the case of multiple entries like this:

    /ns_1:
      /node_1:
        ros__parameters:
          param_1: 1
          param_2: 2

    /**:
      ros__parameters:
        param_2: 22
        param_3: 33

Signed-off-by: Jacob Perron <[email protected]>

* Use separate test file for test_load_composable_nodes

Rather than reusing the test file that is used in test_node.

Signed-off-by: Jacob Perron <[email protected]>

Co-authored-by: Jacob Perron <[email protected]>

Author: rebecca-butler

launch_ros/launch_ros/actions/load_composable_nodes.py

@@ -243,7 +243,7 @@ def get_composable_node_load_request(
     composable_node_description: ComposableNode,
     context: LaunchContext
 ):
-    """Get the request that will be send to the composable node container."""
+    """Get the request that will be sent to the composable node container."""
     request = composition_interfaces.srv.LoadNode.Request()
     request.package_name = perform_substitutions(
         context, composable_node_description.package
@@ -286,15 +286,17 @@ def get_composable_node_load_request(
     if parameters:
         request.parameters = [
             param.to_parameter_msg() for param in to_parameters_list(
-                context, evaluate_parameters(
+                context, request.node_name, expanded_ns,
+                evaluate_parameters(
                     context, parameters
                 )
             )
         ]
     if composable_node_description.extra_arguments is not None:
         request.extra_arguments = [
             param.to_parameter_msg() for param in to_parameters_list(
-                context, evaluate_parameters(
+                context, request.node_name, expanded_ns,
+                evaluate_parameters(
                     context, composable_node_description.extra_arguments
                 )
             )

launch_ros/launch_ros/utilities/to_parameters_list.py

@@ -16,6 +16,7 @@
 
 import pathlib
 from typing import List
+import warnings
 
 from launch.launch_context import LaunchContext
 import rclpy.parameter
@@ -28,26 +29,81 @@
 from ..parameters_type import EvaluatedParameters
 
 
+def __normalize_parameters_dict(dictionary):
+    """
+    Combine namespaces and their node names into one key and remove the "ros__parameters" key.
+
+    Return an empty dict if the "ros__parameters" key is not found.
+    """
+    # Recursive function
+    def normalize_parameters_dict(dictionary, keys, result_dict):
+        for key, value in dictionary.items():
+            # Base case: found 'ros__parameters' key
+            # Return
+            if key == 'ros__parameters':
+                node_name = '/'.join(keys)
+                result_dict[node_name] = value
+                return result_dict
+
+            if isinstance(value, dict):
+                keys.append(key.lstrip('/'))
+                result_dict = normalize_parameters_dict(value, keys, result_dict)
+                # Reset keys in case there are multiple ros__parameter entries
+                keys = []
+
+        return result_dict
+
+    return normalize_parameters_dict(dictionary, [], {})
+
+
 def to_parameters_list(
     context: LaunchContext,
+    node_name: str,
+    namespace: str,
     evaluated_parameters: EvaluatedParameters
 ) -> List[rclpy.parameter.Parameter]:
     """
     Transform evaluated parameters into a list of rclpy.parameter.Parameter objects.
 
     :param context: to carry out substitutions during normalization of parameter files.
         See `normalize_parameters()` documentation for further reference.
+    :param node_name: node name
+    :param namespace: node namespace
     :param parameters: parameters as either paths to parameter files or name/value pairs.
         See `evaluate_parameters()` documentation for further reference.
     :returns: a list of parameters
     """
     parameters = []  # type: List[rclpy.parameter.Parameter]
+    node_name = node_name.lstrip('/')
+    if namespace:
+        namespace = namespace.strip('/')
+        node_name = f'{namespace}/{node_name}'
+
+    params_set = {}
+    warned_once = False
     for params_set_or_path in evaluated_parameters:
         if isinstance(params_set_or_path, pathlib.Path):
             with open(str(params_set_or_path), 'r') as f:
-                params_set = evaluate_parameter_dict(
-                    context, normalize_parameter_dict(yaml.safe_load(f))
-                )
+                param_dict = yaml.safe_load(f)
+                normalized_param_dict = __normalize_parameters_dict(param_dict)
+
+                if normalized_param_dict:
+                    param_dict.clear()
+                    if '**' in normalized_param_dict:
+                        param_dict = normalized_param_dict['**']
+                    if node_name in normalized_param_dict:
+                        param_dict.update(normalized_param_dict[node_name])
+                    if not warned_once and not node_name:
+                        warnings.warn(
+                            'node name not provided to launch; parameter files will not apply',
+                            UserWarning
+                        )
+                        warned_once = True
+
+                if param_dict:
+                    params_set = evaluate_parameter_dict(
+                        context, normalize_parameter_dict(param_dict)
+                    )
         else:
             params_set = params_set_or_path
         if not isinstance(params_set, dict):

test_launch_ros/test/test_launch_ros/actions/example_parameters_multiple_entries.yaml

@@ -0,0 +1,14 @@
+/ns_1:
+  /node_1:
+    ros__parameters:
+      param_1: 1
+      param_2: 2
+
+/**:
+  ros__parameters:
+    param_2: 22
+    param_3: 33
+
+ns_2/node_2:
+    ros__parameters:
+      param_3: 3

test_launch_ros/test/test_launch_ros/actions/example_parameters_namespace.yaml

@@ -0,0 +1,3 @@
+ns/test_node_name:
+  ros__parameters:
+    param: 1

test_launch_ros/test/test_launch_ros/actions/example_parameters_nested_namespace.yaml

@@ -0,0 +1,10 @@
+/my_ns:
+    my_node:
+        ros__parameters:
+            some_int: 42
+            a_string: "Hello world"
+            no_string: ""
+            some_list:
+                sub_list:
+                    some_integers: [1, 2, 3, 4]
+                    some_doubles : [3.14, 2.718]

test_launch_ros/test/test_launch_ros/actions/example_parameters_no_name.yaml

@@ -0,0 +1,2 @@
+some_int: 42
+a_string: "Hello world"

test_launch_ros/test/test_launch_ros/actions/example_parameters_no_namespace.yaml

@@ -0,0 +1,3 @@
+/test_node_name:
+  ros__parameters:
+    param: 2

test_launch_ros/test/test_launch_ros/actions/example_parameters_wildcard.yaml

@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    param: "wildcard"

test_launch_ros/test/test_launch_ros/actions/test_load_composable_nodes.py

@@ -14,6 +14,7 @@
 
 """Tests for the LoadComposableNodes Action."""
 
+import pathlib
 import threading
 
 from composition_interfaces.srv import LoadNode
@@ -214,6 +215,177 @@ def test_load_node_with_params(mock_component_container):
     assert len(request.extra_arguments) == 0
 
 
+def test_load_node_with_param_file(mock_component_container):
+    """Test loading a node with with parameters specified in yaml files."""
+    parameters_file_dir = pathlib.Path(__file__).resolve().parent
+
+    # Case 1: no node name in yaml file
+    context = _assert_launch_no_errors([
+        _load_composable_node(
+            package='foo_package',
+            plugin='bar_plugin',
+            name='test_node_name',
+            namespace='test_node_namespace',
+            parameters=[
+                parameters_file_dir / 'example_parameters_no_name.yaml'
+            ],
+        )
+    ])
+    request = mock_component_container.requests[0]
+    assert get_node_name_count(context, '/test_node_namespace/test_node_name') == 1
+    assert request.node_name == 'test_node_name'
+    assert request.node_namespace == '/test_node_namespace'
+    assert len(request.parameters) == 2
+    assert request.parameters[0].name == 'some_int'
+    assert request.parameters[0].value.integer_value == 42
+    assert request.parameters[1].name == 'a_string'
+    assert request.parameters[1].value.string_value == 'Hello world'
+
+    # Case 2: node name with namespace
+    context = _assert_launch_no_errors([
+        _load_composable_node(
+            package='foo_package',
+            plugin='bar_plugin',
+            name='test_node_name',
+            namespace='ns',
+            parameters=[
+                parameters_file_dir / 'example_parameters_namespace.yaml'
+            ],
+        )
+    ])
+    request = mock_component_container.requests[1]
+    assert get_node_name_count(context, '/ns/test_node_name') == 1
+    assert request.node_name == 'test_node_name'
+    assert request.node_namespace == '/ns'
+    assert len(request.parameters) == 1
+    assert request.parameters[0].name == 'param'
+    assert request.parameters[0].value.integer_value == 1
+
+    # Case 3: nested node name with namespace
+    context = _assert_launch_no_errors([
+        _load_composable_node(
+            package='foo_package',
+            plugin='bar_plugin',
+            name='my_node',
+            namespace='my_ns',
+            parameters=[
+                parameters_file_dir / 'example_parameters_nested_namespace.yaml'
+            ],
+        )
+    ])
+    request = mock_component_container.requests[2]
+    assert get_node_name_count(context, '/my_ns/my_node') == 1
+    assert request.node_name == 'my_node'
+    assert request.node_namespace == '/my_ns'
+    assert len(request.parameters) == 5
+    assert request.parameters[0].name == 'some_int'
+    assert request.parameters[0].value.integer_value == 42
+    assert request.parameters[1].name == 'a_string'
+    assert request.parameters[1].value.string_value == 'Hello world'
+    assert request.parameters[2].value.string_value == ''
+
+    # Case 4: node name without namespace
+    context = _assert_launch_no_errors([
+        _load_composable_node(
+            package='foo_package',
+            plugin='bar_plugin',
+            name='test_node_name',
+            namespace='',
+            parameters=[
+                parameters_file_dir / 'example_parameters_no_namespace.yaml'
+            ],
+        )
+    ])
+    request = mock_component_container.requests[3]
+    assert get_node_name_count(context, '//test_node_name') == 1
+    assert request.node_name == 'test_node_name'
+    assert request.node_namespace == '/'
+    assert len(request.parameters) == 1
+    assert request.parameters[0].name == 'param'
+    assert request.parameters[0].value.integer_value == 2
+
+    # Case 5: wildcard
+    context = _assert_launch_no_errors([
+        _load_composable_node(
+            package='foo_package',
+            plugin='bar_plugin',
+            name='my_node',
+            namespace='wildcard_ns',
+            parameters=[
+                parameters_file_dir / 'example_parameters_wildcard.yaml'
+            ],
+        )
+    ])
+    request = mock_component_container.requests[4]
+    assert get_node_name_count(context, '/wildcard_ns/my_node') == 1
+    assert request.node_name == 'my_node'
+    assert request.node_namespace == '/wildcard_ns'
+    assert len(request.parameters) == 1
+    assert request.parameters[0].name == 'param'
+    assert request.parameters[0].value.string_value == 'wildcard'
+
+    # Case 6: multiple entries (params with node name should take precedence over wildcard params)
+    context = _assert_launch_no_errors([
+        _load_composable_node(
+            package='foo_package',
+            plugin='bar_plugin',
+            name='node_1',
+            namespace='ns_1',
+            parameters=[
+                parameters_file_dir / 'example_parameters_multiple_entries.yaml'
+            ],
+        ),
+        _load_composable_node(
+            package='foo_package',
+            plugin='bar_plugin',
+            name='node_2',
+            namespace='ns_2',
+            parameters=[
+                parameters_file_dir / 'example_parameters_multiple_entries.yaml'
+            ],
+        )
+    ])
+    request = mock_component_container.requests[5]
+    assert get_node_name_count(context, '/ns_1/node_1') == 1
+    assert request.node_name == 'node_1'
+    assert request.node_namespace == '/ns_1'
+    assert len(request.parameters) == 3
+    assert request.parameters[0].name == 'param_2'
+    assert request.parameters[0].value.integer_value == 2
+    assert request.parameters[1].name == 'param_3'
+    assert request.parameters[1].value.integer_value == 33
+    assert request.parameters[2].name == 'param_1'
+    assert request.parameters[2].value.integer_value == 1
+
+    request = mock_component_container.requests[6]
+    assert get_node_name_count(context, '/ns_2/node_2') == 1
+    assert request.node_name == 'node_2'
+    assert request.node_namespace == '/ns_2'
+    assert len(request.parameters) == 2
+    assert request.parameters[0].name == 'param_2'
+    assert request.parameters[0].value.integer_value == 22
+    assert request.parameters[1].name == 'param_3'
+    assert request.parameters[1].value.integer_value == 3
+
+    # Case 7: node name not found
+    context = _assert_launch_no_errors([
+        _load_composable_node(
+            package='foo_package',
+            plugin='bar_plugin',
+            name='wrong_node_name',
+            namespace='ns',
+            parameters=[
+                parameters_file_dir / 'example_parameters_no_namespace.yaml'
+            ],
+        )
+    ])
+    request = mock_component_container.requests[7]
+    assert get_node_name_count(context, '/ns/wrong_node_name') == 1
+    assert request.node_name == 'wrong_node_name'
+    assert request.node_namespace == '/ns'
+    assert len(request.parameters) == 0
+
+
 def test_load_node_with_global_remaps_in_group(mock_component_container):
     """Test loading a node with global remaps scoped to a group."""
     context = _assert_launch_no_errors([