From 6689fe0f56d9979c642d68e78c718081682e1157 Mon Sep 17 00:00:00 2001 From: Roger Strain Date: Mon, 11 Jan 2021 07:53:12 -0600 Subject: [PATCH 01/19] Refactor Node action into description classes Distro A; OPSEC #4584 Signed-off-by: Roger Strain --- launch_ros/launch_ros/actions/node.py | 261 ++------------ .../launch_ros/descriptions/__init__.py | 6 + launch_ros/launch_ros/descriptions/node.py | 324 ++++++++++++++++++ .../launch_ros/descriptions/node_trait.py | 44 +++ .../launch_ros/descriptions/ros_executable.py | 91 +++++ .../test/test_launch_ros/actions/test_node.py | 8 +- .../actions/test_push_ros_namespace.py | 16 +- .../actions/test_set_parameter.py | 10 + .../test_launch_ros/actions/test_set_remap.py | 10 + 9 files changed, 527 insertions(+), 243 deletions(-) create mode 100644 launch_ros/launch_ros/descriptions/node.py create mode 100644 launch_ros/launch_ros/descriptions/node_trait.py create mode 100644 launch_ros/launch_ros/descriptions/ros_executable.py diff --git a/launch_ros/launch_ros/actions/node.py b/launch_ros/launch_ros/actions/node.py index fd684ce21..c22cc4481 100644 --- a/launch_ros/launch_ros/actions/node.py +++ b/launch_ros/launch_ros/actions/node.py @@ -14,23 +14,19 @@ """Module for the Node action.""" -import os -import pathlib -from tempfile import NamedTemporaryFile from typing import Dict from typing import Iterable from typing import List from typing import Optional from typing import Text # noqa: F401 from typing import Tuple # noqa: F401 -from typing import Union try: import importlib.metadata as importlib_metadata except ModuleNotFoundError: import importlib_metadata -from launch.action import Action +from launch.actions import ExecuteLocal from launch.actions import ExecuteProcess from launch.frontend import Entity from launch.frontend import expose_action @@ -38,33 +34,13 @@ from launch.frontend.type_utils import get_data_type_from_identifier from launch.launch_context import LaunchContext -import launch.logging from launch.some_substitutions_type import SomeSubstitutionsType -from launch.substitutions import LocalSubstitution -from launch.utilities import ensure_argument_type -from launch.utilities import normalize_to_list_of_substitutions -from launch.utilities import perform_substitutions +from launch_ros.descriptions import Node as NodeDescription +from launch_ros.descriptions import ParameterFile +from launch_ros.descriptions import RosExecutable from launch_ros.parameters_type import SomeParameters from launch_ros.remap_rule_type import SomeRemapRules -from launch_ros.substitutions import ExecutableInPackage -from launch_ros.utilities import add_node_name -from launch_ros.utilities import evaluate_parameters -from launch_ros.utilities import get_node_name_count -from launch_ros.utilities import make_namespace_absolute -from launch_ros.utilities import normalize_parameters -from launch_ros.utilities import normalize_remap_rules -from launch_ros.utilities import plugin_support -from launch_ros.utilities import prefix_namespace - - -from rclpy.validate_namespace import validate_namespace -from rclpy.validate_node_name import validate_node_name - -import yaml - -from ..descriptions import Parameter -from ..descriptions import ParameterFile class NodeActionExtension: @@ -110,12 +86,9 @@ def prepare_for_execute(self, context, ros_specific_arguments, node_action): @expose_action('node') -class Node(ExecuteProcess): +class Node(ExecuteLocal): """Action that executes a ROS node.""" - UNSPECIFIED_NODE_NAME = '' - UNSPECIFIED_NODE_NAMESPACE = '' - def __init__( self, *, executable: SomeSubstitutionsType, @@ -198,46 +171,20 @@ def __init__( :param: ros_arguments list of ROS arguments for the node :param: arguments list of extra arguments for the node """ - if package is not None: - cmd = [ExecutableInPackage(package=package, executable=executable)] - else: - cmd = [executable] - cmd += [] if arguments is None else arguments - cmd += [] if ros_arguments is None else ['--ros-args'] + ros_arguments - # Reserve space for ros specific arguments. - # The substitutions will get expanded when the action is executed. - cmd += ['--ros-args'] # Prepend ros specific arguments with --ros-args flag - if name is not None: - cmd += ['-r', LocalSubstitution( - "ros_specific_arguments['name']", description='node name')] - if parameters is not None: - 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). - normalized_params = normalize_parameters(parameters) - # Forward 'exec_name' as to ExecuteProcess constructor - kwargs['name'] = exec_name - super().__init__(cmd=cmd, **kwargs) - self.__package = package - self.__node_executable = executable - self.__node_name = name - self.__node_namespace = namespace - self.__parameters = [] if parameters is None else normalized_params - self.__remappings = [] if remappings is None else list(normalize_remap_rules(remappings)) - self.__ros_arguments = ros_arguments - self.__arguments = arguments - - self.__expanded_node_name = self.UNSPECIFIED_NODE_NAME - self.__expanded_node_namespace = self.UNSPECIFIED_NODE_NAMESPACE - self.__expanded_parameter_arguments = None # type: Optional[List[Tuple[Text, bool]]] - self.__final_node_name = None # type: Optional[Text] - self.__expanded_remappings = None # type: Optional[List[Tuple[Text, Text]]] - - self.__substitutions_performed = False - - self.__logger = launch.logging.get_logger(__name__) - + self.__node_desc = NodeDescription(node_name=name, node_namespace=namespace, + parameters=parameters, remappings=remappings, + arguments=arguments) + self.__ros_exec = RosExecutable(package=package, executable=executable, + nodes=[self.__node_desc]) self.__extensions = get_extensions(self.__logger) + super().__init__(process_description=self.__ros_exec, **kwargs) + + def _perform_substitutions(self, lc: LaunchContext): + self.__node_desc.prepare(lc, self.__ros_exec) + + def is_node_name_fully_specified(self): + return self.__node_desc.is_node_name_fully_specified() + @staticmethod def parse_nested_parameters(params, parser): @@ -306,7 +253,7 @@ def get_nested_dictionary_from_nested_key_value_pairs(params): def parse(cls, entity: Entity, parser: Parser): """Parse node.""" # See parse method of `ExecuteProcess` - _, kwargs = super().parse(entity, parser, ignore=['cmd']) + _, kwargs = ExecuteProcess.parse(entity, parser, ignore=['cmd']) args = entity.get_attr('args', optional=True) if args is not None: kwargs['arguments'] = super()._parse_cmdline(args, parser) @@ -348,187 +295,27 @@ def parse(cls, entity: Entity, parser: Parser): @property def node_package(self): """Getter for node_package.""" - return self.__package + return self.__node_exec.package @property def node_executable(self): """Getter for node_executable.""" - return self.__node_executable + return self.__node_exec.executable @property def node_name(self): """Getter for node_name.""" - if self.__final_node_name is None: - raise RuntimeError("cannot access 'node_name' before executing action") - return self.__final_node_name - - def is_node_name_fully_specified(self): - keywords = (self.UNSPECIFIED_NODE_NAME, self.UNSPECIFIED_NODE_NAMESPACE) - return all(x not in self.node_name for x in keywords) - - def _create_params_file_from_dict(self, params): - with NamedTemporaryFile(mode='w', prefix='launch_params_', delete=False) as h: - param_file_path = h.name - param_dict = { - self.node_name if self.is_node_name_fully_specified() else '/**': - {'ros__parameters': params} - } - yaml.dump(param_dict, h, default_flow_style=False) - return param_file_path - - def _get_parameter_rule(self, param: 'Parameter', context: LaunchContext): - name, value = param.evaluate(context) - return f'{name}:={yaml.dump(value)}' - - def _perform_substitutions(self, context: LaunchContext) -> None: - # Here to avoid cyclic import - from ..descriptions import Parameter - try: - if self.__substitutions_performed: - # This function may have already been called by a subclass' `execute`, for example. - return - self.__substitutions_performed = True - if self.__node_name is not None: - self.__expanded_node_name = perform_substitutions( - context, normalize_to_list_of_substitutions(self.__node_name)) - validate_node_name(self.__expanded_node_name) - self.__expanded_node_name.lstrip('/') - expanded_node_namespace: Optional[Text] = None - if self.__node_namespace is not None: - expanded_node_namespace = perform_substitutions( - context, normalize_to_list_of_substitutions(self.__node_namespace)) - base_ns = context.launch_configurations.get('ros_namespace', None) - expanded_node_namespace = make_namespace_absolute( - prefix_namespace(base_ns, expanded_node_namespace)) - if expanded_node_namespace is not None: - self.__expanded_node_namespace = expanded_node_namespace - cmd_extension = ['-r', LocalSubstitution("ros_specific_arguments['ns']")] - self.cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_extension]) - validate_namespace(self.__expanded_node_namespace) - except Exception: - self.__logger.error( - "Error while expanding or validating node name or namespace for '{}':" - .format('package={}, executable={}, name={}, namespace={}'.format( - self.__package, - self.__node_executable, - self.__node_name, - self.__node_namespace, - )) - ) - raise - self.__final_node_name = prefix_namespace( - self.__expanded_node_namespace, self.__expanded_node_name) - - # Expand global parameters first, - # so they can be overridden with specific parameters of this Node - # The params_container list is expected to contain name-value pairs (tuples) - # and/or strings representing paths to parameter files. - params_container = context.launch_configurations.get('global_params', None) - - if any(x is not None for x in (params_container, self.__parameters)): - self.__expanded_parameter_arguments = [] - if params_container is not None: - for param in params_container: - if isinstance(param, tuple): - name, value = param - cmd_extension = ['-p', f'{name}:={value}'] - self.cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_extension]) - else: - param_file_path = os.path.abspath(param) - self.__expanded_parameter_arguments.append((param_file_path, True)) - cmd_extension = ['--params-file', f'{param_file_path}'] - assert os.path.isfile(param_file_path) - self.cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_extension]) - - # expand parameters too - if self.__parameters is not None: - evaluated_parameters = evaluate_parameters(context, self.__parameters) - for params in evaluated_parameters: - is_file = False - if isinstance(params, dict): - param_argument = self._create_params_file_from_dict(params) - is_file = True - assert os.path.isfile(param_argument) - elif isinstance(params, pathlib.Path): - param_argument = str(params) - is_file = True - elif isinstance(params, Parameter): - param_argument = self._get_parameter_rule(params, context) - else: - raise RuntimeError('invalid normalized parameters {}'.format(repr(params))) - if is_file and not os.path.isfile(param_argument): - self.__logger.warning( - 'Parameter file path is not a file: {}'.format(param_argument), - ) - continue - self.__expanded_parameter_arguments.append((param_argument, is_file)) - cmd_extension = ['--params-file' if is_file else '-p', f'{param_argument}'] - self.cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_extension]) - # expand remappings too - global_remaps = context.launch_configurations.get('ros_remaps', None) - if global_remaps or self.__remappings: - self.__expanded_remappings = [] - if global_remaps: - self.__expanded_remappings.extend(global_remaps) - if self.__remappings: - self.__expanded_remappings.extend([ - (perform_substitutions(context, src), perform_substitutions(context, dst)) - for src, dst in self.__remappings - ]) - if self.__expanded_remappings: - cmd_extension = [] - for src, dst in self.__expanded_remappings: - cmd_extension.extend(['-r', f'{src}:={dst}']) - self.cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_extension]) - - def execute(self, context: LaunchContext) -> Optional[List[Action]]: - """ - Execute the action. - - Delegated to :meth:`launch.actions.ExecuteProcess.execute`. - """ - self._perform_substitutions(context) - # Prepare the ros_specific_arguments list and add it to the context so that the - # LocalSubstitution placeholders added to the the cmd can be expanded using the contents. - ros_specific_arguments: Dict[str, Union[str, List[str]]] = {} - if self.__node_name is not None: - ros_specific_arguments['name'] = '__node:={}'.format(self.__expanded_node_name) - if self.__expanded_node_namespace != '': - ros_specific_arguments['ns'] = '__ns:={}'.format(self.__expanded_node_namespace) - - # Give extensions a chance to prepare for execution - for extension in self.__extensions.values(): - cmd_extension, ros_specific_arguments = extension.prepare_for_execute( - context, - ros_specific_arguments, - self - ) - self.cmd.extend(cmd_extension) - - context.extend_locals({'ros_specific_arguments': ros_specific_arguments}) - ret = super().execute(context) - - if self.is_node_name_fully_specified(): - add_node_name(context, self.node_name) - node_name_count = get_node_name_count(context, self.node_name) - if node_name_count > 1: - execute_process_logger = launch.logging.get_logger(self.name) - execute_process_logger.warning( - 'there are now at least {} nodes with the name {} created within this ' - 'launch context'.format(node_name_count, self.node_name) - ) - - return ret + return self.__node_desc.node_name @property def expanded_node_namespace(self): """Getter for expanded_node_namespace.""" - return self.__expanded_node_namespace + return self.__node_desc.expanded_node_namespace @property def expanded_remapping_rules(self): """Getter for expanded_remappings.""" - return self.__expanded_remappings + return self.__node_desc.expanded_remappings def instantiate_extension( diff --git a/launch_ros/launch_ros/descriptions/__init__.py b/launch_ros/launch_ros/descriptions/__init__.py index fd5d9bebe..b920ce6ac 100644 --- a/launch_ros/launch_ros/descriptions/__init__.py +++ b/launch_ros/launch_ros/descriptions/__init__.py @@ -15,6 +15,9 @@ """descriptions Module.""" from .composable_node import ComposableNode +from .node import Node +from .node_trait import NodeTrait +from .ros_executable import RosExecutable from ..parameter_descriptions import Parameter from ..parameter_descriptions import ParameterFile from ..parameter_descriptions import ParameterValue @@ -22,7 +25,10 @@ __all__ = [ 'ComposableNode', + 'Node', + 'NodeTrait', 'Parameter', 'ParameterFile', 'ParameterValue', + 'RosExecutable', ] diff --git a/launch_ros/launch_ros/descriptions/node.py b/launch_ros/launch_ros/descriptions/node.py new file mode 100644 index 000000000..c0c3ecf04 --- /dev/null +++ b/launch_ros/launch_ros/descriptions/node.py @@ -0,0 +1,324 @@ +# Copyright 2021 Southwest Research Institute, All Rights Reserved. +# +# 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. +# +# DISTRIBUTION A. Approved for public release; distribution unlimited. +# OPSEC #4584. +# +# Delivered to the U.S. Government with Unlimited Rights, as defined in DFARS +# Part 252.227-7013 or 7014 (Feb 2014). +# +# This notice must appear in all copies of this file and its derivatives. + +"""Module for a description of a Node.""" + +import os +import pathlib + +from tempfile import NamedTemporaryFile + +from typing import Dict +from typing import Iterable +from typing import List +from typing import Optional +from typing import Text +from typing import Tuple +from typing import Union + +from launch import LaunchContext +from launch import SomeSubstitutionsType +from launch.descriptions import Executable +import launch.logging +from launch.substitutions import LocalSubstitution +from launch.utilities import ensure_argument_type +from launch.utilities import normalize_to_list_of_substitutions +from launch.utilities import perform_substitutions + +from launch_ros.utilities import add_node_name +from launch_ros.utilities import evaluate_parameters +from launch_ros.utilities import get_node_name_count +from launch_ros.utilities import make_namespace_absolute +from launch_ros.utilities import normalize_parameters +from launch_ros.utilities import normalize_remap_rules +from launch_ros.utilities import prefix_namespace + +from rclpy.validate_namespace import validate_namespace +from rclpy.validate_node_name import validate_node_name + +import yaml + +from .node_trait import NodeTrait +from ..parameter_descriptions import Parameter +from ..parameters_type import SomeParameters +from ..remap_rule_type import SomeRemapRules + + +class Node: + """Describes a ROS node.""" + + UNSPECIFIED_NODE_NAME = '' + UNSPECIFIED_NODE_NAMESPACE = '' + + def __init__( + self, *, + node_name: Optional[SomeSubstitutionsType] = None, + node_namespace: Optional[SomeSubstitutionsType] = None, + parameters: Optional[SomeParameters] = None, + remappings: Optional[SomeRemapRules] = None, + arguments: Optional[Iterable[SomeSubstitutionsType]] = None, + traits: Optional[Iterable[NodeTrait]] = None, + **kwargs + ) -> None: + """ + Construct an Node description. + + Many arguments are passed eventually to + :class:`launch.actions.ExecuteProcess`, so see the documentation of + that class for additional details. + + If the node name is not given (or is None) then no name is passed to + the node on creation and instead the default name specified within the + code of the node is used instead. + + The namespace can either be absolute (i.e. starts with /) or + relative. + If absolute, then nothing else is considered and this is passed + directly to the node to set the namespace. + If relative, the namespace in the 'ros_namespace' LaunchConfiguration + will be prepended to the given relative node namespace. + If no namespace is given, then the default namespace `/` is + assumed. + + The parameters are passed as a list, with each element either a yaml + file that contains parameter rules (string or pathlib.Path to the full + path of the file), or a dictionary that specifies parameter rules. + Keys of the dictionary can be strings or an iterable of Substitutions + that will be expanded to a string. + Values in the dictionary can be strings, integers, floats, or tuples + of Substitutions that will be expanded to a string. + Additionally, values in the dictionary can be lists of the + aforementioned types, or another dictionary with the same properties. + A yaml file with the resulting parameters from the dictionary will be + written to a temporary file, the path to which will be passed to the + node. + Multiple dictionaries/files can be passed: each file path will be + passed in in order to the node (where the last definition of a + parameter takes effect). + + :param: node_name the name of the node + :param: node_namespace the ROS namespace for this Node + :param: parameters list of names of yaml files with parameter rules, + or dictionaries of parameters. + :param: remappings ordered list of 'to' and 'from' string pairs to be + passed to the node as ROS remapping rules + :param: arguments list of extra arguments for the node + :param: traits list of special traits of the node + """ + if parameters is not None: + 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). + normalized_params = normalize_parameters(parameters) + + self.__node_name = node_name + self.__node_namespace = node_namespace + self.__parameters = [] if parameters is None else normalized_params + self.__remappings = [] if remappings is None else list(normalize_remap_rules(remappings)) + self.__arguments = arguments + self.__traits = traits + + self.__expanded_node_name = self.UNSPECIFIED_NODE_NAME + self.__expanded_node_namespace = self.UNSPECIFIED_NODE_NAMESPACE + self.__expanded_parameter_arguments = None # type: Optional[List[Tuple[Text, bool]]] + self.__final_node_name = None # type: Optional[Text] + self.__expanded_remappings = None # type: Optional[List[Tuple[Text, Text]]] + + self.__substitutions_performed = False + + self.__logger = launch.logging.get_logger(__name__) + + @property + def node_name(self): + """Getter for node_name.""" + if self.__final_node_name is None: + raise RuntimeError("cannot access 'node_name' before executing action") + return self.__final_node_name + + @property + def node_namespace(self): + """Getter for node_namespace.""" + return self.__node_namespace + + @property + def parameters(self): + """Getter for parameters.""" + return self.__parameters + + @property + def remappings(self): + """Getter for remappings.""" + return self.__remappings + + @property + def arguments(self): + """Getter for arguments.""" + return self.__arguments + + @property + def traits(self): + """Getter for traits.""" + return self.__traits + + @property + def expanded_node_name(self): + """Getter for expanded_node_name.""" + return self.__expanded_node_name + + @property + def expanded_node_namespace(self): + """Getter for expanded_node_namespace.""" + return self.__expanded_node_namespace + + @property + def expanded_parameter_arguments(self): + """Getter for expanded_parameter_arguments.""" + return self.__expanded_parameter_arguments + + @property + def expanded_remappings(self): + """Getter for expanded_remappings.""" + return self.__expanded_remappings + + def is_node_name_fully_specified(self): + keywords = (self.UNSPECIFIED_NODE_NAME, self.UNSPECIFIED_NODE_NAMESPACE) + return all(x not in self.node_name for x in keywords) + + def _create_params_file_from_dict(self, params): + with NamedTemporaryFile(mode='w', prefix='launch_params_', delete=False) as h: + param_file_path = h.name + param_dict = { + self.node_name if self.is_node_name_fully_specified() else '/**': + {'ros__parameters': params} + } + yaml.dump(param_dict, h, default_flow_style=False) + return param_file_path + + def _get_parameter_rule(self, param: 'Parameter', context: LaunchContext): + name, value = param.evaluate(context) + return f'{name}:={yaml.dump(value)}' + + def prepare(self, context: LaunchContext, executable: Executable) -> None: + try: + if self.__substitutions_performed: + # This function may have already been called by a subclass' `execute`, for example. + return + self.__substitutions_performed = True + cmd_ext = ['--ros-args'] # Prepend ros specific arguments with --ros-args flag + executable.cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_ext]) + if self.__node_name is not None: + self.__expanded_node_name = perform_substitutions( + context, normalize_to_list_of_substitutions(self.__node_name)) + validate_node_name(self.__expanded_node_name) + self.__expanded_node_name.lstrip('/') + expanded_node_namespace: Optional[Text] = None + if self.__node_namespace is not None: + expanded_node_namespace = perform_substitutions( + context, normalize_to_list_of_substitutions(self.__node_namespace)) + base_ns = context.launch_configurations.get('ros_namespace', None) + expanded_node_namespace = make_namespace_absolute( + prefix_namespace(base_ns, expanded_node_namespace)) + if expanded_node_namespace is not None: + self.__expanded_node_namespace = expanded_node_namespace + cmd_ext = ['-r', LocalSubstitution("ros_specific_arguments['ns']")] + executable.cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_ext]) + validate_namespace(self.__expanded_node_namespace) + except Exception: + self.__logger.error( + "Error while expanding or validating node name or namespace for '{}':" + .format('name={}, namespace={}'.format( + self.__node_name, + self.__node_namespace, + )) + ) + raise + self.__final_node_name = prefix_namespace( + self.__expanded_node_namespace, 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_arguments = [] + if global_params is not None: + param_file_path = self._create_params_file_from_dict(global_params) + self.__expanded_parameter_arguments.append((param_file_path, True)) + cmd_ext = ['--params-file', f'{param_file_path}'] + executable.cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_ext]) + assert os.path.isfile(param_file_path) + # expand parameters too + if self.__parameters is not None: + evaluated_parameters = evaluate_parameters(context, self.__parameters) + for params in evaluated_parameters: + is_file = False + if isinstance(params, dict): + param_argument = self._create_params_file_from_dict(params) + is_file = True + assert os.path.isfile(param_argument) + elif isinstance(params, pathlib.Path): + param_argument = str(params) + is_file = True + elif isinstance(params, Parameter): + param_argument = self._get_parameter_rule(params, context) + else: + raise RuntimeError('invalid normalized parameters {}'.format(repr(params))) + if is_file and not os.path.isfile(param_argument): + self.__logger.warning( + 'Parameter file path is not a file: {}'.format(param_argument), + ) + continue + self.__expanded_parameter_arguments.append((param_argument, is_file)) + cmd_ext = ['--params-file' if is_file else '-p', f'{param_argument}'] + executable.cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_ext]) + # expand remappings too + global_remaps = context.launch_configurations.get('ros_remaps', None) + if global_remaps or self.__remappings: + self.__expanded_remappings = [] + if global_remaps: + self.__expanded_remappings.extend(global_remaps) + if self.__remappings: + self.__expanded_remappings.extend([ + (perform_substitutions(context, src), perform_substitutions(context, dst)) + for src, dst in self.__remappings + ]) + if self.__expanded_remappings: + cmd_ext = [] + for src, dst in self.__expanded_remappings: + cmd_ext.extend(['-r', f'{src}:={dst}']) + executable.cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_ext]) + # Prepare the ros_specific_arguments list and add it to the context so that the + # LocalSubstitution placeholders added to the the cmd can be expanded using the contents. + ros_specific_arguments: Dict[str, Union[str, List[str]]] = {} + if self.__node_name is not None: + ros_specific_arguments['name'] = '__node:={}'.format(self.__expanded_node_name) + if self.__expanded_node_namespace != '': + ros_specific_arguments['ns'] = '__ns:={}'.format(self.__expanded_node_namespace) + context.extend_locals({'ros_specific_arguments': ros_specific_arguments}) + + if self.is_node_name_fully_specified(): + add_node_name(context, self.node_name) + node_name_count = get_node_name_count(context, self.node_name) + if node_name_count > 1: + execute_process_logger = launch.logging.get_logger(self.name) + execute_process_logger.warning( + 'there are now at least {} nodes with the name {} created within this ' + 'launch context'.format(node_name_count, self.node_name) + ) diff --git a/launch_ros/launch_ros/descriptions/node_trait.py b/launch_ros/launch_ros/descriptions/node_trait.py new file mode 100644 index 000000000..12778d4bd --- /dev/null +++ b/launch_ros/launch_ros/descriptions/node_trait.py @@ -0,0 +1,44 @@ +# Copyright 2021 Southwest Research Institute, All Rights Reserved. +# +# 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. +# +# DISTRIBUTION A. Approved for public release; distribution unlimited. +# OPSEC #4584. +# +# Delivered to the U.S. Government with Unlimited Rights, as defined in DFARS +# Part 252.227-7013 or 7014 (Feb 2014). +# +# This notice must appear in all copies of this file and its derivatives. + +"""Module for a description of a NodeTrait.""" + +from launch import Action +from launch import LaunchContext + + +class NodeTrait: + """Describes a trait of a node.""" + + def __init__(self) -> None: + """ + Initialize a NodeTrait description. + + Note that this class provides no functionality itself, and is used + as a base class for traits which provide actual functionality. As + such, the base class itself should not be directly used by application + code. + """ + pass + + def prepare(self, node, context: LaunchContext, action: Action): + """Perform any actions necessary to prepare the node for execution.""" diff --git a/launch_ros/launch_ros/descriptions/ros_executable.py b/launch_ros/launch_ros/descriptions/ros_executable.py new file mode 100644 index 000000000..107f96bc7 --- /dev/null +++ b/launch_ros/launch_ros/descriptions/ros_executable.py @@ -0,0 +1,91 @@ +# Copyright 2021 Southwest Research Institute, All Rights Reserved. +# +# 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. +# +# DISTRIBUTION A. Approved for public release; distribution unlimited. +# OPSEC #4584. +# +# Delivered to the U.S. Government with Unlimited Rights, as defined in DFARS +# Part 252.227-7013 or 7014 (Feb 2014). +# +# This notice must appear in all copies of this file and its derivatives. + +"""Module for a description of a ROS Executable.""" + +from typing import Iterable +from typing import Optional + +from launch import LaunchContext +from launch import SomeSubstitutionsType +from launch.descriptions import Executable + +from launch_ros.substitutions import ExecutableInPackage + +from ..descriptions import Node + + +class RosExecutable(Executable): + """Describes an executable with ROS features which may be run by the launch system.""" + + def __init__( + self, *, + executable: Optional[SomeSubstitutionsType] = None, + package: Optional[SomeSubstitutionsType] = None, + nodes: Iterable[Node] = None, + **kwargs + ) -> None: + """ + Initialize an Executable description. + + :param: executable the name of the executable to find if a package + is provided or otherwise a path to the executable to run. + :param: package the package in which the node executable can be found + :param: nodes the ROS node(s) included in the executable + """ + if package is not None: + cmd = [ExecutableInPackage(package=package, executable=executable)] + else: + cmd = [executable] + + self.__package = package + self.__executable = executable + self.__nodes = nodes + super().__init__(cmd=cmd, **kwargs) + + @property + def package(self): + """Getter for package.""" + return self.__package + + @property + def executable(self): + """Getter for executable.""" + return self.__executable + + @property + def nodes(self): + """Getter for nodes.""" + return self.__nodes + + def apply_context(self, context: LaunchContext): + """ + Prepare a ROS executable description for execution in a given environment. + + This does the following: + - prepares all nodes + - performs substitutions on various properties + """ + for node in self.__nodes: + node.prepare(context, self) + + super().apply_context(context) diff --git a/test_launch_ros/test/test_launch_ros/actions/test_node.py b/test_launch_ros/test/test_launch_ros/actions/test_node.py index a00485c99..0e33dee8e 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_node.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_node.py @@ -88,7 +88,7 @@ def test_launch_node_with_remappings(self): self._assert_launch_no_errors([node_action]) # Check the expanded parameters. - expanded_remappings = node_action._Node__expanded_remappings + expanded_remappings = node_action._Node__node_desc.expanded_remappings assert len(expanded_remappings) == 2 for i in range(2): assert expanded_remappings[i] == ('chatter', 'new_chatter') @@ -147,7 +147,7 @@ def test_launch_node_with_parameter_files(self): self._assert_launch_no_errors([node_action]) # Check the expanded parameters. - expanded_parameter_arguments = node_action._Node__expanded_parameter_arguments + expanded_parameter_arguments = node_action._Node__node_desc.expanded_parameter_arguments assert len(expanded_parameter_arguments) == 3 for i in range(3): assert expanded_parameter_arguments[i] == (str(parameters_file_path), True) @@ -184,7 +184,7 @@ def test_launch_node_with_parameter_descriptions(self): ) self._assert_launch_no_errors([node_action]) - expanded_parameter_arguments = node_action._Node__expanded_parameter_arguments + expanded_parameter_arguments = node_action._Node__node_desc.expanded_parameter_arguments assert len(expanded_parameter_arguments) == 5 parameters = [] for item, is_file in expanded_parameter_arguments: @@ -221,7 +221,7 @@ def test_launch_node_with_parameter_dict(self): self._assert_launch_no_errors([node_action]) # Check the expanded parameters (will be written to a file). - expanded_parameter_arguments = node_action._Node__expanded_parameter_arguments + expanded_parameter_arguments = node_action._Node__node_desc.expanded_parameter_arguments assert len(expanded_parameter_arguments) == 1 file_path, is_file = expanded_parameter_arguments[0] assert is_file diff --git a/test_launch_ros/test/test_launch_ros/actions/test_push_ros_namespace.py b/test_launch_ros/test/test_launch_ros/actions/test_push_ros_namespace.py index da37e5dbb..d04cd1544 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_push_ros_namespace.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_push_ros_namespace.py @@ -14,10 +14,13 @@ """Tests for the PushROSNamespace Action.""" +from _collections import defaultdict + from launch_ros.actions import Node from launch_ros.actions import PushROSNamespace from launch_ros.actions.load_composable_nodes import get_composable_node_load_request from launch_ros.descriptions import ComposableNode +from launch_ros.descriptions import Node as NodeDescription import pytest @@ -26,6 +29,14 @@ class MockContext: def __init__(self): self.launch_configurations = {} + self.locals = lambda: None + self.locals.unique_ros_node_names = defaultdict(int) + + def extend_globals(self, val): + pass + + def extend_locals(self, val): + pass def perform_substitution(self, sub): return sub.perform(None) @@ -118,10 +129,11 @@ def test_push_ros_namespace(config): ) node._perform_substitutions(lc) expected_ns = ( - config.expected_ns if config.expected_ns is not None else Node.UNSPECIFIED_NODE_NAMESPACE + config.expected_ns if config.expected_ns is not None else + NodeDescription.UNSPECIFIED_NODE_NAMESPACE ) expected_name = ( - config.node_name if config.node_name is not None else Node.UNSPECIFIED_NODE_NAME + config.node_name if config.node_name is not None else NodeDescription.UNSPECIFIED_NODE_NAME ) expected_fqn = expected_ns.rstrip('/') + '/' + expected_name assert expected_ns == node.expanded_node_namespace diff --git a/test_launch_ros/test/test_launch_ros/actions/test_set_parameter.py b/test_launch_ros/test/test_launch_ros/actions/test_set_parameter.py index ddf866f74..b551ed2e1 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_set_parameter.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_set_parameter.py @@ -16,6 +16,8 @@ import os.path +from _collections import defaultdict + from launch import LaunchContext from launch.actions import PopLaunchConfigurations from launch.actions import PushLaunchConfigurations @@ -36,6 +38,14 @@ class MockContext: def __init__(self): self.launch_configurations = {} + self.locals = lambda: None + self.locals.unique_ros_node_names = defaultdict(int) + + def extend_globals(self, val): + pass + + def extend_locals(self, val): + pass def perform_substitution(self, sub): return sub.perform(None) diff --git a/test_launch_ros/test/test_launch_ros/actions/test_set_remap.py b/test_launch_ros/test/test_launch_ros/actions/test_set_remap.py index a7c820a30..bafb5dea1 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_set_remap.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_set_remap.py @@ -14,6 +14,8 @@ """Tests for the SetRemap Action.""" +from _collections import defaultdict + from launch import LaunchContext from launch.actions import PopLaunchConfigurations from launch.actions import PushLaunchConfigurations @@ -30,6 +32,14 @@ class MockContext: def __init__(self): self.launch_configurations = {} + self.locals = lambda: None + self.locals.unique_ros_node_names = defaultdict(int) + + def extend_globals(self, val): + pass + + def extend_locals(self, val): + pass def perform_substitution(self, sub): return sub.perform(None) From e5ef999ed2b8c58d386f4d8ad106e19404aa7f35 Mon Sep 17 00:00:00 2001 From: Roger Strain Date: Tue, 9 Feb 2021 11:41:03 -0600 Subject: [PATCH 02/19] Refactor arguments from nodes, address feedback Distro A; OPSEC #4584 Signed-off-by: Roger Strain --- .../launch_ros/actions/lifecycle_node.py | 2 +- launch_ros/launch_ros/actions/node.py | 17 +++++++--- launch_ros/launch_ros/descriptions/node.py | 31 ++++++++++--------- .../launch_ros/descriptions/node_trait.py | 7 ++++- .../launch_ros/descriptions/ros_executable.py | 7 +++-- .../actions/test_lifecycle_node.py | 2 +- .../test/test_launch_ros/actions/test_node.py | 2 +- .../actions/test_push_ros_namespace.py | 2 +- .../test_launch_ros/actions/test_set_remap.py | 2 +- .../frontend/test_node_frontend.py | 4 +-- 10 files changed, 46 insertions(+), 30 deletions(-) diff --git a/launch_ros/launch_ros/actions/lifecycle_node.py b/launch_ros/launch_ros/actions/lifecycle_node.py index dd5cb46b2..a6a89d118 100644 --- a/launch_ros/launch_ros/actions/lifecycle_node.py +++ b/launch_ros/launch_ros/actions/lifecycle_node.py @@ -142,7 +142,7 @@ def execute(self, context: launch.LaunchContext) -> Optional[List[Action]]: Delegated to :meth:`launch.actions.ExecuteProcess.execute`. """ - self._perform_substitutions(context) # ensure self.node_name is expanded + self.prepare(context) # ensure self.node_name is expanded if '' in self.node_name: raise RuntimeError('node_name unexpectedly incomplete for lifecycle node') node = get_ros_node(context) diff --git a/launch_ros/launch_ros/actions/node.py b/launch_ros/launch_ros/actions/node.py index c22cc4481..ea2f563ef 100644 --- a/launch_ros/launch_ros/actions/node.py +++ b/launch_ros/launch_ros/actions/node.py @@ -100,6 +100,7 @@ def __init__( remappings: Optional[SomeRemapRules] = None, ros_arguments: Optional[Iterable[SomeSubstitutionsType]] = None, arguments: Optional[Iterable[SomeSubstitutionsType]] = None, + additional_env: Optional[Dict[SomeSubstitutionsType, SomeSubstitutionsType]] = None, **kwargs ) -> None: """ @@ -170,17 +171,21 @@ def __init__( passed to the node as ROS remapping rules :param: ros_arguments list of ROS arguments for the node :param: arguments list of extra arguments for the node + :param additional_env: Dictionary of environment variables to be added. If env was + None, they are added to the current environment. If not, env is updated with + additional_env. """ self.__node_desc = NodeDescription(node_name=name, node_namespace=namespace, - parameters=parameters, remappings=remappings, - arguments=arguments) + parameters=parameters, remappings=remappings) + ros_exec_kwargs = {'additional_env': additional_env, 'arguments': arguments} self.__ros_exec = RosExecutable(package=package, executable=executable, nodes=[self.__node_desc]) self.__extensions = get_extensions(self.__logger) super().__init__(process_description=self.__ros_exec, **kwargs) - def _perform_substitutions(self, lc: LaunchContext): - self.__node_desc.prepare(lc, self.__ros_exec) + def prepare(self, context: LaunchContext): + self.__node_desc.prepare(context, self.__ros_exec, self) + super().prepare(context) def is_node_name_fully_specified(self): return self.__node_desc.is_node_name_fully_specified() @@ -253,6 +258,10 @@ def get_nested_dictionary_from_nested_key_value_pairs(params): def parse(cls, entity: Entity, parser: Parser): """Parse node.""" # See parse method of `ExecuteProcess` + # Note: This class originally was a direct descendant of ExecuteProcess, + # but has been refactored to better divide the concept of a node vs an + # executable process. This class remains as a compatibility layer, and + # must hand off parsing duties to its original ancestor. _, kwargs = ExecuteProcess.parse(entity, parser, ignore=['cmd']) args = entity.get_attr('args', optional=True) if args is not None: diff --git a/launch_ros/launch_ros/descriptions/node.py b/launch_ros/launch_ros/descriptions/node.py index c0c3ecf04..ba22f4b05 100644 --- a/launch_ros/launch_ros/descriptions/node.py +++ b/launch_ros/launch_ros/descriptions/node.py @@ -35,6 +35,7 @@ from typing import Tuple from typing import Union +from launch import Action from launch import LaunchContext from launch import SomeSubstitutionsType from launch.descriptions import Executable @@ -75,7 +76,6 @@ def __init__( node_namespace: Optional[SomeSubstitutionsType] = None, parameters: Optional[SomeParameters] = None, remappings: Optional[SomeRemapRules] = None, - arguments: Optional[Iterable[SomeSubstitutionsType]] = None, traits: Optional[Iterable[NodeTrait]] = None, **kwargs ) -> None: @@ -121,7 +121,6 @@ def __init__( or dictionaries of parameters. :param: remappings ordered list of 'to' and 'from' string pairs to be passed to the node as ROS remapping rules - :param: arguments list of extra arguments for the node :param: traits list of special traits of the node """ if parameters is not None: @@ -134,7 +133,6 @@ def __init__( self.__node_namespace = node_namespace self.__parameters = [] if parameters is None else normalized_params self.__remappings = [] if remappings is None else list(normalize_remap_rules(remappings)) - self.__arguments = arguments self.__traits = traits self.__expanded_node_name = self.UNSPECIFIED_NODE_NAME @@ -169,11 +167,6 @@ def remappings(self): """Getter for remappings.""" return self.__remappings - @property - def arguments(self): - """Getter for arguments.""" - return self.__arguments - @property def traits(self): """Getter for traits.""" @@ -217,14 +210,22 @@ def _get_parameter_rule(self, param: 'Parameter', context: LaunchContext): name, value = param.evaluate(context) return f'{name}:={yaml.dump(value)}' - def prepare(self, context: LaunchContext, executable: Executable) -> None: + def prepare(self, context: LaunchContext, executable: Executable, action: Action) -> None: + self._perform_substitutions(context, executable.cmd) + + # Prepare any traits which may be defined for this node + if self.__traits is not None: + for trait in self.__traits: + trait.prepare(self, context, action) + + def _perform_substitutions(self, context: LaunchContext, cmd: List) -> None: try: if self.__substitutions_performed: # This function may have already been called by a subclass' `execute`, for example. return self.__substitutions_performed = True cmd_ext = ['--ros-args'] # Prepend ros specific arguments with --ros-args flag - executable.cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_ext]) + cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_ext]) if self.__node_name is not None: self.__expanded_node_name = perform_substitutions( context, normalize_to_list_of_substitutions(self.__node_name)) @@ -240,7 +241,7 @@ def prepare(self, context: LaunchContext, executable: Executable) -> None: if expanded_node_namespace is not None: self.__expanded_node_namespace = expanded_node_namespace cmd_ext = ['-r', LocalSubstitution("ros_specific_arguments['ns']")] - executable.cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_ext]) + cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_ext]) validate_namespace(self.__expanded_node_namespace) except Exception: self.__logger.error( @@ -262,7 +263,7 @@ def prepare(self, context: LaunchContext, executable: Executable) -> None: param_file_path = self._create_params_file_from_dict(global_params) self.__expanded_parameter_arguments.append((param_file_path, True)) cmd_ext = ['--params-file', f'{param_file_path}'] - executable.cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_ext]) + cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_ext]) assert os.path.isfile(param_file_path) # expand parameters too if self.__parameters is not None: @@ -287,7 +288,7 @@ def prepare(self, context: LaunchContext, executable: Executable) -> None: continue self.__expanded_parameter_arguments.append((param_argument, is_file)) cmd_ext = ['--params-file' if is_file else '-p', f'{param_argument}'] - executable.cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_ext]) + cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_ext]) # expand remappings too global_remaps = context.launch_configurations.get('ros_remaps', None) if global_remaps or self.__remappings: @@ -303,7 +304,7 @@ def prepare(self, context: LaunchContext, executable: Executable) -> None: cmd_ext = [] for src, dst in self.__expanded_remappings: cmd_ext.extend(['-r', f'{src}:={dst}']) - executable.cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_ext]) + cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_ext]) # Prepare the ros_specific_arguments list and add it to the context so that the # LocalSubstitution placeholders added to the the cmd can be expanded using the contents. ros_specific_arguments: Dict[str, Union[str, List[str]]] = {} @@ -321,4 +322,4 @@ def prepare(self, context: LaunchContext, executable: Executable) -> None: execute_process_logger.warning( 'there are now at least {} nodes with the name {} created within this ' 'launch context'.format(node_name_count, self.node_name) - ) + ) \ No newline at end of file diff --git a/launch_ros/launch_ros/descriptions/node_trait.py b/launch_ros/launch_ros/descriptions/node_trait.py index 12778d4bd..f30ab93b1 100644 --- a/launch_ros/launch_ros/descriptions/node_trait.py +++ b/launch_ros/launch_ros/descriptions/node_trait.py @@ -22,9 +22,14 @@ """Module for a description of a NodeTrait.""" +from typing import TYPE_CHECKING + from launch import Action from launch import LaunchContext +if TYPE_CHECKING: + from . import Node + class NodeTrait: """Describes a trait of a node.""" @@ -40,5 +45,5 @@ def __init__(self) -> None: """ pass - def prepare(self, node, context: LaunchContext, action: Action): + def prepare(self, node: 'Node', context: LaunchContext, action: Action): """Perform any actions necessary to prepare the node for execution.""" diff --git a/launch_ros/launch_ros/descriptions/ros_executable.py b/launch_ros/launch_ros/descriptions/ros_executable.py index 107f96bc7..a11410599 100644 --- a/launch_ros/launch_ros/descriptions/ros_executable.py +++ b/launch_ros/launch_ros/descriptions/ros_executable.py @@ -25,6 +25,7 @@ from typing import Iterable from typing import Optional +from launch import Action from launch import LaunchContext from launch import SomeSubstitutionsType from launch.descriptions import Executable @@ -77,7 +78,7 @@ def nodes(self): """Getter for nodes.""" return self.__nodes - def apply_context(self, context: LaunchContext): + def prepare(self, context: LaunchContext, action: Action): """ Prepare a ROS executable description for execution in a given environment. @@ -86,6 +87,6 @@ def apply_context(self, context: LaunchContext): - performs substitutions on various properties """ for node in self.__nodes: - node.prepare(context, self) + node.prepare(context, self, action) - super().apply_context(context) + super().prepare(context, action) diff --git a/test_launch_ros/test/test_launch_ros/actions/test_lifecycle_node.py b/test_launch_ros/test/test_launch_ros/actions/test_lifecycle_node.py index d195a57fe..e2f372d2a 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_lifecycle_node.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_lifecycle_node.py @@ -51,5 +51,5 @@ def test_node_name(): namespace='my_ns', ) lc = LaunchContext() - node_object._perform_substitutions(lc) + node_object._Node__node_desc._perform_substitutions(lc, []) assert node_object.is_node_name_fully_specified() is True diff --git a/test_launch_ros/test/test_launch_ros/actions/test_node.py b/test_launch_ros/test/test_launch_ros/actions/test_node.py index 0e33dee8e..f691a09b8 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_node.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_node.py @@ -356,5 +356,5 @@ def get_test_node_name_parameters(): ) def test_node_name(node_object, expected_result): lc = LaunchContext() - node_object._perform_substitutions(lc) + node_object._Node__node_desc._perform_substitutions(lc, []) assert node_object.is_node_name_fully_specified() is expected_result diff --git a/test_launch_ros/test/test_launch_ros/actions/test_push_ros_namespace.py b/test_launch_ros/test/test_launch_ros/actions/test_push_ros_namespace.py index d04cd1544..456fe02ad 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_push_ros_namespace.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_push_ros_namespace.py @@ -127,7 +127,7 @@ def test_push_ros_namespace(config): namespace=config.node_ns, name=config.node_name ) - node._perform_substitutions(lc) + node._Node__node_desc._perform_substitutions(lc, []) expected_ns = ( config.expected_ns if config.expected_ns is not None else NodeDescription.UNSPECIFIED_NODE_NAMESPACE diff --git a/test_launch_ros/test/test_launch_ros/actions/test_set_remap.py b/test_launch_ros/test/test_launch_ros/actions/test_set_remap.py index bafb5dea1..37a2c3074 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_set_remap.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_set_remap.py @@ -93,7 +93,7 @@ def test_set_remap_with_node(): ) set_remap = SetRemap('from1', 'to1') set_remap.execute(lc) - node._perform_substitutions(lc) + node._Node__node_desc._perform_substitutions(lc, []) assert len(node.expanded_remapping_rules) == 2 assert node.expanded_remapping_rules == [('from1', 'to1'), ('from2', 'to2')] 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 9ae1a7b50..414395b57 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 @@ -159,7 +159,7 @@ def check_launch_node(file): assert 0 == ls.run() evaluated_parameters = evaluate_parameters( ls.context, - ld.describe_sub_entities()[3]._Node__parameters + ld.describe_sub_entities()[3].process_description.nodes[0]._Node__parameters ) assert len(evaluated_parameters) == 3 assert isinstance(evaluated_parameters[0], dict) @@ -200,7 +200,7 @@ def check_launch_node(file): assert param_dict['param_group1.param15'] == ['2', '5', '8'] # Check remappings exist - remappings = ld.describe_sub_entities()[3]._Node__remappings + remappings = ld.describe_sub_entities()[3].process_description.nodes[0]._Node__remappings assert remappings is not None assert len(remappings) == 2 From 83c4529c0c2e097ac6585c031a40a579d7ecd990 Mon Sep 17 00:00:00 2001 From: Roger Strain Date: Wed, 3 Mar 2021 09:15:33 -0600 Subject: [PATCH 03/19] Fix missing node name parameter Distro A; OPSEC #4584 Signed-off-by: Roger Strain --- launch_ros/launch_ros/descriptions/node.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/launch_ros/launch_ros/descriptions/node.py b/launch_ros/launch_ros/descriptions/node.py index ba22f4b05..ce94f6f83 100644 --- a/launch_ros/launch_ros/descriptions/node.py +++ b/launch_ros/launch_ros/descriptions/node.py @@ -229,6 +229,8 @@ def _perform_substitutions(self, context: LaunchContext, cmd: List) -> None: if self.__node_name is not None: self.__expanded_node_name = perform_substitutions( context, normalize_to_list_of_substitutions(self.__node_name)) + cmd_ext = ['-r', LocalSubstitution("ros_specific_arguments['name']")] + cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_ext]) validate_node_name(self.__expanded_node_name) self.__expanded_node_name.lstrip('/') expanded_node_namespace: Optional[Text] = None From 87948a8ff05e72465d7eb263cb92151f8d0f95dc Mon Sep 17 00:00:00 2001 From: Roger Strain Date: Mon, 22 Mar 2021 16:49:47 -0500 Subject: [PATCH 04/19] Improve node name handling, unit test fixes, more Distro A; OPSEC #4584 Signed-off-by: Roger Strain --- launch_ros/launch_ros/actions/node.py | 9 ++--- launch_ros/launch_ros/descriptions/node.py | 34 ++++++++++++++----- .../launch_ros/descriptions/node_trait.py | 1 + .../actions/test_lifecycle_node.py | 3 +- .../test/test_launch_ros/actions/test_node.py | 15 +++++--- .../actions/test_push_ros_namespace.py | 3 +- .../actions/test_set_parameter.py | 3 +- .../test_launch_ros/actions/test_set_remap.py | 3 +- 8 files changed, 50 insertions(+), 21 deletions(-) diff --git a/launch_ros/launch_ros/actions/node.py b/launch_ros/launch_ros/actions/node.py index ea2f563ef..da87cbe3a 100644 --- a/launch_ros/launch_ros/actions/node.py +++ b/launch_ros/launch_ros/actions/node.py @@ -183,10 +183,6 @@ def __init__( self.__extensions = get_extensions(self.__logger) super().__init__(process_description=self.__ros_exec, **kwargs) - def prepare(self, context: LaunchContext): - self.__node_desc.prepare(context, self.__ros_exec, self) - super().prepare(context) - def is_node_name_fully_specified(self): return self.__node_desc.is_node_name_fully_specified() @@ -316,6 +312,11 @@ def node_name(self): """Getter for node_name.""" return self.__node_desc.node_name + @property + def ros_exec(self): + """Getter for ros_exec.""" + return self.__ros_exec + @property def expanded_node_namespace(self): """Getter for expanded_node_namespace.""" diff --git a/launch_ros/launch_ros/descriptions/node.py b/launch_ros/launch_ros/descriptions/node.py index ce94f6f83..caffa7877 100644 --- a/launch_ros/launch_ros/descriptions/node.py +++ b/launch_ros/launch_ros/descriptions/node.py @@ -74,17 +74,14 @@ def __init__( self, *, node_name: Optional[SomeSubstitutionsType] = None, node_namespace: Optional[SomeSubstitutionsType] = None, + original_node_name: Optional[SomeSubstitutionsType] = None, parameters: Optional[SomeParameters] = None, remappings: Optional[SomeRemapRules] = None, traits: Optional[Iterable[NodeTrait]] = None, **kwargs ) -> None: """ - Construct an Node description. - - Many arguments are passed eventually to - :class:`launch.actions.ExecuteProcess`, so see the documentation of - that class for additional details. + Construct a Node description. If the node name is not given (or is None) then no name is passed to the node on creation and instead the default name specified within the @@ -117,6 +114,9 @@ def __init__( :param: node_name the name of the node :param: node_namespace the ROS namespace for this Node + :param: original_node_name the name of the node before remapping; if not specified, + remappings/parameters (including node name/namespace changes) may be applied + to all nodes which share a command line executable with this one :param: parameters list of names of yaml files with parameter rules, or dictionaries of parameters. :param: remappings ordered list of 'to' and 'from' string pairs to be @@ -131,12 +131,14 @@ def __init__( self.__node_name = node_name self.__node_namespace = node_namespace + self.__original_node_name = original_node_name self.__parameters = [] if parameters is None else normalized_params self.__remappings = [] if remappings is None else list(normalize_remap_rules(remappings)) self.__traits = traits self.__expanded_node_name = self.UNSPECIFIED_NODE_NAME self.__expanded_node_namespace = self.UNSPECIFIED_NODE_NAMESPACE + self.__expanded_original_node_name = self.UNSPECIFIED_NODE_NAME self.__expanded_parameter_arguments = None # type: Optional[List[Tuple[Text, bool]]] self.__final_node_name = None # type: Optional[Text] self.__expanded_remappings = None # type: Optional[List[Tuple[Text, Text]]] @@ -157,6 +159,11 @@ def node_namespace(self): """Getter for node_namespace.""" return self.__node_namespace + @property + def original_node_name(self): + """Getter for original_node_name.""" + return self.__original_node_name + @property def parameters(self): """Getter for parameters.""" @@ -245,6 +252,10 @@ def _perform_substitutions(self, context: LaunchContext, cmd: List) -> None: cmd_ext = ['-r', LocalSubstitution("ros_specific_arguments['ns']")] cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_ext]) validate_namespace(self.__expanded_node_namespace) + if self.__original_node_name is not None: + self.__expanded_original_node_name = perform_substitutions( + context, normalize_to_list_of_substitutions(self.__original_node_name)) + self.__expanded_node_name.lstrip('/') except Exception: self.__logger.error( "Error while expanding or validating node name or namespace for '{}':" @@ -310,10 +321,17 @@ def _perform_substitutions(self, context: LaunchContext, cmd: List) -> None: # Prepare the ros_specific_arguments list and add it to the context so that the # LocalSubstitution placeholders added to the the cmd can be expanded using the contents. ros_specific_arguments: Dict[str, Union[str, List[str]]] = {} + original_name_prefix = "" + if self.__expanded_original_node_name is not self.UNSPECIFIED_NODE_NAME: + original_name_prefix = '{}:'.format(self.__expanded_original_node_name) if self.__node_name is not None: - ros_specific_arguments['name'] = '__node:={}'.format(self.__expanded_node_name) + ros_specific_arguments['name'] = '{}__node:={}'.format( + original_name_prefix, self.__expanded_node_name + ) if self.__expanded_node_namespace != '': - ros_specific_arguments['ns'] = '__ns:={}'.format(self.__expanded_node_namespace) + ros_specific_arguments['ns'] = '{}__ns:={}'.format( + original_name_prefix, self.__expanded_node_namespace + ) context.extend_locals({'ros_specific_arguments': ros_specific_arguments}) if self.is_node_name_fully_specified(): @@ -324,4 +342,4 @@ def _perform_substitutions(self, context: LaunchContext, cmd: List) -> None: execute_process_logger.warning( 'there are now at least {} nodes with the name {} created within this ' 'launch context'.format(node_name_count, self.node_name) - ) \ No newline at end of file + ) diff --git a/launch_ros/launch_ros/descriptions/node_trait.py b/launch_ros/launch_ros/descriptions/node_trait.py index f30ab93b1..ad98cc881 100644 --- a/launch_ros/launch_ros/descriptions/node_trait.py +++ b/launch_ros/launch_ros/descriptions/node_trait.py @@ -47,3 +47,4 @@ def __init__(self) -> None: def prepare(self, node: 'Node', context: LaunchContext, action: Action): """Perform any actions necessary to prepare the node for execution.""" + pass diff --git a/test_launch_ros/test/test_launch_ros/actions/test_lifecycle_node.py b/test_launch_ros/test/test_launch_ros/actions/test_lifecycle_node.py index e2f372d2a..98ca78cfc 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_lifecycle_node.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_lifecycle_node.py @@ -51,5 +51,6 @@ def test_node_name(): namespace='my_ns', ) lc = LaunchContext() - node_object._Node__node_desc._perform_substitutions(lc, []) + for node in node_object.ros_exec.nodes: + node._perform_substitutions(lc, []) assert node_object.is_node_name_fully_specified() is True diff --git a/test_launch_ros/test/test_launch_ros/actions/test_node.py b/test_launch_ros/test/test_launch_ros/actions/test_node.py index f691a09b8..e58b46766 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_node.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_node.py @@ -88,7 +88,8 @@ def test_launch_node_with_remappings(self): self._assert_launch_no_errors([node_action]) # Check the expanded parameters. - expanded_remappings = node_action._Node__node_desc.expanded_remappings + for node in node_action.ros_exec.nodes: + expanded_remappings = node.expanded_remappings assert len(expanded_remappings) == 2 for i in range(2): assert expanded_remappings[i] == ('chatter', 'new_chatter') @@ -147,7 +148,8 @@ def test_launch_node_with_parameter_files(self): self._assert_launch_no_errors([node_action]) # Check the expanded parameters. - expanded_parameter_arguments = node_action._Node__node_desc.expanded_parameter_arguments + for node in node_action.ros_exec.nodes: + expanded_parameter_arguments = node.expanded_parameter_arguments assert len(expanded_parameter_arguments) == 3 for i in range(3): assert expanded_parameter_arguments[i] == (str(parameters_file_path), True) @@ -184,7 +186,8 @@ def test_launch_node_with_parameter_descriptions(self): ) self._assert_launch_no_errors([node_action]) - expanded_parameter_arguments = node_action._Node__node_desc.expanded_parameter_arguments + for node in node_action.ros_exec.nodes: + expanded_parameter_arguments = node.expanded_parameter_arguments assert len(expanded_parameter_arguments) == 5 parameters = [] for item, is_file in expanded_parameter_arguments: @@ -221,7 +224,8 @@ def test_launch_node_with_parameter_dict(self): self._assert_launch_no_errors([node_action]) # Check the expanded parameters (will be written to a file). - expanded_parameter_arguments = node_action._Node__node_desc.expanded_parameter_arguments + for node in node_action.ros_exec.nodes: + expanded_parameter_arguments = node.expanded_parameter_arguments assert len(expanded_parameter_arguments) == 1 file_path, is_file = expanded_parameter_arguments[0] assert is_file @@ -356,5 +360,6 @@ def get_test_node_name_parameters(): ) def test_node_name(node_object, expected_result): lc = LaunchContext() - node_object._Node__node_desc._perform_substitutions(lc, []) + for node in node_object.ros_exec.nodes: + node._perform_substitutions(lc, []) assert node_object.is_node_name_fully_specified() is expected_result diff --git a/test_launch_ros/test/test_launch_ros/actions/test_push_ros_namespace.py b/test_launch_ros/test/test_launch_ros/actions/test_push_ros_namespace.py index 456fe02ad..9ab1f59cc 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_push_ros_namespace.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_push_ros_namespace.py @@ -127,7 +127,8 @@ def test_push_ros_namespace(config): namespace=config.node_ns, name=config.node_name ) - node._Node__node_desc._perform_substitutions(lc, []) + for node in node_object.ros_exec.nodes: + node._perform_substitutions(lc, []) expected_ns = ( config.expected_ns if config.expected_ns is not None else NodeDescription.UNSPECIFIED_NODE_NAMESPACE diff --git a/test_launch_ros/test/test_launch_ros/actions/test_set_parameter.py b/test_launch_ros/test/test_launch_ros/actions/test_set_parameter.py index b551ed2e1..8f2dc9df9 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_set_parameter.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_set_parameter.py @@ -119,7 +119,8 @@ def test_set_param_with_node(): ) set_param = SetParameter(name='my_param', value='my_value') set_param.execute(lc) - node._perform_substitutions(lc) + for node_instance in node.ros_exec.nodes: + node_instance._perform_substitutions(lc, []) actual_command = [perform_substitutions(lc, item) for item in node.cmd if type(item[0]) == TextSubstitution] assert actual_command.count('--params-file') == 1 diff --git a/test_launch_ros/test/test_launch_ros/actions/test_set_remap.py b/test_launch_ros/test/test_launch_ros/actions/test_set_remap.py index 37a2c3074..4cab14cc9 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_set_remap.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_set_remap.py @@ -93,7 +93,8 @@ def test_set_remap_with_node(): ) set_remap = SetRemap('from1', 'to1') set_remap.execute(lc) - node._Node__node_desc._perform_substitutions(lc, []) + for node_instance in node.ros_exec.nodes: + node_instance._perform_substitutions(lc, []) assert len(node.expanded_remapping_rules) == 2 assert node.expanded_remapping_rules == [('from1', 'to1'), ('from2', 'to2')] From 2f4937d8a158d51851e41695c5466a410bc1787c Mon Sep 17 00:00:00 2001 From: Roger Strain Date: Fri, 26 Mar 2021 14:47:55 -0500 Subject: [PATCH 05/19] Add node prefix to command line remaps Distro A; OPSEC #4584 Signed-off-by: Roger Strain --- launch_ros/launch_ros/descriptions/node.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/launch_ros/launch_ros/descriptions/node.py b/launch_ros/launch_ros/descriptions/node.py index caffa7877..45a4fd1db 100644 --- a/launch_ros/launch_ros/descriptions/node.py +++ b/launch_ros/launch_ros/descriptions/node.py @@ -215,7 +215,7 @@ def _create_params_file_from_dict(self, params): def _get_parameter_rule(self, param: 'Parameter', context: LaunchContext): name, value = param.evaluate(context) - return f'{name}:={yaml.dump(value)}' + return f'{self.__expanded_node_name}:{name}:={yaml.dump(value)}' def prepare(self, context: LaunchContext, executable: Executable, action: Action) -> None: self._perform_substitutions(context, executable.cmd) @@ -316,7 +316,7 @@ def _perform_substitutions(self, context: LaunchContext, cmd: List) -> None: if self.__expanded_remappings: cmd_ext = [] for src, dst in self.__expanded_remappings: - cmd_ext.extend(['-r', f'{src}:={dst}']) + cmd_ext.extend(['-r', f'{self.__expanded_node_name}:{src}:={dst}']) cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_ext]) # Prepare the ros_specific_arguments list and add it to the context so that the # LocalSubstitution placeholders added to the the cmd can be expanded using the contents. From b38ea738454dd0080d8a7e67d79b164fa0929af3 Mon Sep 17 00:00:00 2001 From: "matthew.lanting" Date: Mon, 8 Nov 2021 15:04:48 -0500 Subject: [PATCH 06/19] Move NodeActionExtension to descriptions/Node.py Distro A, OPSEC #4584. You may have additional rights; please see https://rosmilitary.org/faq/?category=ros-2-license Signed-off-by: matthew.lanting --- launch_ros/launch_ros/actions/node.py | 116 +-------------------- launch_ros/launch_ros/descriptions/node.py | 114 +++++++++++++++++++- 2 files changed, 114 insertions(+), 116 deletions(-) diff --git a/launch_ros/launch_ros/actions/node.py b/launch_ros/launch_ros/actions/node.py index da87cbe3a..2c908d7c8 100644 --- a/launch_ros/launch_ros/actions/node.py +++ b/launch_ros/launch_ros/actions/node.py @@ -21,11 +21,6 @@ from typing import Text # noqa: F401 from typing import Tuple # noqa: F401 -try: - import importlib.metadata as importlib_metadata -except ModuleNotFoundError: - import importlib_metadata - from launch.actions import ExecuteLocal from launch.actions import ExecuteProcess from launch.frontend import Entity @@ -43,48 +38,6 @@ from launch_ros.remap_rule_type import SomeRemapRules -class NodeActionExtension: - """ - The extension point for launch_ros node action extensions. - - The following properties must be defined: - * `NAME` (will be set to the entry point name) - - The following methods may be defined: - * `command_extension` - * `execute` - """ - - NAME = None - EXTENSION_POINT_VERSION = '0.1' - - def __init__(self): - super(NodeActionExtension, self).__init__() - plugin_support.satisfies_version(self.EXTENSION_POINT_VERSION, '^0.1') - - def prepare_for_execute(self, context, ros_specific_arguments, node_action): - """ - Perform any actions prior to the node's process being launched. - - `context` is the context within which the launch is taking place, - containing amongst other things the command line arguments provided by - the user. - - `ros_specific_arguments` is a dictionary of command line arguments that - will be passed to the executable and are specific to ROS. - - `node_action` is the Node action instance that is calling the - extension. - - This method must return a tuple of command line additions as a list of - launch.substitutions.TextSubstitution objects, and - `ros_specific_arguments` with any modifications made to it. If no - modifications are made, it should return - `[], ros_specific_arguments`. - """ - return [], ros_specific_arguments - - @expose_action('node') class Node(ExecuteLocal): """Action that executes a ROS node.""" @@ -180,7 +133,6 @@ def __init__( ros_exec_kwargs = {'additional_env': additional_env, 'arguments': arguments} self.__ros_exec = RosExecutable(package=package, executable=executable, nodes=[self.__node_desc]) - self.__extensions = get_extensions(self.__logger) super().__init__(process_description=self.__ros_exec, **kwargs) def is_node_name_fully_specified(self): @@ -300,12 +252,12 @@ def parse(cls, entity: Entity, parser: Parser): @property def node_package(self): """Getter for node_package.""" - return self.__node_exec.package + return self.__ros_exec.package @property def node_executable(self): """Getter for node_executable.""" - return self.__node_exec.executable + return self.__ros_exec.executable @property def node_name(self): @@ -326,67 +278,3 @@ def expanded_node_namespace(self): def expanded_remapping_rules(self): """Getter for expanded_remappings.""" return self.__node_desc.expanded_remappings - - -def instantiate_extension( - group_name, - extension_name, - extension_class, - extensions, - logger, - *, - unique_instance=False -): - if not unique_instance and extension_class in extensions: - return extensions[extension_name] - try: - extension_instance = extension_class() - except plugin_support.PluginException as e: # noqa: F841 - logger.warning( - f"Failed to instantiate '{group_name}' extension " - f"'{extension_name}': {e}") - return None - except Exception as e: # noqa: F841 - logger.error( - f"Failed to instantiate '{group_name}' extension " - f"'{extension_name}': {e}") - return None - if not unique_instance: - extensions[extension_name] = extension_instance - return extension_instance - - -g_entry_points_impl = None - - -def get_extensions(logger): - global g_entry_points_impl - group_name = 'launch_ros.node_action' - if g_entry_points_impl is None: - g_entry_points_impl = importlib_metadata.entry_points() - entry_points_impl = g_entry_points_impl - if hasattr(entry_points_impl, 'select'): - groups = entry_points_impl.select(group=group_name) - else: - groups = entry_points_impl.get(group_name, []) - entry_points = {} - for entry_point in groups: - entry_points[entry_point.name] = entry_point - extension_types = {} - for entry_point in entry_points: - try: - extension_type = entry_points[entry_point].load() - except Exception as e: # noqa: F841 - logger.warning(f"Failed to load entry point '{entry_points[entry_point].name}': {e}") - continue - extension_types[entry_points[entry_point].name] = extension_type - - extensions = {} - for extension_name, extension_class in extension_types.items(): - extension_instance = instantiate_extension( - group_name, extension_name, extension_class, extensions, logger) - if extension_instance is None: - continue - extension_instance.NAME = extension_name - extensions[extension_name] = extension_instance - return extensions diff --git a/launch_ros/launch_ros/descriptions/node.py b/launch_ros/launch_ros/descriptions/node.py index 45a4fd1db..c758af43f 100644 --- a/launch_ros/launch_ros/descriptions/node.py +++ b/launch_ros/launch_ros/descriptions/node.py @@ -35,6 +35,11 @@ from typing import Tuple from typing import Union +try: + import importlib.metadata as importlib_metadata +except ModuleNotFoundError: + import importlib_metadata + from launch import Action from launch import LaunchContext from launch import SomeSubstitutionsType @@ -52,6 +57,7 @@ from launch_ros.utilities import normalize_parameters from launch_ros.utilities import normalize_remap_rules from launch_ros.utilities import prefix_namespace +from launch_ros.utilities import plugin_support from rclpy.validate_namespace import validate_namespace from rclpy.validate_node_name import validate_node_name @@ -64,6 +70,48 @@ from ..remap_rule_type import SomeRemapRules +class NodeActionExtension: + """ + The extension point for launch_ros node action extensions. + + The following properties must be defined: + * `NAME` (will be set to the entry point name) + + The following methods may be defined: + * `command_extension` + * `execute` + """ + + NAME = None + EXTENSION_POINT_VERSION = '0.1' + + def __init__(self): + super(NodeActionExtension, self).__init__() + plugin_support.satisfies_version(self.EXTENSION_POINT_VERSION, '^0.1') + + def prepare_for_execute(self, context, ros_specific_arguments, node_action): + """ + Perform any actions prior to the node's process being launched. + + `context` is the context within which the launch is taking place, + containing amongst other things the command line arguments provided by + the user. + + `ros_specific_arguments` is a dictionary of command line arguments that + will be passed to the executable and are specific to ROS. + + `node_action` is the Node action instance that is calling the + extension. + + This method must return a tuple of command line additions as a list of + launch.substitutions.TextSubstitution objects, and + `ros_specific_arguments` with any modifications made to it. If no + modifications are made, it should return + `[], ros_specific_arguments`. + """ + return [], ros_specific_arguments + + class Node: """Describes a ROS node.""" @@ -146,6 +194,7 @@ def __init__( self.__substitutions_performed = False self.__logger = launch.logging.get_logger(__name__) + self.__extensions = get_extensions(self.__logger) @property def node_name(self): @@ -218,14 +267,14 @@ def _get_parameter_rule(self, param: 'Parameter', context: LaunchContext): return f'{self.__expanded_node_name}:{name}:={yaml.dump(value)}' def prepare(self, context: LaunchContext, executable: Executable, action: Action) -> None: - self._perform_substitutions(context, executable.cmd) + self._perform_substitutions(context, executable.cmd, action) # Prepare any traits which may be defined for this node if self.__traits is not None: for trait in self.__traits: trait.prepare(self, context, action) - def _perform_substitutions(self, context: LaunchContext, cmd: List) -> None: + def _perform_substitutions(self, context: LaunchContext, cmd: List, action: Action) -> None: try: if self.__substitutions_performed: # This function may have already been called by a subclass' `execute`, for example. @@ -332,6 +381,14 @@ def _perform_substitutions(self, context: LaunchContext, cmd: List) -> None: ros_specific_arguments['ns'] = '{}__ns:={}'.format( original_name_prefix, self.__expanded_node_namespace ) + # Give extensions a chance to prepare for execution + for extension in self.__extensions.values(): + cmd_extension, ros_specific_arguments = extension.prepare_for_execute( + context, + ros_specific_arguments, + action + ) + cmd.extend(cmd_extension) context.extend_locals({'ros_specific_arguments': ros_specific_arguments}) if self.is_node_name_fully_specified(): @@ -343,3 +400,56 @@ def _perform_substitutions(self, context: LaunchContext, cmd: List) -> None: 'there are now at least {} nodes with the name {} created within this ' 'launch context'.format(node_name_count, self.node_name) ) + + +def instantiate_extension( + group_name, + extension_name, + extension_class, + extensions, + logger, + *, + unique_instance=False +): + if not unique_instance and extension_class in extensions: + return extensions[extension_name] + try: + extension_instance = extension_class() + except plugin_support.PluginException as e: # noqa: F841 + logger.warning( + f"Failed to instantiate '{group_name}' extension " + f"'{extension_name}': {e}") + return None + except Exception as e: # noqa: F841 + logger.error( + f"Failed to instantiate '{group_name}' extension " + f"'{extension_name}': {e}") + return None + if not unique_instance: + extensions[extension_name] = extension_instance + return extension_instance + + +def get_extensions(logger): + group_name = 'launch_ros.node_action' + entry_points = {} + for entry_point in importlib_metadata.entry_points().get(group_name, []): + entry_points[entry_point.name] = entry_point + extension_types = {} + for entry_point in entry_points: + try: + extension_type = entry_points[entry_point].load() + except Exception as e: # noqa: F841 + logger.warning(f"Failed to load entry point '{entry_point.name}': {e}") + continue + extension_types[entry_points[entry_point].name] = extension_type + + extensions = {} + for extension_name, extension_class in extension_types.items(): + extension_instance = instantiate_extension( + group_name, extension_name, extension_class, extensions, logger) + if extension_instance is None: + continue + extension_instance.NAME = extension_name + extensions[extension_name] = extension_instance + return extensions From 4be19e5fa13b89f1592fb83dfd9cf983182ef449 Mon Sep 17 00:00:00 2001 From: "matthew.lanting" Date: Fri, 19 Nov 2021 14:50:25 -0500 Subject: [PATCH 07/19] Use node_description and ros_executable instead of node_action. Distro A, OPSEC #4584. You may have additional rights; please see https://rosmilitary.org/faq/?category=ros-2-license Signed-off-by: matthew.lanting --- launch_ros/launch_ros/descriptions/node.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/launch_ros/launch_ros/descriptions/node.py b/launch_ros/launch_ros/descriptions/node.py index c758af43f..4be9855c4 100644 --- a/launch_ros/launch_ros/descriptions/node.py +++ b/launch_ros/launch_ros/descriptions/node.py @@ -267,14 +267,15 @@ def _get_parameter_rule(self, param: 'Parameter', context: LaunchContext): return f'{self.__expanded_node_name}:{name}:={yaml.dump(value)}' def prepare(self, context: LaunchContext, executable: Executable, action: Action) -> None: - self._perform_substitutions(context, executable.cmd, action) + self._perform_substitutions(context, executable) # Prepare any traits which may be defined for this node if self.__traits is not None: for trait in self.__traits: trait.prepare(self, context, action) - def _perform_substitutions(self, context: LaunchContext, cmd: List, action: Action) -> None: + def _perform_substitutions(self, context: LaunchContext, executable: Executable) -> None: + cmd = executable.cmd try: if self.__substitutions_performed: # This function may have already been called by a subclass' `execute`, for example. @@ -386,7 +387,8 @@ def _perform_substitutions(self, context: LaunchContext, cmd: List, action: Acti cmd_extension, ros_specific_arguments = extension.prepare_for_execute( context, ros_specific_arguments, - action + executable, + self ) cmd.extend(cmd_extension) context.extend_locals({'ros_specific_arguments': ros_specific_arguments}) From 9ad74b527c0e0b2181c5ca669b52708f4676d456 Mon Sep 17 00:00:00 2001 From: "matthew.lanting" Date: Mon, 22 Nov 2021 13:44:53 -0500 Subject: [PATCH 08/19] Updated function signature and accompanying documentation. Distro A, OPSEC #4584. You may have additional rights; please see https://rosmilitary.org/faq/?category=ros-2-license Signed-off-by: matthew.lanting --- launch_ros/launch_ros/descriptions/node.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/launch_ros/launch_ros/descriptions/node.py b/launch_ros/launch_ros/descriptions/node.py index 4be9855c4..6c13d1d20 100644 --- a/launch_ros/launch_ros/descriptions/node.py +++ b/launch_ros/launch_ros/descriptions/node.py @@ -89,7 +89,7 @@ def __init__(self): super(NodeActionExtension, self).__init__() plugin_support.satisfies_version(self.EXTENSION_POINT_VERSION, '^0.1') - def prepare_for_execute(self, context, ros_specific_arguments, node_action): + def prepare_for_execute(self, context, ros_specific_arguments, ros_executable, node_description): """ Perform any actions prior to the node's process being launched. @@ -100,7 +100,10 @@ def prepare_for_execute(self, context, ros_specific_arguments, node_action): `ros_specific_arguments` is a dictionary of command line arguments that will be passed to the executable and are specific to ROS. - `node_action` is the Node action instance that is calling the + `ros_executable` is the RosExecutable description instance that the node + is a part of. + + `node_description` is the Node description instance that is calling the extension. This method must return a tuple of command line additions as a list of From f1ae02050f7b7a236105dc9c87548253d366f3c5 Mon Sep 17 00:00:00 2001 From: "matthew.lanting" Date: Fri, 17 Dec 2021 11:10:03 -0500 Subject: [PATCH 09/19] WIP Fixes for failing tests. Distro A, OPSEC #4584. You may have additional rights; please see https://rosmilitary.org/faq/?category=ros-2-license Signed-off-by: matthew.lanting --- launch_ros/launch_ros/actions/node.py | 7 +++--- launch_ros/launch_ros/descriptions/node.py | 15 +++++++++---- .../launch_ros/descriptions/ros_executable.py | 7 +++++- .../actions/test_lifecycle_node.py | 2 +- .../test/test_launch_ros/actions/test_node.py | 12 +++++----- .../actions/test_push_ros_namespace.py | 4 ++-- .../actions/test_set_parameter.py | 4 ++-- .../actions/test_set_parameters_from_file.py | 22 +++++++++++++++---- .../test_launch_ros/actions/test_set_remap.py | 2 +- 9 files changed, 50 insertions(+), 25 deletions(-) diff --git a/launch_ros/launch_ros/actions/node.py b/launch_ros/launch_ros/actions/node.py index 2c908d7c8..8de98eb62 100644 --- a/launch_ros/launch_ros/actions/node.py +++ b/launch_ros/launch_ros/actions/node.py @@ -28,7 +28,6 @@ from launch.frontend import Parser from launch.frontend.type_utils import get_data_type_from_identifier -from launch.launch_context import LaunchContext from launch.some_substitutions_type import SomeSubstitutionsType from launch_ros.descriptions import Node as NodeDescription @@ -130,15 +129,15 @@ def __init__( """ self.__node_desc = NodeDescription(node_name=name, node_namespace=namespace, parameters=parameters, remappings=remappings) - ros_exec_kwargs = {'additional_env': additional_env, 'arguments': arguments} + ros_exec_kwargs = {'additional_env': additional_env} self.__ros_exec = RosExecutable(package=package, executable=executable, - nodes=[self.__node_desc]) + arguments=arguments, ros_arguments=ros_arguments, + nodes=[self.__node_desc], **ros_exec_kwargs) super().__init__(process_description=self.__ros_exec, **kwargs) def is_node_name_fully_specified(self): return self.__node_desc.is_node_name_fully_specified() - @staticmethod def parse_nested_parameters(params, parser): """Normalize parameters as expected by Node constructor argument.""" diff --git a/launch_ros/launch_ros/descriptions/node.py b/launch_ros/launch_ros/descriptions/node.py index 6c13d1d20..1e384f02b 100644 --- a/launch_ros/launch_ros/descriptions/node.py +++ b/launch_ros/launch_ros/descriptions/node.py @@ -56,8 +56,8 @@ from launch_ros.utilities import make_namespace_absolute from launch_ros.utilities import normalize_parameters from launch_ros.utilities import normalize_remap_rules -from launch_ros.utilities import prefix_namespace from launch_ros.utilities import plugin_support +from launch_ros.utilities import prefix_namespace from rclpy.validate_namespace import validate_namespace from rclpy.validate_node_name import validate_node_name @@ -89,7 +89,13 @@ def __init__(self): super(NodeActionExtension, self).__init__() plugin_support.satisfies_version(self.EXTENSION_POINT_VERSION, '^0.1') - def prepare_for_execute(self, context, ros_specific_arguments, ros_executable, node_description): + def prepare_for_execute( + self, + context, + ros_specific_arguments, + ros_executable, + node_description + ): """ Perform any actions prior to the node's process being launched. @@ -271,7 +277,7 @@ def _get_parameter_rule(self, param: 'Parameter', context: LaunchContext): def prepare(self, context: LaunchContext, executable: Executable, action: Action) -> None: self._perform_substitutions(context, executable) - + # Prepare any traits which may be defined for this node if self.__traits is not None: for trait in self.__traits: @@ -374,7 +380,7 @@ def _perform_substitutions(self, context: LaunchContext, executable: Executable) # Prepare the ros_specific_arguments list and add it to the context so that the # LocalSubstitution placeholders added to the the cmd can be expanded using the contents. ros_specific_arguments: Dict[str, Union[str, List[str]]] = {} - original_name_prefix = "" + original_name_prefix = '' if self.__expanded_original_node_name is not self.UNSPECIFIED_NODE_NAME: original_name_prefix = '{}:'.format(self.__expanded_original_node_name) if self.__node_name is not None: @@ -386,6 +392,7 @@ def _perform_substitutions(self, context: LaunchContext, executable: Executable) original_name_prefix, self.__expanded_node_namespace ) # Give extensions a chance to prepare for execution + cmd_extension = [] for extension in self.__extensions.values(): cmd_extension, ros_specific_arguments = extension.prepare_for_execute( context, diff --git a/launch_ros/launch_ros/descriptions/ros_executable.py b/launch_ros/launch_ros/descriptions/ros_executable.py index a11410599..62cd84db5 100644 --- a/launch_ros/launch_ros/descriptions/ros_executable.py +++ b/launch_ros/launch_ros/descriptions/ros_executable.py @@ -43,6 +43,8 @@ def __init__( executable: Optional[SomeSubstitutionsType] = None, package: Optional[SomeSubstitutionsType] = None, nodes: Iterable[Node] = None, + ros_arguments: Optional[Iterable[SomeSubstitutionsType]] = None, + arguments: Optional[Iterable[SomeSubstitutionsType]] = None, **kwargs ) -> None: """ @@ -58,10 +60,13 @@ def __init__( else: cmd = [executable] + if ros_arguments is not None: + arguments += ['--ros-args'] + ros_arguments + self.__package = package self.__executable = executable self.__nodes = nodes - super().__init__(cmd=cmd, **kwargs) + super().__init__(cmd=cmd, arguments=arguments, **kwargs) @property def package(self): diff --git a/test_launch_ros/test/test_launch_ros/actions/test_lifecycle_node.py b/test_launch_ros/test/test_launch_ros/actions/test_lifecycle_node.py index 98ca78cfc..ba6379b34 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_lifecycle_node.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_lifecycle_node.py @@ -52,5 +52,5 @@ def test_node_name(): ) lc = LaunchContext() for node in node_object.ros_exec.nodes: - node._perform_substitutions(lc, []) + node._perform_substitutions(lc, node_object.ros_exec) assert node_object.is_node_name_fully_specified() is True diff --git a/test_launch_ros/test/test_launch_ros/actions/test_node.py b/test_launch_ros/test/test_launch_ros/actions/test_node.py index e58b46766..81467fa6a 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_node.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_node.py @@ -195,11 +195,11 @@ def test_launch_node_with_parameter_descriptions(self): name, value = item.split(':=') parameters.append((name, yaml.safe_load(value))) assert parameters == [ - ('param1', 'param1_value'), - ('param2', ['param2', '_value']), - ('param_group1.list_params', [1.2, 3.4]), - ('param_group1.param_group2.param2_values', ['param2_value']), - ('param3', ''), + ('my_node:param1', 'param1_value'), + ('my_node:param2', ['param2', '_value']), + ('my_node:param_group1.list_params', [1.2, 3.4]), + ('my_node:param_group1.param_group2.param2_values', ['param2_value']), + ('my_node:param3', ''), ] def test_launch_node_with_parameter_dict(self): @@ -361,5 +361,5 @@ def get_test_node_name_parameters(): def test_node_name(node_object, expected_result): lc = LaunchContext() for node in node_object.ros_exec.nodes: - node._perform_substitutions(lc, []) + node._perform_substitutions(lc, node_object.ros_exec) assert node_object.is_node_name_fully_specified() is expected_result diff --git a/test_launch_ros/test/test_launch_ros/actions/test_push_ros_namespace.py b/test_launch_ros/test/test_launch_ros/actions/test_push_ros_namespace.py index 9ab1f59cc..7917252d7 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_push_ros_namespace.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_push_ros_namespace.py @@ -121,14 +121,14 @@ def test_push_ros_namespace(config): if config.second_push_ns is not None: pns2 = PushROSNamespace(config.second_push_ns) pns2.execute(lc) - node = Node( + node_object = Node( package='dont_care', executable='whatever', namespace=config.node_ns, name=config.node_name ) for node in node_object.ros_exec.nodes: - node._perform_substitutions(lc, []) + node._perform_substitutions(lc, node_object.ros_exec) expected_ns = ( config.expected_ns if config.expected_ns is not None else NodeDescription.UNSPECIFIED_NODE_NAMESPACE diff --git a/test_launch_ros/test/test_launch_ros/actions/test_set_parameter.py b/test_launch_ros/test/test_launch_ros/actions/test_set_parameter.py index 8f2dc9df9..51aa6a5b6 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_set_parameter.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_set_parameter.py @@ -120,9 +120,9 @@ def test_set_param_with_node(): set_param = SetParameter(name='my_param', value='my_value') set_param.execute(lc) for node_instance in node.ros_exec.nodes: - node_instance._perform_substitutions(lc, []) + node_instance._perform_substitutions(lc, node.ros_exec) actual_command = [perform_substitutions(lc, item) for item in - node.cmd if type(item[0]) == TextSubstitution] + node.ros_exec.cmd if type(item[0]) == TextSubstitution] assert actual_command.count('--params-file') == 1 assert actual_command.count('-p') == 1 diff --git a/test_launch_ros/test/test_launch_ros/actions/test_set_parameters_from_file.py b/test_launch_ros/test/test_launch_ros/actions/test_set_parameters_from_file.py index acb3a5f24..460a330d3 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_set_parameters_from_file.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_set_parameters_from_file.py @@ -16,6 +16,8 @@ import os +from _collections import defaultdict + from launch import LaunchContext from launch.actions import PopLaunchConfigurations from launch.actions import PushLaunchConfigurations @@ -34,6 +36,14 @@ class MockContext: def __init__(self): self.launch_configurations = {} + self.locals = lambda: None + self.locals.unique_ros_node_names = defaultdict(int) + + def extend_globals(self, val): + pass + + def extend_locals(self, val): + pass def perform_substitution(self, sub): return sub.perform(None) @@ -74,14 +84,18 @@ def test_set_param_with_node(): set_param_single = SetParameter(name='my_param', value='my_value') set_param_file.execute(lc) set_param_single.execute(lc) - node_1._perform_substitutions(lc) - node_2._perform_substitutions(lc) + # node_1._perform_substitutions(lc) + for node_description in node_1.ros_exec.nodes: + node_description._perform_substitutions(lc, node_1.ros_exec) + # node_2._perform_substitutions(lc) + for node_description in node_2.ros_exec.nodes: + node_description._perform_substitutions(lc, node_2.ros_exec) actual_command_1 = [perform_substitutions(lc, item) for item in - node_1.cmd if type(item[0]) == TextSubstitution] + node_1.ros_exec.cmd if type(item[0]) == TextSubstitution] actual_command_2 = [perform_substitutions(lc, item) for item in - node_2.cmd if type(item[0]) == TextSubstitution] + node_2.ros_exec.cmd if type(item[0]) == TextSubstitution] assert actual_command_1[3] == '--params-file' assert os.path.isfile(actual_command_1[4]) diff --git a/test_launch_ros/test/test_launch_ros/actions/test_set_remap.py b/test_launch_ros/test/test_launch_ros/actions/test_set_remap.py index 4cab14cc9..98fa34c11 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_set_remap.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_set_remap.py @@ -94,7 +94,7 @@ def test_set_remap_with_node(): set_remap = SetRemap('from1', 'to1') set_remap.execute(lc) for node_instance in node.ros_exec.nodes: - node_instance._perform_substitutions(lc, []) + node_instance._perform_substitutions(lc, node.ros_exec) assert len(node.expanded_remapping_rules) == 2 assert node.expanded_remapping_rules == [('from1', 'to1'), ('from2', 'to2')] From 5f4ac9f99656eb64c861d6ff675e4cdb411da7b5 Mon Sep 17 00:00:00 2001 From: "matthew.lanting" Date: Thu, 6 Jan 2022 13:51:36 -0500 Subject: [PATCH 10/19] Change "ros_params" string to "global_params" when getting params from context in descriptions/node.py. Distro A, OPSEC #4584. You may have additional rights; please see https://rosmilitary.org/faq/?category=ros-2-license Signed-off-by: matthew.lanting --- launch_ros/launch_ros/descriptions/node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch_ros/launch_ros/descriptions/node.py b/launch_ros/launch_ros/descriptions/node.py index 1e384f02b..93efdf152 100644 --- a/launch_ros/launch_ros/descriptions/node.py +++ b/launch_ros/launch_ros/descriptions/node.py @@ -328,7 +328,7 @@ def _perform_substitutions(self, context: LaunchContext, executable: Executable) self.__expanded_node_namespace, 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) + global_params = context.launch_configurations.get('global_params', None) if global_params is not None or self.__parameters is not None: self.__expanded_parameter_arguments = [] if global_params is not None: From 1bb350ea25a81e16ae327c7f3798e3112aeb6a09 Mon Sep 17 00:00:00 2001 From: "matthew.lanting" Date: Wed, 12 Jan 2022 15:28:18 -0500 Subject: [PATCH 11/19] More unit test fixes. Distro A, OPSEC #4584. You may have additional rights; please see https://rosmilitary.org/faq/?category=ros-2-license Signed-off-by: matthew.lanting --- launch_ros/launch_ros/actions/node.py | 4 +-- launch_ros/launch_ros/descriptions/node.py | 32 ++++++++++++------- .../frontend/test_component_container.py | 10 +++--- 3 files changed, 28 insertions(+), 18 deletions(-) diff --git a/launch_ros/launch_ros/actions/node.py b/launch_ros/launch_ros/actions/node.py index 8de98eb62..3a4d33e7c 100644 --- a/launch_ros/launch_ros/actions/node.py +++ b/launch_ros/launch_ros/actions/node.py @@ -212,10 +212,10 @@ def parse(cls, entity: Entity, parser: Parser): _, kwargs = ExecuteProcess.parse(entity, parser, ignore=['cmd']) args = entity.get_attr('args', optional=True) if args is not None: - kwargs['arguments'] = super()._parse_cmdline(args, parser) + kwargs['arguments'] = ExecuteProcess._parse_cmdline(args, parser) ros_args = entity.get_attr('ros_args', optional=True) if ros_args is not None: - kwargs['ros_arguments'] = super()._parse_cmdline(ros_args, parser) + kwargs['ros_arguments'] = ExecuteProcess._parse_cmdline(ros_args, parser) node_name = entity.get_attr('node-name', optional=True) if node_name is not None: kwargs['node_name'] = parser.parse_substitution(node_name) diff --git a/launch_ros/launch_ros/descriptions/node.py b/launch_ros/launch_ros/descriptions/node.py index 93efdf152..3e5aa2d0d 100644 --- a/launch_ros/launch_ros/descriptions/node.py +++ b/launch_ros/launch_ros/descriptions/node.py @@ -326,17 +326,27 @@ def _perform_substitutions(self, context: LaunchContext, executable: Executable) raise self.__final_node_name = prefix_namespace( self.__expanded_node_namespace, 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('global_params', None) - if global_params is not None or self.__parameters is not None: + + # Expand global parameters first, + # so they can be overridden with specific parameters of this Node + # The params_container list is expected to contain name-value pairs (tuples) + # and/or strings representing paths to parameter files. + params_container = context.launch_configurations.get('global_params', None) + + if any(x is not None for x in (params_container, self.__parameters)): self.__expanded_parameter_arguments = [] - if global_params is not None: - param_file_path = self._create_params_file_from_dict(global_params) - self.__expanded_parameter_arguments.append((param_file_path, True)) - cmd_ext = ['--params-file', f'{param_file_path}'] - cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_ext]) - assert os.path.isfile(param_file_path) + if params_container is not None: + for param in params_container: + if isinstance(param, tuple): + name, value = param + cmd_extension = ['-p', f'{name}:={value}'] + cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_extension]) + else: + param_file_path = os.path.abspath(param) + self.__expanded_parameter_arguments.append((param_file_path, True)) + cmd_extension = ['--params-file', f'{param_file_path}'] + assert os.path.isfile(param_file_path) + cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_extension]) # expand parameters too if self.__parameters is not None: evaluated_parameters = evaluate_parameters(context, self.__parameters) @@ -407,7 +417,7 @@ def _perform_substitutions(self, context: LaunchContext, executable: Executable) add_node_name(context, self.node_name) node_name_count = get_node_name_count(context, self.node_name) if node_name_count > 1: - execute_process_logger = launch.logging.get_logger(self.name) + execute_process_logger = launch.logging.get_logger(self.node_name) execute_process_logger.warning( 'there are now at least {} nodes with the name {} created within this ' 'launch context'.format(node_name_count, self.node_name) diff --git a/test_launch_ros/test/test_launch_ros/frontend/test_component_container.py b/test_launch_ros/test/test_launch_ros/frontend/test_component_container.py index 0ed304852..dbc77c038 100644 --- a/test_launch_ros/test/test_launch_ros/frontend/test_component_container.py +++ b/test_launch_ros/test/test_launch_ros/frontend/test_component_container.py @@ -114,11 +114,11 @@ def perform(substitution): return perform_substitutions(ls.context, substitution) # Check container params - assert perform(node_container._Node__package) == 'rclcpp_components' - assert perform(node_container._Node__node_executable) == 'component_container' - assert perform(node_container._Node__node_name) == 'my_container' - assert perform(node_container._Node__node_namespace) == '' - assert perform(node_container._Node__arguments[0]) == 'test_args' + assert perform(node_container.node_package) == 'rclcpp_components' + assert perform(node_container.node_executable) == 'component_container' + assert perform(node_container._Node__node_desc._Node__node_name) == 'my_container' + assert perform(node_container._Node__node_desc._Node__node_namespace) == '' + assert perform(node_container._Node__ros_exec._Executable__arguments[0]) == 'test_args' assert perform(load_composable_node._LoadComposableNodes__target_container) == 'my_container' From 29788c466236087c06cbd8076dfb0127eeb539a6 Mon Sep 17 00:00:00 2001 From: "matthew.lanting" Date: Thu, 20 Jan 2022 14:52:35 -0500 Subject: [PATCH 12/19] More unit test fixes. Distro A, OPSEC #4584. You may have additional rights; please see https://rosmilitary.org/faq/?category=ros-2-license Signed-off-by: matthew.lanting --- launch_ros/launch_ros/descriptions/node.py | 4 ++-- .../test/test_launch_ros/actions/test_node.py | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/launch_ros/launch_ros/descriptions/node.py b/launch_ros/launch_ros/descriptions/node.py index 3e5aa2d0d..8271b6195 100644 --- a/launch_ros/launch_ros/descriptions/node.py +++ b/launch_ros/launch_ros/descriptions/node.py @@ -273,7 +273,7 @@ def _create_params_file_from_dict(self, params): def _get_parameter_rule(self, param: 'Parameter', context: LaunchContext): name, value = param.evaluate(context) - return f'{self.__expanded_node_name}:{name}:={yaml.dump(value)}' + return f'{name}:={yaml.dump(value)}' def prepare(self, context: LaunchContext, executable: Executable, action: Action) -> None: self._perform_substitutions(context, executable) @@ -385,7 +385,7 @@ def _perform_substitutions(self, context: LaunchContext, executable: Executable) if self.__expanded_remappings: cmd_ext = [] for src, dst in self.__expanded_remappings: - cmd_ext.extend(['-r', f'{self.__expanded_node_name}:{src}:={dst}']) + cmd_ext.extend(['-r', f'{src}:={dst}']) cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_ext]) # Prepare the ros_specific_arguments list and add it to the context so that the # LocalSubstitution placeholders added to the the cmd can be expanded using the contents. diff --git a/test_launch_ros/test/test_launch_ros/actions/test_node.py b/test_launch_ros/test/test_launch_ros/actions/test_node.py index 81467fa6a..89e563ac1 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_node.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_node.py @@ -195,11 +195,11 @@ def test_launch_node_with_parameter_descriptions(self): name, value = item.split(':=') parameters.append((name, yaml.safe_load(value))) assert parameters == [ - ('my_node:param1', 'param1_value'), - ('my_node:param2', ['param2', '_value']), - ('my_node:param_group1.list_params', [1.2, 3.4]), - ('my_node:param_group1.param_group2.param2_values', ['param2_value']), - ('my_node:param3', ''), + ('param1', 'param1_value'), + ('param2', ['param2', '_value']), + ('param_group1.list_params', [1.2, 3.4]), + ('param_group1.param_group2.param2_values', ['param2_value']), + ('param3', ''), ] def test_launch_node_with_parameter_dict(self): From 43f19122958ca018e839578065a9c52dfbe26c2d Mon Sep 17 00:00:00 2001 From: "matthew.lanting" Date: Thu, 20 Jan 2022 14:56:37 -0500 Subject: [PATCH 13/19] Increase tolerance for timer tests from 0.1 seconds to 0.5 seconds Distro A, OPSEC #4584. You may have additional rights; please see https://rosmilitary.org/faq/?category=ros-2-license Signed-off-by: matthew.lanting --- test_launch_ros/test/test_launch_ros/actions/test_ros_timer.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test_launch_ros/test/test_launch_ros/actions/test_ros_timer.py b/test_launch_ros/test/test_launch_ros/actions/test_ros_timer.py index b1f6dc976..82ac57891 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_ros_timer.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_ros_timer.py @@ -195,6 +195,6 @@ def timer_callback(publisher, time_msg): # Timer is using sim time which is 100x faster than system time, # so 200 sec timer should finish in 2 sec - tolerance = 0.1 + tolerance = 0.5 assert (end_time - start_time) > 2 - tolerance assert (end_time - start_time) < 2 + tolerance From b9531dd4a0f267d5e545b23725dc90f7f08f430a Mon Sep 17 00:00:00 2001 From: "matthew.lanting" Date: Fri, 18 Feb 2022 10:53:28 -0500 Subject: [PATCH 14/19] removed license language to match aggreed upon format Distro A, OPSEC #4584. You may have additional rights; please see https://rosmilitary.org/faq/?category=ros-2-license Signed-off-by: matthew.lanting --- launch_ros/launch_ros/descriptions/node.py | 8 -------- launch_ros/launch_ros/descriptions/node_trait.py | 8 -------- launch_ros/launch_ros/descriptions/ros_executable.py | 8 -------- 3 files changed, 24 deletions(-) diff --git a/launch_ros/launch_ros/descriptions/node.py b/launch_ros/launch_ros/descriptions/node.py index 8271b6195..eb9f41281 100644 --- a/launch_ros/launch_ros/descriptions/node.py +++ b/launch_ros/launch_ros/descriptions/node.py @@ -11,14 +11,6 @@ # 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. -# -# DISTRIBUTION A. Approved for public release; distribution unlimited. -# OPSEC #4584. -# -# Delivered to the U.S. Government with Unlimited Rights, as defined in DFARS -# Part 252.227-7013 or 7014 (Feb 2014). -# -# This notice must appear in all copies of this file and its derivatives. """Module for a description of a Node.""" diff --git a/launch_ros/launch_ros/descriptions/node_trait.py b/launch_ros/launch_ros/descriptions/node_trait.py index ad98cc881..c566ce033 100644 --- a/launch_ros/launch_ros/descriptions/node_trait.py +++ b/launch_ros/launch_ros/descriptions/node_trait.py @@ -11,14 +11,6 @@ # 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. -# -# DISTRIBUTION A. Approved for public release; distribution unlimited. -# OPSEC #4584. -# -# Delivered to the U.S. Government with Unlimited Rights, as defined in DFARS -# Part 252.227-7013 or 7014 (Feb 2014). -# -# This notice must appear in all copies of this file and its derivatives. """Module for a description of a NodeTrait.""" diff --git a/launch_ros/launch_ros/descriptions/ros_executable.py b/launch_ros/launch_ros/descriptions/ros_executable.py index 62cd84db5..d32380430 100644 --- a/launch_ros/launch_ros/descriptions/ros_executable.py +++ b/launch_ros/launch_ros/descriptions/ros_executable.py @@ -11,14 +11,6 @@ # 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. -# -# DISTRIBUTION A. Approved for public release; distribution unlimited. -# OPSEC #4584. -# -# Delivered to the U.S. Government with Unlimited Rights, as defined in DFARS -# Part 252.227-7013 or 7014 (Feb 2014). -# -# This notice must appear in all copies of this file and its derivatives. """Module for a description of a ROS Executable.""" From d5b83d58012dcc082dfe0b47d20e7c0a626f6bc9 Mon Sep 17 00:00:00 2001 From: "matthew.lanting" Date: Tue, 8 Mar 2022 14:16:13 -0500 Subject: [PATCH 15/19] Move Executable specific keyword args to kwargs instead of duplicating parameters between classes. Distro A, OPSEC #4584. You may have additional rights; please see https://rosmilitary.org/faq/?category=ros-2-license Signed-off-by: matthew.lanting --- launch_ros/launch_ros/actions/node.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/launch_ros/launch_ros/actions/node.py b/launch_ros/launch_ros/actions/node.py index 3a4d33e7c..5f258d2c5 100644 --- a/launch_ros/launch_ros/actions/node.py +++ b/launch_ros/launch_ros/actions/node.py @@ -14,6 +14,8 @@ """Module for the Node action.""" +import inspect + from typing import Dict from typing import Iterable from typing import List @@ -23,6 +25,7 @@ from launch.actions import ExecuteLocal from launch.actions import ExecuteProcess +from launch.descriptions import Executable from launch.frontend import Entity from launch.frontend import expose_action from launch.frontend import Parser @@ -123,13 +126,11 @@ def __init__( passed to the node as ROS remapping rules :param: ros_arguments list of ROS arguments for the node :param: arguments list of extra arguments for the node - :param additional_env: Dictionary of environment variables to be added. If env was - None, they are added to the current environment. If not, env is updated with - additional_env. """ self.__node_desc = NodeDescription(node_name=name, node_namespace=namespace, parameters=parameters, remappings=remappings) - ros_exec_kwargs = {'additional_env': additional_env} + executable_keywords = inspect.signature(Executable).parameters.keys() + ros_exec_kwargs = {key: val for key, val in kwargs.items() if key in executable_keywords} self.__ros_exec = RosExecutable(package=package, executable=executable, arguments=arguments, ros_arguments=ros_arguments, nodes=[self.__node_desc], **ros_exec_kwargs) From 4b45bc5c705a26f90894eb68476d2e6b0ae98946 Mon Sep 17 00:00:00 2001 From: "matthew.lanting" Date: Wed, 9 Mar 2022 12:53:34 -0500 Subject: [PATCH 16/19] Remove 'node_' prefix from members and arguments. Distro A, OPSEC #4584. You may have additional rights; please see https://rosmilitary.org/faq/?category=ros-2-license Signed-off-by: matthew.lanting --- .../launch_ros/actions/lifecycle_node.py | 12 +- .../actions/load_composable_nodes.py | 8 +- launch_ros/launch_ros/actions/node.py | 38 ++--- .../descriptions/composable_node.py | 16 +-- launch_ros/launch_ros/descriptions/node.py | 132 +++++++++--------- .../actions/test_lifecycle_node.py | 2 +- .../test/test_launch_ros/actions/test_node.py | 2 +- .../actions/test_push_ros_namespace.py | 4 +- .../frontend/test_component_container.py | 16 +-- 9 files changed, 115 insertions(+), 115 deletions(-) diff --git a/launch_ros/launch_ros/actions/lifecycle_node.py b/launch_ros/launch_ros/actions/lifecycle_node.py index a6a89d118..59cd12010 100644 --- a/launch_ros/launch_ros/actions/lifecycle_node.py +++ b/launch_ros/launch_ros/actions/lifecycle_node.py @@ -123,7 +123,7 @@ def unblock(future): self.__logger.error( "Failed to make transition '{}' for LifecycleNode '{}'".format( ChangeState.valid_transitions[request.transition.id], - self.node_name, + self.name, ) ) @@ -142,20 +142,20 @@ def execute(self, context: launch.LaunchContext) -> Optional[List[Action]]: Delegated to :meth:`launch.actions.ExecuteProcess.execute`. """ - self.prepare(context) # ensure self.node_name is expanded - if '' in self.node_name: - raise RuntimeError('node_name unexpectedly incomplete for lifecycle node') + self.prepare(context) # ensure self.name is expanded + if '' in self.name: + raise RuntimeError('name unexpectedly incomplete for lifecycle node') node = get_ros_node(context) # Create a subscription to monitor the state changes of the subprocess. self.__rclpy_subscription = node.create_subscription( lifecycle_msgs.msg.TransitionEvent, - '{}/transition_event'.format(self.node_name), + '{}/transition_event'.format(self.name), functools.partial(self._on_transition_event, context), 10) # Create a service client to change state on demand. self.__rclpy_change_state_client = node.create_client( lifecycle_msgs.srv.ChangeState, - '{}/change_state'.format(self.node_name)) + '{}/change_state'.format(self.name)) # Register an event handler to change states on a ChangeState lifecycle event. context.register_event_handler(launch.EventHandler( matcher=lambda event: isinstance(event, ChangeState), diff --git a/launch_ros/launch_ros/actions/load_composable_nodes.py b/launch_ros/launch_ros/actions/load_composable_nodes.py index fda356a29..e1278d5fc 100644 --- a/launch_ros/launch_ros/actions/load_composable_nodes.py +++ b/launch_ros/launch_ros/actions/load_composable_nodes.py @@ -210,7 +210,7 @@ def execute( # resolve target container node name if is_a_subclass(self.__target_container, ComposableNodeContainer): - self.__final_target_container_name = self.__target_container.node_name + self.__final_target_container_name = self.__target_container.name elif isinstance(self.__target_container, SomeSubstitutionsType_types_tuple): subs = normalize_to_list_of_substitutions(self.__target_container) self.__final_target_container_name = perform_substitutions( @@ -261,11 +261,11 @@ def get_composable_node_load_request( request.plugin_name = perform_substitutions( context, composable_node_description.node_plugin ) - if composable_node_description.node_name is not None: + if composable_node_description.name is not None: request.node_name = perform_substitutions( - context, composable_node_description.node_name + context, composable_node_description.name ) - expanded_ns = composable_node_description.node_namespace + expanded_ns = composable_node_description.namespace if expanded_ns is not None: expanded_ns = perform_substitutions(context, expanded_ns) base_ns = context.launch_configurations.get('ros_namespace', None) diff --git a/launch_ros/launch_ros/actions/node.py b/launch_ros/launch_ros/actions/node.py index 5f258d2c5..9e010f176 100644 --- a/launch_ros/launch_ros/actions/node.py +++ b/launch_ros/launch_ros/actions/node.py @@ -127,7 +127,7 @@ def __init__( :param: ros_arguments list of ROS arguments for the node :param: arguments list of extra arguments for the node """ - self.__node_desc = NodeDescription(node_name=name, node_namespace=namespace, + self.__node_desc = NodeDescription(name=name, namespace=namespace, parameters=parameters, remappings=remappings) executable_keywords = inspect.signature(Executable).parameters.keys() ros_exec_kwargs = {key: val for key, val in kwargs.items() if key in executable_keywords} @@ -136,8 +136,8 @@ def __init__( nodes=[self.__node_desc], **ros_exec_kwargs) super().__init__(process_description=self.__ros_exec, **kwargs) - def is_node_name_fully_specified(self): - return self.__node_desc.is_node_name_fully_specified() + def is_name_fully_specified(self): + return self.__node_desc.is_name_fully_specified() @staticmethod def parse_nested_parameters(params, parser): @@ -217,12 +217,12 @@ def parse(cls, entity: Entity, parser: Parser): ros_args = entity.get_attr('ros_args', optional=True) if ros_args is not None: kwargs['ros_arguments'] = ExecuteProcess._parse_cmdline(ros_args, parser) - node_name = entity.get_attr('node-name', optional=True) - if node_name is not None: - kwargs['node_name'] = parser.parse_substitution(node_name) - node_name = entity.get_attr('name', optional=True) - if node_name is not None: - kwargs['name'] = parser.parse_substitution(node_name) + name = entity.get_attr('node-name', optional=True) + if name is not None: + kwargs['name'] = parser.parse_substitution(name) + name = entity.get_attr('name', optional=True) + if name is not None: + kwargs['name'] = parser.parse_substitution(name) exec_name = entity.get_attr('exec_name', optional=True) if exec_name is not None: kwargs['exec_name'] = parser.parse_substitution(exec_name) @@ -250,19 +250,19 @@ def parse(cls, entity: Entity, parser: Parser): return cls, kwargs @property - def node_package(self): - """Getter for node_package.""" + def package(self): + """Getter for package.""" return self.__ros_exec.package @property - def node_executable(self): - """Getter for node_executable.""" + def executable(self): + """Getter for executable.""" return self.__ros_exec.executable @property - def node_name(self): - """Getter for node_name.""" - return self.__node_desc.node_name + def name(self): + """Getter for name.""" + return self.__node_desc.name @property def ros_exec(self): @@ -270,9 +270,9 @@ def ros_exec(self): return self.__ros_exec @property - def expanded_node_namespace(self): - """Getter for expanded_node_namespace.""" - return self.__node_desc.expanded_node_namespace + def expanded_namespace(self): + """Getter for expanded_namespace.""" + return self.__node_desc.expanded_namespace @property def expanded_remapping_rules(self): diff --git a/launch_ros/launch_ros/descriptions/composable_node.py b/launch_ros/launch_ros/descriptions/composable_node.py index 8ad35c5ce..9f4bdca21 100644 --- a/launch_ros/launch_ros/descriptions/composable_node.py +++ b/launch_ros/launch_ros/descriptions/composable_node.py @@ -62,13 +62,13 @@ def __init__( self.__package = normalize_to_list_of_substitutions(package) self.__node_plugin = normalize_to_list_of_substitutions(plugin) - self.__node_name = None # type: Optional[List[Substitution]] + self.__name = None # type: Optional[List[Substitution]] if name is not None: - self.__node_name = normalize_to_list_of_substitutions(name) + self.__name = normalize_to_list_of_substitutions(name) - self.__node_namespace = None # type: Optional[List[Substitution]] + self.__namespace = None # type: Optional[List[Substitution]] if namespace is not None: - self.__node_namespace = normalize_to_list_of_substitutions(namespace) + self.__namespace = normalize_to_list_of_substitutions(namespace) self.__parameters = None # type: Optional[Parameters] if parameters is not None: @@ -154,14 +154,14 @@ def node_plugin(self) -> List[Substitution]: return self.__node_plugin @property - def node_name(self) -> Optional[List[Substitution]]: + def name(self) -> Optional[List[Substitution]]: """Get node name as a sequence of substitutions to be performed.""" - return self.__node_name + return self.__name @property - def node_namespace(self) -> Optional[List[Substitution]]: + def namespace(self) -> Optional[List[Substitution]]: """Get node namespace as a sequence of substitutions to be performed.""" - return self.__node_namespace + return self.__namespace @property def parameters(self) -> Optional[Parameters]: diff --git a/launch_ros/launch_ros/descriptions/node.py b/launch_ros/launch_ros/descriptions/node.py index eb9f41281..8038b1a41 100644 --- a/launch_ros/launch_ros/descriptions/node.py +++ b/launch_ros/launch_ros/descriptions/node.py @@ -121,9 +121,9 @@ class Node: def __init__( self, *, - node_name: Optional[SomeSubstitutionsType] = None, - node_namespace: Optional[SomeSubstitutionsType] = None, - original_node_name: Optional[SomeSubstitutionsType] = None, + name: Optional[SomeSubstitutionsType] = None, + namespace: Optional[SomeSubstitutionsType] = None, + original_name: Optional[SomeSubstitutionsType] = None, parameters: Optional[SomeParameters] = None, remappings: Optional[SomeRemapRules] = None, traits: Optional[Iterable[NodeTrait]] = None, @@ -161,9 +161,9 @@ def __init__( passed in in order to the node (where the last definition of a parameter takes effect). - :param: node_name the name of the node - :param: node_namespace the ROS namespace for this Node - :param: original_node_name the name of the node before remapping; if not specified, + :param: name the name of the node + :param: namespace the ROS namespace for this Node + :param: original_name the name of the node before remapping; if not specified, remappings/parameters (including node name/namespace changes) may be applied to all nodes which share a command line executable with this one :param: parameters list of names of yaml files with parameter rules, @@ -178,18 +178,18 @@ def __init__( # evaluate to paths), or dictionaries of parameters (fields can be substitutions). normalized_params = normalize_parameters(parameters) - self.__node_name = node_name - self.__node_namespace = node_namespace - self.__original_node_name = original_node_name + self.__name = name + self.__namespace = namespace + self.__original_name = original_name self.__parameters = [] if parameters is None else normalized_params self.__remappings = [] if remappings is None else list(normalize_remap_rules(remappings)) self.__traits = traits - self.__expanded_node_name = self.UNSPECIFIED_NODE_NAME - self.__expanded_node_namespace = self.UNSPECIFIED_NODE_NAMESPACE - self.__expanded_original_node_name = self.UNSPECIFIED_NODE_NAME + self.__expanded_name = self.UNSPECIFIED_NODE_NAME + self.__expanded_namespace = self.UNSPECIFIED_NODE_NAMESPACE + self.__expanded_original_name = self.UNSPECIFIED_NODE_NAME self.__expanded_parameter_arguments = None # type: Optional[List[Tuple[Text, bool]]] - self.__final_node_name = None # type: Optional[Text] + self.__final_name = None # type: Optional[Text] self.__expanded_remappings = None # type: Optional[List[Tuple[Text, Text]]] self.__substitutions_performed = False @@ -198,21 +198,21 @@ def __init__( self.__extensions = get_extensions(self.__logger) @property - def node_name(self): - """Getter for node_name.""" - if self.__final_node_name is None: - raise RuntimeError("cannot access 'node_name' before executing action") - return self.__final_node_name + def name(self): + """Getter for name.""" + if self.__final_name is None: + raise RuntimeError("cannot access 'name' before executing action") + return self.__final_name @property - def node_namespace(self): - """Getter for node_namespace.""" - return self.__node_namespace + def namespace(self): + """Getter for namespace.""" + return self.__namespace @property - def original_node_name(self): - """Getter for original_node_name.""" - return self.__original_node_name + def original_name(self): + """Getter for original_name.""" + return self.__original_name @property def parameters(self): @@ -230,14 +230,14 @@ def traits(self): return self.__traits @property - def expanded_node_name(self): - """Getter for expanded_node_name.""" - return self.__expanded_node_name + def expanded_name(self): + """Getter for expanded_name.""" + return self.__expanded_name @property - def expanded_node_namespace(self): - """Getter for expanded_node_namespace.""" - return self.__expanded_node_namespace + def expanded_namespace(self): + """Getter for expanded_namespace.""" + return self.__expanded_namespace @property def expanded_parameter_arguments(self): @@ -249,15 +249,15 @@ def expanded_remappings(self): """Getter for expanded_remappings.""" return self.__expanded_remappings - def is_node_name_fully_specified(self): + def is_name_fully_specified(self): keywords = (self.UNSPECIFIED_NODE_NAME, self.UNSPECIFIED_NODE_NAMESPACE) - return all(x not in self.node_name for x in keywords) + return all(x not in self.name for x in keywords) def _create_params_file_from_dict(self, params): with NamedTemporaryFile(mode='w', prefix='launch_params_', delete=False) as h: param_file_path = h.name param_dict = { - self.node_name if self.is_node_name_fully_specified() else '/**': + self.name if self.is_name_fully_specified() else '/**': {'ros__parameters': params} } yaml.dump(param_dict, h, default_flow_style=False) @@ -284,40 +284,40 @@ def _perform_substitutions(self, context: LaunchContext, executable: Executable) self.__substitutions_performed = True cmd_ext = ['--ros-args'] # Prepend ros specific arguments with --ros-args flag cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_ext]) - if self.__node_name is not None: - self.__expanded_node_name = perform_substitutions( - context, normalize_to_list_of_substitutions(self.__node_name)) + if self.__name is not None: + self.__expanded_name = perform_substitutions( + context, normalize_to_list_of_substitutions(self.__name)) cmd_ext = ['-r', LocalSubstitution("ros_specific_arguments['name']")] cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_ext]) - validate_node_name(self.__expanded_node_name) - self.__expanded_node_name.lstrip('/') - expanded_node_namespace: Optional[Text] = None - if self.__node_namespace is not None: - expanded_node_namespace = perform_substitutions( - context, normalize_to_list_of_substitutions(self.__node_namespace)) + validate_node_name(self.__expanded_name) + self.__expanded_name.lstrip('/') + expanded_namespace: Optional[Text] = None + if self.__namespace is not None: + expanded_namespace = perform_substitutions( + context, normalize_to_list_of_substitutions(self.__namespace)) base_ns = context.launch_configurations.get('ros_namespace', None) - expanded_node_namespace = make_namespace_absolute( - prefix_namespace(base_ns, expanded_node_namespace)) - if expanded_node_namespace is not None: - self.__expanded_node_namespace = expanded_node_namespace + expanded_namespace = make_namespace_absolute( + prefix_namespace(base_ns, expanded_namespace)) + if expanded_namespace is not None: + self.__expanded_namespace = expanded_namespace cmd_ext = ['-r', LocalSubstitution("ros_specific_arguments['ns']")] cmd.extend([normalize_to_list_of_substitutions(x) for x in cmd_ext]) - validate_namespace(self.__expanded_node_namespace) - if self.__original_node_name is not None: - self.__expanded_original_node_name = perform_substitutions( - context, normalize_to_list_of_substitutions(self.__original_node_name)) - self.__expanded_node_name.lstrip('/') + validate_namespace(self.__expanded_namespace) + if self.__original_name is not None: + self.__expanded_original_name = perform_substitutions( + context, normalize_to_list_of_substitutions(self.__original_name)) + self.__expanded_name.lstrip('/') except Exception: self.__logger.error( "Error while expanding or validating node name or namespace for '{}':" .format('name={}, namespace={}'.format( - self.__node_name, - self.__node_namespace, + self.__name, + self.__namespace, )) ) raise - self.__final_node_name = prefix_namespace( - self.__expanded_node_namespace, self.__expanded_node_name) + self.__final_name = prefix_namespace( + self.__expanded_namespace, self.__expanded_name) # Expand global parameters first, # so they can be overridden with specific parameters of this Node @@ -383,15 +383,15 @@ def _perform_substitutions(self, context: LaunchContext, executable: Executable) # LocalSubstitution placeholders added to the the cmd can be expanded using the contents. ros_specific_arguments: Dict[str, Union[str, List[str]]] = {} original_name_prefix = '' - if self.__expanded_original_node_name is not self.UNSPECIFIED_NODE_NAME: - original_name_prefix = '{}:'.format(self.__expanded_original_node_name) - if self.__node_name is not None: + if self.__expanded_original_name is not self.UNSPECIFIED_NODE_NAME: + original_name_prefix = '{}:'.format(self.__expanded_original_name) + if self.__name is not None: ros_specific_arguments['name'] = '{}__node:={}'.format( - original_name_prefix, self.__expanded_node_name + original_name_prefix, self.__expanded_name ) - if self.__expanded_node_namespace != '': + if self.__expanded_namespace != '': ros_specific_arguments['ns'] = '{}__ns:={}'.format( - original_name_prefix, self.__expanded_node_namespace + original_name_prefix, self.__expanded_namespace ) # Give extensions a chance to prepare for execution cmd_extension = [] @@ -405,14 +405,14 @@ def _perform_substitutions(self, context: LaunchContext, executable: Executable) cmd.extend(cmd_extension) context.extend_locals({'ros_specific_arguments': ros_specific_arguments}) - if self.is_node_name_fully_specified(): - add_node_name(context, self.node_name) - node_name_count = get_node_name_count(context, self.node_name) + if self.is_name_fully_specified(): + add_node_name(context, self.name) + node_name_count = get_node_name_count(context, self.name) if node_name_count > 1: - execute_process_logger = launch.logging.get_logger(self.node_name) + execute_process_logger = launch.logging.get_logger(self.name) execute_process_logger.warning( 'there are now at least {} nodes with the name {} created within this ' - 'launch context'.format(node_name_count, self.node_name) + 'launch context'.format(node_name_count, self.name) ) diff --git a/test_launch_ros/test/test_launch_ros/actions/test_lifecycle_node.py b/test_launch_ros/test/test_launch_ros/actions/test_lifecycle_node.py index ba6379b34..22b27410d 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_lifecycle_node.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_lifecycle_node.py @@ -53,4 +53,4 @@ def test_node_name(): lc = LaunchContext() for node in node_object.ros_exec.nodes: node._perform_substitutions(lc, node_object.ros_exec) - assert node_object.is_node_name_fully_specified() is True + assert node_object.is_name_fully_specified() is True diff --git a/test_launch_ros/test/test_launch_ros/actions/test_node.py b/test_launch_ros/test/test_launch_ros/actions/test_node.py index 89e563ac1..808c71bee 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_node.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_node.py @@ -362,4 +362,4 @@ def test_node_name(node_object, expected_result): lc = LaunchContext() for node in node_object.ros_exec.nodes: node._perform_substitutions(lc, node_object.ros_exec) - assert node_object.is_node_name_fully_specified() is expected_result + assert node_object.is_name_fully_specified() is expected_result diff --git a/test_launch_ros/test/test_launch_ros/actions/test_push_ros_namespace.py b/test_launch_ros/test/test_launch_ros/actions/test_push_ros_namespace.py index 7917252d7..d377dbd41 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_push_ros_namespace.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_push_ros_namespace.py @@ -137,8 +137,8 @@ def test_push_ros_namespace(config): config.node_name if config.node_name is not None else NodeDescription.UNSPECIFIED_NODE_NAME ) expected_fqn = expected_ns.rstrip('/') + '/' + expected_name - assert expected_ns == node.expanded_node_namespace - assert expected_fqn == node.node_name + assert expected_ns == node.expanded_namespace + assert expected_fqn == node.name @pytest.mark.parametrize('config', get_test_cases()) diff --git a/test_launch_ros/test/test_launch_ros/frontend/test_component_container.py b/test_launch_ros/test/test_launch_ros/frontend/test_component_container.py index dbc77c038..88f80954d 100644 --- a/test_launch_ros/test/test_launch_ros/frontend/test_component_container.py +++ b/test_launch_ros/test/test_launch_ros/frontend/test_component_container.py @@ -114,10 +114,10 @@ def perform(substitution): return perform_substitutions(ls.context, substitution) # Check container params - assert perform(node_container.node_package) == 'rclcpp_components' - assert perform(node_container.node_executable) == 'component_container' - assert perform(node_container._Node__node_desc._Node__node_name) == 'my_container' - assert perform(node_container._Node__node_desc._Node__node_namespace) == '' + assert perform(node_container.package) == 'rclcpp_components' + assert perform(node_container.executable) == 'component_container' + assert perform(node_container._Node__node_desc._Node__name) == 'my_container' + assert perform(node_container._Node__node_desc._Node__namespace) == '' assert perform(node_container._Node__ros_exec._Executable__arguments[0]) == 'test_args' assert perform(load_composable_node._LoadComposableNodes__target_container) == 'my_container' @@ -135,16 +135,16 @@ def perform(substitution): assert perform(talker._ComposableNode__package) == 'composition' assert perform(talker._ComposableNode__node_plugin) == 'composition::Talker' - assert perform(talker._ComposableNode__node_name) == 'talker' - assert perform(talker._ComposableNode__node_namespace) == 'test_namespace' + assert perform(talker._ComposableNode__name) == 'talker' + assert perform(talker._ComposableNode__namespace) == 'test_namespace' assert (perform(talker_remappings[0][0]), perform(talker_remappings[0][1])) == ('chatter', '/remap/chatter') assert talker_params[0]['use_sim_time'] is True assert perform(listener._ComposableNode__package) == 'composition' assert perform(listener._ComposableNode__node_plugin) == 'composition::Listener' - assert perform(listener._ComposableNode__node_name) == 'listener' - assert perform(listener._ComposableNode__node_namespace) == 'test_namespace' + assert perform(listener._ComposableNode__name) == 'listener' + assert perform(listener._ComposableNode__namespace) == 'test_namespace' assert (perform(listener_remappings[0][0]), perform(listener_remappings[0][1])) == ('chatter', '/remap/chatter') assert listener_params[0]['use_sim_time'] is True From 846295c66b826cfc3330987d2443cce5789f5c37 Mon Sep 17 00:00:00 2001 From: "matthew.lanting" Date: Wed, 9 Mar 2022 15:02:35 -0500 Subject: [PATCH 17/19] extend instead of assign expanded_parameter_arguments in tests. Distro A, OPSEC #4584. You may have additional rights; please see https://rosmilitary.org/faq/?category=ros-2-license Signed-off-by: matthew.lanting --- test_launch_ros/test/test_launch_ros/actions/test_node.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/test_launch_ros/test/test_launch_ros/actions/test_node.py b/test_launch_ros/test/test_launch_ros/actions/test_node.py index 808c71bee..acc4fd073 100644 --- a/test_launch_ros/test/test_launch_ros/actions/test_node.py +++ b/test_launch_ros/test/test_launch_ros/actions/test_node.py @@ -186,8 +186,9 @@ def test_launch_node_with_parameter_descriptions(self): ) self._assert_launch_no_errors([node_action]) + expanded_parameter_arguments = [] for node in node_action.ros_exec.nodes: - expanded_parameter_arguments = node.expanded_parameter_arguments + expanded_parameter_arguments.extend(node.expanded_parameter_arguments) assert len(expanded_parameter_arguments) == 5 parameters = [] for item, is_file in expanded_parameter_arguments: @@ -224,8 +225,9 @@ def test_launch_node_with_parameter_dict(self): self._assert_launch_no_errors([node_action]) # Check the expanded parameters (will be written to a file). + expanded_parameter_arguments = [] for node in node_action.ros_exec.nodes: - expanded_parameter_arguments = node.expanded_parameter_arguments + expanded_parameter_arguments.extend(node.expanded_parameter_arguments) assert len(expanded_parameter_arguments) == 1 file_path, is_file = expanded_parameter_arguments[0] assert is_file From 32409afd08f238d41cb6d6293345097645712906 Mon Sep 17 00:00:00 2001 From: "veronica.knisley" Date: Mon, 18 Sep 2023 10:52:26 -0500 Subject: [PATCH 18/19] Resolve SelectableGroups deprecation warning, using approach used to resolve https://github.com/ros2/ros2/issues/1254 by clalancette. Signed-off-by: veronica.knisley --- launch_ros/launch_ros/descriptions/node.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/launch_ros/launch_ros/descriptions/node.py b/launch_ros/launch_ros/descriptions/node.py index 8038b1a41..8ae681093 100644 --- a/launch_ros/launch_ros/descriptions/node.py +++ b/launch_ros/launch_ros/descriptions/node.py @@ -446,8 +446,13 @@ def instantiate_extension( def get_extensions(logger): group_name = 'launch_ros.node_action' + entry_points_impl = importlib_metadata.entry_points() + if hasattr(entry_points_impl, 'select'): + groups = entry_points_impl.select(group=group_name) + else: + groups = entry_points_impl.get(group_name, []) entry_points = {} - for entry_point in importlib_metadata.entry_points().get(group_name, []): + for entry_point in groups: entry_points[entry_point.name] = entry_point extension_types = {} for entry_point in entry_points: From 6e95e60c1094f290cbe15a19d8ee69c5b49363f2 Mon Sep 17 00:00:00 2001 From: "veronica.knisley" Date: Mon, 18 Sep 2023 11:11:24 -0500 Subject: [PATCH 19/19] Resolve arguments typing issue Signed-off-by: veronica.knisley --- launch_ros/launch_ros/descriptions/ros_executable.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/launch_ros/launch_ros/descriptions/ros_executable.py b/launch_ros/launch_ros/descriptions/ros_executable.py index d32380430..ade0f5cfd 100644 --- a/launch_ros/launch_ros/descriptions/ros_executable.py +++ b/launch_ros/launch_ros/descriptions/ros_executable.py @@ -53,7 +53,11 @@ def __init__( cmd = [executable] if ros_arguments is not None: - arguments += ['--ros-args'] + ros_arguments + if arguments is not None: + arguments.append('--ros-args') + else: + arguments = ['--ros-args'] + arguments.extend(ros_arguments) self.__package = package self.__executable = executable