diff --git a/rclpy_components/package.xml b/rclpy_components/package.xml new file mode 100644 index 000000000..5c5c0122e --- /dev/null +++ b/rclpy_components/package.xml @@ -0,0 +1,24 @@ + + + + rclpy_components + 1.1.0 + Enables dynamically composing rcly nodes into the same process. + Zhen Ju + Aditya Pande + Shane Loretz + Apache License 2.0 + + rclpy + composition_interfaces + + ament_copyright + ament_flake8 + ament_pep257 + composition_interfaces + python3-pytest + + + ament_python + + diff --git a/rclpy_components/rclpy_components/__init__.py b/rclpy_components/rclpy_components/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/rclpy_components/rclpy_components/component_container.py b/rclpy_components/rclpy_components/component_container.py new file mode 100644 index 000000000..1ddbc5688 --- /dev/null +++ b/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() diff --git a/rclpy_components/rclpy_components/component_container_mt.py b/rclpy_components/rclpy_components/component_container_mt.py new file mode 100644 index 000000000..3d9feccb2 --- /dev/null +++ b/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() diff --git a/rclpy_components/rclpy_components/component_manager.py b/rclpy_components/rclpy_components/component_manager.py new file mode 100644 index 000000000..329048eb0 --- /dev/null +++ b/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 diff --git a/rclpy_components/rclpy_components/test/__init__.py b/rclpy_components/rclpy_components/test/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/rclpy_components/rclpy_components/test/foo_node.py b/rclpy_components/rclpy_components/test/foo_node.py new file mode 100644 index 000000000..08dd1bc4c --- /dev/null +++ b/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) diff --git a/rclpy_components/resource/rclpy_components b/rclpy_components/resource/rclpy_components new file mode 100644 index 000000000..e69de29bb diff --git a/rclpy_components/resource/test_composition b/rclpy_components/resource/test_composition new file mode 100644 index 000000000..e69de29bb diff --git a/rclpy_components/setup.cfg b/rclpy_components/setup.cfg new file mode 100644 index 000000000..4d3f2656a --- /dev/null +++ b/rclpy_components/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rclpy_components +[install] +install_scripts=$base/lib/rclpy_components diff --git a/rclpy_components/setup.py b/rclpy_components/setup.py new file mode 100644 index 000000000..e0a4b3e44 --- /dev/null +++ b/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='aditya.pande@openrobotics.org, sloretz@openrobotics.org', + 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', + ] + }, +) diff --git a/rclpy_components/test/test_component_manager.py b/rclpy_components/test/test_component_manager.py new file mode 100644 index 000000000..8c9211c71 --- /dev/null +++ b/rclpy_components/test/test_component_manager.py @@ -0,0 +1,250 @@ +# 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 threading +from unittest.mock import MagicMock +from unittest.mock import patch + +from composition_interfaces.srv import ListNodes, LoadNode, UnloadNode +import pytest +import rclpy +from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor +from rclpy_components.component_manager import ComponentManager + + +class FakeEntryPoint: + name: str + value: str + group: str + mock_class: MagicMock + + def __init__(self, name: str, value: str, group: str = 'rclpy_components'): + self.name = name + self.value = value + self.group = group + self.mock_class = MagicMock() + + def load(self): + return self.mock_class + + +class FakeEntryPointFailsToLoad(FakeEntryPoint): + + def load(self): + raise ValueError('Oops I failed to load') + + +EXECUTORS = (SingleThreadedExecutor, MultiThreadedExecutor) + + +@pytest.fixture(params=EXECUTORS) +def component_manager(request): + rclpy.init() + executor = request.param() + component_manager = ComponentManager(executor, 'ut_container') + executor.add_node(component_manager) + t = threading.Thread(target=executor.spin, daemon=True) + t.start() + + yield component_manager + + executor.remove_node(component_manager) + component_manager.destroy_node() + executor.shutdown() + t.join() + rclpy.shutdown() + + +@pytest.fixture() +def node_and_context(): + context = rclpy.context.Context() + rclpy.init(context=context) + node = rclpy.create_node('test_node', namespace='test_component_manager', context=context) + executor = SingleThreadedExecutor() + executor.add_node(node) + t = threading.Thread(target=executor.spin, daemon=True) + t.start() + + yield (node, context) + + node.destroy_node() + executor.shutdown() + t.join() + rclpy.shutdown(context=context) + + +def test_list_nodes_no_nodes(component_manager): + res = ListNodes.Response() + req = ListNodes.Request() + component_manager.on_list_node(req, res) + assert len(res.full_node_names) == 0 + assert len(res.unique_ids) == 0 + + +def test_load_node_no_such_node(component_manager): + res = LoadNode.Response() + req = LoadNode.Request() + req.package_name = 'fake_package' + req.plugin_name = 'FakePluginName' + + component_manager.on_load_node(req, res) + + assert 'not found' in res.error_message + assert res.success is False + + +def test_load_then_unload_node(component_manager): + # Load + load_res = LoadNode.Response() + load_req = LoadNode.Request() + load_req.package_name = 'rclpy_components' + load_req.plugin_name = 'FooNode' + + component_manager.on_load_node(load_req, load_res) + + assert load_res.success is True + + # List + list_res = ListNodes.Response() + list_req = ListNodes.Request() + + component_manager.on_list_node(list_req, list_res) + + assert len(list_res.full_node_names) == 1 + assert len(list_res.unique_ids) == 1 + assert load_res.unique_id in list_res.unique_ids + + # Unload + unload_res = UnloadNode.Response() + unload_req = UnloadNode.Request() + unload_req.unique_id = load_res.unique_id + + component_manager.on_unload_node(unload_req, unload_res) + + assert unload_res.success is True + + # List + list_res = ListNodes.Response() + list_req = ListNodes.Request() + + component_manager.on_list_node(list_req, list_res) + + assert len(list_res.full_node_names) == 0 + assert len(list_res.unique_ids) == 0 + + +def test_unload_non_existant_node(component_manager): + unload_res = UnloadNode.Response() + unload_req = UnloadNode.Request() + unload_req.unique_id = 42 + + component_manager.on_unload_node(unload_req, unload_res) + + assert unload_res.success is False + assert '42' in unload_res.error_message + + +def test_load_fails(component_manager): + load_res = LoadNode.Response() + load_req = LoadNode.Request() + load_req.package_name = 'fake_package' + load_req.plugin_name = 'FakePlugin' + + with patch('rclpy_components.component_manager.entry_points') as m: + m.return_value.select.return_value = [ + FakeEntryPointFailsToLoad( + f'{load_req.package_name}::{load_req.plugin_name}', 'not.used:here'), + ] + component_manager.on_load_node(load_req, load_res) + + assert load_res.success is False + assert 'Oops I failed to load' in load_res.error_message + + +def test_load_node_wont_init(component_manager): + load_res = LoadNode.Response() + load_req = LoadNode.Request() + load_req.package_name = 'fake_package' + load_req.plugin_name = 'FakePlugin' + + with patch('rclpy_components.component_manager.entry_points') as m: + fake_ep = FakeEntryPoint( + f'{load_req.package_name}::{load_req.plugin_name}', 'not.used:here') + fake_ep.mock_class.side_effect = ValueError('Oops I failed to construct') + m.return_value.select.return_value = [fake_ep] + component_manager.on_load_node(load_req, load_res) + + assert load_res.success is False + assert 'Oops I failed to construct' in load_res.error_message + + +def test_load_list_unload_via_services(component_manager, node_and_context): + node, context = node_and_context + manager_fqn = component_manager.get_fully_qualified_name() + + load_node_service = f'{manager_fqn}/_container/load_node' + unload_node_service = f'{manager_fqn}/_container/unload_node' + list_node_service = f'{manager_fqn}/_container/list_nodes' + + load_cli = node.create_client(LoadNode, load_node_service) + unload_cli = node.create_client(UnloadNode, unload_node_service) + list_cli = node.create_client(ListNodes, list_node_service) + + assert load_cli.wait_for_service(timeout_sec=5.0) + assert unload_cli.wait_for_service(timeout_sec=5.0) + assert list_cli.wait_for_service(timeout_sec=5.0) + + # Load one node + load_req = LoadNode.Request() + load_req.package_name = 'rclpy_components' + load_req.plugin_name = 'FooNode' + + load_res = load_cli.call(load_req) + assert load_res.success is True + + # List the node + list_res = list_cli.call(ListNodes.Request()) + assert len(list_res.full_node_names) == 1 + assert len(list_res.unique_ids) == 1 + assert load_res.unique_id in list_res.unique_ids + + # Unload it + + unload_req = UnloadNode.Request() + unload_req.unique_id = load_res.unique_id + unload_res = unload_cli.call(unload_req) + assert unload_res.success is True + + +def test_load_new_name_and_namespace(component_manager): + # Load + load_res = LoadNode.Response() + load_req = LoadNode.Request() + load_req.package_name = 'rclpy_components' + load_req.plugin_name = 'FooNode' + load_req.node_name = 'kitty' + load_req.node_namespace = '/hello' + + component_manager.on_load_node(load_req, load_res) + + assert load_res.success is True + + # List + list_res = ListNodes.Response() + list_req = ListNodes.Request() + + component_manager.on_list_node(list_req, list_res) + + assert len(list_res.full_node_names) == 1 + assert '/hello/kitty' in list_res.full_node_names diff --git a/rclpy_components/test/test_copyright.py b/rclpy_components/test/test_copyright.py new file mode 100644 index 000000000..6851b977c --- /dev/null +++ b/rclpy_components/test/test_copyright.py @@ -0,0 +1,23 @@ +# 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 ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/rclpy_components/test/test_flake8.py b/rclpy_components/test/test_flake8.py new file mode 100644 index 000000000..0cdb78d63 --- /dev/null +++ b/rclpy_components/test/test_flake8.py @@ -0,0 +1,25 @@ +# 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 ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/rclpy_components/test/test_pep257.py b/rclpy_components/test/test_pep257.py new file mode 100644 index 000000000..77a96f049 --- /dev/null +++ b/rclpy_components/test/test_pep257.py @@ -0,0 +1,23 @@ +# 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 ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings'