Add a SetParameter action that sets a parameter to all nodes in the same scope (#158) (#187)

Signed-off-by: Ivan Santiago Paunovic <[email protected]>

Co-authored-by: Ivan Santiago Paunovic <[email protected]>

Author: jacobperron

launch_ros/launch_ros/actions/__init__.py

@@ -18,10 +18,12 @@
 from .lifecycle_node import LifecycleNode
 from .load_composable_nodes import LoadComposableNodes
 from .node import Node
+from .set_parameter import SetParameter
 
 __all__ = [
     'ComposableNodeContainer',
     'LifecycleNode',
     'LoadComposableNodes',
     'Node',
+    'SetParameter',
 ]

launch_ros/launch_ros/actions/load_composable_nodes.py

@@ -30,6 +30,7 @@
 from ..descriptions import ComposableNode
 from ..utilities import evaluate_parameters
 from ..utilities import to_parameters_list
+from ..utilities.normalize_parameters import normalize_parameter_dict
 
 
 class LoadComposableNodes(Action):
@@ -85,44 +86,7 @@ def _load_node(
                     )
                 )
                 return
-        request = composition_interfaces.srv.LoadNode.Request()
-        request.package_name = perform_substitutions(
-            context, composable_node_description.package
-        )
-        request.plugin_name = perform_substitutions(
-            context, composable_node_description.node_plugin
-        )
-        if composable_node_description.node_name is not None:
-            request.node_name = perform_substitutions(
-                context, composable_node_description.node_name
-            )
-        if composable_node_description.node_namespace is not None:
-            request.node_namespace = perform_substitutions(
-                context, composable_node_description.node_namespace
-            )
-        # request.log_level = perform_substitutions(context, node_description.log_level)
-        if composable_node_description.remappings is not None:
-            for from_, to in composable_node_description.remappings:
-                request.remap_rules.append('{}:={}'.format(
-                    perform_substitutions(context, list(from_)),
-                    perform_substitutions(context, list(to)),
-                ))
-        if composable_node_description.parameters is not None:
-            request.parameters = [
-                param.to_parameter_msg() for param in to_parameters_list(
-                    context, evaluate_parameters(
-                        context, composable_node_description.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, composable_node_description.extra_arguments
-                    )
-                )
-            ]
+        request = get_composable_node_load_request(composable_node_description, context)
         response = self.__rclpy_load_node_client.call(request)
         if not response.success:
             self.__logger.error(
@@ -173,3 +137,55 @@ def execute(
                 None, self._load_in_sequence, self.__composable_node_descriptions, context
             )
         )
+
+
+def get_composable_node_load_request(
+    composable_node_description: ComposableNode,
+    context: LaunchContext
+):
+    """Get the request that will be send to the composable node container."""
+    request = composition_interfaces.srv.LoadNode.Request()
+    request.package_name = perform_substitutions(
+        context, composable_node_description.package
+    )
+    request.plugin_name = perform_substitutions(
+        context, composable_node_description.node_plugin
+    )
+    if composable_node_description.node_name is not None:
+        request.node_name = perform_substitutions(
+            context, composable_node_description.node_name
+        )
+    if composable_node_description.node_namespace is not None:
+        request.node_namespace = perform_substitutions(
+            context, composable_node_description.node_namespace
+        )
+    # request.log_level = perform_substitutions(context, node_description.log_level)
+    if composable_node_description.remappings is not None:
+        for from_, to in composable_node_description.remappings:
+            request.remap_rules.append('{}:={}'.format(
+                perform_substitutions(context, list(from_)),
+                perform_substitutions(context, list(to)),
+            ))
+    global_params = context.launch_configurations.get('ros_params', None)
+    parameters = []
+    if global_params is not None:
+        parameters.append(normalize_parameter_dict(global_params))
+    if composable_node_description.parameters is not None:
+        parameters.extend(list(composable_node_description.parameters))
+    if parameters:
+        request.parameters = [
+            param.to_parameter_msg() for param in to_parameters_list(
+                context, 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, composable_node_description.extra_arguments
+                )
+            )
+        ]
+    return request

launch_ros/launch_ros/actions/node.py

@@ -136,13 +136,6 @@ def __init__(
             ensure_argument_type(parameters, (list), 'parameters', 'Node')
             # All elements in the list are paths to files with parameters (or substitutions that
             # evaluate to paths), or dictionaries of parameters (fields can be substitutions).
-            i = 0
-            for param in parameters:
-                i += 1
-                cmd += [LocalSubstitution(
-                    'ros_specific_arguments[{}]'.format(ros_args_index),
-                    description='parameter {}'.format(i))]
-                ros_args_index += 1
             normalized_params = normalize_parameters(parameters)
         if remappings is not None:
             i = 0
@@ -164,8 +157,8 @@ def __init__(
 
         self.__expanded_node_name = '<node_name_unspecified>'
         self.__expanded_node_namespace = '/'
-        self.__final_node_name = None  # type: Optional[Text]
         self.__expanded_parameter_files = None  # type: Optional[List[Text]]
+        self.__final_node_name = None  # type: Optional[Text]
         self.__expanded_remappings = None  # type: Optional[List[Tuple[Text, Text]]]
 
         self.__substitutions_performed = False
@@ -219,13 +212,24 @@ def _perform_substitutions(self, context: LaunchContext) -> None:
         if self.__expanded_node_namespace not in ['', '/']:
             self.__final_node_name += self.__expanded_node_namespace
         self.__final_node_name += '/' + self.__expanded_node_name
+        # expand global parameters first,
+        # so they can be overriden with specific parameters of this Node
+        global_params = context.launch_configurations.get('ros_params', None)
+        if global_params is not None or self.__parameters is not None:
+            self.__expanded_parameter_files = []
+        if global_params is not None:
+            param_file_path = self._create_params_file_from_dict(global_params)
+            self.__expanded_parameter_files.append(param_file_path)
+            cmd_extension = ['--params-file', f'{param_file_path}']
+            self.cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_extension])
+            assert os.path.isfile(param_file_path)
         # expand parameters too
         if self.__parameters is not None:
