diff --git a/ros2param/ros2param/api/__init__.py b/ros2param/ros2param/api/__init__.py index cd8a4b29a..84303273b 100644 --- a/ros2param/ros2param/api/__init__.py +++ b/ros2param/ros2param/api/__init__.py @@ -12,6 +12,9 @@ # 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 rcl_interfaces.srv import DescribeParameters @@ -19,7 +22,9 @@ from rcl_interfaces.srv import ListParameters from rcl_interfaces.srv import SetParameters import rclpy +from rclpy.parameter import PARAMETER_SEPARATOR_STRING from ros2cli.node.direct import DirectNode + import yaml @@ -92,6 +97,65 @@ def get_parameter_value(*, string_value): return value +def parse_parameter_dict(*, namespace, parameter_dict): + parameters = [] + for param_name, param_value in parameter_dict.items(): + full_param_name = namespace + param_name + # Unroll nested parameters + if type(param_value) == dict: + parameters += parse_parameter_dict( + namespace=full_param_name + PARAMETER_SEPARATOR_STRING, + parameter_dict=param_value) + else: + parameter = Parameter() + parameter.name = full_param_name + parameter.value = get_parameter_value(string_value=str(param_value)) + parameters.append(parameter) + return parameters + + +def load_parameter_dict(*, node, node_name, parameter_dict): + + parameters = parse_parameter_dict(namespace='', parameter_dict=parameter_dict) + response = call_set_parameters( + node=node, node_name=node_name, parameters=parameters) + + # output response + assert len(response.results) == len(parameters) + for i in range(0, len(response.results)): + result = response.results[i] + param_name = parameters[i].name + if result.successful: + msg = 'Set parameter {} successful'.format(param_name) + if result.reason: + msg += ': ' + result.reason + print(msg) + else: + msg = 'Set parameter {} failed'.format(param_name) + if result.reason: + msg += ': ' + result.reason + print(msg, file=sys.stderr) + + +def load_parameter_file(*, node, node_name, parameter_file): + # Remove leading slash and namespaces + internal_node_name = node_name.split('/')[-1] + with open(parameter_file, 'r') as f: + param_file = yaml.safe_load(f) + if internal_node_name not in param_file: + raise RuntimeError('Param file does not contain parameters for {}, ' + ' only for namespaces: {}' .format(internal_node_name, + param_file.keys())) + + value = param_file[internal_node_name] + if type(value) != dict or 'ros__parameters' not in value: + raise RuntimeError('Invalid structure of parameter file in namespace {}' + 'expected same format as provided by ros2 param dump' + .format(internal_node_name)) + load_parameter_dict(node=node, node_name=node_name, + parameter_dict=value['ros__parameters']) + + def call_describe_parameters(*, node, node_name, parameter_names=None): # create client client = node.create_client( diff --git a/ros2param/ros2param/verb/load.py b/ros2param/ros2param/verb/load.py new file mode 100644 index 000000000..72a72824e --- /dev/null +++ b/ros2param/ros2param/verb/load.py @@ -0,0 +1,50 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ros2cli.node.direct import DirectNode +from ros2cli.node.strategy import add_arguments +from ros2cli.node.strategy import NodeStrategy +from ros2node.api import get_absolute_node_name +from ros2node.api import get_node_names +from ros2node.api import NodeNameCompleter +from ros2param.api import load_parameter_file +from ros2param.verb import VerbExtension + + +class LoadVerb(VerbExtension): + """Load parameter file for a node.""" + + 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') + arg = parser.add_argument( + 'parameter_file', help='Parameter file') + + 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) + + node_name = get_absolute_node_name(args.node_name) + if node_name not in {n.full_name for n in node_names}: + return 'Node not found' + + with DirectNode(args) as node: + load_parameter_file(node=node, node_name=node_name, parameter_file=args.parameter_file) diff --git a/ros2param/setup.py b/ros2param/setup.py index e59e3a301..55a982059 100644 --- a/ros2param/setup.py +++ b/ros2param/setup.py @@ -46,6 +46,7 @@ 'get = ros2param.verb.get:GetVerb', 'list = ros2param.verb.list:ListVerb', 'set = ros2param.verb.set:SetVerb', + 'load = ros2param.verb.load:LoadVerb', ], } ) diff --git a/ros2param/test/fixtures/parameter_node.py b/ros2param/test/fixtures/parameter_node.py index 36385f18d..9df269ac0 100644 --- a/ros2param/test/fixtures/parameter_node.py +++ b/ros2param/test/fixtures/parameter_node.py @@ -11,7 +11,6 @@ # 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 import rclpy from rclpy.parameter import PARAMETER_SEPARATOR_STRING @@ -38,9 +37,6 @@ def main(args=None): rclpy.spin(node) except KeyboardInterrupt: print('parameter node stopped cleanly') - except BaseException: - print('exception in parameter node:', file=sys.stderr) - raise finally: node.destroy_node() rclpy.shutdown() diff --git a/ros2param/test/test_verb_load.py b/ros2param/test/test_verb_load.py new file mode 100644 index 000000000..0cfbbb43a --- /dev/null +++ b/ros2param/test/test_verb_load.py @@ -0,0 +1,282 @@ +# Copyright 2021 PAL Robotics S.L. +# +# 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 contextlib +import os +import sys +import tempfile +import time +import unittest +import xmlrpc + +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch_ros.actions import Node +import launch_testing +import launch_testing.actions +import launch_testing.asserts +import launch_testing.markers +import launch_testing.tools +import launch_testing_ros.tools + +import pytest + +import rclpy +from rclpy.utilities import get_available_rmw_implementations + +from ros2cli.node.strategy import NodeStrategy + +TEST_NODE = 'test_node' +TEST_NAMESPACE = '/foo' + +TEST_TIMEOUT = 20.0 + +INPUT_PARAMETER_FILE = ( + f'{TEST_NODE}:\n' + ' ros__parameters:\n' + ' bool_array_param:\n' + ' - true\n' + ' - false\n' + ' - false\n' + ' bool_param: false\n' + ' double_array_param:\n' + ' - 2.125\n' + ' - 1.25\n' + ' - 2.5\n' + ' double_param: 1.3\n' + ' foo:\n' + ' bar:\n' + ' str_param: foo_bar\n' + ' str_param: foobar\n' + ' int_array_param:\n' + ' - 42\n' + ' - 3\n' + ' - 3\n' + ' int_param: -42\n' + ' str_array_param:\n' + ' - a_foo\n' + ' - a_bar\n' + ' - baz\n' + ' str_param: Bye World\n' + ' use_sim_time: false\n' +) + +# Skip cli tests on Windows while they exhibit pathological behavior +# https://github.com/ros2/build_farmer/issues/248 +if sys.platform.startswith('win'): + pytest.skip( + 'CLI tests can block for a pathological amount of time on Windows.', + allow_module_level=True) + + +@pytest.mark.rostest +@launch_testing.parametrize('rmw_implementation', get_available_rmw_implementations()) +def generate_test_description(rmw_implementation): + path_to_fixtures = os.path.join(os.path.dirname(__file__), 'fixtures') + additional_env = {'RMW_IMPLEMENTATION': rmw_implementation} + + # Parameter node test fixture + path_to_parameter_node_script = os.path.join(path_to_fixtures, 'parameter_node.py') + parameter_node = Node( + executable=sys.executable, + name=TEST_NODE, + namespace=TEST_NAMESPACE, + arguments=[path_to_parameter_node_script], + additional_env=additional_env + ) + + return LaunchDescription([ + # TODO(jacobperron): Provide a common RestartCliDaemon launch action in ros2cli + ExecuteProcess( + cmd=['ros2', 'daemon', 'stop'], + name='daemon-stop', + on_exit=[ + ExecuteProcess( + cmd=['ros2', 'daemon', 'start'], + name='daemon-start', + on_exit=[ + parameter_node, + launch_testing.actions.ReadyToTest(), + ], + additional_env=additional_env + ) + ] + ), + ]) + + +class TestVerbDump(unittest.TestCase): + + @classmethod + def setUpClass( + cls, + launch_service, + proc_info, + proc_output, + rmw_implementation + ): + rmw_implementation_filter = launch_testing_ros.tools.basic_output_filter( + filtered_rmw_implementation=rmw_implementation + ) + + @contextlib.contextmanager + def launch_param_load_command(self, arguments): + param_load_command_action = ExecuteProcess( + cmd=['ros2', 'param', 'load', *arguments], + additional_env={ + 'RMW_IMPLEMENTATION': rmw_implementation, + }, + name='ros2param-load-cli', + output='screen' + ) + with launch_testing.tools.launch_process( + launch_service, param_load_command_action, proc_info, proc_output, + output_filter=rmw_implementation_filter + ) as param_load_command: + yield param_load_command + cls.launch_param_load_command = launch_param_load_command + + @contextlib.contextmanager + def launch_param_dump_command(self, arguments): + param_dump_command_action = ExecuteProcess( + cmd=['ros2', 'param', 'dump', *arguments], + additional_env={ + 'RMW_IMPLEMENTATION': rmw_implementation, + }, + name='ros2param-dump-cli', + output='screen' + ) + with launch_testing.tools.launch_process( + launch_service, param_dump_command_action, proc_info, proc_output, + output_filter=rmw_implementation_filter + ) as param_dump_command: + yield param_dump_command + cls.launch_param_dump_command = launch_param_dump_command + + def setUp(self): + # Ensure the daemon node is running and discovers the test node + start_time = time.time() + timed_out = True + with NodeStrategy(None) as node: + while (time.time() - start_time) < TEST_TIMEOUT: + # TODO(jacobperron): Create a generic 'CliNodeError' so we can treat errors + # from DirectNode and DaemonNode the same + try: + services = node.get_service_names_and_types_by_node(TEST_NODE, TEST_NAMESPACE) + except rclpy.node.NodeNameNonExistentError: + continue + except xmlrpc.client.Fault as e: + if 'NodeNameNonExistentError' in e.faultString: + continue + raise + + service_names = [name_type_tuple[0] for name_type_tuple in services] + if ( + len(service_names) > 0 + and f'{TEST_NAMESPACE}/{TEST_NODE}/get_parameters' in service_names + ): + timed_out = False + break + if timed_out: + self.fail(f'CLI daemon failed to find test node after {TEST_TIMEOUT} seconds') + + def _write_param_file(self, tmpdir, filename): + yaml_path = os.path.join(tmpdir, filename) + with open(yaml_path, 'w') as f: + f.write(INPUT_PARAMETER_FILE) + return yaml_path + + def _output_file(self): + return f'{TEST_NAMESPACE}/{TEST_NODE}'.lstrip('/').replace('/', '__') + '.yaml' + + def test_verb_load_missing_args(self): + with self.launch_param_load_command(arguments=[]) as param_load_command: + assert param_load_command.wait_for_shutdown(timeout=TEST_TIMEOUT) + assert param_load_command.exit_code != launch_testing.asserts.EXIT_OK + assert launch_testing.tools.expect_output( + expected_lines=['ros2 param load: error: the following arguments are required: ' + 'node_name, parameter_file'], + text=param_load_command.output, + strict=False + ) + with self.launch_param_load_command(arguments=['some_node']) as param_load_command: + assert param_load_command.wait_for_shutdown(timeout=TEST_TIMEOUT) + assert param_load_command.exit_code != launch_testing.asserts.EXIT_OK + assert launch_testing.tools.expect_output( + expected_lines=['ros2 param load: error: the following arguments are required: ' + 'parameter_file'], + text=param_load_command.output, + strict=False + ) + + def test_verb_load_invalid_node(self): + with tempfile.TemporaryDirectory() as tmpdir: + filepath = self._write_param_file(tmpdir, 'params.yaml') + with self.launch_param_load_command( + arguments=['invalid_node', filepath] + ) as param_load_command: + assert param_load_command.wait_for_shutdown(timeout=TEST_TIMEOUT) + assert param_load_command.exit_code != launch_testing.asserts.EXIT_OK + assert launch_testing.tools.expect_output( + expected_lines=['Node not found'], + text=param_load_command.output, + strict=True + ) + with self.launch_param_load_command( + arguments=[f'invalid_ns/{TEST_NODE}', filepath] + ) as param_load_command: + assert param_load_command.wait_for_shutdown(timeout=TEST_TIMEOUT) + assert param_load_command.exit_code != launch_testing.asserts.EXIT_OK + assert launch_testing.tools.expect_output( + expected_lines=['Node not found'], + text=param_load_command.output, + strict=True + ) + + def test_verb_load_invalid_path(self): + with self.launch_param_load_command( + arguments=[f'{TEST_NAMESPACE}/{TEST_NODE}', 'invalid_path'] + ) as param_load_command: + assert param_load_command.wait_for_shutdown(timeout=TEST_TIMEOUT) + assert param_load_command.exit_code != launch_testing.asserts.EXIT_OK + assert launch_testing.tools.expect_output( + expected_lines=['No such file or directory'], + text=param_load_command.output, + strict=False + ) + + def test_verb_load(self): + with tempfile.TemporaryDirectory() as tmpdir: + filepath = self._write_param_file(tmpdir, 'params.yaml') + with self.launch_param_load_command( + arguments=[f'{TEST_NAMESPACE}/{TEST_NODE}', filepath] + ) as param_load_command: + assert param_load_command.wait_for_shutdown(timeout=TEST_TIMEOUT) + assert param_load_command.exit_code == launch_testing.asserts.EXIT_OK + assert launch_testing.tools.expect_output( + expected_lines=[''], + text=param_load_command.output, + strict=True + ) + # Dump with ros2 param dump and compare that output matches input file + with self.launch_param_dump_command( + arguments=[f'{TEST_NAMESPACE}/{TEST_NODE}', '--print'] + ) as param_dump_command: + assert param_dump_command.wait_for_shutdown(timeout=TEST_TIMEOUT) + assert param_dump_command.exit_code == launch_testing.asserts.EXIT_OK + assert launch_testing.tools.expect_output( + expected_text=INPUT_PARAMETER_FILE + '\n', + text=param_dump_command.output, + strict=True + )