From d0cc82a0727f34bcbf8fbbbd7dafb7a85a0496df Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 12 Mar 2019 12:19:30 -0700 Subject: [PATCH 01/20] Add ros2action package Contains ros2cli command 'action' with verbs: list and show. The list verb lists action names for any running action servers and action clients. The show verb prints the definition for a given action type. Signed-off-by: Jacob Perron --- ros2action/package.xml | 27 ++++++++++ ros2action/ros2action/__init__.py | 0 ros2action/ros2action/api/__init__.py | 61 +++++++++++++++++++++++ ros2action/ros2action/command/__init__.py | 0 ros2action/ros2action/command/action.py | 38 ++++++++++++++ ros2action/ros2action/verb/__init__.py | 44 ++++++++++++++++ ros2action/ros2action/verb/list.py | 44 ++++++++++++++++ ros2action/ros2action/verb/show.py | 38 ++++++++++++++ ros2action/setup.py | 43 ++++++++++++++++ ros2action/test/test_api.py | 48 ++++++++++++++++++ ros2action/test/test_copyright.py | 23 +++++++++ ros2action/test/test_flake8.py | 23 +++++++++ ros2action/test/test_pep257.py | 23 +++++++++ 13 files changed, 412 insertions(+) create mode 100644 ros2action/package.xml create mode 100644 ros2action/ros2action/__init__.py create mode 100644 ros2action/ros2action/api/__init__.py create mode 100644 ros2action/ros2action/command/__init__.py create mode 100644 ros2action/ros2action/command/action.py create mode 100644 ros2action/ros2action/verb/__init__.py create mode 100644 ros2action/ros2action/verb/list.py create mode 100644 ros2action/ros2action/verb/show.py create mode 100644 ros2action/setup.py create mode 100644 ros2action/test/test_api.py create mode 100644 ros2action/test/test_copyright.py create mode 100644 ros2action/test/test_flake8.py create mode 100644 ros2action/test/test_pep257.py diff --git a/ros2action/package.xml b/ros2action/package.xml new file mode 100644 index 000000000..b7be5c6bb --- /dev/null +++ b/ros2action/package.xml @@ -0,0 +1,27 @@ + + + + ros2action + 0.6.3 + + The action command for ROS 2 command line tools. + + Dirk Thomas + Apache License 2.0 + + Jacob Perron + + ros2cli + + ament_index_python + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + test_msgs + + + ament_python + + diff --git a/ros2action/ros2action/__init__.py b/ros2action/ros2action/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/ros2action/ros2action/api/__init__.py b/ros2action/ros2action/api/__init__.py new file mode 100644 index 000000000..51fe7bc84 --- /dev/null +++ b/ros2action/ros2action/api/__init__.py @@ -0,0 +1,61 @@ +# Copyright 2019 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 os +import re + +from ament_index_python import get_resource +from ament_index_python import has_resource + + +def get_action_names_and_types(*, node): + service_names_and_types = node.get_service_names_and_types() + # Assumption: actions have a hidden 'send_goal' service with the name: + # '///_action/send_goal' (18 trailing characters) + # And the service type has the suffix '_SendGoal' (9 trailing characters) + action_names_and_types = [ + (name[:-18], [t[:-9] for t in type_with_suffix]) + for (name, type_with_suffix) in service_names_and_types + if re.match('.*/_action/send_goal$', name)] + return action_names_and_types + + +def get_action_names(*, node): + action_names_and_types = get_action_names_and_types(node=node) + return [n for (n, t) in action_names_and_types] + + +def get_action_types(package_name): + if not has_resource('packages', package_name): + raise LookupError('Unknown package name') + try: + content, _ = get_resource('rosidl_interfaces', package_name) + except LookupError: + return [] + interface_names = content.splitlines() + # TODO(jacobperron) this logic should come from a rosidl related package + # Only return actions in action folder + return list(sorted({ + n[7:-7] + for n in interface_names + if n.startswith('action/') and n[-7:] in ('.idl', '.action')})) + + +def get_action_path(package_name, action_name): + action_types = get_action_types(package_name) + if action_name not in action_types: + raise LookupError('Unknown action type') + prefix_path = has_resource('packages', package_name) + # TODO(jacobperron) this logic should come from a rosidl related package + return os.path.join(prefix_path, 'share', package_name, 'action', action_name + '.action') diff --git a/ros2action/ros2action/command/__init__.py b/ros2action/ros2action/command/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/ros2action/ros2action/command/action.py b/ros2action/ros2action/command/action.py new file mode 100644 index 000000000..24bcb21a3 --- /dev/null +++ b/ros2action/ros2action/command/action.py @@ -0,0 +1,38 @@ +# Copyright 2019 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 ActionCommand(CommandExtension): + """Various action related sub-commands.""" + + def add_arguments(self, parser, cli_name): + self._subparser = parser + # Get verb extensions and let them add their arguments and sub-commands + verb_extensions = get_verb_extensions('ros2action.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/ros2action/ros2action/verb/__init__.py b/ros2action/ros2action/verb/__init__.py new file mode 100644 index 000000000..551267d47 --- /dev/null +++ b/ros2action/ros2action/verb/__init__.py @@ -0,0 +1,44 @@ +# 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 ros2cli.plugin_system import PLUGIN_SYSTEM_VERSION +from ros2cli.plugin_system import satisfies_version + + +class VerbExtension: + """ + The extension point for 'action' 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/ros2action/ros2action/verb/list.py b/ros2action/ros2action/verb/list.py new file mode 100644 index 000000000..1286d8c39 --- /dev/null +++ b/ros2action/ros2action/verb/list.py @@ -0,0 +1,44 @@ +# Copyright 2019 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 ros2action.api import get_action_names_and_types +from ros2action.verb import VerbExtension +from ros2cli.node.strategy import NodeStrategy + + +class ListVerb(VerbExtension): + """Output a list of action names.""" + + def add_arguments(self, parser, cli_name): + parser.add_argument( + '-t', '--show-types', action='store_true', + help='Additionally show the action type') + parser.add_argument( + '-c', '--count-actions', action='store_true', + help='Only display the number of actions discovered') + + def main(self, *, args): + with NodeStrategy(args) as node: + action_names_and_types = get_action_names_and_types(node=node) + + if args.count_actions: + print(len(action_names_and_types)) + return + + for name, types in action_names_and_types: + if args.show_types: + types_formatted = ', '.join(types) + print('{name} [{types_formatted}]'.format_map(locals())) + else: + print('{name}'.format_map(locals())) diff --git a/ros2action/ros2action/verb/show.py b/ros2action/ros2action/verb/show.py new file mode 100644 index 000000000..2fd3709d0 --- /dev/null +++ b/ros2action/ros2action/verb/show.py @@ -0,0 +1,38 @@ +# Copyright 2019 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 ros2action.api import get_action_path +# from ros2action.api import action_type_completer +from ros2action.verb import VerbExtension + + +class ShowVerb(VerbExtension): + """Output the action definition.""" + + def add_arguments(self, parser, cli_name): + parser.add_argument( + 'action_type', + help="Type of the ROS action (e.g. 'examples_interfaces/Fibonacci')") + # arg.completer = action_type_completer + + def main(self, *, args): + package_name, action_name = args.action_type.split('/', 2) + if not package_name or not action_name: + raise RuntimeError('The passed action type is invalid') + try: + path = get_action_path(package_name, action_name) + except LookupError as e: + return str(e) + with open(path, 'r') as action_file: + print(action_file.read(), end='') diff --git a/ros2action/setup.py b/ros2action/setup.py new file mode 100644 index 000000000..2ba44ab18 --- /dev/null +++ b/ros2action/setup.py @@ -0,0 +1,43 @@ +from setuptools import find_packages +from setuptools import setup + +setup( + name='ros2action', + version='0.6.3', + packages=find_packages(exclude=['test']), + install_requires=['ros2cli'], + zip_safe=True, + author='Jacob Perron', + author_email='jacob@openrobotics.org', + maintainer='Dirk Thomas', + maintainer_email='dthomas@osrfoundation.org', + url='https://github.com/ros2/ros2cli/tree/master/ros2action', + 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 action command for ROS 2 command line tools.', + long_description="""\ +The package provides the action command for the ROS 2 command line tools.""", + license='Apache License, Version 2.0', + tests_require=['pytest'], + entry_points={ + 'ros2cli.command': [ + 'action = ros2action.command.action:ActionCommand', + ], + 'ros2cli.extension_point': [ + 'ros2action.verb = ros2action.verb:VerbExtension', + ], + 'ros2action.verb': [ + # 'echo = ros2action.verb.echo:EchoVerb', + # 'info = ros2action.verb.info:InfoVerb', + 'list = ros2action.verb.list:ListVerb', + # 'send_goal = ros2action.verb.send_goal:SendGoalVerb', + 'show = ros2action.verb.show:ShowVerb', + ], + } +) diff --git a/ros2action/test/test_api.py b/ros2action/test/test_api.py new file mode 100644 index 000000000..d46c34af9 --- /dev/null +++ b/ros2action/test/test_api.py @@ -0,0 +1,48 @@ +# Copyright 2019 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 os + +import pytest + +from ros2action.api import get_action_names +from ros2action.api import get_action_path +from ros2action.api import get_action_types +from ros2cli.node.strategy import NodeStrategy + + +def test_get_action_names(): + get_action_names(node=NodeStrategy(object())) + + +def test_get_action_path(): + action_path = get_action_path('test_msgs', 'Fibonacci') + assert os.path.isfile(action_path) + + with pytest.raises(LookupError): + get_action_path('_not_a_real_package_name', 'Fibonacci') + + +def test_get_action_types(): + action_types = get_action_types('test_msgs') + # Expect only strings + for t in action_types: + assert isinstance(t, str) + # Explicit dependencies of this package should be available + assert 'Fibonacci' in action_types + assert 'NestedMessage' in action_types + # Some things that should not be in the list + assert '' not in action_types + assert 'test_msgs' not in action_types + assert 'NotAnActionMessage' not in action_types diff --git a/ros2action/test/test_copyright.py b/ros2action/test/test_copyright.py new file mode 100644 index 000000000..cf0fae31f --- /dev/null +++ b/ros2action/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/ros2action/test/test_flake8.py b/ros2action/test/test_flake8.py new file mode 100644 index 000000000..eff829969 --- /dev/null +++ b/ros2action/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/ros2action/test/test_pep257.py b/ros2action/test/test_pep257.py new file mode 100644 index 000000000..0e38a6c60 --- /dev/null +++ b/ros2action/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' From 7d022a482402812dd71d5c51f8caea8d36e309d7 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 13 Mar 2019 16:11:03 -0700 Subject: [PATCH 02/20] Add 'info' verb to action command Prints a list of node names that have an action client or server for a given action name. Signed-off-by: Jacob Perron --- ros2action/ros2action/api/__init__.py | 30 +++++++++++++++++ ros2action/ros2action/verb/info.py | 46 +++++++++++++++++++++++++++ ros2action/setup.py | 2 +- ros2action/test/test_api.py | 11 +++++++ 4 files changed, 88 insertions(+), 1 deletion(-) create mode 100644 ros2action/ros2action/verb/info.py diff --git a/ros2action/ros2action/api/__init__.py b/ros2action/ros2action/api/__init__.py index 51fe7bc84..099b9a241 100644 --- a/ros2action/ros2action/api/__init__.py +++ b/ros2action/ros2action/api/__init__.py @@ -19,6 +19,36 @@ from ament_index_python import has_resource +def _is_action_status_topic(topic_name, action_name): + return action_name + '/_action/status' == topic_name + + +def get_action_clients_and_servers(*, node, action_name): + action_clients = [] + action_servers = [] + + node_names_and_ns = node.get_node_names_and_namespaces() + for node_name, node_ns in node_names_and_ns: + # Construct fully qualified name + node_fqn = '/'.join(node_ns) + node_name + + # If a subscription is on an action status topic, the node must contain an action client + sub_names_and_types = node.get_subscriber_names_and_types_by_node(node_name, node_ns) + for sub_name, sub_type in sub_names_and_types: + if _is_action_status_topic(sub_name, action_name): + # TODO(jacobperron): Infer the action type and return that too + action_clients.append(node_fqn) + + # If a publisher is on an action status topic, the node must contain an action server + pub_names_and_types = node.get_publisher_names_and_types_by_node(node_name, node_ns) + for pub_name, pub_type in pub_names_and_types: + if _is_action_status_topic(pub_name, action_name): + # TODO(jacobperron): Infer the action type and return that too + action_servers.append(node_fqn) + + return (action_clients, action_servers) + + def get_action_names_and_types(*, node): service_names_and_types = node.get_service_names_and_types() # Assumption: actions have a hidden 'send_goal' service with the name: diff --git a/ros2action/ros2action/verb/info.py b/ros2action/ros2action/verb/info.py new file mode 100644 index 000000000..ca14b1171 --- /dev/null +++ b/ros2action/ros2action/verb/info.py @@ -0,0 +1,46 @@ +# Copyright 2019 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 ros2action.api import get_action_clients_and_servers +from ros2action.verb import VerbExtension +from ros2cli.node.direct import DirectNode + + +class InfoVerb(VerbExtension): + """Print information about an action.""" + + def add_arguments(self, parser, cli_name): + parser.add_argument( + 'action_name', + help="Name of the ROS action to get info (e.g. '/fibonacci')") + parser.add_argument( + '-c', '--count', action='store_true', + help='Only display the number of action clients and action servers') + + def main(self, *, args): + with DirectNode(args) as node: + action_clients, action_servers = get_action_clients_and_servers( + node=node, + action_name=args.action_name, + ) + + print('Action: {}'.format(args.action_name)) + print('Action clients: {}'.format(len(action_clients))) + if not args.count: + for client in action_clients: + print(' {}'.format(client)) + print('Action servers: {}'.format(len(action_servers))) + if not args.count: + for server in action_servers: + print(' {}'.format(server)) diff --git a/ros2action/setup.py b/ros2action/setup.py index 2ba44ab18..3f0b68545 100644 --- a/ros2action/setup.py +++ b/ros2action/setup.py @@ -34,7 +34,7 @@ ], 'ros2action.verb': [ # 'echo = ros2action.verb.echo:EchoVerb', - # 'info = ros2action.verb.info:InfoVerb', + 'info = ros2action.verb.info:InfoVerb', 'list = ros2action.verb.list:ListVerb', # 'send_goal = ros2action.verb.send_goal:SendGoalVerb', 'show = ros2action.verb.show:ShowVerb', diff --git a/ros2action/test/test_api.py b/ros2action/test/test_api.py index d46c34af9..095b16130 100644 --- a/ros2action/test/test_api.py +++ b/ros2action/test/test_api.py @@ -16,12 +16,23 @@ import pytest +from ros2action.api import get_action_clients_and_servers from ros2action.api import get_action_names from ros2action.api import get_action_path from ros2action.api import get_action_types +from ros2cli.node.strategy import DirectNode from ros2cli.node.strategy import NodeStrategy +def test_get_action_clients_and_servers(): + clients, servers = get_action_clients_and_servers( + node=DirectNode(None), + action_name='/test_action_name', + ) + assert len(clients) == 0 + assert len(servers) == 0 + + def test_get_action_names(): get_action_names(node=NodeStrategy(object())) From d81668c4fbfda553a5833d704f87df57045ef2b5 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 13 Mar 2019 16:14:46 -0700 Subject: [PATCH 03/20] Use None as argument to test node Signed-off-by: Jacob Perron --- ros2action/test/test_api.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2action/test/test_api.py b/ros2action/test/test_api.py index 095b16130..d5ec0ec2d 100644 --- a/ros2action/test/test_api.py +++ b/ros2action/test/test_api.py @@ -34,7 +34,7 @@ def test_get_action_clients_and_servers(): def test_get_action_names(): - get_action_names(node=NodeStrategy(object())) + get_action_names(node=NodeStrategy(None)) def test_get_action_path(): From d0c4d2335e1f4466d57a709d04720441b86f63fb Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 13 Mar 2019 16:25:24 -0700 Subject: [PATCH 04/20] Add TODOs to move action query functions to rclpy (and rcl_action) The tool shouldn't need to know details about the implementation of actions. Signed-off-by: Jacob Perron --- ros2action/ros2action/api/__init__.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ros2action/ros2action/api/__init__.py b/ros2action/ros2action/api/__init__.py index 099b9a241..a104c490f 100644 --- a/ros2action/ros2action/api/__init__.py +++ b/ros2action/ros2action/api/__init__.py @@ -23,6 +23,7 @@ def _is_action_status_topic(topic_name, action_name): return action_name + '/_action/status' == topic_name +# TODO(jacobperron): This should be provided by rclpy (and probably rcl_action) def get_action_clients_and_servers(*, node, action_name): action_clients = [] action_servers = [] @@ -49,6 +50,7 @@ def get_action_clients_and_servers(*, node, action_name): return (action_clients, action_servers) +# TODO(jacobperron): This should be provided by rclpy (and probably rcl_action) def get_action_names_and_types(*, node): service_names_and_types = node.get_service_names_and_types() # Assumption: actions have a hidden 'send_goal' service with the name: From e6eca00c3470356ab3a4520ab0bcedf5f5bfaa5a Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 13 Mar 2019 16:27:48 -0700 Subject: [PATCH 05/20] Add dependency to rclpy Signed-off-by: Jacob Perron --- ros2action/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/ros2action/package.xml b/ros2action/package.xml index b7be5c6bb..00361ba34 100644 --- a/ros2action/package.xml +++ b/ros2action/package.xml @@ -11,6 +11,7 @@ Jacob Perron + rclpy ros2cli ament_index_python From f5077f8bcf89bacf6b73489562948e3214787bd3 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 13 Mar 2019 18:06:16 -0700 Subject: [PATCH 06/20] Add 'send_goal' verb to action command Signed-off-by: Jacob Perron --- ros2action/package.xml | 2 + ros2action/ros2action/verb/send_goal.py | 206 ++++++++++++++++++++++++ ros2action/setup.py | 2 +- 3 files changed, 209 insertions(+), 1 deletion(-) create mode 100644 ros2action/ros2action/verb/send_goal.py diff --git a/ros2action/package.xml b/ros2action/package.xml index 00361ba34..1434c901c 100644 --- a/ros2action/package.xml +++ b/ros2action/package.xml @@ -14,7 +14,9 @@ rclpy ros2cli + action_msgs ament_index_python + ros2topic ament_copyright ament_flake8 diff --git a/ros2action/ros2action/verb/send_goal.py b/ros2action/ros2action/verb/send_goal.py new file mode 100644 index 000000000..379039730 --- /dev/null +++ b/ros2action/ros2action/verb/send_goal.py @@ -0,0 +1,206 @@ +# Copyright 2019 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 collections import OrderedDict +import importlib +import sys +import yaml + +from action_msgs.msg import GoalStatus +import rclpy +from rclpy.action import ActionClient +from ros2action.verb import VerbExtension +from ros2cli.node import NODE_NAME_PREFIX +# TODO(jacobperron): Move 'set_msg_fields' to ros2cli package to remove dependency to ros2topic +from ros2topic.api import set_msg_fields +from ros2topic.api import SetFieldError + + +class SendGoalVerb(VerbExtension): + """Send an action goal.""" + + def add_arguments(self, parser, cli_name): + parser.add_argument( + 'action_name', + help="Name of the ROS action (e.g. '/fibonacci')") + parser.add_argument( + 'action_type', + help="Type of the ROS action (e.g. 'example_interfaces/Fibonacci')") + parser.add_argument( + 'goal', + help="Goal request values in YAML format (e.g. '{order: 10}')") + parser.add_argument( + '-f', '--feedback', action='store_true', + help='Echo feedback messages for the goal') + + def main(self, *, args): + register_yaml_representer() + feedback_callback = None + if args.feedback: + feedback_callback = _feedback_callback + return send_goal(args.action_name, args.action_type, args.goal, feedback_callback) + + +def _goal_status_to_string(status): + if GoalStatus.STATUS_ACCEPTED == status: + return 'ACCEPTED' + elif GoalStatus.STATUS_EXECUTING == status: + return 'EXECUTING' + elif GoalStatus.STATUS_CANCELING == status: + return 'CANCELING' + elif GoalStatus.STATUS_SUCCEEDED == status: + return 'SUCCEEDED' + elif GoalStatus.STATUS_CANCELED == status: + return 'CANCELED' + elif GoalStatus.STATUS_ABORTED == status: + return 'ABORTED' + else: + return 'UNKNOWN' + +# TODO(jacobperron): Move to common place in ros2cli for all packages to use +def register_yaml_representer(): + # Register our custom representer for YAML output + yaml.add_representer(OrderedDict, represent_ordereddict) + + +# TODO(jacobperron): Move to common place in ros2cli for all packages to use +# Custom representer for getting clean YAML output that preserves the order in +# an OrderedDict. +# Inspired by: +# http://stackoverflow.com/a/16782282/7169408 +def represent_ordereddict(dumper, data): + items = [] + for k, v in data.items(): + items.append((dumper.represent_data(k), dumper.represent_data(v))) + return yaml.nodes.MappingNode(u'tag:yaml.org,2002:map', items) + + +# TODO(jacobperron): Move to common place in ros2cli for all packages to use +def _convert_value(value, truncate_length=None): + if isinstance(value, bytes): + if truncate_length is not None and len(value) > truncate_length: + value = ''.join([chr(c) for c in value[:truncate_length]]) + '...' + else: + value = ''.join([chr(c) for c in value]) + elif isinstance(value, str): + if truncate_length is not None and len(value) > truncate_length: + value = value[:truncate_length] + '...' + elif isinstance(value, tuple) or isinstance(value, list): + if truncate_length is not None and len(value) > truncate_length: + # Truncate the sequence + value = value[:truncate_length] + # Truncate every item in the sequence + value = type(value)([_convert_value(v, truncate_length) for v in value] + ['...']) + else: + # Truncate every item in the list + value = type(value)([_convert_value(v, truncate_length) for v in value]) + elif isinstance(value, dict) or isinstance(value, OrderedDict): + # convert each key and value in the mapping + new_value = {} if isinstance(value, dict) else OrderedDict() + for k, v in value.items(): + # don't truncate keys because that could result in key collisions and data loss + new_value[_convert_value(k)] = _convert_value(v, truncate_length=truncate_length) + value = new_value + elif not any(isinstance(value, t) for t in (bool, float, int)): + # assuming value is a message + # since it is neither a collection nor a primitive type + value = msg_to_ordereddict(value, truncate_length=truncate_length) + return value + + +# TODO(jacobperron): Move to common place in ros2cli for all packages to use +def msg_to_ordereddict(msg, truncate_length=None): + d = OrderedDict() + # We rely on __slots__ retaining the order of the fields in the .msg file. + for field_name in msg.__slots__: + value = getattr(msg, field_name, None) + value = _convert_value(value, truncate_length=truncate_length) + # remove leading underscore from field name + d[field_name[1:]] = value + return d + + +# TODO(jacobperron): Move to common place in ros2cli for all packages to use +def msg_to_yaml(msg): + return yaml.dump( + msg_to_ordereddict( + msg, + truncate_length=128 + # truncate_length=args.truncate_length if not args.full_length else None + ), width=sys.maxsize) + + +def _feedback_callback(feedback): + print('Feedback:\n {}'.format(msg_to_yaml(feedback.feedback))) + # print(msg_to_yaml(feedback.feedback)) + + +def send_goal(action_name, action_type, goal_values, feedback_callback): + # TODO(jacobperron): This logic should come from a rosidl related package + package_name, action_type = action_type.split('/', 2) + if not package_name or not action_type: + raise RuntimeError('The passed action type is invalid') + + module = importlib.import_module(package_name + '.action') + action_module = getattr(module, action_type) + goal_dict = yaml.load(goal_values) + + rclpy.init() + + node_name = NODE_NAME_PREFIX + '_send_goal_{}_{}'.format(package_name, action_type) + node = rclpy.create_node(node_name) + + action_client = ActionClient(node, action_module, action_name) + + goal = action_module.Goal() + + try: + set_msg_fields(goal, goal_dict) + except SetFieldError as ex: + return "Failed to populate field '{ex.field_name}': {ex.exception}" \ + .format_map(locals()) + + print('Waiting for an action server to become available...') + action_client.wait_for_server() + + print('Sending goal:\n {}'.format(msg_to_yaml(goal))) + # print(msg_to_yaml(goal)) + goal_future = action_client.send_goal_async(goal, feedback_callback) + rclpy.spin_until_future_complete(node, goal_future) + + goal_handle = goal_future.result() + + if goal_handle is None: + raise RuntimeError('Exeception while sending goal: {!r}'.format(goal_future.exception())) + + if not goal_handle.accepted: + print('Goal was rejected.') + return + + result_future = goal_handle.get_result_async() + rclpy.spin_until_future_complete(node, result_future) + + result = result_future.result() + + if result is None: + raise RuntimeError( + 'Exeception while getting result: {!r}'.format(result_future.exception())) + + print('Result:\n {}'.format(msg_to_yaml(result.result))) + # print(msg_to_yaml(result.result)) + print('Goal finished with status: {}'.format(_goal_status_to_string(result.status))) + + action_client.destroy() + node.destroy_node() + rclpy.shutdown() diff --git a/ros2action/setup.py b/ros2action/setup.py index 3f0b68545..770d09dff 100644 --- a/ros2action/setup.py +++ b/ros2action/setup.py @@ -36,7 +36,7 @@ # 'echo = ros2action.verb.echo:EchoVerb', 'info = ros2action.verb.info:InfoVerb', 'list = ros2action.verb.list:ListVerb', - # 'send_goal = ros2action.verb.send_goal:SendGoalVerb', + 'send_goal = ros2action.verb.send_goal:SendGoalVerb', 'show = ros2action.verb.show:ShowVerb', ], } From 32ecb3257fe3027e6c919ca12580c345e540097a Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Thu, 14 Mar 2019 16:26:59 -0700 Subject: [PATCH 07/20] Migrate message utility functions to rosidl_runtime_py Signed-off-by: Jacob Perron --- ros2action/package.xml | 2 +- ros2action/ros2action/verb/send_goal.py | 95 +++---------------------- 2 files changed, 9 insertions(+), 88 deletions(-) diff --git a/ros2action/package.xml b/ros2action/package.xml index 1434c901c..30dcff1b1 100644 --- a/ros2action/package.xml +++ b/ros2action/package.xml @@ -16,7 +16,7 @@ action_msgs ament_index_python - ros2topic + rosidl_runtime_py ament_copyright ament_flake8 diff --git a/ros2action/ros2action/verb/send_goal.py b/ros2action/ros2action/verb/send_goal.py index 379039730..2b2a9fc40 100644 --- a/ros2action/ros2action/verb/send_goal.py +++ b/ros2action/ros2action/verb/send_goal.py @@ -22,9 +22,8 @@ from rclpy.action import ActionClient from ros2action.verb import VerbExtension from ros2cli.node import NODE_NAME_PREFIX -# TODO(jacobperron): Move 'set_msg_fields' to ros2cli package to remove dependency to ros2topic -from ros2topic.api import set_msg_fields -from ros2topic.api import SetFieldError +from rosidl_runtime_py import message_to_yaml +from rosidl_runtime_py import set_message_fields class SendGoalVerb(VerbExtension): @@ -45,7 +44,6 @@ def add_arguments(self, parser, cli_name): help='Echo feedback messages for the goal') def main(self, *, args): - register_yaml_representer() feedback_callback = None if args.feedback: feedback_callback = _feedback_callback @@ -68,82 +66,8 @@ def _goal_status_to_string(status): else: return 'UNKNOWN' -# TODO(jacobperron): Move to common place in ros2cli for all packages to use -def register_yaml_representer(): - # Register our custom representer for YAML output - yaml.add_representer(OrderedDict, represent_ordereddict) - - -# TODO(jacobperron): Move to common place in ros2cli for all packages to use -# Custom representer for getting clean YAML output that preserves the order in -# an OrderedDict. -# Inspired by: -# http://stackoverflow.com/a/16782282/7169408 -def represent_ordereddict(dumper, data): - items = [] - for k, v in data.items(): - items.append((dumper.represent_data(k), dumper.represent_data(v))) - return yaml.nodes.MappingNode(u'tag:yaml.org,2002:map', items) - - -# TODO(jacobperron): Move to common place in ros2cli for all packages to use -def _convert_value(value, truncate_length=None): - if isinstance(value, bytes): - if truncate_length is not None and len(value) > truncate_length: - value = ''.join([chr(c) for c in value[:truncate_length]]) + '...' - else: - value = ''.join([chr(c) for c in value]) - elif isinstance(value, str): - if truncate_length is not None and len(value) > truncate_length: - value = value[:truncate_length] + '...' - elif isinstance(value, tuple) or isinstance(value, list): - if truncate_length is not None and len(value) > truncate_length: - # Truncate the sequence - value = value[:truncate_length] - # Truncate every item in the sequence - value = type(value)([_convert_value(v, truncate_length) for v in value] + ['...']) - else: - # Truncate every item in the list - value = type(value)([_convert_value(v, truncate_length) for v in value]) - elif isinstance(value, dict) or isinstance(value, OrderedDict): - # convert each key and value in the mapping - new_value = {} if isinstance(value, dict) else OrderedDict() - for k, v in value.items(): - # don't truncate keys because that could result in key collisions and data loss - new_value[_convert_value(k)] = _convert_value(v, truncate_length=truncate_length) - value = new_value - elif not any(isinstance(value, t) for t in (bool, float, int)): - # assuming value is a message - # since it is neither a collection nor a primitive type - value = msg_to_ordereddict(value, truncate_length=truncate_length) - return value - - -# TODO(jacobperron): Move to common place in ros2cli for all packages to use -def msg_to_ordereddict(msg, truncate_length=None): - d = OrderedDict() - # We rely on __slots__ retaining the order of the fields in the .msg file. - for field_name in msg.__slots__: - value = getattr(msg, field_name, None) - value = _convert_value(value, truncate_length=truncate_length) - # remove leading underscore from field name - d[field_name[1:]] = value - return d - - -# TODO(jacobperron): Move to common place in ros2cli for all packages to use -def msg_to_yaml(msg): - return yaml.dump( - msg_to_ordereddict( - msg, - truncate_length=128 - # truncate_length=args.truncate_length if not args.full_length else None - ), width=sys.maxsize) - - def _feedback_callback(feedback): - print('Feedback:\n {}'.format(msg_to_yaml(feedback.feedback))) - # print(msg_to_yaml(feedback.feedback)) + print('Feedback:\n {}'.format(message_to_yaml(feedback.feedback, None))) def send_goal(action_name, action_type, goal_values, feedback_callback): @@ -166,16 +90,14 @@ def send_goal(action_name, action_type, goal_values, feedback_callback): goal = action_module.Goal() try: - set_msg_fields(goal, goal_dict) - except SetFieldError as ex: - return "Failed to populate field '{ex.field_name}': {ex.exception}" \ - .format_map(locals()) + set_message_fields(goal, goal_dict) + except Exception as ex: + return "Failed to populate message fields: {!r}".format(ex) print('Waiting for an action server to become available...') action_client.wait_for_server() - print('Sending goal:\n {}'.format(msg_to_yaml(goal))) - # print(msg_to_yaml(goal)) + print('Sending goal:\n {}'.format(message_to_yaml(goal, None))) goal_future = action_client.send_goal_async(goal, feedback_callback) rclpy.spin_until_future_complete(node, goal_future) @@ -197,8 +119,7 @@ def send_goal(action_name, action_type, goal_values, feedback_callback): raise RuntimeError( 'Exeception while getting result: {!r}'.format(result_future.exception())) - print('Result:\n {}'.format(msg_to_yaml(result.result))) - # print(msg_to_yaml(result.result)) + print('Result:\n {}'.format(message_to_yaml(result.result, None))) print('Goal finished with status: {}'.format(_goal_status_to_string(result.status))) action_client.destroy() From 45885862160cec665efffa21f8a501dfb2fa0465 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 3 Apr 2019 15:59:06 -0700 Subject: [PATCH 08/20] Make use of rclpy functions Signed-off-by: Jacob Perron --- ros2action/ros2action/api/__init__.py | 46 +++++++++++++-------------- ros2action/ros2action/verb/info.py | 19 ++++++++--- ros2action/ros2action/verb/list.py | 4 +-- 3 files changed, 39 insertions(+), 30 deletions(-) diff --git a/ros2action/ros2action/api/__init__.py b/ros2action/ros2action/api/__init__.py index a104c490f..140f1ee8d 100644 --- a/ros2action/ros2action/api/__init__.py +++ b/ros2action/ros2action/api/__init__.py @@ -18,12 +18,13 @@ from ament_index_python import get_resource from ament_index_python import has_resource +import rclpy.action + def _is_action_status_topic(topic_name, action_name): return action_name + '/_action/status' == topic_name -# TODO(jacobperron): This should be provided by rclpy (and probably rcl_action) def get_action_clients_and_servers(*, node, action_name): action_clients = [] action_servers = [] @@ -33,34 +34,31 @@ def get_action_clients_and_servers(*, node, action_name): # Construct fully qualified name node_fqn = '/'.join(node_ns) + node_name - # If a subscription is on an action status topic, the node must contain an action client - sub_names_and_types = node.get_subscriber_names_and_types_by_node(node_name, node_ns) - for sub_name, sub_type in sub_names_and_types: - if _is_action_status_topic(sub_name, action_name): - # TODO(jacobperron): Infer the action type and return that too - action_clients.append(node_fqn) - - # If a publisher is on an action status topic, the node must contain an action server - pub_names_and_types = node.get_publisher_names_and_types_by_node(node_name, node_ns) - for pub_name, pub_type in pub_names_and_types: - if _is_action_status_topic(pub_name, action_name): - # TODO(jacobperron): Infer the action type and return that too - action_servers.append(node_fqn) + # Get any action clients associated with the node + client_names_and_types = rclpy.action.get_action_client_names_and_types_by_node( + node, + node_name, + node_ns, + ) + for client_name, client_types in client_names_and_types: + if client_name == action_name: + action_clients.append((node_fqn, client_types)) + + # Get any action servers associated with the node + server_names_and_types = rclpy.action.get_action_server_names_and_types_by_node( + node, + node_name, + node_ns, + ) + for server_name, server_types in server_names_and_types: + if server_name == action_name: + action_servers.append((node_fqn, server_types)) return (action_clients, action_servers) -# TODO(jacobperron): This should be provided by rclpy (and probably rcl_action) def get_action_names_and_types(*, node): - service_names_and_types = node.get_service_names_and_types() - # Assumption: actions have a hidden 'send_goal' service with the name: - # '///_action/send_goal' (18 trailing characters) - # And the service type has the suffix '_SendGoal' (9 trailing characters) - action_names_and_types = [ - (name[:-18], [t[:-9] for t in type_with_suffix]) - for (name, type_with_suffix) in service_names_and_types - if re.match('.*/_action/send_goal$', name)] - return action_names_and_types + return rclpy.action.get_action_names_and_types(node) def get_action_names(*, node): diff --git a/ros2action/ros2action/verb/info.py b/ros2action/ros2action/verb/info.py index ca14b1171..b8ff13173 100644 --- a/ros2action/ros2action/verb/info.py +++ b/ros2action/ros2action/verb/info.py @@ -24,6 +24,9 @@ def add_arguments(self, parser, cli_name): parser.add_argument( 'action_name', help="Name of the ROS action to get info (e.g. '/fibonacci')") + parser.add_argument( + '-t', '--show-types', action='store_true', + help='Additionally show the action type') parser.add_argument( '-c', '--count', action='store_true', help='Only display the number of action clients and action servers') @@ -38,9 +41,17 @@ def main(self, *, args): print('Action: {}'.format(args.action_name)) print('Action clients: {}'.format(len(action_clients))) if not args.count: - for client in action_clients: - print(' {}'.format(client)) + for client_name, client_types in action_clients: + if args.show_types: + types_formatted = ', '.join(client_types) + print(' {client_name} [{types_formatted}]'.format_map(locals())) + else: + print(' {client_name}'.format_map(locals())) print('Action servers: {}'.format(len(action_servers))) if not args.count: - for server in action_servers: - print(' {}'.format(server)) + for server_name, server_types in action_servers: + if args.show_types: + types_formatted = ', '.join(server_types) + print(' {server_name} [{types_formatted}]'.format_map(locals())) + else: + print(' {server_name}'.format_map(locals())) diff --git a/ros2action/ros2action/verb/list.py b/ros2action/ros2action/verb/list.py index 1286d8c39..893d0d9f3 100644 --- a/ros2action/ros2action/verb/list.py +++ b/ros2action/ros2action/verb/list.py @@ -14,7 +14,7 @@ from ros2action.api import get_action_names_and_types from ros2action.verb import VerbExtension -from ros2cli.node.strategy import NodeStrategy +from ros2cli.node.strategy import DirectNode class ListVerb(VerbExtension): @@ -29,7 +29,7 @@ def add_arguments(self, parser, cli_name): help='Only display the number of actions discovered') def main(self, *, args): - with NodeStrategy(args) as node: + with DirectNode(args) as node: action_names_and_types = get_action_names_and_types(node=node) if args.count_actions: From 5e324d1c88973a0d3312f708398dcd6e34cdf8e6 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 3 Apr 2019 17:02:44 -0700 Subject: [PATCH 09/20] Fix lint Signed-off-by: Jacob Perron --- ros2action/ros2action/api/__init__.py | 1 - ros2action/ros2action/verb/send_goal.py | 8 ++++---- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/ros2action/ros2action/api/__init__.py b/ros2action/ros2action/api/__init__.py index 140f1ee8d..f9609590d 100644 --- a/ros2action/ros2action/api/__init__.py +++ b/ros2action/ros2action/api/__init__.py @@ -13,7 +13,6 @@ # limitations under the License. import os -import re from ament_index_python import get_resource from ament_index_python import has_resource diff --git a/ros2action/ros2action/verb/send_goal.py b/ros2action/ros2action/verb/send_goal.py index 2b2a9fc40..d0d50916b 100644 --- a/ros2action/ros2action/verb/send_goal.py +++ b/ros2action/ros2action/verb/send_goal.py @@ -12,10 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from collections import OrderedDict import importlib -import sys -import yaml from action_msgs.msg import GoalStatus import rclpy @@ -25,6 +22,8 @@ from rosidl_runtime_py import message_to_yaml from rosidl_runtime_py import set_message_fields +import yaml + class SendGoalVerb(VerbExtension): """Send an action goal.""" @@ -66,6 +65,7 @@ def _goal_status_to_string(status): else: return 'UNKNOWN' + def _feedback_callback(feedback): print('Feedback:\n {}'.format(message_to_yaml(feedback.feedback, None))) @@ -92,7 +92,7 @@ def send_goal(action_name, action_type, goal_values, feedback_callback): try: set_message_fields(goal, goal_dict) except Exception as ex: - return "Failed to populate message fields: {!r}".format(ex) + return 'Failed to populate message fields: {!r}'.format(ex) print('Waiting for an action server to become available...') action_client.wait_for_server() From eb7ba1f61eac692d628a0dea13613eb9cb0391f6 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 3 Apr 2019 17:20:17 -0700 Subject: [PATCH 10/20] Fix tests Signed-off-by: Jacob Perron --- ros2action/test/test_api.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/ros2action/test/test_api.py b/ros2action/test/test_api.py index d5ec0ec2d..783407e8a 100644 --- a/ros2action/test/test_api.py +++ b/ros2action/test/test_api.py @@ -21,12 +21,13 @@ from ros2action.api import get_action_path from ros2action.api import get_action_types from ros2cli.node.strategy import DirectNode -from ros2cli.node.strategy import NodeStrategy + +g_node = DirectNode(None) def test_get_action_clients_and_servers(): clients, servers = get_action_clients_and_servers( - node=DirectNode(None), + node=g_node, action_name='/test_action_name', ) assert len(clients) == 0 @@ -34,7 +35,7 @@ def test_get_action_clients_and_servers(): def test_get_action_names(): - get_action_names(node=NodeStrategy(None)) + get_action_names(node=g_node) def test_get_action_path(): From 62b2d407281d2b5551ab6011eb0318b215cb69b0 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Thu, 4 Apr 2019 06:56:22 -0700 Subject: [PATCH 11/20] Fix test Signed-off-by: Jacob Perron --- ros2action/test/test_api.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/ros2action/test/test_api.py b/ros2action/test/test_api.py index 783407e8a..8b42245af 100644 --- a/ros2action/test/test_api.py +++ b/ros2action/test/test_api.py @@ -22,20 +22,20 @@ from ros2action.api import get_action_types from ros2cli.node.strategy import DirectNode -g_node = DirectNode(None) - def test_get_action_clients_and_servers(): - clients, servers = get_action_clients_and_servers( - node=g_node, - action_name='/test_action_name', - ) + with DirectNode(None) as node: + clients, servers = get_action_clients_and_servers( + node=node, + action_name='/test_action_name', + ) assert len(clients) == 0 assert len(servers) == 0 def test_get_action_names(): - get_action_names(node=g_node) + with DirectNode(None) as node: + get_action_names(node=node) def test_get_action_path(): From bdab18af5fff6f5ee8432db42f0185d7b27eaeab Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Thu, 4 Apr 2019 07:07:20 -0700 Subject: [PATCH 12/20] Add autocompletion to verbs Signed-off-by: Jacob Perron --- ros2action/ros2action/api/__init__.py | 46 +++++++++++++++++++++++++ ros2action/ros2action/verb/info.py | 4 ++- ros2action/ros2action/verb/send_goal.py | 8 +++-- ros2action/ros2action/verb/show.py | 6 ++-- 4 files changed, 58 insertions(+), 6 deletions(-) diff --git a/ros2action/ros2action/api/__init__.py b/ros2action/ros2action/api/__init__.py index f9609590d..7bafd4d72 100644 --- a/ros2action/ros2action/api/__init__.py +++ b/ros2action/ros2action/api/__init__.py @@ -15,9 +15,11 @@ import os from ament_index_python import get_resource +from ament_index_python import get_resources from ament_index_python import has_resource import rclpy.action +from ros2cli.node.direct import DirectNode def _is_action_status_topic(topic_name, action_name): @@ -81,6 +83,15 @@ def get_action_types(package_name): if n.startswith('action/') and n[-7:] in ('.idl', '.action')})) +def get_all_action_types(): + all_action_types = {} + for package_name in get_resources('rosidl_interfaces'): + action_types = get_action_types(package_name) + if action_types: + all_action_types[package_name] = action_types + return all_action_types + + def get_action_path(package_name, action_name): action_types = get_action_types(package_name) if action_name not in action_types: @@ -88,3 +99,38 @@ def get_action_path(package_name, action_name): prefix_path = has_resource('packages', package_name) # TODO(jacobperron) this logic should come from a rosidl related package return os.path.join(prefix_path, 'share', package_name, 'action', action_name + '.action') + + +def action_name_completer(prefix, parsed_args, **kwargs): + """Callable returning a list of action names.""" + with DirectNode(parsed_args) as node: + return get_action_names(node=node) + + +def action_type_completer(**kwargs): + """Callable returning a list of action types.""" + action_types = [] + for package_name, action_names in get_all_action_types().items(): + for action_name in action_names: + action_types.append( + '{package_name}/{action_name}'.format_map(locals())) + return action_types + + +class ActionTypeCompleter: + """Callable returning a list of action types.""" + + def __init__(self, *, action_name_key=None): + self.action_name_key = action_name_key + + def __call__(self, prefix, parsed_args, **kwargs): + if self.action_name_key is None: + return action_type_completer() + + action_name = getattr(parsed_args, self.action_name_key) + with DirectNode(parsed_args) as node: + names_and_types = get_action_names_and_types(node=node) + for n, t in names_and_types: + if n == action_name: + return t + return [] diff --git a/ros2action/ros2action/verb/info.py b/ros2action/ros2action/verb/info.py index b8ff13173..8cfe5ac4e 100644 --- a/ros2action/ros2action/verb/info.py +++ b/ros2action/ros2action/verb/info.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +from ros2action.api import action_name_completer from ros2action.api import get_action_clients_and_servers from ros2action.verb import VerbExtension from ros2cli.node.direct import DirectNode @@ -21,9 +22,10 @@ class InfoVerb(VerbExtension): """Print information about an action.""" def add_arguments(self, parser, cli_name): - parser.add_argument( + arg = parser.add_argument( 'action_name', help="Name of the ROS action to get info (e.g. '/fibonacci')") + arg.completer = action_name_completer parser.add_argument( '-t', '--show-types', action='store_true', help='Additionally show the action type') diff --git a/ros2action/ros2action/verb/send_goal.py b/ros2action/ros2action/verb/send_goal.py index d0d50916b..d0cef8888 100644 --- a/ros2action/ros2action/verb/send_goal.py +++ b/ros2action/ros2action/verb/send_goal.py @@ -17,6 +17,8 @@ from action_msgs.msg import GoalStatus import rclpy from rclpy.action import ActionClient +from ros2action.api import action_name_completer +from ros2action.api import ActionTypeCompleter from ros2action.verb import VerbExtension from ros2cli.node import NODE_NAME_PREFIX from rosidl_runtime_py import message_to_yaml @@ -29,12 +31,14 @@ class SendGoalVerb(VerbExtension): """Send an action goal.""" def add_arguments(self, parser, cli_name): - parser.add_argument( + arg = parser.add_argument( 'action_name', help="Name of the ROS action (e.g. '/fibonacci')") - parser.add_argument( + arg.completer = action_name_completer + arg = parser.add_argument( 'action_type', help="Type of the ROS action (e.g. 'example_interfaces/Fibonacci')") + arg.completer = ActionTypeCompleter(action_name_key='action_name') parser.add_argument( 'goal', help="Goal request values in YAML format (e.g. '{order: 10}')") diff --git a/ros2action/ros2action/verb/show.py b/ros2action/ros2action/verb/show.py index 2fd3709d0..eacbc820d 100644 --- a/ros2action/ros2action/verb/show.py +++ b/ros2action/ros2action/verb/show.py @@ -12,8 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. +from ros2action.api import action_type_completer from ros2action.api import get_action_path -# from ros2action.api import action_type_completer from ros2action.verb import VerbExtension @@ -21,10 +21,10 @@ class ShowVerb(VerbExtension): """Output the action definition.""" def add_arguments(self, parser, cli_name): - parser.add_argument( + arg = parser.add_argument( 'action_type', help="Type of the ROS action (e.g. 'examples_interfaces/Fibonacci')") - # arg.completer = action_type_completer + arg.completer = action_type_completer def main(self, *, args): package_name, action_name = args.action_type.split('/', 2) From 4f39c2b1f9caf5a22c6d6260b64e1a9851e3ffe4 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Mon, 8 Apr 2019 07:43:59 -0700 Subject: [PATCH 13/20] Update year Signed-off-by: Jacob Perron --- ros2action/ros2action/verb/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2action/ros2action/verb/__init__.py b/ros2action/ros2action/verb/__init__.py index 551267d47..919ef8a0e 100644 --- a/ros2action/ros2action/verb/__init__.py +++ b/ros2action/ros2action/verb/__init__.py @@ -1,4 +1,4 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. +# Copyright 2019 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 0d13d0085dfbda97f65d162fb4c100b7cda93731 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Mon, 8 Apr 2019 08:35:41 -0700 Subject: [PATCH 14/20] Expand and validate action name This also has the side-effect of making the forward slash optional for the action name. Signed-off-by: Jacob Perron --- ros2action/ros2action/api/__init__.py | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/ros2action/ros2action/api/__init__.py b/ros2action/ros2action/api/__init__.py index 7bafd4d72..964f2ac36 100644 --- a/ros2action/ros2action/api/__init__.py +++ b/ros2action/ros2action/api/__init__.py @@ -19,6 +19,8 @@ from ament_index_python import has_resource import rclpy.action +from rclpy.expand_topic_name import expand_topic_name +from rclpy.validate_full_topic_name import validate_full_topic_name from ros2cli.node.direct import DirectNode @@ -30,6 +32,15 @@ def get_action_clients_and_servers(*, node, action_name): action_clients = [] action_servers = [] + try: + expanded_name = expand_topic_name(action_name, node.get_name(), node.get_namespace()) + except ValueError as e: + raise RuntimeError(e) + try: + validate_full_topic_name(expanded_name) + except rclpy.exceptions.InvalidTopicNameException as e: + raise RuntimeError(e) + node_names_and_ns = node.get_node_names_and_namespaces() for node_name, node_ns in node_names_and_ns: # Construct fully qualified name @@ -42,7 +53,7 @@ def get_action_clients_and_servers(*, node, action_name): node_ns, ) for client_name, client_types in client_names_and_types: - if client_name == action_name: + if client_name == expanded_name: action_clients.append((node_fqn, client_types)) # Get any action servers associated with the node @@ -52,7 +63,7 @@ def get_action_clients_and_servers(*, node, action_name): node_ns, ) for server_name, server_types in server_names_and_types: - if server_name == action_name: + if server_name == expanded_name: action_servers.append((node_fqn, server_types)) return (action_clients, action_servers) From 8ef62e00834cf0982d1507601cb96eb803f4e8a8 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Mon, 8 Apr 2019 08:45:10 -0700 Subject: [PATCH 15/20] Print goal ID when sendind a goal Signed-off-by: Jacob Perron --- ros2action/ros2action/verb/send_goal.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ros2action/ros2action/verb/send_goal.py b/ros2action/ros2action/verb/send_goal.py index d0cef8888..2f535343c 100644 --- a/ros2action/ros2action/verb/send_goal.py +++ b/ros2action/ros2action/verb/send_goal.py @@ -114,6 +114,8 @@ def send_goal(action_name, action_type, goal_values, feedback_callback): print('Goal was rejected.') return + print('Goal accepted with ID: {}\n'.format(bytes(goal_handle.goal_id.uuid).hex())) + result_future = goal_handle.get_result_async() rclpy.spin_until_future_complete(node, result_future) From 89902d5d8acbedfd99de116f77b9237cb4a32df8 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Thu, 11 Apr 2019 12:08:36 -0700 Subject: [PATCH 16/20] Cancel goal on SIGINT Wrapped send goal logic in try-finally clause. This ensures that any active goal will be canceled before the CLI command terminates and also ensure that the ROS node is shutdown. Signed-off-by: Jacob Perron --- ros2action/ros2action/verb/send_goal.py | 108 +++++++++++++++--------- 1 file changed, 69 insertions(+), 39 deletions(-) diff --git a/ros2action/ros2action/verb/send_goal.py b/ros2action/ros2action/verb/send_goal.py index 2f535343c..ef7ed9ec6 100644 --- a/ros2action/ros2action/verb/send_goal.py +++ b/ros2action/ros2action/verb/send_goal.py @@ -75,59 +75,89 @@ def _feedback_callback(feedback): def send_goal(action_name, action_type, goal_values, feedback_callback): - # TODO(jacobperron): This logic should come from a rosidl related package - package_name, action_type = action_type.split('/', 2) - if not package_name or not action_type: - raise RuntimeError('The passed action type is invalid') + goal_handle = None + node = None + action_client = None + try: + # TODO(jacobperron): This logic should come from a rosidl related package + package_name, action_type = action_type.split('/', 2) + if not package_name or not action_type: + raise RuntimeError('The passed action type is invalid') - module = importlib.import_module(package_name + '.action') - action_module = getattr(module, action_type) - goal_dict = yaml.load(goal_values) + module = importlib.import_module(package_name + '.action') + action_module = getattr(module, action_type) + goal_dict = yaml.load(goal_values) - rclpy.init() + rclpy.init() - node_name = NODE_NAME_PREFIX + '_send_goal_{}_{}'.format(package_name, action_type) - node = rclpy.create_node(node_name) + node_name = NODE_NAME_PREFIX + '_send_goal_{}_{}'.format(package_name, action_type) + node = rclpy.create_node(node_name) - action_client = ActionClient(node, action_module, action_name) + action_client = ActionClient(node, action_module, action_name) - goal = action_module.Goal() + goal = action_module.Goal() - try: - set_message_fields(goal, goal_dict) - except Exception as ex: - return 'Failed to populate message fields: {!r}'.format(ex) + try: + set_message_fields(goal, goal_dict) + except Exception as ex: + return 'Failed to populate message fields: {!r}'.format(ex) + + print('Waiting for an action server to become available...') + action_client.wait_for_server() + + print('Sending goal:\n {}'.format(message_to_yaml(goal, None))) + goal_future = action_client.send_goal_async(goal, feedback_callback) + rclpy.spin_until_future_complete(node, goal_future) + + goal_handle = goal_future.result() - print('Waiting for an action server to become available...') - action_client.wait_for_server() + if goal_handle is None: + raise RuntimeError( + 'Exeception while sending goal: {!r}'.format(goal_future.exception())) - print('Sending goal:\n {}'.format(message_to_yaml(goal, None))) - goal_future = action_client.send_goal_async(goal, feedback_callback) - rclpy.spin_until_future_complete(node, goal_future) + if not goal_handle.accepted: + print('Goal was rejected.') + return - goal_handle = goal_future.result() + print('Goal accepted with ID: {}\n'.format(bytes(goal_handle.goal_id.uuid).hex())) - if goal_handle is None: - raise RuntimeError('Exeception while sending goal: {!r}'.format(goal_future.exception())) + result_future = goal_handle.get_result_async() + rclpy.spin_until_future_complete(node, result_future) - if not goal_handle.accepted: - print('Goal was rejected.') - return + result = result_future.result() - print('Goal accepted with ID: {}\n'.format(bytes(goal_handle.goal_id.uuid).hex())) + if result is None: + raise RuntimeError( + 'Exeception while getting result: {!r}'.format(result_future.exception())) - result_future = goal_handle.get_result_async() - rclpy.spin_until_future_complete(node, result_future) + print('Result:\n {}'.format(message_to_yaml(result.result, None))) + print('Goal finished with status: {}'.format(_goal_status_to_string(result.status))) + finally: + # Cancel the goal if it's still active + if (goal_handle is not None and + (GoalStatus.STATUS_ACCEPTED == goal_handle.status or + GoalStatus.STATUS_EXECUTING == goal_handle.status)): + print('Canceling goal...') + cancel_future = goal_handle.cancel_goal_async() + rclpy.spin_until_future_complete(node, cancel_future) - result = result_future.result() + cancel_response = cancel_future.result() - if result is None: - raise RuntimeError( - 'Exeception while getting result: {!r}'.format(result_future.exception())) + if cancel_response is None: + raise RuntimeError( + 'Exeception while canceling goal: {!r}'.format(cancel_future.exception())) - print('Result:\n {}'.format(message_to_yaml(result.result, None))) - print('Goal finished with status: {}'.format(_goal_status_to_string(result.status))) + if len(cancel_response.goals_canceling) == 0: + raise RuntimeError('Failed to cancel goal') + if len(cancel_response.goals_canceling) > 1: + raise RuntimeError('More than one goal canceled') + if cancel_response.goals_canceling[0].goal_id != goal_handle.goal_id: + raise RuntimeError('Canceled goal with incorrect goal ID') + print('Goal canceled.') - action_client.destroy() - node.destroy_node() - rclpy.shutdown() + if action_client is not None: + action_client.destroy() + if node is not None: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() From 5bc307fa0f12c932aefe73ce9c95321ddbd52a3c Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 12 Apr 2019 06:45:45 -0700 Subject: [PATCH 17/20] Fix typos Signed-off-by: Jacob Perron --- ros2action/ros2action/verb/show.py | 2 +- ros2action/test/test_copyright.py | 2 +- ros2action/test/test_flake8.py | 2 +- ros2action/test/test_pep257.py | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ros2action/ros2action/verb/show.py b/ros2action/ros2action/verb/show.py index eacbc820d..b2df89326 100644 --- a/ros2action/ros2action/verb/show.py +++ b/ros2action/ros2action/verb/show.py @@ -23,7 +23,7 @@ class ShowVerb(VerbExtension): def add_arguments(self, parser, cli_name): arg = parser.add_argument( 'action_type', - help="Type of the ROS action (e.g. 'examples_interfaces/Fibonacci')") + help="Type of the ROS action (e.g. 'example_interfaces/Fibonacci')") arg.completer = action_type_completer def main(self, *, args): diff --git a/ros2action/test/test_copyright.py b/ros2action/test/test_copyright.py index cf0fae31f..ca44b846e 100644 --- a/ros2action/test/test_copyright.py +++ b/ros2action/test/test_copyright.py @@ -1,4 +1,4 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. +# Copyright 2019 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/ros2action/test/test_flake8.py b/ros2action/test/test_flake8.py index eff829969..b90428249 100644 --- a/ros2action/test/test_flake8.py +++ b/ros2action/test/test_flake8.py @@ -1,4 +1,4 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. +# Copyright 2019 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/ros2action/test/test_pep257.py b/ros2action/test/test_pep257.py index 0e38a6c60..6b8c791ef 100644 --- a/ros2action/test/test_pep257.py +++ b/ros2action/test/test_pep257.py @@ -1,4 +1,4 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. +# Copyright 2019 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 71de0e55d47858e3b6111da44de5b9911a1f9acd Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 12 Apr 2019 15:34:26 -0700 Subject: [PATCH 18/20] Change maintainer Signed-off-by: Jacob Perron --- ros2action/package.xml | 2 +- ros2action/setup.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ros2action/package.xml b/ros2action/package.xml index 30dcff1b1..74d0eef21 100644 --- a/ros2action/package.xml +++ b/ros2action/package.xml @@ -6,7 +6,7 @@ The action command for ROS 2 command line tools. - Dirk Thomas + Jacob Perron Apache License 2.0 Jacob Perron diff --git a/ros2action/setup.py b/ros2action/setup.py index 770d09dff..169b53b84 100644 --- a/ros2action/setup.py +++ b/ros2action/setup.py @@ -9,8 +9,8 @@ zip_safe=True, author='Jacob Perron', author_email='jacob@openrobotics.org', - maintainer='Dirk Thomas', - maintainer_email='dthomas@osrfoundation.org', + maintainer='Jacob Perron', + maintainer_email='jacob@openrobotics.org', url='https://github.com/ros2/ros2cli/tree/master/ros2action', download_url='https://github.com/ros2/ros2cli/releases', keywords=[], From bbd8893419339d692a909b6ddda46fd6d83feb97 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 12 Apr 2019 18:15:59 -0700 Subject: [PATCH 19/20] Move try-except to verb Signed-off-by: Jacob Perron --- ros2action/ros2action/api/__init__.py | 10 ++-------- ros2action/ros2action/verb/info.py | 11 +++++++---- 2 files changed, 9 insertions(+), 12 deletions(-) diff --git a/ros2action/ros2action/api/__init__.py b/ros2action/ros2action/api/__init__.py index 964f2ac36..de9f0d921 100644 --- a/ros2action/ros2action/api/__init__.py +++ b/ros2action/ros2action/api/__init__.py @@ -32,14 +32,8 @@ def get_action_clients_and_servers(*, node, action_name): action_clients = [] action_servers = [] - try: - expanded_name = expand_topic_name(action_name, node.get_name(), node.get_namespace()) - except ValueError as e: - raise RuntimeError(e) - try: - validate_full_topic_name(expanded_name) - except rclpy.exceptions.InvalidTopicNameException as e: - raise RuntimeError(e) + expanded_name = expand_topic_name(action_name, node.get_name(), node.get_namespace()) + validate_full_topic_name(expanded_name) node_names_and_ns = node.get_node_names_and_namespaces() for node_name, node_ns in node_names_and_ns: diff --git a/ros2action/ros2action/verb/info.py b/ros2action/ros2action/verb/info.py index 8cfe5ac4e..53b218b33 100644 --- a/ros2action/ros2action/verb/info.py +++ b/ros2action/ros2action/verb/info.py @@ -35,10 +35,13 @@ def add_arguments(self, parser, cli_name): def main(self, *, args): with DirectNode(args) as node: - action_clients, action_servers = get_action_clients_and_servers( - node=node, - action_name=args.action_name, - ) + try: + action_clients, action_servers = get_action_clients_and_servers( + node=node, + action_name=args.action_name, + ) + except Exception as e: + raise RuntimeError(e) print('Action: {}'.format(args.action_name)) print('Action clients: {}'.format(len(action_clients))) From 6a62cb83cfce5f2aaebb0e04c265d5fbb0215734 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Sun, 14 Apr 2019 06:48:58 -0700 Subject: [PATCH 20/20] Catch expected exceptions only Signed-off-by: Jacob Perron --- ros2action/ros2action/verb/info.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ros2action/ros2action/verb/info.py b/ros2action/ros2action/verb/info.py index 53b218b33..043806fe8 100644 --- a/ros2action/ros2action/verb/info.py +++ b/ros2action/ros2action/verb/info.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import rclpy from ros2action.api import action_name_completer from ros2action.api import get_action_clients_and_servers from ros2action.verb import VerbExtension @@ -40,7 +41,7 @@ def main(self, *, args): node=node, action_name=args.action_name, ) - except Exception as e: + except (ValueError, rclpy.exceptions.InvalidTopicNameException) as e: raise RuntimeError(e) print('Action: {}'.format(args.action_name))