From f815c6cdd497b6d96463762484f1210279689ad8 Mon Sep 17 00:00:00 2001 From: Brian Date: Wed, 15 Jun 2022 14:05:16 -0700 Subject: [PATCH] Revert "implement parameter_client" (#958) See discussion @ https://github.com/ros2/rclpy/pull/956 Signed-off-by: Brian Chen --- rclpy/docs/source/api/parameters.rst | 5 - rclpy/rclpy/__init__.py | 3 +- rclpy/rclpy/parameter.py | 118 +--------- rclpy/rclpy/parameter_client.py | 319 --------------------------- rclpy/test/test_parameter.py | 23 +- rclpy/test/test_parameter_client.py | 144 ------------ 6 files changed, 3 insertions(+), 609 deletions(-) delete mode 100644 rclpy/rclpy/parameter_client.py delete mode 100644 rclpy/test/test_parameter_client.py diff --git a/rclpy/docs/source/api/parameters.rst b/rclpy/docs/source/api/parameters.rst index 5dee741fd..c38ee7d9c 100644 --- a/rclpy/docs/source/api/parameters.rst +++ b/rclpy/docs/source/api/parameters.rst @@ -10,8 +10,3 @@ Parameter Service ----------------- .. automodule:: rclpy.parameter_service - -Parameter Client ------------------ - -.. automodule:: rclpy.parameter_client diff --git a/rclpy/rclpy/__init__.py b/rclpy/rclpy/__init__.py index 26e0dc519..cea1f9163 100644 --- a/rclpy/rclpy/__init__.py +++ b/rclpy/rclpy/__init__.py @@ -43,7 +43,6 @@ from typing import List from typing import Optional from typing import TYPE_CHECKING -from typing import Union from rclpy.context import Context from rclpy.parameter import Parameter @@ -142,7 +141,7 @@ def create_node( use_global_arguments: bool = True, enable_rosout: bool = True, start_parameter_services: bool = True, - parameter_overrides: Union[List[Parameter], None] = None, + parameter_overrides: List[Parameter] = None, allow_undeclared_parameters: bool = False, automatically_declare_parameters_from_overrides: bool = False ) -> 'Node': diff --git a/rclpy/rclpy/parameter.py b/rclpy/rclpy/parameter.py index 6b917aaa0..daad33146 100644 --- a/rclpy/rclpy/parameter.py +++ b/rclpy/rclpy/parameter.py @@ -14,14 +14,9 @@ import array from enum import Enum -from typing import Dict -from typing import List -from typing import Optional from rcl_interfaces.msg import Parameter as ParameterMsg -from rcl_interfaces.msg import ParameterType -from rcl_interfaces.msg import ParameterValue -import yaml +from rcl_interfaces.msg import ParameterType, ParameterValue PARAMETER_SEPARATOR_STRING = '.' @@ -182,50 +177,6 @@ def to_parameter_msg(self): return ParameterMsg(name=self.name, value=self.get_parameter_value()) -def get_parameter_value(string_value: str) -> ParameterValue: - """ - Guess the desired type of the parameter based on the string value. - - :param string_value: The string value to be converted to a ParameterValue. - :return: The ParameterValue. - """ - value = ParameterValue() - try: - yaml_value = yaml.safe_load(string_value) - except yaml.parser.ParserError: - yaml_value = string_value - - if isinstance(yaml_value, bool): - value.type = ParameterType.PARAMETER_BOOL - value.bool_value = yaml_value - elif isinstance(yaml_value, int): - value.type = ParameterType.PARAMETER_INTEGER - value.integer_value = yaml_value - elif isinstance(yaml_value, float): - value.type = ParameterType.PARAMETER_DOUBLE - value.double_value = yaml_value - elif isinstance(yaml_value, list): - if all((isinstance(v, bool) for v in yaml_value)): - value.type = ParameterType.PARAMETER_BOOL_ARRAY - value.bool_array_value = yaml_value - elif all((isinstance(v, int) for v in yaml_value)): - value.type = ParameterType.PARAMETER_INTEGER_ARRAY - value.integer_array_value = yaml_value - elif all((isinstance(v, float) for v in yaml_value)): - value.type = ParameterType.PARAMETER_DOUBLE_ARRAY - value.double_array_value = yaml_value - elif all((isinstance(v, str) for v in yaml_value)): - value.type = ParameterType.PARAMETER_STRING_ARRAY - value.string_array_value = yaml_value - else: - value.type = ParameterType.PARAMETER_STRING - value.string_value = string_value - else: - value.type = ParameterType.PARAMETER_STRING - value.string_value = yaml_value - return value - - def parameter_value_to_python(parameter_value: ParameterValue): """ Get the value for the Python builtin type from a rcl_interfaces/msg/ParameterValue object. @@ -260,70 +211,3 @@ def parameter_value_to_python(parameter_value: ParameterValue): raise RuntimeError(f'unexpected parameter type {parameter_value.type}') return value - - -def parameter_dict_from_yaml_file( - parameter_file: str, - use_wildcard: bool = False, - target_nodes: Optional[List[str]] = None, - namespace: str = '' -) -> Dict[str, ParameterMsg]: - """ - Build a dict of parameters from a YAML file formatted as per ``ros2 param dump``. - - Will load all parameters if ``target_nodes`` is None - - :param parameter_file: Path to the YAML file to load parameters from. - :param use_wildcard: Use wildcard matching for the target nodes. - :param target_nodes: List of nodes in the YAML file to load parameters from. - :param namespace: Namespace to prepend to all parameters. - :return: A dict of Parameter objects keyed by the parameter names - """ - with open(parameter_file, 'r') as f: - param_file = yaml.safe_load(f) - param_keys = set() - param_dict = {} - - if use_wildcard and '/**' in param_file: - param_keys.add('/**') - if target_nodes: - for n in target_nodes: - if n not in param_file.keys(): - raise RuntimeError(f'Param file does not contain parameters for {n},' - f'only for nodes: {list(param_file.keys())} ') - param_keys.add(n) - else: - param_keys = param_file.keys() - - for n in param_keys: - value = param_file[n] - if type(value) != dict or 'ros__parameters' not in value: - raise RuntimeError('Invalid structure of parameter file for node {}' - 'expected same format as provided by ros2 param dump' - .format(n)) - param_dict.update(value['ros__parameters']) - return _unpack_parameter_dict(namespace, param_dict) - - -def _unpack_parameter_dict(namespace, parameter_dict): - """ - Flatten a parameter dictionary recursively. - - :param namespace: The namespace to prepend to the parameter names. - :param parameter_dict: A dictionary of parameters keyed by the parameter names - :return: A dict of Parameter objects keyed by the parameter names - """ - parameters: Dict[str, ParameterMsg] = {} - for param_name, param_value in parameter_dict.items(): - full_param_name = namespace + param_name - # Unroll nested parameters - if type(param_value) == Dict: - parameters += _unpack_parameter_dict( - namespace=full_param_name + PARAMETER_SEPARATOR_STRING, - parameter_dict=param_value) - else: - parameter = ParameterMsg() - parameter.name = full_param_name - parameter.value = get_parameter_value(str(param_value)) - parameters[full_param_name] = parameter - return parameters diff --git a/rclpy/rclpy/parameter_client.py b/rclpy/rclpy/parameter_client.py deleted file mode 100644 index 7757c3231..000000000 --- a/rclpy/rclpy/parameter_client.py +++ /dev/null @@ -1,319 +0,0 @@ -# Copyright 2022 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 time -from typing import Callable, List, Optional, Sequence, Union - -from rcl_interfaces.msg import Parameter as ParameterMsg -from rcl_interfaces.srv import DescribeParameters -from rcl_interfaces.srv import GetParameters -from rcl_interfaces.srv import GetParameterTypes -from rcl_interfaces.srv import ListParameters -from rcl_interfaces.srv import SetParameters -from rcl_interfaces.srv import SetParametersAtomically - -from rclpy.callback_groups import CallbackGroup -from rclpy.node import Node -from rclpy.parameter import Parameter as Parameter -from rclpy.parameter import parameter_dict_from_yaml_file -from rclpy.qos import qos_profile_services_default -from rclpy.qos import QoSProfile -from rclpy.task import Future - - -class AsyncParameterClient: - def __init__( - self, - node: Node, - remote_node_name: str, - qos_profile: QoSProfile = qos_profile_services_default, - callback_group: Optional[CallbackGroup] = None): - """ - Create an AsyncParameterClient. - - An AsyncParameterClient that uses services offered by a remote node - to query and modify parameters in a streamlined way. - - Usage example: - - .. code-block:: python - - import rclpy - from rclpy.parameter import Parameter - node = rclpy.create_node('my_node') - - client = AsyncParameterClient(node, 'example_node') - - # set parameters - future = client.set_parameters([ - Parameter('int_param', Parameter.Type.INTEGER, 88).to_parameter_msg(), - Parameter('string/param', Parameter.Type.STRING, 'hello world').to_parameter_msg(), - ]) - self.executor.spin_until_future_complete(future) - results = future.result() # rcl_interfaces.srv.SetParameters.Response - - For more on service names, see: `ROS 2 docs`_. - - .. _ROS 2 docs: https://docs.ros.org/en/rolling/Concepts/About-ROS-2-Parameters.html#interacting-with-parameters # noqa E501 - - :param node: Node used to create clients that will interact with the remote node - :param remote_node_name: Name of remote node for which the parameters will be managed - """ - self.remote_node_name = remote_node_name - self.node = node - self._get_parameter_client = self.node.create_client( - GetParameters, f'{remote_node_name}/get_parameters', - qos_profile=qos_profile, callback_group=callback_group - ) - self._list_parameter_client = self.node.create_client( - ListParameters, f'{remote_node_name}/list_parameters', - qos_profile=qos_profile, callback_group=callback_group - ) - self._set_parameter_client = self.node.create_client( - SetParameters, f'{remote_node_name}/set_parameters', - qos_profile=qos_profile, callback_group=callback_group - ) - self._get_parameter_types_client = self.node.create_client( - GetParameterTypes, f'{remote_node_name}/get_parameter_types', - qos_profile=qos_profile, callback_group=callback_group - ) - self._describe_parameters_client = self.node.create_client( - DescribeParameters, f'{remote_node_name}/describe_parameters', - qos_profile=qos_profile, callback_group=callback_group - ) - self._set_parameters_atomically_client = self.node.create_client( - SetParametersAtomically, f'{remote_node_name}/set_parameters_atomically', - qos_profile=qos_profile, callback_group=callback_group - ) - - def services_are_ready(self) -> bool: - """ - Check if all services are ready. - - :return: ``True`` if all services are available, False otherwise. - """ - return all([ - self._list_parameter_client.service_is_ready(), - self._set_parameter_client.service_is_ready(), - self._get_parameter_client.service_is_ready(), - self._get_parameter_types_client.service_is_ready(), - self._describe_parameters_client.service_is_ready(), - self._set_parameters_atomically_client.service_is_ready(), - ]) - - def wait_for_services(self, timeout_sec: Optional[float] = None) -> bool: - """ - Wait for all parameter services to be available. - - :param timeout_sec: Seconds to wait. If ``None``, then wait forever. - :return: ``True`` if all services were waite , ``False`` otherwise. - """ - client_wait_fns = [ - self._list_parameter_client.wait_for_service, - self._set_parameter_client.wait_for_service, - self._get_parameter_client.wait_for_service, - self._get_parameter_types_client.wait_for_service, - self._describe_parameters_client.wait_for_service, - self._set_parameters_atomically_client.wait_for_service, - ] - - if timeout_sec is None: - return all([fn() for fn in client_wait_fns]) - - prev = time.time() - for wait_for_service in client_wait_fns: - if timeout_sec < 0 or not wait_for_service(timeout_sec): - return False - timeout_sec -= time.time() - prev - prev = time.time() - return True - - def list_parameters( - self, - prefixes: Optional[List[str]] = None, - depth: int = 1, - callback: Optional[Callable] = None - ) -> Future: - """ - List all parameters with given prefixs. - - :param prefixes: List of prefixes to filter by. - :param depth: Depth of the parameter tree to list. - :param callback: Callback function to call when the request is complete. - :return: ``Future`` with the result of the request. - """ - request = ListParameters.Request() - if prefixes: - request.prefixes = prefixes - request.depth = depth - future = self._list_parameter_client.call_async(request) - if callback: - future.add_done_callback(callback) - return future - - def get_parameters(self, names: List[str], callback: Optional[Callable] = None) -> Future: - """ - Get parameters given names. - - :param names: List of parameter names to get. - :param callback: Callback function to call when the request is complete. - :return: ``Future`` with the result of the request. - """ - request = GetParameters.Request() - request.names = names - future = self._get_parameter_client.call_async(request) - if callback: - future.add_done_callback(callback) - return future - - def set_parameters( - self, - parameters: Sequence[Union[Parameter, ParameterMsg]], - callback: Optional[Callable] = None - ) -> Future: - """ - Set parameters given a list of parameters. - - The result after the returned future is complete - will be of type ``rcl_interfaces.srv.SetParameters.Response``. - - :param parameters: Sequence of parameters to set. - :param callback: Callback function to call when the request is complete. - :return: ``Future`` with the result of the request. - """ - request = SetParameters.Request() - request.parameters = [ - param.to_parameter_msg() - if isinstance(param, Parameter) else param - for param in parameters - ] - future = self._set_parameter_client.call_async(request) - if callback: - future.add_done_callback(callback) - return future - - def describe_parameters( - self, - names: List[str], - callback: Optional[Callable] = None - ) -> Future: - """ - Describe parameters given names. - - The result after the returned future is complete - will be of type ``rcl_interfaces.srv.DescribeParameters.Response``. - - :param names: List of parameter names to describe - :param callback: Callback function to call when the request is complete. - :return: ``Future`` with the result of the request. - """ - request = DescribeParameters.Request() - request.names = names - future = self._describe_parameters_client.call_async(request) - if callback: - future.add_done_callback(callback) - return future - - def get_parameter_types( - self, - names: List[str], - callback: Optional[Callable] = None - ) -> Future: - """ - Get parameter types given names. - - The result after the returned future is complete - will be of type ``rcl_interfaces.srv.GetParameterTypes.Response``. - - Parameter type definitions are given in Parameter.Type - - :param names: List of parameter names to get types for. - :param callback: Callback function to call when the request is complete. - :return: ``Future`` with the result of the request. - """ - request = GetParameterTypes.Request() - request.names = names - future = self._get_parameter_types_client.call_async(request) - if callback: - future.add_done_callback(callback) - return future - - def set_parameters_atomically( - self, - parameters: Sequence[Union[Parameter, ParameterMsg]], - callback: Optional[Callable] = None - ) -> Future: - """ - Set parameters atomically. - - The result after the returned future is complete - will be of type ``rcl_interfaces.srv.SetParametersAtomically.Response``. - - :param parameters: Sequence of parameters to set. - :param callback: Callback function to call when the request is complete. - :return: ``Future`` with the result of the request. - """ - request = SetParametersAtomically.Request() - request.parameters = [ - param.to_parameter_msg() - if isinstance(param, Parameter) else param - for param in parameters - ] - future = self._set_parameters_atomically_client.call_async(request) - if callback: - future.add_done_callback(callback) - return future - - def delete_parameters( - self, names: List[str], callback: Optional[Callable] = None - ) -> Future: - """ - Unset parameters with given names. - - The result after the returned future is complete - will be of type ``rcl_interfaces.srv.SetParameters.Response``. - - Note: Only parameters that have been declared as dynamically typed can be unset. - - :param names: List of parameter names to unset. - :param callback: Callback function to call when the request is complete. - :return: ``Future`` with the result of the request. - """ - request = SetParameters.Request() - request.parameters = [Parameter(name=i).to_parameter_msg() for i in names] - future = self._set_parameter_client.call_async(request) - if callback: - future.add_done_callback(callback) - return future - - def load_parameter_file( - self, - parameter_file: str, - use_wildcard: bool = False, - ) -> Future: - """ - Load parameters from a yaml file. - - Wrapper around `rclpy.parameter.parameter_dict_from_yaml_file`. - - The result after the returned future is complete - will be of type ``rcl_interfaces.srv.SetParameters.Response``. - - :param parameter_file: Path to the parameter file. - :param use_wildcard: Whether to use wildcard expansion. - :return: Future with the result from the set_parameter call. - """ - param_dict = parameter_dict_from_yaml_file(parameter_file, use_wildcard) - future = self.set_parameters(list(param_dict.values())) - return future diff --git a/rclpy/test/test_parameter.py b/rclpy/test/test_parameter.py index acf6b9b69..4a4a7f7d3 100644 --- a/rclpy/test/test_parameter.py +++ b/rclpy/test/test_parameter.py @@ -13,15 +13,14 @@ # limitations under the License. from array import array -from tempfile import NamedTemporaryFile import unittest import pytest + from rcl_interfaces.msg import Parameter as ParameterMsg from rcl_interfaces.msg import ParameterType from rcl_interfaces.msg import ParameterValue from rclpy.parameter import Parameter -from rclpy.parameter import parameter_dict_from_yaml_file from rclpy.parameter import parameter_value_to_python @@ -214,26 +213,6 @@ def test_parameter_value_to_python(self): with pytest.raises(RuntimeError): parameter_value_to_python(parameter_value) - def test_parameter_dict_from_yaml_file(self): - yaml_string = """/param_test_target: - ros__parameters: - param_1: 1 - param_str: string - """ - expected = { - 'param_1': Parameter('param_1', Parameter.Type.INTEGER, 1).to_parameter_msg(), - 'param_str': Parameter('param_str', Parameter.Type.STRING, 'string').to_parameter_msg() - } - - with NamedTemporaryFile(mode='w') as f: - f.write(yaml_string) - f.flush() - parameter_dict = parameter_dict_from_yaml_file(f.name) - print(parameter_dict) - assert parameter_dict == expected - - self.assertRaises(FileNotFoundError, parameter_dict_from_yaml_file, 'unknown_file') - if __name__ == '__main__': unittest.main() diff --git a/rclpy/test/test_parameter_client.py b/rclpy/test/test_parameter_client.py deleted file mode 100644 index 49ddc0d40..000000000 --- a/rclpy/test/test_parameter_client.py +++ /dev/null @@ -1,144 +0,0 @@ -# Copyright 2022 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 tempfile import NamedTemporaryFile -import unittest - -import rcl_interfaces.msg -from rcl_interfaces.msg import ParameterType -import rcl_interfaces.srv -import rclpy -import rclpy.context -from rclpy.executors import SingleThreadedExecutor -from rclpy.parameter import Parameter -from rclpy.parameter_client import AsyncParameterClient - - -class TestParameterClient(unittest.TestCase): - - def setUp(self): - self.context = rclpy.context.Context() - rclpy.init(context=self.context) - self.client_node = rclpy.create_node('test_parameter_client', - namespace='/rclpy', - context=self.context) - self.target_node = rclpy.create_node('test_parameter_client_target', - namespace='rclpy', - allow_undeclared_parameters=True, - context=self.context) - self.target_node.declare_parameter('int_arr_param', [1, 2, 3]) - self.target_node.declare_parameter('float/param//', 3.14) - - self.client = AsyncParameterClient(self.client_node, 'test_parameter_client_target') - self.executor = SingleThreadedExecutor(context=self.context) - self.executor.add_node(self.client_node) - self.executor.add_node(self.target_node) - - def tearDown(self): - self.executor.shutdown() - self.client_node.destroy_node() - self.target_node.destroy_node() - rclpy.shutdown(context=self.context) - - def test_set_parameter(self): - future = self.client.set_parameters([ - Parameter('int_param', Parameter.Type.INTEGER, 88).to_parameter_msg(), - Parameter('string/param', Parameter.Type.STRING, 'hello world'), - ]) - self.executor.spin_until_future_complete(future) - results = future.result() - assert results is not None - assert len(results.results) == 2 - res = [i.successful for i in results.results] - assert all(res) - - def test_get_parameter(self): - future = self.client.get_parameters(['int_arr_param', 'float/param//']) - self.executor.spin_until_future_complete(future) - results = future.result() - assert results is not None - assert len(results.values) == 2 - assert list(results.values[0].integer_array_value) == [1, 2, 3] - assert results.values[1].double_value == 3.14 - - def test_list_parameters(self): - future = self.client.list_parameters() - self.executor.spin_until_future_complete(future) - results = future.result() - assert results is not None - assert 'int_arr_param' in results.result.names - assert 'float/param//' in results.result.names - - def test_describe_parameters(self): - future = self.client.describe_parameters(['int_arr_param']) - self.executor.spin_until_future_complete(future) - results = future.result() - assert results is not None - assert len(results.descriptors) == 1 - assert results.descriptors[0].type == ParameterType.PARAMETER_INTEGER_ARRAY - assert results.descriptors[0].name == 'int_arr_param' - - def test_get_paramter_types(self): - future = self.client.get_parameter_types(['int_arr_param']) - self.executor.spin_until_future_complete(future) - results = future.result() - assert results is not None - assert len(results.types) == 1 - assert results.types[0] == ParameterType.PARAMETER_INTEGER_ARRAY - - def test_set_parameters_atomically(self): - future = self.client.set_parameters_atomically([ - Parameter('int_param', Parameter.Type.INTEGER, 888), - Parameter('string/param', Parameter.Type.STRING, 'Hello World').to_parameter_msg(), - ]) - self.executor.spin_until_future_complete(future) - results = future.result() - assert results is not None - assert results.result.successful - - def test_delete_parameters(self): - self.target_node.declare_parameter('delete_param', 10) - descriptor = rcl_interfaces.msg.ParameterDescriptor(dynamic_typing=True) - self.target_node.declare_parameter('delete_param_dynamic', 10, descriptor=descriptor) - - future = self.client.delete_parameters(['delete_param']) - self.executor.spin_until_future_complete(future) - result = future.result() - assert result is not None - assert len(result.results) == 1 - assert not result.results[0].successful - assert result.results[0].reason == 'Static parameter cannot be undeclared' - - future = self.client.delete_parameters(['delete_param_dynamic']) - self.executor.spin_until_future_complete(future) - result = future.result() - assert result is not None - assert len(result.results) == 1 - assert result.results[0].successful - - def test_load_parameter_file(self): - yaml_string = """/param_test_target: - ros__parameters: - param_1: 1 - param_str: "string" - """ - with NamedTemporaryFile(mode='w') as f: - f.write(yaml_string) - f.flush() - future = self.client.load_parameter_file(f.name) - self.executor.spin_until_future_complete(future) - result = future.result() - assert result is not None - assert len(result.results) == 2 - assert all([i.successful for i in result.results])