Fix ComposableNode ignoring PushRosNamespace actions (#162)

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

Author: ivanpauno

launch_ros/launch_ros/actions/load_composable_nodes.py

@@ -38,6 +38,8 @@
 from ..utilities import add_node_name
 from ..utilities import evaluate_parameters
 from ..utilities import get_node_name_count
+from ..utilities import make_namespace_absolute
+from ..utilities import prefix_namespace
 from ..utilities import to_parameters_list
 from ..utilities.normalize_parameters import normalize_parameter_dict
 
@@ -190,10 +192,13 @@ def get_composable_node_load_request(
         request.node_name = perform_substitutions(
             context, composable_node_description.node_name
         )
-    if composable_node_description.node_namespace is not None:
-        request.node_namespace = perform_substitutions(
-            context, composable_node_description.node_namespace
-        )
+    expanded_ns = composable_node_description.node_namespace
+    if expanded_ns is not None:
+        expanded_ns = perform_substitutions(context, expanded_ns)
+    base_ns = context.launch_configurations.get('ros_namespace', None)
+    combined_ns = make_namespace_absolute(prefix_namespace(base_ns, expanded_ns))
+    if combined_ns is not None:
+        request.node_namespace = combined_ns
     # request.log_level = perform_substitutions(context, node_description.log_level)
     if composable_node_description.remappings is not None:
         for from_, to in composable_node_description.remappings:

launch_ros/launch_ros/actions/node.py

@@ -48,8 +48,10 @@
 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
@@ -343,24 +345,15 @@ def _perform_substitutions(self, context: LaunchContext) -> None:
                     context, normalize_to_list_of_substitutions(self.__node_name))
                 validate_node_name(self.__expanded_node_name)
             self.__expanded_node_name.lstrip('/')
-            expanded_node_namespace = None
+            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)
-            if base_ns is not None or expanded_node_namespace is not None:
-                if expanded_node_namespace is None:
-                    expanded_node_namespace = ''
-                if base_ns is None:
-                    base_ns = ''
-                if not expanded_node_namespace.startswith('/'):
-                    expanded_node_namespace = (
-                        base_ns + '/' + expanded_node_namespace
-                    ).rstrip('/')
-                if not expanded_node_namespace.startswith('/'):
-                    expanded_node_namespace = '/' + expanded_node_namespace
-                self.__expanded_node_namespace = expanded_node_namespace
+            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)
@@ -375,10 +368,8 @@ def _perform_substitutions(self, context: LaunchContext) -> None:
                 ))
             )
             raise
-        self.__final_node_name = ''
-        if self.__expanded_node_namespace != '/':
-            self.__final_node_name += self.__expanded_node_namespace
-        self.__final_node_name += '/' + self.__expanded_node_name
+        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)

launch_ros/launch_ros/actions/push_ros_namespace.py

@@ -27,6 +27,9 @@
 from launch.utilities import normalize_to_list_of_substitutions
 from launch.utilities import perform_substitutions
 
+from launch_ros.utilities import make_namespace_absolute
+from launch_ros.utilities import prefix_namespace
+
 from rclpy.validate_namespace import validate_namespace
 
 
@@ -63,19 +66,16 @@ def namespace(self) -> List[Substitution]:
     def execute(self, context: LaunchContext):
         """Execute the action."""
         pushed_namespace = perform_substitutions(context, self.namespace)
