Composition API

rclpy_components/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rclpy_components</name>
+  <version>1.1.0</version>
+  <description>Enables dynamically composing rcly nodes into the same process.</description>
+  <author email="[email protected]">Zhen Ju</author>
+  <maintainer email="[email protected]">Aditya Pande</maintainer>
+  <maintainer email="[email protected]">Shane Loretz</maintainer>
+  <license>Apache License 2.0</license>
+
+  <depend>rclpy</depend>
+  <depend>composition_interfaces</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>composition_interfaces</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

rclpy_components/rclpy_components/component_container.py

@@ -0,0 +1,38 @@
+# 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.
+
+import rclpy
+from rclpy.executors import SingleThreadedExecutor
+
+from .component_manager import ComponentManager
+
+
+def main():
+    rclpy.init()
+
+    executor = SingleThreadedExecutor()
+    component_manager = ComponentManager(executor, 'PyComponentManager')
+
+    executor.add_node(component_manager)
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        print('KeyboardInterrupt received, exit')
+
+    component_manager.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

rclpy_components/rclpy_components/component_container_mt.py

@@ -0,0 +1,39 @@
+# 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.
+
+import rclpy
+from rclpy.executors import MultiThreadedExecutor
+
+from .component_manager import ComponentManager
+
+
+def main():
+    rclpy.init()
+
+    executor = MultiThreadedExecutor()
+    component_manager = ComponentManager(executor, 'PyComponentManager')
+
+    executor.add_node(component_manager)
+
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        print('KeyboardInterrupt received, exit')
+
+    component_manager.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

rclpy_components/rclpy_components/component_manager.py

@@ -0,0 +1,150 @@
+# 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.
+
+from importlib.metadata import entry_points
+
+from composition_interfaces.srv import ListNodes, LoadNode, UnloadNode
+from rclpy.executors import Executor
+from rclpy.node import Node
+
+
+class ComponentManager(Node):
+
+    def __init__(self, executor: Executor, name='py_component_manager', **kwargs):
+        super().__init__(name, **kwargs)
+        self.executor = executor
+        # Implement the 3 services described in
+        # http://design.ros2.org/articles/roslaunch.html#command-line-arguments
+        self._list_node_srv = self.create_service(
+            ListNodes, '~/_container/list_nodes', self.on_list_node)
+        self._load_node_srv = self.create_service(
+            LoadNode, '~/_container/load_node', self.on_load_node)
+        self._unload_node_srv = self.create_service(
+            UnloadNode, '~/_container/unload_node', self.on_unload_node)
+
+        self._components = {}  # key: unique_id, value: full node name and component instance
+        self._unique_id_index = 0
+
+    def _next_id(self):
+        self._unique_id_index += 1
+        return self._unique_id_index
+
+    def _does_name_exist(self, fully_qualified_name: str) -> bool:
+        """Return true iff there is already a node with the given fully qualified name."""
+        if fully_qualified_name == self.get_fully_qualified_name():
+            return True
+        for name, instance in self._components.values():
+            if fully_qualified_name == name:
+                return True
+        return False
+
+    def on_list_node(self, req: ListNodes.Request, res: ListNodes.Response):
+        for key, name_instance in self._components.items():
+            res.unique_ids.append(key)
+            res.full_node_names.append(name_instance[0])
+
+        return res
+
+    def on_load_node(self, req: LoadNode.Request, res: LoadNode.Response):
+        component_entry_points = entry_points().select(group='rclpy_components')
+        if not component_entry_points:
+            res.success = False
+            res.error_message = 'No rclpy components registered'
+            return res
+
+        request_ep_name = f'{req.package_name}::{req.plugin_name}'
+        component_entry_point = None
+        for ep in component_entry_points:
+            if ep.name == request_ep_name:
+                component_entry_point = ep
+                break
+
+        if not component_entry_point:
+            res.success = False
+            res.error_message = f'Rclpy component not found: {request_ep_name}'
+            return res
+
+        try:
+            component_class = component_entry_point.load()
+        except Exception as e:
+            error_message = str(e)
+            self.get_logger().error(f'Failed to load entry point: {error_message}')
+            res.success = False
+            res.error_message = error_message
+            return res
+
+        if req.node_name:
+            node_name = req.node_name
+        else:
+            # TODO(sloretz) use remap rule like rclcpp_components
+            node_name = str.lower(str.split(component_entry_point.value, ':')[1])
+
+        params_dict = {
+            'use_global_arguments': False,
+            'context': self.context,
+            'cli_args': ['--ros-args'],
+        }
+
+        if req.parameters:
+            params_dict['parameter_overrides'] = req.parameters
+
+        if req.node_namespace:
+            params_dict['cli_args'].append('-r')
+            params_dict['cli_args'].append(f'__ns:={req.node_namespace}')
+
+        if req.node_name:
+            params_dict['cli_args'].append('-r')
+            params_dict['cli_args'].append(f'__node:={req.node_name}')
+
+        if req.remap_rules:
+            for rule in req.remap_rules:
+                params_dict['cli_args'].extend(['-r', rule])
+
+        self.get_logger().info(
+                f'Instantiating {component_entry_point.value} with {node_name}, {params_dict}')
+        try:
+            component = component_class(**params_dict)
+        except Exception as e:
+            error_message = str(e)
+            self.get_logger().error(f'Failed to instantiate node: {error_message}')
+            res.success = False
+            res.error_message = error_message
+            return res
+
+        res.full_node_name = component.get_fully_qualified_name()
+        if self._does_name_exist(res.full_node_name):
+            error_message = f'Node with name {res.full_node_name} already exists in this process.'
+            self.get_logger().error(f'Failed to load node: {error_message}')
+            res.success = False
+            res.error_message = error_message
+            component.destroy_node()
+            return res
+
+        res.unique_id = self._next_id()
+        res.success = True
+        self._components[res.unique_id] = (res.full_node_name, component)
+        self.executor.add_node(component)
+        return res
+
+    def on_unload_node(self, req: UnloadNode.Request, res: UnloadNode.Response):
+        uid = req.unique_id
+        if uid not in self._components:
+            res.success = False
+            res.error_message = f'No node found with id: {uid}'
+            return res
+
+        _, component_instance = self._components.pop(uid)
+        self.executor.remove_node(component_instance)
+        res.success = True
+        return res

rclpy_components/rclpy_components/test/foo_node.py

@@ -0,0 +1,21 @@
+# 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.
+
+from rclpy.node import Node
+
+
+class FooNode(Node):
+
+    def __init__(self, **kwargs):
+        super().__init__('test_foo', **kwargs)

rclpy_components/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/rclpy_components
+[install]
+install_scripts=$base/lib/rclpy_components

rclpy_components/setup.py

@@ -0,0 +1,30 @@
+from setuptools import find_packages, setup
+
+package_name = 'rclpy_components'
+
+setup(
+    name=package_name,
+    version='1.1.0',
+    packages=find_packages(),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='Aditya Pande, Shane Loretz',
+    maintainer_email='[email protected], [email protected]',
+    description='The dynamic node management package',
+    license='Apache License 2.0',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'component_container = rclpy_components.component_container:main',
+            'component_container_mt = rclpy_components.component_container_mt:main',
+        ],
+        'rclpy_components': [
+            'rclpy_components::FooNode = rclpy_components.test.foo_node:FooNode',
+        ]
+    },
+)