-            self.__expanded_parameter_files = []
             evaluated_parameters = evaluate_parameters(context, self.__parameters)
-            for params in evaluated_parameters:
+            for i, params in enumerate(evaluated_parameters):
                 if isinstance(params, dict):
                     param_file_path = self._create_params_file_from_dict(params)
+                    assert os.path.isfile(param_file_path)
                 elif isinstance(params, pathlib.Path):
                     param_file_path = str(params)
                 else:
@@ -234,9 +238,10 @@ def _perform_substitutions(self, context: LaunchContext) -> None:
                     self.__logger.warning(
                         'Parameter file path is not a file: {}'.format(param_file_path),
                     )
-                    # Don't skip adding the file to the parameter list since space has been
-                    # reserved for it in the ros_specific_arguments.
+                    continue
                 self.__expanded_parameter_files.append(param_file_path)
+                cmd_extension = ['--params-file', f'{param_file_path}']
+                self.cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_extension])
         # expand remappings too
         if self.__remappings is not None:
             self.__expanded_remappings = []
@@ -259,9 +264,6 @@ def execute(self, context: LaunchContext) -> Optional[List[Action]]:
             ros_specific_arguments.append('__node:={}'.format(self.__expanded_node_name))
         if self.__node_namespace is not None:
             ros_specific_arguments.append('__ns:={}'.format(self.__expanded_node_namespace))
-        if self.__expanded_parameter_files is not None:
-            for param_file_path in self.__expanded_parameter_files:
-                ros_specific_arguments.append('__params:={}'.format(param_file_path))
         if self.__expanded_remappings is not None:
             for remapping_from, remapping_to in self.__expanded_remappings:
                 ros_specific_arguments.append('{}:={}'.format(remapping_from, remapping_to))

launch_ros/launch_ros/actions/set_parameter.py

@@ -0,0 +1,90 @@
+# Copyright 2020 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+"""Module for the `SetParameter` action."""
+
+from typing import List
+
+from launch import Action
+from launch import Substitution
+from launch.frontend import Entity
+from launch.frontend import expose_action
+from launch.frontend import Parser
+from launch.launch_context import LaunchContext
+from launch.some_substitutions_type import SomeSubstitutionsType
+
+from launch_ros.parameters_type import SomeParameterValue
+from launch_ros.utilities.evaluate_parameters import evaluate_parameter_dict
+from launch_ros.utilities.normalize_parameters import normalize_parameter_dict
+
+
+@expose_action('set_parameter')
+class SetParameter(Action):
+    """
+    Action that sets a parameter in the current context.
+
+    This parameter will be set in all the nodes launched in the same scope.
+    e.g.:
+    ```python3
+        LaunchDescription([
+            ...,
+            GroupAction(
+                actions = [
+                    ...,
+                    SetParameter(name='my_param', value='2'),
+                    ...,
+                    Node(...),  // the param will be passed to this node
+                    ...,
+                ]
+            ),
+            Node(...),  // here it won't be passed, as it's not in the same scope
+            ...
+        ])
+    ```
+    """
+
+    def __init__(
+        self,
+        name: SomeSubstitutionsType,
+        value: SomeParameterValue,
+        **kwargs
+    ) -> None:
+        """Create a SetParameter action."""
+        super().__init__(**kwargs)
+        self.__param_dict = normalize_parameter_dict({name: value})
+
+    @classmethod
+    def parse(cls, entity: Entity, parser: Parser):
+        """Return `SetParameter` action and kwargs for constructing it."""
+        _, kwargs = super().parse(entity, parser)
+        kwargs['name'] = parser.parse_substitution(entity.get_attr('name'))
+        kwargs['value'] = parser.parse_substitution(entity.get_attr('value'))
+        return cls, kwargs
+
+    @property
+    def name(self) -> List[Substitution]:
+        """Getter for name."""
+        return self.__param_dict.keys()[0]
+
+    @property
+    def value(self) -> List[Substitution]:
+        """Getter for value."""
+        return self.__param_dict.values()[0]
+
+    def execute(self, context: LaunchContext):
+        """Execute the action."""
+        eval_param_dict = evaluate_parameter_dict(context, self.__param_dict)
+        global_params = context.launch_configurations.get('ros_params', {})
+        global_params.update(eval_param_dict)
+        context.launch_configurations['ros_params'] = global_params

launch_ros/launch_ros/utilities/__init__.py

@@ -26,7 +26,9 @@
 
 __all__ = [
     'evaluate_parameters',
+    'evaluate_parameters_dict',
     'normalize_parameters',
+    'normalize_parameters_dict',
     'normalize_remap_rule',
     'normalize_remap_rules',
     'to_parameters_list',

test_launch_ros/test/test_launch_ros/actions/test_node.py

@@ -30,13 +30,13 @@ class TestNode(unittest.TestCase):
 
     def _assert_launch_errors(self, actions):
         ld = LaunchDescription(actions)
-        ls = LaunchService()
+        ls = LaunchService(debug=True)
         ls.include_launch_description(ld)
         assert 0 != ls.run()
 
     def _assert_launch_no_errors(self, actions):
         ld = LaunchDescription(actions)
-        ls = LaunchService()
+        ls = LaunchService(debug=True)
         ls.include_launch_description(ld)
         assert 0 == ls.run()