-        previous_namespace = context.launch_configurations.get('ros_namespace', '')
-        namespace = pushed_namespace
-        if not pushed_namespace.startswith('/'):
-            namespace = previous_namespace + '/' + pushed_namespace
-        namespace = namespace.rstrip('/')
-        if namespace != '':
-            try:
-                validate_namespace(namespace)
-            except Exception:
-                raise SubstitutionFailure(
-                    'The resulting namespace is invalid:'
-                    " [previous_namespace='{}', pushed_namespace='{}']".format(
-                        previous_namespace, pushed_namespace
-                    )
+        previous_namespace = context.launch_configurations.get('ros_namespace', None)
+        namespace = make_namespace_absolute(
+            prefix_namespace(previous_namespace, pushed_namespace))
+        try:
+            validate_namespace(namespace)
+        except Exception:
+            raise SubstitutionFailure(
+                'The resulting namespace is invalid:'
+                " [previous_namespace='{}', pushed_namespace='{}']".format(
+                    previous_namespace, pushed_namespace
                 )
+            )
         context.launch_configurations['ros_namespace'] = namespace

launch_ros/launch_ros/utilities/__init__.py

@@ -19,6 +19,10 @@
 """
 
 from .evaluate_parameters import evaluate_parameters
+from .namespace_utils import is_namespace_absolute
+from .namespace_utils import is_root_namespace
+from .namespace_utils import make_namespace_absolute
+from .namespace_utils import prefix_namespace
 from .normalize_parameters import normalize_parameters
 from .normalize_remap_rule import normalize_remap_rule
 from .normalize_remap_rule import normalize_remap_rules
@@ -31,9 +35,13 @@
     'evaluate_parameters',
     'evaluate_parameters_dict',
     'get_node_name_count',
+    'is_namespace_absolute',
+    'is_root_namespace',
+    'make_namespace_absolute',
     'normalize_parameters',
     'normalize_parameters_dict',
     'normalize_remap_rule',
     'normalize_remap_rules',
+    'prefix_namespace',
     'to_parameters_list',
 ]

launch_ros/launch_ros/utilities/namespace_utils.py

@@ -0,0 +1,95 @@
+# Copyright 2020 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Module with utility functions for handling namespaces."""
+
+
+from typing import Optional
+from typing import Text
+from typing import Type
+from typing import TypeVar
+
+
+def is_root_namespace(ns: Text) -> bool:
+    """Return `True` if `ns` is `'/'`."""
+    return ns == '/'
+
+
+def is_namespace_absolute(ns: Text) -> bool:
+    """Return `True` if `ns` is absolute."""
+    return ns.startswith('/')
+
+
+def prefix_namespace(
+    base_ns: Optional[Text],
+    ns: Optional[Text],
+) -> Optional[Text]:
+    """
+    Return `ns` prefixed with `base_ns` if `ns` is relative, return `ns` if not.
+
+    :param `base_ns`: Prefix to be added to `ns`.
+    :param `ns`: Namespace to be prefixed.
+    :return: `None` if both `base_ns` and `ns` are `None`, or
+        `base_ns` if `ns` is `None`, or
+        `ns` if `base_ns` is `None`, or
+        `ns` if `ns` is absolute, or
+        `ns` prefixed with `base_ns`.
+        In all cases, trailing `/` are stripped from the result.
+
+    ## Examples:
+
+    ```python3
+    combined_ns = prefix_namespace('my_ns', 'original_ns')
+    assert combined_ns == 'my_ns/original_ns'
+
+    combined_ns = prefix_namespace('/my_ns', 'original_ns')
+    assert combined_ns == '/my_ns/original_ns'
+
+    combined_ns = prefix_namespace('my_ns', '/original_ns')
+    assert combined_ns == '/original_ns'
+
+    combined_ns = prefix_namespace(None, 'original_ns')
+    assert combined_ns == 'original_ns'
+
+    combined_ns = prefix_namespace('my_ns', None)
+    assert combined_ns == 'my_ns'
+    ```
+    """
+    combined_ns: Optional[Text] = None
+    if base_ns is not None or ns is not None:
+        if ns is None:
+            combined_ns = base_ns
+        elif not base_ns or is_namespace_absolute(ns):
+            combined_ns = ns
+        else:
+            if is_root_namespace(base_ns):
+                base_ns = ''
+            combined_ns = (
+                base_ns + '/' + ns
+            )
+        if not is_root_namespace(combined_ns):
+            combined_ns = combined_ns.rstrip('/')
+    return combined_ns
+
+
+OptionalText = TypeVar('OptionalText', Text, Type[None])
+
+
+def make_namespace_absolute(ns: OptionalText) -> OptionalText:
+    """Make a relative namespace absolute."""
+    if ns is None:
+        return None
+    if not is_namespace_absolute(ns):
+        return '/' + ns
+    return ns

test_launch_ros/test/test_launch_ros/actions/test_push_ros_namespace.py

@@ -14,14 +14,23 @@
 
 """Tests for the PushRosNamespace Action."""
 
-from launch import LaunchContext
-
 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
 
 import pytest
 
 
+class MockContext:
+
+    def __init__(self):
+        self.launch_configurations = {}
+
+    def perform_substitution(self, sub):
+        return sub.perform(None)
+
+
 class Config:
 
     def __init__(
@@ -39,51 +48,62 @@ def __init__(
         self.expected_ns = expected_ns
         self.second_push_ns = second_push_ns
 
+    def __repr__(self):
+        return (
+            f'TestConfig(node_name={self.node_name}, node_ns={self.node_ns}, '
+            f'push_ns={self.push_ns}, expected_ns={self.expected_ns}, '
+            f'second_push_ns={self.second_push_ns})'
+        )
 
-@pytest.mark.parametrize('config', (
-    Config(
-        push_ns='relative_ns',
-        node_ns='node_ns',
-        expected_ns='/relative_ns/node_ns'),
-    Config(
-        push_ns='relative_ns',
-        node_ns='/node_ns',
-        expected_ns='/node_ns'),
-    Config(
-        push_ns='relative_ns',
-        node_ns='/',
-        expected_ns='/'),
-    Config(
-        push_ns='relative_ns',
-        node_ns='',
-        expected_ns='/relative_ns'),
-    Config(
-        push_ns='relative_ns',
-        second_push_ns='another_relative_ns',
-        node_ns='node_ns',
-        expected_ns='/relative_ns/another_relative_ns/node_ns'),
-    Config(
-        push_ns='relative_ns',
-        second_push_ns='/absolute_ns',
-        node_ns='node_ns',
-        expected_ns='/absolute_ns/node_ns'),
-    Config(
-        node_name='my_node',
-        push_ns='relative_ns',
-        second_push_ns='/absolute_ns',
-        node_ns='node_ns',
-        expected_ns='/absolute_ns/node_ns'),
-    Config(
-        node_name='my_node',
-        node_ns='node_ns',
-        expected_ns='/node_ns'),
-    Config(),
-    Config(
-        push_ns='',
-        expected_ns='/'),
-))
+
+def get_test_cases():
+    return (
+        Config(
+            push_ns='relative_ns',
+            node_ns='node_ns',
+            expected_ns='/relative_ns/node_ns'),
+        Config(
+            push_ns='relative_ns',
+            node_ns='/node_ns',
+            expected_ns='/node_ns'),
+        Config(
+            push_ns='relative_ns',
+            node_ns='/',
+            expected_ns='/'),
+        Config(
+            push_ns='relative_ns',
+            node_ns='',
+            expected_ns='/relative_ns'),
+        Config(
+            push_ns='relative_ns',
+            second_push_ns='another_relative_ns',
+            node_ns='node_ns',
+            expected_ns='/relative_ns/another_relative_ns/node_ns'),
+        Config(
+            push_ns='relative_ns',
+            second_push_ns='/absolute_ns',
+            node_ns='node_ns',
+            expected_ns='/absolute_ns/node_ns'),
+        Config(
+            node_name='my_node',
+            push_ns='relative_ns',
+            second_push_ns='/absolute_ns',
+            node_ns='node_ns',
+            expected_ns='/absolute_ns/node_ns'),
+        Config(
+            node_name='my_node',
+            node_ns='node_ns',
+            expected_ns='/node_ns'),
+        Config(),
+        Config(
+            push_ns='',
+            expected_ns='/'),
+    )
+
+
+@pytest.mark.parametrize('config', get_test_cases())
 def test_push_ros_namespace(config):
-    lc = LaunchContext()
+    lc = MockContext()
     if config.push_ns is not None:
         pns1 = PushRosNamespace(config.push_ns)
         pns1.execute(lc)
@@ -106,3 +126,23 @@ def test_push_ros_namespace(config):
     expected_fqn = expected_ns.rstrip('/') + '/' + expected_name
     assert expected_ns == node.expanded_node_namespace
     assert expected_fqn == node.node_name
+
+
+@pytest.mark.parametrize('config', get_test_cases())
+def test_push_ros_namespace_with_composable_node(config):
+    lc = MockContext()
+    if config.push_ns is not None:
+        pns1 = PushRosNamespace(config.push_ns)
+        pns1.execute(lc)
+    if config.second_push_ns is not None:
+        pns2 = PushRosNamespace(config.second_push_ns)
+        pns2.execute(lc)
+    node_description = ComposableNode(
+        package='asd',
+        plugin='my_plugin',
+        namespace=config.node_ns,
+        name=config.node_name,
+    )
+    request = get_composable_node_load_request(node_description, lc)
+    expected_ns = config.expected_ns if config.expected_ns is not None else ''
+    assert expected_ns == request.node_namespace