From 529ee58518e0348a1045a7a5804921bd1c72309b Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Mon, 11 Jun 2018 18:07:38 -0700 Subject: [PATCH] add ros2 lifecycle (#97) * add ros2 lifecycle * remove debug print --- ros2lifecycle/package.xml | 28 +++ ros2lifecycle/ros2lifecycle/__init__.py | 0 ros2lifecycle/ros2lifecycle/api/__init__.py | 168 ++++++++++++++++++ .../ros2lifecycle/command/__init__.py | 0 .../ros2lifecycle/command/lifecycle.py | 40 +++++ ros2lifecycle/ros2lifecycle/verb/__init__.py | 44 +++++ ros2lifecycle/ros2lifecycle/verb/get.py | 95 ++++++++++ ros2lifecycle/ros2lifecycle/verb/list.py | 41 +++++ ros2lifecycle/ros2lifecycle/verb/set.py | 87 +++++++++ ros2lifecycle/setup.py | 41 +++++ ros2lifecycle/test/test_copyright.py | 23 +++ ros2lifecycle/test/test_flake8.py | 23 +++ ros2lifecycle/test/test_pep257.py | 23 +++ 13 files changed, 613 insertions(+) create mode 100644 ros2lifecycle/package.xml create mode 100644 ros2lifecycle/ros2lifecycle/__init__.py create mode 100644 ros2lifecycle/ros2lifecycle/api/__init__.py create mode 100644 ros2lifecycle/ros2lifecycle/command/__init__.py create mode 100644 ros2lifecycle/ros2lifecycle/command/lifecycle.py create mode 100644 ros2lifecycle/ros2lifecycle/verb/__init__.py create mode 100644 ros2lifecycle/ros2lifecycle/verb/get.py create mode 100644 ros2lifecycle/ros2lifecycle/verb/list.py create mode 100644 ros2lifecycle/ros2lifecycle/verb/set.py create mode 100644 ros2lifecycle/setup.py create mode 100644 ros2lifecycle/test/test_copyright.py create mode 100644 ros2lifecycle/test/test_flake8.py create mode 100644 ros2lifecycle/test/test_pep257.py diff --git a/ros2lifecycle/package.xml b/ros2lifecycle/package.xml new file mode 100644 index 000000000..83a1e344b --- /dev/null +++ b/ros2lifecycle/package.xml @@ -0,0 +1,28 @@ + + + + ros2lifecycle + 0.4.0 + + The lifecycle command for ROS 2 command line tools. + + Dirk Thomas + Apache License 2.0 + + rcl_interfaces + rclpy + ros2cli + + python3-yaml + ros2node + ros2service + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/ros2lifecycle/ros2lifecycle/__init__.py b/ros2lifecycle/ros2lifecycle/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/ros2lifecycle/ros2lifecycle/api/__init__.py b/ros2lifecycle/ros2lifecycle/api/__init__.py new file mode 100644 index 000000000..6ad419593 --- /dev/null +++ b/ros2lifecycle/ros2lifecycle/api/__init__.py @@ -0,0 +1,168 @@ +# Copyright 2018 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 lifecycle_msgs.srv import ChangeState +from lifecycle_msgs.srv import GetAvailableTransitions +from lifecycle_msgs.srv import GetState +from ros2node.api import get_node_names as get_all_node_names +from ros2service.api import get_service_names_and_types + + +def get_node_names(*, node, include_hidden_nodes=False): + node_names = get_all_node_names( + node=node, include_hidden_nodes=include_hidden_nodes) + service_names_and_types = get_service_names_and_types(node=node) + return [ + n for n in node_names + if _has_lifecycle(n, service_names_and_types)] + + +def _has_lifecycle(node_name, service_names_and_types): + for (service_name, service_types) in service_names_and_types: + if ( + service_name == '/{node_name}/get_state'.format_map(locals()) and + 'lifecycle_msgs/GetState' in service_types + ): + return True + return False + + +def call_get_states(*, node, node_names): + clients = {} + futures = {} + # create clients + for node_name in node_names: + client = node.create_client( + GetState, + '/{node_name}/get_state'.format_map(locals())) + clients[node_name] = client + + # wait until all clients have been called + while True: + for node_name in [n for n in node_names if n not in futures]: + # call as soon as ready + client = clients[node_name] + if client.service_is_ready(): + request = GetState.Request() + request.node_name = node_name + future = client.call_async(request) + futures[node_name] = future + + if len(futures) == len(clients): + break + rclpy.spin_once(node, timeout_sec=1.0) + + # wait for all responses + for future in futures.values(): + rclpy.spin_until_future_complete(node, future) + + # return current state or exception for each node + states = {} + for node_name, future in futures.items(): + if future.result() is not None: + response = future.result() + states[node_name] = response.current_state + else: + states[node_name] = future.exception() + return states + + +def call_get_available_transitions(*, node, states): + clients = {} + futures = {} + # create clients + for node_name in states.keys(): + client = node.create_client( + GetAvailableTransitions, + '/{node_name}/get_available_transitions'.format_map(locals())) + clients[node_name] = client + + # wait until all clients have been called + while True: + for node_name in [n for n in states.keys() if n not in futures]: + # call as soon as ready + client = clients[node_name] + if client.service_is_ready(): + request = GetAvailableTransitions.Request() + request.node_name = node_name + future = client.call_async(request) + futures[node_name] = future + + if len(futures) == len(clients): + break + rclpy.spin_once(node, timeout_sec=1.0) + + # wait for all responses + for future in futures.values(): + rclpy.spin_until_future_complete(node, future) + + # return transitions from current state or exception for each node + transitions = {} + for node_name, future in futures.items(): + if future.result() is not None: + response = future.result() + transitions[node_name] = [] + for transition_description in response.available_transitions: + if ( + states[node_name] is None or + transition_description.start_state == states[node_name] + ): + transitions[node_name].append( + transition_description.transition) + else: + transitions[node_name] = future.exception() + return transitions + + +def call_change_states(*, node, transitions): + clients = {} + futures = {} + # create clients + for node_name in transitions.keys(): + client = node.create_client( + ChangeState, + '/{node_name}/change_state'.format_map(locals())) + clients[node_name] = client + + # wait until all clients have been called + while True: + for node_name in [n for n in transitions.keys() if n not in futures]: + # call as soon as ready + client = clients[node_name] + if client.service_is_ready(): + request = ChangeState.Request() + request.node_name = node_name + request.transition = transitions[node_name] + future = client.call_async(request) + futures[node_name] = future + + if len(futures) == len(clients): + break + rclpy.spin_once(node, timeout_sec=1.0) + + # wait for all responses + for future in futures.values(): + rclpy.spin_until_future_complete(node, future) + + # return success flag or exception for each node + results = {} + for node_name, future in futures.items(): + if future.result() is not None: + response = future.result() + results[node_name] = response.success + else: + results[node_name] = future.exception() + return results diff --git a/ros2lifecycle/ros2lifecycle/command/__init__.py b/ros2lifecycle/ros2lifecycle/command/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/ros2lifecycle/ros2lifecycle/command/lifecycle.py b/ros2lifecycle/ros2lifecycle/command/lifecycle.py new file mode 100644 index 000000000..1a9ad8c26 --- /dev/null +++ b/ros2lifecycle/ros2lifecycle/command/lifecycle.py @@ -0,0 +1,40 @@ +# Copyright 2018 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 ros2cli.command import add_subparsers +from ros2cli.command import CommandExtension +from ros2cli.verb import get_verb_extensions + + +class LifecycleCommand(CommandExtension): + """Various lifecycle related sub-commands.""" + + def add_arguments(self, parser, cli_name): + self._subparser = parser + + # get verb extensions and let them add their arguments + verb_extensions = get_verb_extensions('ros2lifecycle.verb') + add_subparsers( + parser, cli_name, '_verb', verb_extensions, required=False) + + def main(self, *, parser, args): + if not hasattr(args, '_verb'): + # in case no verb was passed + self._subparser.print_help() + return 0 + + extension = getattr(args, '_verb') + + # call the verb's main method + return extension.main(args=args) diff --git a/ros2lifecycle/ros2lifecycle/verb/__init__.py b/ros2lifecycle/ros2lifecycle/verb/__init__.py new file mode 100644 index 000000000..15c7fc2f4 --- /dev/null +++ b/ros2lifecycle/ros2lifecycle/verb/__init__.py @@ -0,0 +1,44 @@ +# Copyright 2018 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 ros2cli.plugin_system import PLUGIN_SYSTEM_VERSION +from ros2cli.plugin_system import satisfies_version + + +class VerbExtension: + """ + The extension point for 'lifecycle' verb extensions. + + The following properties must be defined: + * `NAME` (will be set to the entry point name) + + The following methods must be defined: + * `main` + + The following methods can be defined: + * `add_arguments` + """ + + NAME = None + EXTENSION_POINT_VERSION = '0.1' + + def __init__(self): + super(VerbExtension, self).__init__() + satisfies_version(PLUGIN_SYSTEM_VERSION, '^0.1') + + def add_arguments(self, parser, cli_name): + pass + + def main(self, *, args): + raise NotImplementedError() diff --git a/ros2lifecycle/ros2lifecycle/verb/get.py b/ros2lifecycle/ros2lifecycle/verb/get.py new file mode 100644 index 000000000..317bbd418 --- /dev/null +++ b/ros2lifecycle/ros2lifecycle/verb/get.py @@ -0,0 +1,95 @@ +# Copyright 2018 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 sys + +from ros2cli.node.direct import DirectNode +from ros2cli.node.strategy import add_arguments +from ros2cli.node.strategy import NodeStrategy +from ros2lifecycle.api import call_get_available_transitions +from ros2lifecycle.api import call_get_states +from ros2lifecycle.api import get_node_names +from ros2lifecycle.verb import VerbExtension +from ros2node.api import NodeNameCompleter + + +class GetVerb(VerbExtension): + """Get lifecycle state.""" + + def add_arguments(self, parser, cli_name): # noqa: D102 + add_arguments(parser) + arg = parser.add_argument( + 'node_name', nargs='?', help='Name of the ROS node') + arg.completer = NodeNameCompleter( + include_hidden_nodes_key='include_hidden_nodes') + parser.add_argument( + '--include-hidden-nodes', action='store_true', + help='Consider hidden nodes as well') + parser.add_argument( + '--transitions', action='store_true', + help='Output possible transitions') + + def main(self, *, args): # noqa: D102 + with NodeStrategy(args) as node: + node_names = get_node_names( + node=node, include_hidden_nodes=args.include_hidden_nodes) + + if args.node_name: + if args.node_name not in node_names: + return 'Node not found' + node_names = [args.node_name] + + with DirectNode(args) as node: + states = call_get_states(node=node, node_names=node_names) + + # output exceptions + for node_name in sorted(states.keys()): + state = states[node_name] + if isinstance(state, Exception): + print( + 'Exception while calling service of node ' + "'{node_name}': {state}".format_map(locals()), + file=sys.stderr) + del states[node_name] + if args.node_name: + return 1 + + if args.transitions: + transitions = call_get_available_transitions( + node=node, states=states) + + # output current states + for node_name in sorted(states.keys()): + state = states[node_name] + prefix = '' + if not args.node_name: + prefix = '{node_name}: '.format_map(locals()) + print( + prefix + '{state.label} [{state.id}]'.format_map(locals())) + + if args.transitions: + if isinstance(transitions[node_name], Exception): + print( + 'Exception while calling service of node ' + "'{node_name}': {transitions[node_name]}" + .format_map(locals()), file=sys.stderr) + if args.node_name: + return 1 + elif transitions[node_name]: + for transition in transitions[node_name]: + print( + '-> {transition.label} [{transition.id}]' + .format_map(locals())) + else: + print(' no transitions available') diff --git a/ros2lifecycle/ros2lifecycle/verb/list.py b/ros2lifecycle/ros2lifecycle/verb/list.py new file mode 100644 index 000000000..fa4ce5d08 --- /dev/null +++ b/ros2lifecycle/ros2lifecycle/verb/list.py @@ -0,0 +1,41 @@ +# Copyright 2018 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 ros2cli.node.strategy import add_arguments +from ros2cli.node.strategy import NodeStrategy +from ros2lifecycle.api import get_node_names +from ros2lifecycle.verb import VerbExtension + + +class ListVerb(VerbExtension): + """Output a list of nodes with lifecycle.""" + + def add_arguments(self, parser, cli_name): # noqa: D102 + add_arguments(parser) + parser.add_argument( + '-a', '--all', action='store_true', + help='Display all nodes even hidden ones') + parser.add_argument( + '-c', '--count-nodes', action='store_true', + help='Only display the number of nodes discovered') + + def main(self, *, args): # noqa: D102 + with NodeStrategy(args) as node: + node_names = get_node_names( + node=node, include_hidden_nodes=args.all) + + if args.count_nodes: + print(len(node_names)) + elif node_names: + print(*node_names, sep='\n') diff --git a/ros2lifecycle/ros2lifecycle/verb/set.py b/ros2lifecycle/ros2lifecycle/verb/set.py new file mode 100644 index 000000000..677b293a5 --- /dev/null +++ b/ros2lifecycle/ros2lifecycle/verb/set.py @@ -0,0 +1,87 @@ +# Copyright 2018 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 sys + +from ros2cli.node.direct import DirectNode +from ros2cli.node.strategy import add_arguments +from ros2cli.node.strategy import NodeStrategy +from ros2lifecycle.api import call_change_states +from ros2lifecycle.api import call_get_available_transitions +from ros2node.api import get_node_names +from ros2node.api import NodeNameCompleter +from ros2param.verb import VerbExtension + + +class SetVerb(VerbExtension): + """Trigger lifecycle state transition.""" + + def add_arguments(self, parser, cli_name): # noqa: D102 + add_arguments(parser) + arg = parser.add_argument( + 'node_name', help='Name of the ROS node') + arg.completer = NodeNameCompleter( + include_hidden_nodes_key='include_hidden_nodes') + parser.add_argument( + '--include-hidden-nodes', action='store_true', + help='Consider hidden nodes as well') + parser.add_argument( + 'transition', help='The lifecycle transition') + + def main(self, *, args): # noqa: D102 + with NodeStrategy(args) as node: + node_names = get_node_names( + node=node, include_hidden_nodes=args.include_hidden_nodes) + + if args.node_name not in node_names: + return 'Node not found' + + with DirectNode(args) as node: + transitions = call_get_available_transitions( + node=node, states={args.node_name: None}) + transitions = transitions[args.node_name] + if isinstance(transitions, Exception): + return 'Exception while calling service of node ' \ + "'{args.node_name}': {transitions}" \ + .format_map(locals()) + + # identify requested transition + for transition in transitions: + if transition.label == args.transition: + break + else: + for transition in transitions: + if str(transition.id) == args.transition: + break + else: + return \ + 'Unknown transition requested, available ones are:' + \ + ''.join( + '\n- {t.label} [{t.id}]'.format_map(locals()) + for t in transitions) + + results = call_change_states( + node=node, transitions={args.node_name: transition}) + result = results[args.node_name] + + # output response + if isinstance(result, Exception): + print( + 'Exception while calling service of node ' + "'{args.node_name}': {result}" + .format_map(locals()), file=sys.stderr) + elif result: + print('Transitioning successful') + else: + print('Transitioning failed', file=sys.stderr) diff --git a/ros2lifecycle/setup.py b/ros2lifecycle/setup.py new file mode 100644 index 000000000..6f962ccaf --- /dev/null +++ b/ros2lifecycle/setup.py @@ -0,0 +1,41 @@ +from setuptools import find_packages +from setuptools import setup + +setup( + name='ros2lifecycle', + version='0.4.0', + packages=find_packages(exclude=['test']), + install_requires=['ros2cli'], + zip_safe=True, + author='Dirk Thomas', + author_email='dthomas@osrfoundation.org', + maintainer='Dirk Thomas', + maintainer_email='dthomas@osrfoundation.org', + url='https://github.com/ros2/ros2cli/tree/master/ros2lifecycle', + download_url='https://github.com/ros2/ros2cli/releases', + keywords=[], + classifiers=[ + 'Environment :: Console', + 'Intended Audience :: Developers', + 'License :: OSI Approved :: Apache Software License', + 'Programming Language :: Python', + ], + description='The lifecycle command for ROS 2 command line tools.', + long_description="""\ +The package provides the lifecycle command for the ROS 2 command line tools.""", + license='Apache License, Version 2.0', + tests_require=['pytest'], + entry_points={ + 'ros2cli.command': [ + 'lifecycle = ros2lifecycle.command.lifecycle:LifecycleCommand', + ], + 'ros2cli.extension_point': [ + 'ros2lifecycle.verb = ros2lifecycle.verb:VerbExtension', + ], + 'ros2lifecycle.verb': [ + 'get = ros2lifecycle.verb.get:GetVerb', + 'list = ros2lifecycle.verb.list:ListVerb', + 'set = ros2lifecycle.verb.set:SetVerb', + ], + } +) diff --git a/ros2lifecycle/test/test_copyright.py b/ros2lifecycle/test/test_copyright.py new file mode 100644 index 000000000..cf0fae31f --- /dev/null +++ b/ros2lifecycle/test/test_copyright.py @@ -0,0 +1,23 @@ +# 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_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/ros2lifecycle/test/test_flake8.py b/ros2lifecycle/test/test_flake8.py new file mode 100644 index 000000000..eff829969 --- /dev/null +++ b/ros2lifecycle/test/test_flake8.py @@ -0,0 +1,23 @@ +# 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 +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc = main(argv=[]) + assert rc == 0, 'Found errors' diff --git a/ros2lifecycle/test/test_pep257.py b/ros2lifecycle/test/test_pep257.py new file mode 100644 index 000000000..0e38a6c60 --- /dev/null +++ b/ros2lifecycle/test/test_pep257.py @@ -0,0 +1,23 @@ +# 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_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[]) + assert rc == 0, 'Found code style errors / warnings'