diff --git a/ros2param/package.xml b/ros2param/package.xml new file mode 100644 index 000000000..44a57b806 --- /dev/null +++ b/ros2param/package.xml @@ -0,0 +1,27 @@ + + + + ros2param + 0.4.0 + + The param command for ROS 2 command line tools. + + Dirk Thomas + Apache License 2.0 + + rcl_interfaces + rclpy + ros2cli + + python3-yaml + ros2node + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/ros2param/ros2param/__init__.py b/ros2param/ros2param/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/ros2param/ros2param/api/__init__.py b/ros2param/ros2param/api/__init__.py new file mode 100644 index 000000000..9f8e10718 --- /dev/null +++ b/ros2param/ros2param/api/__init__.py @@ -0,0 +1,105 @@ +# 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 rcl_interfaces.msg import ParameterType +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import GetParameters +from rcl_interfaces.srv import SetParameters +import rclpy + + +def get_parameter_value(*, string_value): + """Guess the desired type of the parameter based on the string value.""" + value = ParameterValue() + if string_value.lower() in ('true', 'false'): + value.type = ParameterType.PARAMETER_BOOL + value.bool_value = string_value.lower() == 'true' + elif _is_integer(string_value): + value.type = ParameterType.PARAMETER_INTEGER + value.integer_value = int(string_value) + elif _is_float(string_value): + value.type = ParameterType.PARAMETER_DOUBLE + value.double_value = float(string_value) + else: + value.type = ParameterType.PARAMETER_STRING + value.string_value = string_value + return value + + +def _is_integer(string_value): + try: + integer_value = int(string_value) + except ValueError: + return False + return str(integer_value) == string_value + + +def _is_float(string_value): + try: + float(string_value) + except ValueError: + return False + return True + + +def call_get_parameters(*, node, node_name, parameter_names): + # create client + client = node.create_client( + GetParameters, + '/{node_name}/get_parameters'.format_map(locals())) + + # call as soon as ready + ready = client.wait_for_service(timeout_sec=5.0) + if not ready: + raise RuntimeError('Wait for service timed out') + + request = GetParameters.Request() + request.names = parameter_names + future = client.call_async(request) + rclpy.spin_until_future_complete(node, future) + + # handle response + response = future.result() + if response is None: + e = future.exception() + raise RuntimeError( + 'Exception while calling service of node ' + "'{args.node_name}': {e}".format_map(locals())) + return response + + +def call_set_parameters(*, node, node_name, parameters): + # create client + client = node.create_client( + SetParameters, + '/{node_name}/set_parameters'.format_map(locals())) + + # call as soon as ready + ready = client.wait_for_service(timeout_sec=5.0) + if not ready: + raise RuntimeError('Wait for service timed out') + + request = SetParameters.Request() + request.parameters = parameters + future = client.call_async(request) + rclpy.spin_until_future_complete(node, future) + + # handle response + response = future.result() + if response is None: + e = future.exception() + raise RuntimeError( + 'Exception while calling service of node ' + "'{args.node_name}': {e}".format_map(locals())) + return response diff --git a/ros2param/ros2param/command/__init__.py b/ros2param/ros2param/command/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/ros2param/ros2param/command/param.py b/ros2param/ros2param/command/param.py new file mode 100644 index 000000000..eff64a23e --- /dev/null +++ b/ros2param/ros2param/command/param.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 ParamCommand(CommandExtension): + """Various param 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('ros2param.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/ros2param/ros2param/verb/__init__.py b/ros2param/ros2param/verb/__init__.py new file mode 100644 index 000000000..853e87372 --- /dev/null +++ b/ros2param/ros2param/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 'param' 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/ros2param/ros2param/verb/delete.py b/ros2param/ros2param/verb/delete.py new file mode 100644 index 000000000..2caf43572 --- /dev/null +++ b/ros2param/ros2param/verb/delete.py @@ -0,0 +1,74 @@ +# 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 rcl_interfaces.msg import Parameter +from rcl_interfaces.msg import ParameterType +from rcl_interfaces.msg import ParameterValue +from ros2cli.node.direct import DirectNode +from ros2cli.node.strategy import add_arguments +from ros2cli.node.strategy import NodeStrategy +from ros2node.api import get_node_names +from ros2node.api import NodeNameCompleter +from ros2param.api import call_set_parameters +from ros2param.verb import VerbExtension + + +class DeleteVerb(VerbExtension): + """Delete parameter.""" + + 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( + 'name', help='Name of the parameter') + + 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: + parameter = Parameter() + Parameter.name = args.name + value = ParameterValue() + value.type = ParameterType.PARAMETER_NOT_SET + parameter.value = value + + response = call_set_parameters( + node=node, node_name=args.node_name, parameters=[parameter]) + + # output response + assert len(response.results) == 1 + result = response.results[0] + if result.successful: + msg = 'Deleted parameter successfully' + if result.reason: + msg += ': ' + result.reason + print(msg) + else: + msg = 'Deleting parameter failed' + if result.reason: + msg += ': ' + result.reason + print(msg, file=sys.stderr) diff --git a/ros2param/ros2param/verb/get.py b/ros2param/ros2param/verb/get.py new file mode 100644 index 000000000..2b6e32430 --- /dev/null +++ b/ros2param/ros2param/verb/get.py @@ -0,0 +1,99 @@ +# 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 rcl_interfaces.msg import ParameterType +from ros2cli.node.direct import DirectNode +from ros2cli.node.strategy import add_arguments +from ros2cli.node.strategy import NodeStrategy +from ros2node.api import get_node_names +from ros2node.api import NodeNameCompleter +from ros2param.api import call_get_parameters +from ros2param.verb import VerbExtension + + +class GetVerb(VerbExtension): + """Get parameter.""" + + 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( + 'name', help='Name of the parameter') + parser.add_argument( + '--hide-type', action='store_true', + help='Hide the type information') + + 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: + response = call_get_parameters( + node=node, node_name=args.node_name, + parameter_names=[args.name]) + + assert len(response.values) <= 1 + + # requested parameter not set + if not response.values: + return 'Parameter not set' + + # extract type specific value + pvalue = response.values[0] + if pvalue.type == ParameterType.PARAMETER_BOOL: + label = 'Boolean value is:' + value = pvalue.bool_value + elif pvalue.type == ParameterType.PARAMETER_INTEGER: + label = 'Integer value is:' + value = pvalue.integer_value + elif pvalue.type == ParameterType.PARAMETER_DOUBLE: + label = 'Double value is:' + value = pvalue.double_value + elif pvalue.type == ParameterType.PARAMETER_STRING: + label = 'String value is:' + value = pvalue.string_value + elif pvalue.type == ParameterType.PARAMETER_BYTE_ARRAY: + label = 'Byte values are:' + value = pvalue.byte_array_value + elif pvalue.type == ParameterType.PARAMETER_BOOL_ARRAY: + label = 'Boolean values are:' + value = pvalue.bool_array_value + elif pvalue.type == ParameterType.PARAMETER_INTEGER_ARRAY: + label = 'Integer values are:' + value = pvalue.integer_array_value + elif pvalue.type == ParameterType.PARAMETER_DOUBLE_ARRAY: + label = 'Double values are:' + value = pvalue.double_array_value + elif pvalue.type == ParameterType.PARAMETER_STRING_ARRAY: + label = 'String values are:' + value = pvalue.string_array_value + else: + return "Unknown parameter type '{pvalue.type}'" \ + .format_map(locals()) + + # output response + if not args.hide_type: + print(label, value) + else: + print(value) diff --git a/ros2param/ros2param/verb/list.py b/ros2param/ros2param/verb/list.py new file mode 100644 index 000000000..589cc3a9e --- /dev/null +++ b/ros2param/ros2param/verb/list.py @@ -0,0 +1,91 @@ +# 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 rcl_interfaces.srv import ListParameters +import rclpy +from ros2cli.node.direct import DirectNode +from ros2cli.node.strategy import add_arguments +from ros2cli.node.strategy import NodeStrategy +from ros2node.api import get_node_names +from ros2node.api import NodeNameCompleter +from ros2param.verb import VerbExtension + + +class ListVerb(VerbExtension): + """Output a list of available parameters.""" + + 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') + + 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: + clients = {} + futures = {} + # create clients + for node_name in node_names: + client = node.create_client( + ListParameters, + '/{node_name}/list_parameters'.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 = ListParameters.Request() + 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 node_name in node_names: + rclpy.spin_until_future_complete(node, futures[node_name]) + + # print responses + for node_name in sorted(futures.keys()): + future = futures[node_name] + if future.result() is not None: + print('{node_name}:'.format_map(locals())) + response = future.result() + for name in sorted(response.result.names): + print(' {name}'.format_map(locals())) + else: + e = future.exception() + print( + 'Exception while calling service of node ' + "'{node_name}': {e}".format_map(locals()), + file=sys.stderr) diff --git a/ros2param/ros2param/verb/set.py b/ros2param/ros2param/verb/set.py new file mode 100644 index 000000000..2335a322e --- /dev/null +++ b/ros2param/ros2param/verb/set.py @@ -0,0 +1,73 @@ +# 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 rcl_interfaces.msg import Parameter +from ros2cli.node.direct import DirectNode +from ros2cli.node.strategy import add_arguments +from ros2cli.node.strategy import NodeStrategy +from ros2node.api import get_node_names +from ros2node.api import NodeNameCompleter +from ros2param.api import call_set_parameters +from ros2param.api import get_parameter_value +from ros2param.verb import VerbExtension + + +class SetVerb(VerbExtension): + """Set parameter.""" + + 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( + 'name', help='Name of the parameter') + parser.add_argument( + 'value', help='Value of the parameter') + + 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: + parameter = Parameter() + Parameter.name = args.name + parameter.value = get_parameter_value(string_value=args.value) + + response = call_set_parameters( + node=node, node_name=args.node_name, parameters=[parameter]) + + # output response + assert len(response.results) == 1 + result = response.results[0] + if result.successful: + msg = 'Set parameter successful' + if result.reason: + msg += ': ' + result.reason + print(msg) + else: + msg = 'Setting parameter failed' + if result.reason: + msg += ': ' + result.reason + print(msg, file=sys.stderr) diff --git a/ros2param/setup.py b/ros2param/setup.py new file mode 100644 index 000000000..88f71544b --- /dev/null +++ b/ros2param/setup.py @@ -0,0 +1,42 @@ +from setuptools import find_packages +from setuptools import setup + +setup( + name='ros2param', + 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/ros2param', + 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 param command for ROS 2 command line tools.', + long_description="""\ +The package provides the param command for the ROS 2 command line tools.""", + license='Apache License, Version 2.0', + tests_require=['pytest'], + entry_points={ + 'ros2cli.command': [ + 'param = ros2param.command.param:ParamCommand', + ], + 'ros2cli.extension_point': [ + 'ros2param.verb = ros2param.verb:VerbExtension', + ], + 'ros2param.verb': [ + 'delete = ros2param.verb.delete:DeleteVerb', + 'get = ros2param.verb.get:GetVerb', + 'list = ros2param.verb.list:ListVerb', + 'set = ros2param.verb.set:SetVerb', + ], + } +) diff --git a/ros2param/test/test_copyright.py b/ros2param/test/test_copyright.py new file mode 100644 index 000000000..cf0fae31f --- /dev/null +++ b/ros2param/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/ros2param/test/test_flake8.py b/ros2param/test/test_flake8.py new file mode 100644 index 000000000..eff829969 --- /dev/null +++ b/ros2param/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/ros2param/test/test_pep257.py b/ros2param/test/test_pep257.py new file mode 100644 index 000000000..0e38a6c60 --- /dev/null +++ b/ros2param/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'