From 077707dee3ff95370f6265c39f5aba6f66464fb9 Mon Sep 17 00:00:00 2001 From: Zhen Ju Date: Thu, 9 Jul 2020 18:45:23 +0800 Subject: [PATCH 01/19] Add the rclpy_component sub project The project provide component container(special Node) and component_manager Signed-off-by: Zhen Ju --- rclpy_components/package.xml | 21 +++ rclpy_components/rclpy_components/__init__.py | 13 ++ .../rclpy_components/component_container.py | 42 ++++++ .../component_container_mt.py | 41 ++++++ .../rclpy_components/component_manager.py | 127 ++++++++++++++++++ rclpy_components/resource/rclpy_components | 0 rclpy_components/setup.cfg | 4 + rclpy_components/setup.py | 27 ++++ rclpy_components/test/test_copyright.py | 23 ++++ rclpy_components/test/test_flake8.py | 25 ++++ rclpy_components/test/test_pep257.py | 23 ++++ 11 files changed, 346 insertions(+) create mode 100644 rclpy_components/package.xml create mode 100644 rclpy_components/rclpy_components/__init__.py create mode 100644 rclpy_components/rclpy_components/component_container.py create mode 100644 rclpy_components/rclpy_components/component_container_mt.py create mode 100644 rclpy_components/rclpy_components/component_manager.py create mode 100644 rclpy_components/resource/rclpy_components create mode 100644 rclpy_components/setup.cfg create mode 100644 rclpy_components/setup.py create mode 100644 rclpy_components/test/test_copyright.py create mode 100644 rclpy_components/test/test_flake8.py create mode 100644 rclpy_components/test/test_pep257.py diff --git a/rclpy_components/package.xml b/rclpy_components/package.xml new file mode 100644 index 000000000..de35717f4 --- /dev/null +++ b/rclpy_components/package.xml @@ -0,0 +1,21 @@ + + + + rclpy_components + 0.0.0 + TODO: Package description + root + TODO: License declaration + + rclpy + std_msgs + + ament_copyright + ament_flake8 + ament_pep257 + 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..fdbd08251 --- /dev/null +++ b/rclpy_components/rclpy_components/__init__.py @@ -0,0 +1,13 @@ +# 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. \ No newline at end of file diff --git a/rclpy_components/rclpy_components/component_container.py b/rclpy_components/rclpy_components/component_container.py new file mode 100644 index 000000000..f4fafd095 --- /dev/null +++ b/rclpy_components/rclpy_components/component_container.py @@ -0,0 +1,42 @@ +# 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 signal +import rclpy +from rclpy.executors import SingleThreadedExecutor +from rclpy.component import ComponentManager + +def main(): + print("run component_container") + try: + _main() + except KeyboardInterrupt: + print('KeyboardInterrupt received, exit') + return signal.SIGINT + + +def _main(): + rclpy.init() + + executor = SingleThreadedExecutor() + component_manager = ComponentManager(executor, "PyComponentManager") + + executor.add_node(component_manager) + executor.spin() + + 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..46524e411 --- /dev/null +++ b/rclpy_components/rclpy_components/component_container_mt.py @@ -0,0 +1,41 @@ +# 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 signal +import rclpy +from rclpy.executors import MultiThreadedExecutor +from rclpy.component import ComponentManager + +def main(): + try: + _main() + except KeyboardInterrupt: + print('KeyboardInterrupt received, exit') + return signal.SIGINT + + +def _main(): + rclpy.init() + + executor = MultiThreadedExecutor() + component_manager = ComponentManager(executor, "PyComponentManager") + + executor.add_node(component_manager) + executor.spin() + + 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..8d21b6ef3 --- /dev/null +++ b/rclpy_components/rclpy_components/component_manager.py @@ -0,0 +1,127 @@ +# 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 +from rclpy.executors import Executor +from rclpy.logging import get_logger +from rclpy.exceptions import InvalidNodeNameException, InvalidNamespaceException +from composition_interfaces.srv import ListNodes, LoadNode, UnloadNode +try: + from importlib.metadata import entry_points +except ImportError: + from importlib_metadata import entry_points + +RCLPY_COMPONENTS = 'rclpy_components' +logger = get_logger('ComponentManager') + + +class ComponentManager(Node): + + def __init__(self, executor: Executor, *args, name="py_component_manager", **kwargs): + # TODO Handle the py args equivalent to rclcpp 'NodeOptions' + super().__init__(name, *args, **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.unload_node_srv_ = self.create_service(Empty, "~/_container/supported_types", self.on_supported_types) + + self.components = {} # key: unique_id, value: full node name and component instance + self.unique_id_index = 0 + + self.executor.spin() + + def gen_unique_id(self): + self.unique_id_index += 1 + return self.unique_id_index + + def on_list_node(self, req: ListNodes.Request, res: ListNodes.Response): + res.unique_ids = [int(key) for key in self.components.keys()] + res.full_node_names = [v[0] for v in self.components.values()] + + return res + + def on_load_node(self, req: LoadNode.Request, res: LoadNode.Response): + component_entry_points = entry_points().get(RCLPY_COMPONENTS, None) + if not component_entry_points: + logger.error('No rclpy components registered') + res.success = False + return res + + component_entry_point = None + for ep in component_entry_points: + if ep.name == req.plugin_name: + component_entry_point = ep + break + + if not component_entry_point: + logger.error('No rclpy component found by %s' % req.plugin_name) + res.success = False + return res + + component_class = component_entry_point.load() + node_name = req.node_name if req.node_name else \ + str.lower(str.split(component_entry_point.value, ':')[1]) + + params_dict = {} + if req.parameters: + params_dict['parameter_overrides'] = req.parameters + + if req.node_namespace: + params_dict['namespace'] = req.node_namespace + + if req.remap_rules: + params_dict['cli_args'] = ['--ros-args'] + for rule in req.remap_rules: + params_dict['cli_args'].extend(['-r', rule]) + + # TODO Handle the priority of req.node_namespace and req.remap_rules '__ns:=' + try: + component = component_class(node_name, **params_dict) + + # TODO Handle the node_name, node_namespace, and remapping rules. + + res.unique_id = self.gen_unique_id() + res.full_node_name = '/{}'.format(node_name) + + if req.node_namespace: + res.full_node_name = '/{}{}'.format(req.node_namespace, res.full_node_name) + self.components[str(res.unique_id)] = (res.full_node_name, component) + self.executor.add_node(component) + res.success = True + return res + except (InvalidNodeNameException, InvalidNamespaceException, TypeError) as e: + error_message = str(e) + logger.error('Failed to load node: %s' % error_message) + res.success = False + res.error_message = error_message + return res + + def on_unload_node(self, req: UnloadNode.Request, res: UnloadNode.Response): + uid = str(req.unique_id) + if uid not in self.components: + res._error_message = 'No node found with unique_id: %s' % uid + res.success = False + return res + + _, component_instance = self.components.pop(uid) + self.executor.remove_node(component_instance) + res.success = True + return res + + # def on_supported_types(self, req: SupportedComponentTypes.Request, res: SupportedComponentTypes.Response): + # res.supported_types = ['rclpy_components'] + # return res 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/setup.cfg b/rclpy_components/setup.cfg new file mode 100644 index 000000000..38a4b02f9 --- /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..3984ac8a8 --- /dev/null +++ b/rclpy_components/setup.py @@ -0,0 +1,27 @@ +from setuptools import setup + +package_name = 'rclpy_components' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='root', + maintainer_email='root@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'component_container = rclpy_components.component_container:main', + 'component_container_mt = rclpy_components.component_container_mt:main', + ], + }, +) diff --git a/rclpy_components/test/test_copyright.py b/rclpy_components/test/test_copyright.py new file mode 100644 index 000000000..cc8ff03f7 --- /dev/null +++ b/rclpy_components/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 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..27ee1078f --- /dev/null +++ b/rclpy_components/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 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..b234a3840 --- /dev/null +++ b/rclpy_components/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 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' From 1f58c71be1cb473265f6553df13e3a863fe52c65 Mon Sep 17 00:00:00 2001 From: Zhen Ju Date: Thu, 9 Jul 2020 23:36:03 +0800 Subject: [PATCH 02/19] Refer to ComponentManager from project rclpy_components Signed-off-by: Zhen Ju --- rclpy_components/rclpy_components/component_container.py | 3 ++- rclpy_components/rclpy_components/component_manager.py | 3 +-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rclpy_components/rclpy_components/component_container.py b/rclpy_components/rclpy_components/component_container.py index f4fafd095..945591be0 100644 --- a/rclpy_components/rclpy_components/component_container.py +++ b/rclpy_components/rclpy_components/component_container.py @@ -15,7 +15,8 @@ import signal import rclpy from rclpy.executors import SingleThreadedExecutor -from rclpy.component import ComponentManager +from .component_manager import ComponentManager + def main(): print("run component_container") diff --git a/rclpy_components/rclpy_components/component_manager.py b/rclpy_components/rclpy_components/component_manager.py index 8d21b6ef3..1954f324b 100644 --- a/rclpy_components/rclpy_components/component_manager.py +++ b/rclpy_components/rclpy_components/component_manager.py @@ -28,7 +28,7 @@ class ComponentManager(Node): - def __init__(self, executor: Executor, *args, name="py_component_manager", **kwargs): + def __init__(self, executor: Executor, name="py_component_manager", *args, **kwargs): # TODO Handle the py args equivalent to rclcpp 'NodeOptions' super().__init__(name, *args, **kwargs) self.executor = executor @@ -93,7 +93,6 @@ def on_load_node(self, req: LoadNode.Request, res: LoadNode.Response): component = component_class(node_name, **params_dict) # TODO Handle the node_name, node_namespace, and remapping rules. - res.unique_id = self.gen_unique_id() res.full_node_name = '/{}'.format(node_name) From 1bea84a7a3aff204104301ac225effa720d13ecd Mon Sep 17 00:00:00 2001 From: Zhen Ju Date: Tue, 14 Jul 2020 11:46:11 +0800 Subject: [PATCH 03/19] Set 'use_global_arguments' to False when creating node instance. This is to get rid of the impact of global remapping rules on node name, which will create nodes with duplicated names. Remove *args in ComponentManager's constructor. Signed-off-by: Zhen Ju --- .../rclpy_components/component_container.py | 1 - .../rclpy_components/component_container_mt.py | 1 + .../rclpy_components/component_manager.py | 18 ++++++------------ 3 files changed, 7 insertions(+), 13 deletions(-) diff --git a/rclpy_components/rclpy_components/component_container.py b/rclpy_components/rclpy_components/component_container.py index 945591be0..e4afe818a 100644 --- a/rclpy_components/rclpy_components/component_container.py +++ b/rclpy_components/rclpy_components/component_container.py @@ -19,7 +19,6 @@ def main(): - print("run component_container") try: _main() except KeyboardInterrupt: diff --git a/rclpy_components/rclpy_components/component_container_mt.py b/rclpy_components/rclpy_components/component_container_mt.py index 46524e411..095073628 100644 --- a/rclpy_components/rclpy_components/component_container_mt.py +++ b/rclpy_components/rclpy_components/component_container_mt.py @@ -17,6 +17,7 @@ from rclpy.executors import MultiThreadedExecutor from rclpy.component import ComponentManager + def main(): try: _main() diff --git a/rclpy_components/rclpy_components/component_manager.py b/rclpy_components/rclpy_components/component_manager.py index 1954f324b..ea86ad4fb 100644 --- a/rclpy_components/rclpy_components/component_manager.py +++ b/rclpy_components/rclpy_components/component_manager.py @@ -28,16 +28,15 @@ class ComponentManager(Node): - def __init__(self, executor: Executor, name="py_component_manager", *args, **kwargs): + def __init__(self, executor: Executor, name="py_component_manager", **kwargs): # TODO Handle the py args equivalent to rclcpp 'NodeOptions' - super().__init__(name, *args, **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.unload_node_srv_ = self.create_service(Empty, "~/_container/supported_types", self.on_supported_types) self.components = {} # key: unique_id, value: full node name and component instance self.unique_id_index = 0 @@ -76,7 +75,7 @@ def on_load_node(self, req: LoadNode.Request, res: LoadNode.Response): node_name = req.node_name if req.node_name else \ str.lower(str.split(component_entry_point.value, ':')[1]) - params_dict = {} + params_dict = {'use_global_arguments': False} if req.parameters: params_dict['parameter_overrides'] = req.parameters @@ -88,19 +87,18 @@ def on_load_node(self, req: LoadNode.Request, res: LoadNode.Response): for rule in req.remap_rules: params_dict['cli_args'].extend(['-r', rule]) - # TODO Handle the priority of req.node_namespace and req.remap_rules '__ns:=' try: + logger.info('Instantiating {} with {}, {}'.format(component_entry_point.value, node_name, params_dict)) component = component_class(node_name, **params_dict) - # TODO Handle the node_name, node_namespace, and remapping rules. res.unique_id = self.gen_unique_id() + # TODO Assign the full_node_name with node.get_fully_qualified_name res.full_node_name = '/{}'.format(node_name) - if req.node_namespace: res.full_node_name = '/{}{}'.format(req.node_namespace, res.full_node_name) + res.success = True self.components[str(res.unique_id)] = (res.full_node_name, component) self.executor.add_node(component) - res.success = True return res except (InvalidNodeNameException, InvalidNamespaceException, TypeError) as e: error_message = str(e) @@ -120,7 +118,3 @@ def on_unload_node(self, req: UnloadNode.Request, res: UnloadNode.Response): self.executor.remove_node(component_instance) res.success = True return res - - # def on_supported_types(self, req: SupportedComponentTypes.Request, res: SupportedComponentTypes.Response): - # res.supported_types = ['rclpy_components'] - # return res From 9a43691e9bbe8981cce6150951bf9bf928113dc7 Mon Sep 17 00:00:00 2001 From: Zhen Ju Date: Mon, 19 Oct 2020 15:22:03 +0800 Subject: [PATCH 04/19] Fixes & improments following the code review comments. * Update the meta info in package.xml and setup.py * Try-except executor.spin instead of main in executables * Use f string to format infos * Pass error message to response objectc instead of logging it * Use node's logger instead of init a global one * Call node.get_qualified_name() to assign response obj's full_node_name * Other super tiny changes Signed-off-by: Zhen Ju --- rclpy_components/package.xml | 11 ++--- rclpy_components/rclpy_components/__init__.py | 2 +- .../rclpy_components/component_container.py | 16 +++----- .../component_container_mt.py | 19 ++++----- .../rclpy_components/component_manager.py | 41 ++++++++----------- rclpy_components/setup.py | 14 +++---- 6 files changed, 45 insertions(+), 58 deletions(-) diff --git a/rclpy_components/package.xml b/rclpy_components/package.xml index de35717f4..d71de5b8b 100644 --- a/rclpy_components/package.xml +++ b/rclpy_components/package.xml @@ -2,17 +2,18 @@ rclpy_components - 0.0.0 - TODO: Package description - root - TODO: License declaration + 1.1.0 + The dynamic node management package + Zhen Ju + Apache License 2.0 rclpy - std_msgs + composition_interfaces ament_copyright ament_flake8 ament_pep257 + composition_interfaces python3-pytest diff --git a/rclpy_components/rclpy_components/__init__.py b/rclpy_components/rclpy_components/__init__.py index fdbd08251..a71789ec8 100644 --- a/rclpy_components/rclpy_components/__init__.py +++ b/rclpy_components/rclpy_components/__init__.py @@ -10,4 +10,4 @@ # 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. \ No newline at end of file +# limitations under the License. diff --git a/rclpy_components/rclpy_components/component_container.py b/rclpy_components/rclpy_components/component_container.py index e4afe818a..0d7f11433 100644 --- a/rclpy_components/rclpy_components/component_container.py +++ b/rclpy_components/rclpy_components/component_container.py @@ -12,29 +12,25 @@ # See the License for the specific language governing permissions and # limitations under the License. -import signal import rclpy from rclpy.executors import SingleThreadedExecutor from .component_manager import ComponentManager def main(): - try: - _main() - except KeyboardInterrupt: - print('KeyboardInterrupt received, exit') - return signal.SIGINT - - -def _main(): rclpy.init() executor = SingleThreadedExecutor() component_manager = ComponentManager(executor, "PyComponentManager") executor.add_node(component_manager) - executor.spin() + try: + executor.spin() + except KeyboardInterrupt: + print('KeyboardInterrupt received, exit') + pass + component_manager.destroy_node() rclpy.shutdown() diff --git a/rclpy_components/rclpy_components/component_container_mt.py b/rclpy_components/rclpy_components/component_container_mt.py index 095073628..3c40c554f 100644 --- a/rclpy_components/rclpy_components/component_container_mt.py +++ b/rclpy_components/rclpy_components/component_container_mt.py @@ -12,29 +12,26 @@ # See the License for the specific language governing permissions and # limitations under the License. -import signal import rclpy from rclpy.executors import MultiThreadedExecutor -from rclpy.component import ComponentManager +from .component_manager import ComponentManager def main(): - try: - _main() - except KeyboardInterrupt: - print('KeyboardInterrupt received, exit') - return signal.SIGINT - - -def _main(): rclpy.init() executor = MultiThreadedExecutor() component_manager = ComponentManager(executor, "PyComponentManager") executor.add_node(component_manager) - executor.spin() + try: + executor.spin() + except KeyboardInterrupt: + print('KeyboardInterrupt received, exit') + pass + + component_manager.destroy_node() rclpy.shutdown() diff --git a/rclpy_components/rclpy_components/component_manager.py b/rclpy_components/rclpy_components/component_manager.py index ea86ad4fb..0662f2171 100644 --- a/rclpy_components/rclpy_components/component_manager.py +++ b/rclpy_components/rclpy_components/component_manager.py @@ -14,7 +14,6 @@ from rclpy.node import Node from rclpy.executors import Executor -from rclpy.logging import get_logger from rclpy.exceptions import InvalidNodeNameException, InvalidNamespaceException from composition_interfaces.srv import ListNodes, LoadNode, UnloadNode try: @@ -22,27 +21,24 @@ except ImportError: from importlib_metadata import entry_points -RCLPY_COMPONENTS = 'rclpy_components' -logger = get_logger('ComponentManager') - class ComponentManager(Node): def __init__(self, executor: Executor, name="py_component_manager", **kwargs): - # TODO Handle the py args equivalent to rclcpp 'NodeOptions' 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._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 - self.executor.spin() - def gen_unique_id(self): self.unique_id_index += 1 return self.unique_id_index @@ -54,10 +50,10 @@ def on_list_node(self, req: ListNodes.Request, res: ListNodes.Response): return res def on_load_node(self, req: LoadNode.Request, res: LoadNode.Response): - component_entry_points = entry_points().get(RCLPY_COMPONENTS, None) + component_entry_points = entry_points().get('rclpy_components', None) if not component_entry_points: - logger.error('No rclpy components registered') res.success = False + res.error_message = 'No rclpy components registered' return res component_entry_point = None @@ -67,15 +63,15 @@ def on_load_node(self, req: LoadNode.Request, res: LoadNode.Response): break if not component_entry_point: - logger.error('No rclpy component found by %s' % req.plugin_name) res.success = False + res.error_message = f'No rclpy component found by {req.plugin_name}' return res component_class = component_entry_point.load() node_name = req.node_name if req.node_name else \ str.lower(str.split(component_entry_point.value, ':')[1]) - params_dict = {'use_global_arguments': False} + params_dict = {'use_global_arguments': False, 'context': self.context} if req.parameters: params_dict['parameter_overrides'] = req.parameters @@ -88,30 +84,27 @@ def on_load_node(self, req: LoadNode.Request, res: LoadNode.Response): params_dict['cli_args'].extend(['-r', rule]) try: - logger.info('Instantiating {} with {}, {}'.format(component_entry_point.value, node_name, params_dict)) + self.get_logger().info( + f'Instantiating {component_entry_point.value} with {node_name}, {params_dict}') component = component_class(node_name, **params_dict) - res.unique_id = self.gen_unique_id() - # TODO Assign the full_node_name with node.get_fully_qualified_name - res.full_node_name = '/{}'.format(node_name) - if req.node_namespace: - res.full_node_name = '/{}{}'.format(req.node_namespace, res.full_node_name) + res.full_node_name = component.get_fully_qualified_name() res.success = True - self.components[str(res.unique_id)] = (res.full_node_name, component) + self.components[res.unique_id] = (res.full_node_name, component) self.executor.add_node(component) return res except (InvalidNodeNameException, InvalidNamespaceException, TypeError) as e: error_message = str(e) - logger.error('Failed to load node: %s' % error_message) + self.get_logger().error(f'Failed to load node: {error_message}') res.success = False res.error_message = error_message return res def on_unload_node(self, req: UnloadNode.Request, res: UnloadNode.Response): - uid = str(req.unique_id) + uid = req.unique_id if uid not in self.components: - res._error_message = 'No node found with unique_id: %s' % uid res.success = False + res.error_message = f'No node found with unique_id: {uid}' return res _, component_instance = self.components.pop(uid) diff --git a/rclpy_components/setup.py b/rclpy_components/setup.py index 3984ac8a8..f2bf627c3 100644 --- a/rclpy_components/setup.py +++ b/rclpy_components/setup.py @@ -1,11 +1,11 @@ -from setuptools import setup +from setuptools import setup, find_packages package_name = 'rclpy_components' setup( name=package_name, - version='0.0.0', - packages=[package_name], + version='1.1.0', + packages=find_packages(), data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), @@ -13,10 +13,10 @@ ], install_requires=['setuptools'], zip_safe=True, - maintainer='root', - maintainer_email='root@todo.todo', - description='TODO: Package description', - license='TODO: License declaration', + maintainer='Zhen Ju', + maintainer_email='juzhen@huawei.com', + description='The dynamic node management package', + license='Apache License 2.0', tests_require=['pytest'], entry_points={ 'console_scripts': [ From 875d84c8ca2f90af8e95e5f860e227a3ce40adec Mon Sep 17 00:00:00 2001 From: Zhen Ju Date: Mon, 19 Oct 2020 15:45:46 +0800 Subject: [PATCH 05/19] Add test cases for composition API. Signed-off-by: Zhen Ju --- .../rclpy_components_test/__init__.py | 20 ++ rclpy_components/resource/test_composition | 0 rclpy_components/setup.py | 5 + .../test/test_component_manager.py | 198 ++++++++++++++++++ .../test/test_component_manager_ut.py | 110 ++++++++++ rclpy_components/test/test_copyright.py | 2 +- rclpy_components/test/test_flake8.py | 2 +- rclpy_components/test/test_pep257.py | 2 +- 8 files changed, 336 insertions(+), 3 deletions(-) create mode 100644 rclpy_components/rclpy_components_test/__init__.py create mode 100644 rclpy_components/resource/test_composition create mode 100644 rclpy_components/test/test_component_manager.py create mode 100644 rclpy_components/test/test_component_manager_ut.py diff --git a/rclpy_components/rclpy_components_test/__init__.py b/rclpy_components/rclpy_components_test/__init__.py new file mode 100644 index 000000000..90b535c56 --- /dev/null +++ b/rclpy_components/rclpy_components_test/__init__.py @@ -0,0 +1,20 @@ +# 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 TestFoo(Node): + def __init__(self, node_name='test_foo', **kwargs): + super().__init__(node_name, **kwargs) 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.py b/rclpy_components/setup.py index f2bf627c3..5dc11a4f8 100644 --- a/rclpy_components/setup.py +++ b/rclpy_components/setup.py @@ -10,6 +10,8 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + ('share/ament_index/resource_index/packages', + ['resource/' + 'test_composition']) ], install_requires=['setuptools'], zip_safe=True, @@ -23,5 +25,8 @@ 'component_container = rclpy_components.component_container:main', 'component_container_mt = rclpy_components.component_container_mt:main', ], + 'rclpy_components': [ + 'test_composition::TestFoo = rclpy_components_test:TestFoo', + ] }, ) diff --git a/rclpy_components/test/test_component_manager.py b/rclpy_components/test/test_component_manager.py new file mode 100644 index 000000000..a05674ec3 --- /dev/null +++ b/rclpy_components/test/test_component_manager.py @@ -0,0 +1,198 @@ +# 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 unittest +import rclpy +from multiprocessing import Process +from rclpy.client import Client +from rclpy.executors import SingleThreadedExecutor, MultiThreadedExecutor +from rclpy.node import Node +from composition_interfaces.srv import ListNodes, LoadNode, UnloadNode +from rclpy_components.component_manager import ComponentManager + +TEST_COMPOSITION = 'test_composition' +TEST_COMPOSITION_FOO = 'test_composition::TestFoo' +TEST_COMPOSITION_NODE_NAME = '/testfoo' + + +def run_container(container_name, multi_thread): + rclpy.init() + if multi_thread: + executor = MultiThreadedExecutor() + else: + executor = SingleThreadedExecutor() + + component_manager = ComponentManager(executor, container_name) + executor.add_node(component_manager) + try: + executor.spin() + except Exception as e: + print(e) + pass + + component_manager.destroy_node() + rclpy.shutdown() + + +class TestComponentManager(unittest.TestCase): + helper_node: Node = None + container_process: Process = None + + container_name = 'TestComponentManager' + # service names & clients will be generated with container_name + load_node_svc_name = "" + unload_node_svc_name = "" + list_node_svc_name = "" + load_cli: Client = None + unload_cli: Client = None + list_cli: Client = None + + use_multi_threaded_executor = False + + @classmethod + def setUpClass(cls): + cls.load_node_svc_name = f'{cls.container_name}/_container/load_node' + cls.unload_node_svc_name = f'{cls.container_name}/_container/unload_node' + cls.list_node_svc_name = f'{cls.container_name}/_container/list_nodes' + + # Start the test component manager in the background + cls.container_process = Process(target=run_container, + args=(cls.container_name, cls.use_multi_threaded_executor)) + cls.container_process.start() + + # Setup the helper_node, which will help create client and test the services + rclpy.init() + cls.helper_node = rclpy.create_node('helper_node') + cls.load_cli = cls.helper_node.create_client(LoadNode, cls.load_node_svc_name) + cls.unload_cli = cls.helper_node.create_client(UnloadNode, cls.unload_node_svc_name) + cls.list_cli = cls.helper_node.create_client(ListNodes, cls.list_node_svc_name) + + @classmethod + def tearDownClass(cls): + cls.container_process.terminate() + cls.load_cli.destroy() + cls.unload_cli.destroy() + cls.list_cli.destroy() + cls.helper_node.destroy_node() + rclpy.shutdown() + + @classmethod + def load_node(cls, package_name, plugin_name, node_name="", node_namespace=""): + if not cls.load_cli.wait_for_service(timeout_sec=5.0): + raise RuntimeError(f'No load service found in /{cls.container_name}') + + load_req = LoadNode.Request() + load_req.package_name = package_name + load_req.plugin_name = plugin_name + + if node_name != "": + load_req.node_name = node_name + if node_namespace != "": + load_req.node_namespace = node_namespace + + future = cls.load_cli.call_async(load_req) + rclpy.spin_until_future_complete(cls.helper_node, future) + return future.result() + + @classmethod + def unload_node(cls, unique_id): + if not cls.unload_cli.wait_for_service(timeout_sec=5.0): + raise RuntimeError(f'No unload service found in /{cls.container_name}') + + unload_req = UnloadNode.Request() + unload_req.unique_id = unique_id + + future = cls.unload_cli.call_async(unload_req) + rclpy.spin_until_future_complete(cls.helper_node, future) + return future.result() + + @classmethod + def list_nodes(cls): + if not cls.list_cli.wait_for_service(timeout_sec=5.0): + raise RuntimeError(f'No list service found in {cls.container_name}') + list_req = ListNodes.Request() + future = cls.list_cli.call_async(list_req) + rclpy.spin_until_future_complete(cls.helper_node, future) + return future.result() + + def load_node_test(self): + load_res = self.__class__.load_node(TEST_COMPOSITION, TEST_COMPOSITION_FOO) + assert load_res.success is True + assert load_res.error_message == "" + assert load_res.unique_id == 1 + assert load_res.full_node_name == TEST_COMPOSITION_NODE_NAME + + node_name = "renamed_node" + node_ns = 'testing_ns' + remap_load_res = self.__class__.load_node( + TEST_COMPOSITION, TEST_COMPOSITION_FOO, node_name=node_name, + node_namespace=node_ns) + assert remap_load_res.success is True + assert remap_load_res.error_message == "" + assert remap_load_res.unique_id == 2 + assert remap_load_res.full_node_name == f'/{node_ns}/{node_name}' + + list_res: ListNodes.Response = self.__class__.list_nodes() + assert len(list_res.unique_ids) == len(list_res.full_node_names) == 2 + + def unload_node_test(self): + if not self.__class__.unload_cli.wait_for_service(timeout_sec=5.0): + raise RuntimeError(f'no unload service found in {self.__class__.container_name}') + + unload_res: UnloadNode.Response = self.__class__.unload_node(1) + assert unload_res.success is True + assert unload_res.error_message == "" + + # Should be only 1 node left + list_res: ListNodes.Response = self.__class__.list_nodes() + assert len(list_res.full_node_names) == len(list_res.unique_ids) == 1 + + # The index definitely won't exist + unload_res: UnloadNode.Response = self.__class__.unload_node(1000) + assert unload_res.error_message != "" + assert unload_res.success is False + list_res: ListNodes.Response = self.__class__.list_nodes() + assert len(list_res.full_node_names) == len(list_res.unique_ids) == 1 + + # Unload the last node + unload_req = UnloadNode.Request() + unload_req.unique_id = 2 + future = self.__class__.unload_cli.call_async(unload_req) + rclpy.spin_until_future_complete(self.__class__.helper_node, future) + unload_res: UnloadNode.Response = future.result() + assert unload_res.success is True + assert unload_res.error_message == "" + + # Won't be any node in the container + list_res: ListNodes.Response = self.__class__.list_nodes() + assert len(list_res.full_node_names) == len(list_res.unique_ids) == 0 + + def list_nodes_test(self): + container_name = self.__class__.container_name + print(f'{container_name}: list_nodes tested within test_load_node and test_unload_node') + + def test_composition_api(self): + # Control the order of test + self.load_node_test() + self.unload_node_test() + self.list_nodes_test() + + +class TestComponentManagerMT(TestComponentManager): + use_multi_threaded_executor = True + container_name = 'TestComponentManagerMT' + + +if __name__ == '__main__': + unittest.main() diff --git a/rclpy_components/test/test_component_manager_ut.py b/rclpy_components/test/test_component_manager_ut.py new file mode 100644 index 000000000..7357fa67e --- /dev/null +++ b/rclpy_components/test/test_component_manager_ut.py @@ -0,0 +1,110 @@ +# 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 unittest +import rclpy +from composition_interfaces.srv import ListNodes, LoadNode, UnloadNode +from rclpy.executors import Executor, SingleThreadedExecutor, MultiThreadedExecutor +from rclpy_components.component_manager import ComponentManager + +TEST_COMPOSITION = 'test_composition' +TEST_COMPOSITION_FOO = 'test_composition::TestFoo' + + +class TestComponentManagerUT(unittest.TestCase): + executor: Executor + component_manager: ComponentManager + use_multi_threaded_executor: bool = False + container_name: str = "ut_container" + + @classmethod + def setUpClass(cls) -> None: + rclpy.init() + + if cls.use_multi_threaded_executor: + cls.executor = MultiThreadedExecutor() + else: + cls.executor = SingleThreadedExecutor() + + cls.executor = SingleThreadedExecutor() + cls.component_manager = ComponentManager(cls.executor, cls.container_name) + cls.executor.add_node(cls.component_manager) + + @classmethod + def tearDownClass(cls) -> None: + cls.executor.remove_node(cls.component_manager) + cls.executor.shutdown() + rclpy.shutdown() + + def list_nodes(self): + res = ListNodes.Response() + req = ListNodes.Request() + self.__class__.component_manager.on_list_node(req, res) + + return res.unique_ids, res.full_node_names + + def test_gen_unique_id(self): + current_index = self.component_manager.gen_unique_id() + assert current_index == 1 # The unique id start from 1 + + def test_list_node(self): + unique_ids, full_node_names = self.list_nodes() + assert len(full_node_names) == 0 + assert len(unique_ids) == 0 + + def test_load_node(self): + mock_res = LoadNode.Response() + mock_req = LoadNode.Request() + mock_req.package_name = TEST_COMPOSITION + mock_req.plugin_name = TEST_COMPOSITION_FOO + self.component_manager.on_load_node(mock_req, mock_res) + + print(mock_res.success, mock_res.error_message) + + unique_ids, full_node_names = self.list_nodes() + assert len(unique_ids) == 1 + assert len(full_node_names) == 1 + + def test_unload_node(self): + mock_res = UnloadNode.Response() + mock_req = UnloadNode.Request() + + # Unload a non-existing node + mock_req.unique_id = 0 + self.component_manager.on_unload_node(mock_req, mock_res) + assert mock_res.error_message + assert (not mock_res.success) + + # Unload the first node + ids, node_names = self.list_nodes() + mock_req.unique_id = ids[0] + # Don't forget to remove the previous test results + mock_res = UnloadNode.Response() + self.component_manager.on_unload_node(mock_req, mock_res) + assert mock_res.success + assert (not mock_res.error_message) + + # There should be (n-1) nodes left + unique_ids, full_node_names = self.list_nodes() + assert len(unique_ids) == len(ids) - 1 + assert len(full_node_names) == len(node_names) - 1 + + +class TestComponentManagerUTMT(TestComponentManagerUT): + use_multi_threaded_executor = True + container_name = 'ut_component_mt' + + +if __name__ == '__main__': + unittest.main() diff --git a/rclpy_components/test/test_copyright.py b/rclpy_components/test/test_copyright.py index cc8ff03f7..6851b977c 100644 --- a/rclpy_components/test/test_copyright.py +++ b/rclpy_components/test/test_copyright.py @@ -1,4 +1,4 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. +# 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. diff --git a/rclpy_components/test/test_flake8.py b/rclpy_components/test/test_flake8.py index 27ee1078f..0cdb78d63 100644 --- a/rclpy_components/test/test_flake8.py +++ b/rclpy_components/test/test_flake8.py @@ -1,4 +1,4 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. +# 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. diff --git a/rclpy_components/test/test_pep257.py b/rclpy_components/test/test_pep257.py index b234a3840..77a96f049 100644 --- a/rclpy_components/test/test_pep257.py +++ b/rclpy_components/test/test_pep257.py @@ -1,4 +1,4 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. +# 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. From 874752b2600e337a8a666bb11fc1cd1a0f59a28e Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Sun, 31 Mar 2024 00:50:38 +0000 Subject: [PATCH 06/19] Move test node into test subpackage Signed-off-by: Shane Loretz --- .../__init__.py => rclpy_components/test/test_foo.py} | 0 rclpy_components/setup.py | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) rename rclpy_components/{rclpy_components_test/__init__.py => rclpy_components/test/test_foo.py} (100%) diff --git a/rclpy_components/rclpy_components_test/__init__.py b/rclpy_components/rclpy_components/test/test_foo.py similarity index 100% rename from rclpy_components/rclpy_components_test/__init__.py rename to rclpy_components/rclpy_components/test/test_foo.py diff --git a/rclpy_components/setup.py b/rclpy_components/setup.py index 5dc11a4f8..d8602ea0a 100644 --- a/rclpy_components/setup.py +++ b/rclpy_components/setup.py @@ -26,7 +26,7 @@ 'component_container_mt = rclpy_components.component_container_mt:main', ], 'rclpy_components': [ - 'test_composition::TestFoo = rclpy_components_test:TestFoo', + 'rclpy_components::TestFoo = rclpy_components.test.test_foo:TestFoo', ] }, ) From d23d9be31e5c01b84f6da16ed9d6c5df664679e2 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Sun, 31 Mar 2024 00:51:18 +0000 Subject: [PATCH 07/19] Remove unused test package marker file Signed-off-by: Shane Loretz --- rclpy_components/setup.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/rclpy_components/setup.py b/rclpy_components/setup.py index d8602ea0a..fef2fa5b0 100644 --- a/rclpy_components/setup.py +++ b/rclpy_components/setup.py @@ -10,8 +10,6 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), - ('share/ament_index/resource_index/packages', - ['resource/' + 'test_composition']) ], install_requires=['setuptools'], zip_safe=True, From 734d35ed97f51fca163a5fe4895f72e431702a8b Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Sun, 31 Mar 2024 00:51:49 +0000 Subject: [PATCH 08/19] Copyright not needed for empty file I think Signed-off-by: Shane Loretz --- rclpy_components/rclpy_components/__init__.py | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/rclpy_components/rclpy_components/__init__.py b/rclpy_components/rclpy_components/__init__.py index a71789ec8..e69de29bb 100644 --- a/rclpy_components/rclpy_components/__init__.py +++ b/rclpy_components/rclpy_components/__init__.py @@ -1,13 +0,0 @@ -# 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 a839f770dccd14dbf370c3fe58e6f76bcc7f0ad9 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Sun, 31 Mar 2024 00:52:37 +0000 Subject: [PATCH 09/19] use non-deprecated underscore script_dir and install_scripts Signed-off-by: Shane Loretz --- rclpy_components/setup.cfg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclpy_components/setup.cfg b/rclpy_components/setup.cfg index 38a4b02f9..4d3f2656a 100644 --- a/rclpy_components/setup.cfg +++ b/rclpy_components/setup.cfg @@ -1,4 +1,4 @@ [develop] -script-dir=$base/lib/rclpy_components +script_dir=$base/lib/rclpy_components [install] -install-scripts=$base/lib/rclpy_components +install_scripts=$base/lib/rclpy_components From 57092eab6b7ee5b3d0478ed4c69f40871fdeac77 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Sun, 31 Mar 2024 00:52:59 +0000 Subject: [PATCH 10/19] Add __init__.py for test subpackage Signed-off-by: Shane Loretz --- rclpy_components/rclpy_components/test/__init__.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 rclpy_components/rclpy_components/test/__init__.py 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 From 96a2321f88c32d72424175554ad67d09d0c2c3e4 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Sun, 31 Mar 2024 08:08:32 -0700 Subject: [PATCH 11/19] Consolidate unit tests into one Signed-off-by: Shane Loretz --- .../test/test_component_manager_ut.py | 110 ------------------ 1 file changed, 110 deletions(-) delete mode 100644 rclpy_components/test/test_component_manager_ut.py diff --git a/rclpy_components/test/test_component_manager_ut.py b/rclpy_components/test/test_component_manager_ut.py deleted file mode 100644 index 7357fa67e..000000000 --- a/rclpy_components/test/test_component_manager_ut.py +++ /dev/null @@ -1,110 +0,0 @@ -# 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 unittest -import rclpy -from composition_interfaces.srv import ListNodes, LoadNode, UnloadNode -from rclpy.executors import Executor, SingleThreadedExecutor, MultiThreadedExecutor -from rclpy_components.component_manager import ComponentManager - -TEST_COMPOSITION = 'test_composition' -TEST_COMPOSITION_FOO = 'test_composition::TestFoo' - - -class TestComponentManagerUT(unittest.TestCase): - executor: Executor - component_manager: ComponentManager - use_multi_threaded_executor: bool = False - container_name: str = "ut_container" - - @classmethod - def setUpClass(cls) -> None: - rclpy.init() - - if cls.use_multi_threaded_executor: - cls.executor = MultiThreadedExecutor() - else: - cls.executor = SingleThreadedExecutor() - - cls.executor = SingleThreadedExecutor() - cls.component_manager = ComponentManager(cls.executor, cls.container_name) - cls.executor.add_node(cls.component_manager) - - @classmethod - def tearDownClass(cls) -> None: - cls.executor.remove_node(cls.component_manager) - cls.executor.shutdown() - rclpy.shutdown() - - def list_nodes(self): - res = ListNodes.Response() - req = ListNodes.Request() - self.__class__.component_manager.on_list_node(req, res) - - return res.unique_ids, res.full_node_names - - def test_gen_unique_id(self): - current_index = self.component_manager.gen_unique_id() - assert current_index == 1 # The unique id start from 1 - - def test_list_node(self): - unique_ids, full_node_names = self.list_nodes() - assert len(full_node_names) == 0 - assert len(unique_ids) == 0 - - def test_load_node(self): - mock_res = LoadNode.Response() - mock_req = LoadNode.Request() - mock_req.package_name = TEST_COMPOSITION - mock_req.plugin_name = TEST_COMPOSITION_FOO - self.component_manager.on_load_node(mock_req, mock_res) - - print(mock_res.success, mock_res.error_message) - - unique_ids, full_node_names = self.list_nodes() - assert len(unique_ids) == 1 - assert len(full_node_names) == 1 - - def test_unload_node(self): - mock_res = UnloadNode.Response() - mock_req = UnloadNode.Request() - - # Unload a non-existing node - mock_req.unique_id = 0 - self.component_manager.on_unload_node(mock_req, mock_res) - assert mock_res.error_message - assert (not mock_res.success) - - # Unload the first node - ids, node_names = self.list_nodes() - mock_req.unique_id = ids[0] - # Don't forget to remove the previous test results - mock_res = UnloadNode.Response() - self.component_manager.on_unload_node(mock_req, mock_res) - assert mock_res.success - assert (not mock_res.error_message) - - # There should be (n-1) nodes left - unique_ids, full_node_names = self.list_nodes() - assert len(unique_ids) == len(ids) - 1 - assert len(full_node_names) == len(node_names) - 1 - - -class TestComponentManagerUTMT(TestComponentManagerUT): - use_multi_threaded_executor = True - container_name = 'ut_component_mt' - - -if __name__ == '__main__': - unittest.main() From 85e6f60705a8b2892480228f7152d9597a961612 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Sun, 31 Mar 2024 08:09:01 -0700 Subject: [PATCH 12/19] flake8 fixes Signed-off-by: Shane Loretz --- rclpy_components/rclpy_components/component_container.py | 4 ++-- rclpy_components/rclpy_components/component_container_mt.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rclpy_components/rclpy_components/component_container.py b/rclpy_components/rclpy_components/component_container.py index 0d7f11433..1ddbc5688 100644 --- a/rclpy_components/rclpy_components/component_container.py +++ b/rclpy_components/rclpy_components/component_container.py @@ -14,6 +14,7 @@ import rclpy from rclpy.executors import SingleThreadedExecutor + from .component_manager import ComponentManager @@ -21,14 +22,13 @@ def main(): rclpy.init() executor = SingleThreadedExecutor() - component_manager = ComponentManager(executor, "PyComponentManager") + component_manager = ComponentManager(executor, 'PyComponentManager') executor.add_node(component_manager) try: executor.spin() except KeyboardInterrupt: print('KeyboardInterrupt received, exit') - pass component_manager.destroy_node() rclpy.shutdown() diff --git a/rclpy_components/rclpy_components/component_container_mt.py b/rclpy_components/rclpy_components/component_container_mt.py index 3c40c554f..3d9feccb2 100644 --- a/rclpy_components/rclpy_components/component_container_mt.py +++ b/rclpy_components/rclpy_components/component_container_mt.py @@ -14,6 +14,7 @@ import rclpy from rclpy.executors import MultiThreadedExecutor + from .component_manager import ComponentManager @@ -21,7 +22,7 @@ def main(): rclpy.init() executor = MultiThreadedExecutor() - component_manager = ComponentManager(executor, "PyComponentManager") + component_manager = ComponentManager(executor, 'PyComponentManager') executor.add_node(component_manager) @@ -29,7 +30,6 @@ def main(): executor.spin() except KeyboardInterrupt: print('KeyboardInterrupt received, exit') - pass component_manager.destroy_node() rclpy.shutdown() From c2921d7341a7279cb9dbac4784de892fdcf935b3 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Sun, 31 Mar 2024 08:10:18 -0700 Subject: [PATCH 13/19] Rename TestFoo to FooNode to avoid pytest trying to load tests from it Signed-off-by: Shane Loretz --- rclpy_components/rclpy_components/test/test_foo.py | 3 ++- rclpy_components/setup.py | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/rclpy_components/rclpy_components/test/test_foo.py b/rclpy_components/rclpy_components/test/test_foo.py index 90b535c56..2a8f3a46c 100644 --- a/rclpy_components/rclpy_components/test/test_foo.py +++ b/rclpy_components/rclpy_components/test/test_foo.py @@ -15,6 +15,7 @@ from rclpy.node import Node -class TestFoo(Node): +class FooNode(Node): + def __init__(self, node_name='test_foo', **kwargs): super().__init__(node_name, **kwargs) diff --git a/rclpy_components/setup.py b/rclpy_components/setup.py index fef2fa5b0..ee1fdacd6 100644 --- a/rclpy_components/setup.py +++ b/rclpy_components/setup.py @@ -1,4 +1,4 @@ -from setuptools import setup, find_packages +from setuptools import find_packages, setup package_name = 'rclpy_components' @@ -24,7 +24,7 @@ 'component_container_mt = rclpy_components.component_container_mt:main', ], 'rclpy_components': [ - 'rclpy_components::TestFoo = rclpy_components.test.test_foo:TestFoo', + 'rclpy_components::FooNode = rclpy_components.test.test_foo:FooNode', ] }, ) From ed26ff1d4c330c56a5b00b7cd3601583f2b3368c Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Sun, 31 Mar 2024 08:10:52 -0700 Subject: [PATCH 14/19] Rewrite tests to use pytest and make them independent of each other Signed-off-by: Shane Loretz --- .../test/test_component_manager.py | 373 ++++++++++-------- 1 file changed, 201 insertions(+), 172 deletions(-) diff --git a/rclpy_components/test/test_component_manager.py b/rclpy_components/test/test_component_manager.py index a05674ec3..05d510815 100644 --- a/rclpy_components/test/test_component_manager.py +++ b/rclpy_components/test/test_component_manager.py @@ -12,187 +12,216 @@ # See the License for the specific language governing permissions and # limitations under the License. -import unittest -import rclpy -from multiprocessing import Process -from rclpy.client import Client -from rclpy.executors import SingleThreadedExecutor, MultiThreadedExecutor -from rclpy.node import Node +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 -TEST_COMPOSITION = 'test_composition' -TEST_COMPOSITION_FOO = 'test_composition::TestFoo' -TEST_COMPOSITION_NODE_NAME = '/testfoo' +class FakeEntryPoint: + name: str + value: str + group: str + mock_class: MagicMock -def run_container(container_name, multi_thread): - rclpy.init() - if multi_thread: - executor = MultiThreadedExecutor() - else: - executor = SingleThreadedExecutor() + 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): - component_manager = ComponentManager(executor, container_name) + 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) - try: - executor.spin() - except Exception as e: - print(e) - pass + 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() -class TestComponentManager(unittest.TestCase): - helper_node: Node = None - container_process: Process = None - - container_name = 'TestComponentManager' - # service names & clients will be generated with container_name - load_node_svc_name = "" - unload_node_svc_name = "" - list_node_svc_name = "" - load_cli: Client = None - unload_cli: Client = None - list_cli: Client = None - - use_multi_threaded_executor = False - - @classmethod - def setUpClass(cls): - cls.load_node_svc_name = f'{cls.container_name}/_container/load_node' - cls.unload_node_svc_name = f'{cls.container_name}/_container/unload_node' - cls.list_node_svc_name = f'{cls.container_name}/_container/list_nodes' - - # Start the test component manager in the background - cls.container_process = Process(target=run_container, - args=(cls.container_name, cls.use_multi_threaded_executor)) - cls.container_process.start() - - # Setup the helper_node, which will help create client and test the services - rclpy.init() - cls.helper_node = rclpy.create_node('helper_node') - cls.load_cli = cls.helper_node.create_client(LoadNode, cls.load_node_svc_name) - cls.unload_cli = cls.helper_node.create_client(UnloadNode, cls.unload_node_svc_name) - cls.list_cli = cls.helper_node.create_client(ListNodes, cls.list_node_svc_name) - - @classmethod - def tearDownClass(cls): - cls.container_process.terminate() - cls.load_cli.destroy() - cls.unload_cli.destroy() - cls.list_cli.destroy() - cls.helper_node.destroy_node() - rclpy.shutdown() - - @classmethod - def load_node(cls, package_name, plugin_name, node_name="", node_namespace=""): - if not cls.load_cli.wait_for_service(timeout_sec=5.0): - raise RuntimeError(f'No load service found in /{cls.container_name}') - - load_req = LoadNode.Request() - load_req.package_name = package_name - load_req.plugin_name = plugin_name - - if node_name != "": - load_req.node_name = node_name - if node_namespace != "": - load_req.node_namespace = node_namespace - - future = cls.load_cli.call_async(load_req) - rclpy.spin_until_future_complete(cls.helper_node, future) - return future.result() - - @classmethod - def unload_node(cls, unique_id): - if not cls.unload_cli.wait_for_service(timeout_sec=5.0): - raise RuntimeError(f'No unload service found in /{cls.container_name}') - - unload_req = UnloadNode.Request() - unload_req.unique_id = unique_id - - future = cls.unload_cli.call_async(unload_req) - rclpy.spin_until_future_complete(cls.helper_node, future) - return future.result() - - @classmethod - def list_nodes(cls): - if not cls.list_cli.wait_for_service(timeout_sec=5.0): - raise RuntimeError(f'No list service found in {cls.container_name}') - list_req = ListNodes.Request() - future = cls.list_cli.call_async(list_req) - rclpy.spin_until_future_complete(cls.helper_node, future) - return future.result() - - def load_node_test(self): - load_res = self.__class__.load_node(TEST_COMPOSITION, TEST_COMPOSITION_FOO) - assert load_res.success is True - assert load_res.error_message == "" - assert load_res.unique_id == 1 - assert load_res.full_node_name == TEST_COMPOSITION_NODE_NAME - - node_name = "renamed_node" - node_ns = 'testing_ns' - remap_load_res = self.__class__.load_node( - TEST_COMPOSITION, TEST_COMPOSITION_FOO, node_name=node_name, - node_namespace=node_ns) - assert remap_load_res.success is True - assert remap_load_res.error_message == "" - assert remap_load_res.unique_id == 2 - assert remap_load_res.full_node_name == f'/{node_ns}/{node_name}' - - list_res: ListNodes.Response = self.__class__.list_nodes() - assert len(list_res.unique_ids) == len(list_res.full_node_names) == 2 - - def unload_node_test(self): - if not self.__class__.unload_cli.wait_for_service(timeout_sec=5.0): - raise RuntimeError(f'no unload service found in {self.__class__.container_name}') - - unload_res: UnloadNode.Response = self.__class__.unload_node(1) - assert unload_res.success is True - assert unload_res.error_message == "" - - # Should be only 1 node left - list_res: ListNodes.Response = self.__class__.list_nodes() - assert len(list_res.full_node_names) == len(list_res.unique_ids) == 1 - - # The index definitely won't exist - unload_res: UnloadNode.Response = self.__class__.unload_node(1000) - assert unload_res.error_message != "" - assert unload_res.success is False - list_res: ListNodes.Response = self.__class__.list_nodes() - assert len(list_res.full_node_names) == len(list_res.unique_ids) == 1 - - # Unload the last node - unload_req = UnloadNode.Request() - unload_req.unique_id = 2 - future = self.__class__.unload_cli.call_async(unload_req) - rclpy.spin_until_future_complete(self.__class__.helper_node, future) - unload_res: UnloadNode.Response = future.result() - assert unload_res.success is True - assert unload_res.error_message == "" - - # Won't be any node in the container - list_res: ListNodes.Response = self.__class__.list_nodes() - assert len(list_res.full_node_names) == len(list_res.unique_ids) == 0 - - def list_nodes_test(self): - container_name = self.__class__.container_name - print(f'{container_name}: list_nodes tested within test_load_node and test_unload_node') - - def test_composition_api(self): - # Control the order of test - self.load_node_test() - self.unload_node_test() - self.list_nodes_test() - - -class TestComponentManagerMT(TestComponentManager): - use_multi_threaded_executor = True - container_name = 'TestComponentManagerMT' - - -if __name__ == '__main__': - unittest.main() +@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 From c4cf1823ee7783a2bcb3f8033e7a55a10df46c4f Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Sun, 31 Mar 2024 08:11:16 -0700 Subject: [PATCH 15/19] Misc flake8 fixes and bug fixes Signed-off-by: Shane Loretz --- .../rclpy_components/component_manager.py | 100 +++++++++++------- 1 file changed, 64 insertions(+), 36 deletions(-) diff --git a/rclpy_components/rclpy_components/component_manager.py b/rclpy_components/rclpy_components/component_manager.py index 0662f2171..5c7671fa4 100644 --- a/rclpy_components/rclpy_components/component_manager.py +++ b/rclpy_components/rclpy_components/component_manager.py @@ -12,64 +12,83 @@ # See the License for the specific language governing permissions and # limitations under the License. -from rclpy.node import Node -from rclpy.executors import Executor -from rclpy.exceptions import InvalidNodeNameException, InvalidNamespaceException +from importlib.metadata import entry_points + from composition_interfaces.srv import ListNodes, LoadNode, UnloadNode -try: - from importlib.metadata import entry_points -except ImportError: - from importlib_metadata import entry_points +from rclpy.executors import Executor +from rclpy.node import Node class ComponentManager(Node): - def __init__(self, executor: Executor, name="py_component_manager", **kwargs): + 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) + ListNodes, '~/_container/list_nodes', self.on_list_node) self._load_node_srv = self.create_service( - LoadNode, "~/_container/load_node", self.on_load_node) + LoadNode, '~/_container/load_node', self.on_load_node) self._unload_node_srv = self.create_service( - UnloadNode, "~/_container/unload_node", self.on_unload_node) + 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 - 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 gen_unique_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): - res.unique_ids = [int(key) for key in self.components.keys()] - res.full_node_names = [v[0] for v in self.components.values()] + 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().get('rclpy_components', None) + 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 == req.plugin_name: + if ep.name == request_ep_name: component_entry_point = ep break if not component_entry_point: res.success = False - res.error_message = f'No rclpy component found by {req.plugin_name}' + 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 - component_class = component_entry_point.load() - node_name = req.node_name if req.node_name else \ - str.lower(str.split(component_entry_point.value, ':')[1]) + 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} if req.parameters: @@ -83,31 +102,40 @@ def on_load_node(self, req: LoadNode.Request, res: LoadNode.Response): for rule in req.remap_rules: params_dict['cli_args'].extend(['-r', rule]) - try: - self.get_logger().info( + self.get_logger().info( f'Instantiating {component_entry_point.value} with {node_name}, {params_dict}') + try: component = component_class(node_name, **params_dict) - res.unique_id = self.gen_unique_id() - res.full_node_name = component.get_fully_qualified_name() - res.success = True - self.components[res.unique_id] = (res.full_node_name, component) - self.executor.add_node(component) - return res - except (InvalidNodeNameException, InvalidNamespaceException, TypeError) as e: + 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: + if uid not in self._components: res.success = False - res.error_message = f'No node found with unique_id: {uid}' + res.error_message = f'No node found with id: {uid}' return res - _, component_instance = self.components.pop(uid) + _, component_instance = self._components.pop(uid) self.executor.remove_node(component_instance) res.success = True return res From 1e9478822f34d8630edc02fcdfcb286447d4c132 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Sun, 31 Mar 2024 08:13:21 -0700 Subject: [PATCH 16/19] test_foo.py -> foo_node.py Signed-off-by: Shane Loretz --- .../rclpy_components/test/{test_foo.py => foo_node.py} | 0 rclpy_components/setup.py | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) rename rclpy_components/rclpy_components/test/{test_foo.py => foo_node.py} (100%) diff --git a/rclpy_components/rclpy_components/test/test_foo.py b/rclpy_components/rclpy_components/test/foo_node.py similarity index 100% rename from rclpy_components/rclpy_components/test/test_foo.py rename to rclpy_components/rclpy_components/test/foo_node.py diff --git a/rclpy_components/setup.py b/rclpy_components/setup.py index ee1fdacd6..956ecef08 100644 --- a/rclpy_components/setup.py +++ b/rclpy_components/setup.py @@ -24,7 +24,7 @@ 'component_container_mt = rclpy_components.component_container_mt:main', ], 'rclpy_components': [ - 'rclpy_components::FooNode = rclpy_components.test.test_foo:FooNode', + 'rclpy_components::FooNode = rclpy_components.test.foo_node:FooNode', ] }, ) From a9dd0e753a8c422931c86022f0de0a9de6a0b5ba Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Sun, 31 Mar 2024 08:50:00 -0700 Subject: [PATCH 17/19] Make node name and namespace be passed through remapping args Signed-off-by: Shane Loretz --- .../rclpy_components/component_manager.py | 17 ++++++++++---- .../rclpy_components/test/foo_node.py | 4 ++-- .../test/test_component_manager.py | 23 +++++++++++++++++++ 3 files changed, 38 insertions(+), 6 deletions(-) diff --git a/rclpy_components/rclpy_components/component_manager.py b/rclpy_components/rclpy_components/component_manager.py index 5c7671fa4..329048eb0 100644 --- a/rclpy_components/rclpy_components/component_manager.py +++ b/rclpy_components/rclpy_components/component_manager.py @@ -90,22 +90,31 @@ def on_load_node(self, req: LoadNode.Request, res: LoadNode.Response): # 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} + 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['namespace'] = 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: - params_dict['cli_args'] = ['--ros-args'] 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(node_name, **params_dict) + component = component_class(**params_dict) except Exception as e: error_message = str(e) self.get_logger().error(f'Failed to instantiate node: {error_message}') diff --git a/rclpy_components/rclpy_components/test/foo_node.py b/rclpy_components/rclpy_components/test/foo_node.py index 2a8f3a46c..08dd1bc4c 100644 --- a/rclpy_components/rclpy_components/test/foo_node.py +++ b/rclpy_components/rclpy_components/test/foo_node.py @@ -17,5 +17,5 @@ class FooNode(Node): - def __init__(self, node_name='test_foo', **kwargs): - super().__init__(node_name, **kwargs) + def __init__(self, **kwargs): + super().__init__('test_foo', **kwargs) diff --git a/rclpy_components/test/test_component_manager.py b/rclpy_components/test/test_component_manager.py index 05d510815..8c9211c71 100644 --- a/rclpy_components/test/test_component_manager.py +++ b/rclpy_components/test/test_component_manager.py @@ -225,3 +225,26 @@ def test_load_list_unload_via_services(component_manager, node_and_context): 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 From 8b1066f01c7417bb9b17ee17750d2e4c251c7c26 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Sun, 31 Mar 2024 08:53:00 -0700 Subject: [PATCH 18/19] Update author an maintainer entries Signed-off-by: Shane Loretz --- rclpy_components/package.xml | 4 +++- rclpy_components/setup.py | 4 ++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/rclpy_components/package.xml b/rclpy_components/package.xml index d71de5b8b..ca59bb11c 100644 --- a/rclpy_components/package.xml +++ b/rclpy_components/package.xml @@ -4,7 +4,9 @@ rclpy_components 1.1.0 The dynamic node management package - Zhen Ju + Zhen Ju + Aditya Pande + Shane Loretz Apache License 2.0 rclpy diff --git a/rclpy_components/setup.py b/rclpy_components/setup.py index 956ecef08..e0a4b3e44 100644 --- a/rclpy_components/setup.py +++ b/rclpy_components/setup.py @@ -13,8 +13,8 @@ ], install_requires=['setuptools'], zip_safe=True, - maintainer='Zhen Ju', - maintainer_email='juzhen@huawei.com', + 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'], From dcb7da625b47be606928051329e89c6fd015102d Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Sun, 31 Mar 2024 08:54:13 -0700 Subject: [PATCH 19/19] Update description for package Signed-off-by: Shane Loretz --- rclpy_components/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclpy_components/package.xml b/rclpy_components/package.xml index ca59bb11c..5c5c0122e 100644 --- a/rclpy_components/package.xml +++ b/rclpy_components/package.xml @@ -3,7 +3,7 @@ rclpy_components 1.1.0 - The dynamic node management package + Enables dynamically composing rcly nodes into the same process. Zhen Ju Aditya Pande Shane Loretz