From abb82d74b804162bcdec9f4830ad33b6d2560f89 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Thu, 19 May 2022 17:05:19 -0700 Subject: [PATCH 01/34] Misc. typing fixes Signed-off-by: Brian Chen --- rclpy/rclpy/__init__.py | 10 +++++----- rclpy/rclpy/client.py | 3 ++- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/rclpy/rclpy/__init__.py b/rclpy/rclpy/__init__.py index cea1f9163..cf034b0e6 100644 --- a/rclpy/rclpy/__init__.py +++ b/rclpy/rclpy/__init__.py @@ -42,6 +42,7 @@ from typing import List from typing import Optional +from typing import Union from typing import TYPE_CHECKING from rclpy.context import Context @@ -131,17 +132,16 @@ def shutdown(*, context: Context = None, uninstall_handlers: Optional[bool] = No ): uninstall_signal_handlers() - def create_node( node_name: str, *, - context: Context = None, - cli_args: List[str] = None, - namespace: str = None, + context: Union[Context, None] = None, + cli_args: Union[List[str], None] = None, + namespace: Union[str, None] = None, use_global_arguments: bool = True, enable_rosout: bool = True, start_parameter_services: bool = True, - parameter_overrides: List[Parameter] = None, + parameter_overrides: Union[List[Parameter], None] = None, allow_undeclared_parameters: bool = False, automatically_declare_parameters_from_overrides: bool = False ) -> 'Node': diff --git a/rclpy/rclpy/client.py b/rclpy/rclpy/client.py index 024b100d9..557b824d4 100644 --- a/rclpy/rclpy/client.py +++ b/rclpy/rclpy/client.py @@ -16,6 +16,7 @@ import time from typing import Dict from typing import TypeVar +from typing import Union from rclpy.callback_groups import CallbackGroup from rclpy.context import Context @@ -159,7 +160,7 @@ def service_is_ready(self) -> bool: with self.handle: return self.__client.service_server_is_available() - def wait_for_service(self, timeout_sec: float = None) -> bool: + def wait_for_service(self, timeout_sec: Union[float, None] = None) -> bool: """ Wait for a service server to become ready. From 6e435e69f19e28e2edb3a62e70f03ac1d67f8db2 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Thu, 19 May 2022 17:05:33 -0700 Subject: [PATCH 02/34] Start AsyncParameterClient class Signed-off-by: Brian Chen --- rclpy/rclpy/parameter_client_async.py | 126 ++++++++++++++++++++++++++ 1 file changed, 126 insertions(+) create mode 100644 rclpy/rclpy/parameter_client_async.py diff --git a/rclpy/rclpy/parameter_client_async.py b/rclpy/rclpy/parameter_client_async.py new file mode 100644 index 000000000..e42afab51 --- /dev/null +++ b/rclpy/rclpy/parameter_client_async.py @@ -0,0 +1,126 @@ +import rclpy +from rclpy import Parameter + + + +from typing import Union +from typing import List +from typing import Callable +from rclpy.task import Future + +from rcl_interfaces.srv import ListParameters +from rcl_interfaces.srv import DescribeParameters +from rcl_interfaces.srv import GetParameters +from rcl_interfaces.srv import GetParameterTypes +from rcl_interfaces.srv import SetParameters +from rcl_interfaces.srv import SetParametersAtomically + +from rcl_interfaces.msg import ListParametersResult + +class AsyncParameterClient: + def __init__(self, target_node_name): + """ + Creates an AsyncParameterClient + + :param target_node_name: + :type target_node_name: + """ + self.target_node = target_node_name + self.node = rclpy.create_node(f'async_param_client__{target_node_name}') + self.list_parameter_client_ = self.node.create_client(ListParameters, f'{target_node_name}/list_parameters') + self.set_parameter_client_ = self.node.create_client(SetParameters, f'{target_node_name}/set_parameters') + self.get_parameter_client_ = self.node.create_client(GetParameters, f'{target_node_name}/get_parameters') + self.get_parameter_types_client_ = self.node.create_client(GetParameterTypes, f'{target_node_name}/get_parameter_types') + + + + def wait_for_service(self, timeout_sec: Union[float, None] = None) -> bool: + """ + Waits for all parameter services to be available. + + :param timeout_sec: Seconds to wait. If ``None``, then wait forever. + :type timeout_sec: Union[float, None] + :return: + :rtype: + + :return: ``True`` if all services are available, False otherwise. + """ + + # TODO(ihasdapie): Wait across all parameter services + return self.list_parameter_client_.wait_for_service(timeout_sec) + + + def list_parameters(self, prefixes: List[str], depth: int, callback: Union[Callable, None] = None) -> Future: + # TODO: add typing/etc to handle listing all + + """ + Lists all parameters with given prefix, + + + :param prefixes: + :type prefixes: List[str] + :param depth: + :type depth: int + :param callback: + :type callback: Union[Callable, None] + :return: + :rtype: ``rclpy.task.Future`` + """ + request = ListParameters.Request() + 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: Union[Callable, None] = None) -> Future: + 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: Union[List[Parameter], Parameter], callback: Union[Callable, None] = None) -> Future: + # NOTE: With list of strs instead + # request = SetParameters.Request() + # request.parameters = parameters + # future = self.set_parameter_client_.call_async(request) + # if callable: + # future.add_done_callback(callback) + # return future + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 8b5d2d99c914cac4390bf8a5d2c239f46622607c Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 20 May 2022 16:47:11 -0700 Subject: [PATCH 03/34] core parameter_client functionality ported Signed-off-by: Brian Chen --- rclpy/rclpy/parameter_client.py | 224 ++++++++++++++++++++++++++ rclpy/rclpy/parameter_client_async.py | 126 --------------- 2 files changed, 224 insertions(+), 126 deletions(-) create mode 100644 rclpy/rclpy/parameter_client.py delete mode 100644 rclpy/rclpy/parameter_client_async.py diff --git a/rclpy/rclpy/parameter_client.py b/rclpy/rclpy/parameter_client.py new file mode 100644 index 000000000..3bae2e6c1 --- /dev/null +++ b/rclpy/rclpy/parameter_client.py @@ -0,0 +1,224 @@ +from typing import Callable, List, Sequence, Union + +from rcl_interfaces.srv import ( + DescribeParameters, + GetParameters, + GetParameterTypes, + ListParameters, + SetParameters, + SetParametersAtomically, +) +from rclpy.parameter import Parameter +from rclpy.task import Future + + +class AsyncParameterClient: + def __init__(self, node, target_node_name): + # TODO: qos_profile, callback group + """Create an AsyncParameterClient. + + :param node: Node for which the parameter clients will be added to + :type node: rclpy.Node + :param target_node_name: Name of remote node for which the parameters will be managed + :type target_node_name: string + """ + # Service names are defined in rclcpp/parameter_service_names.hpp + # TODO: Link to them??? + self.target_node = target_node_name + self.node = node + self.list_parameter_client_ = self.node.create_client( + ListParameters, f'{target_node_name}/list_parameters' + ) + self.set_parameter_client_ = self.node.create_client( + SetParameters, f'{target_node_name}/set_parameters' + ) + self.get_parameter_client_ = self.node.create_client( + GetParameters, f'{target_node_name}/get_parameters' + ) + self.get_parameter_types_client_ = self.node.create_client( + GetParameterTypes, f'{target_node_name}/get_parameter_types' + ) + self.describe_parameters_client_ = self.node.create_client( + DescribeParameters, f'{target_node_name}/describe_parameters' + ) + self.set_parameters_atomically_client_ = self.node.create_client( + SetParametersAtomically, f'{target_node_name}/set_parameters_atomically' + ) + + def wait_for_service(self, timeout_sec: Union[float, None] = None) -> bool: + """Wait for all parameter services to be available. + + :param timeout_sec: Seconds to wait. If ``None``, then wait forever. + :type timeout_sec: Union[float, None] + :return: + :rtype: + + :return: ``True`` if all services are available, False otherwise. + """ + return all( + [ + self.list_parameter_client_.wait_for_service(timeout_sec), + self.set_parameter_client_.wait_for_service(timeout_sec), + self.get_parameter_client_.wait_for_service(timeout_sec), + self.get_parameter_types_client_.wait_for_service(timeout_sec), + self.describe_parameters_client_.wait_for_service(timeout_sec), + self.set_parameters_atomically_client_.wait_for_service(timeout_sec), + ] + ) + + def list_parameters( + self, prefixes: List[str], depth: int, callback: Union[Callable, None] = None + ) -> Future: + # TODO: add typing/etc to handle listing all + + """ List all parameters with given prefixs. + + :param prefixes: + :type prefixes: List[str] + :param depth: + :type depth: int + :param callback: + :type callback: Union[Callable, None] + :return: + :rtype: ``rclpy.task.Future`` + """ + request = ListParameters.Request() + 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: Union[Callable, None] = None + ) -> Future: + """ + + :param names: + :type names: List[str] + :param callback: + :type callback: Union[Callable, None] + :return: + :rtype: + """ + 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[Parameter], callback: Union[Callable, None] = None + ) -> Future: + """ + + :param parameters: + :type parameters: Sequence[Parameter] + :param callback: + :type callback: Union[Callable, None] + :return: + :rtype: + """ + request = SetParameters.Request() + request.parameters = [i.to_parameter_msg() for i in parameters] + future = self.set_parameter_client_.call_async(request) + if callback: + future.add_done_callback(callback) + return future + + def describe_parameters( + self, names: Union[List[str], str], callback: Union[Callable, None] = None + ) -> Future: + """ + + :param names: + :type names: Union[List[str], str] + :param callback: + :type callback: Union[Callable, None] + :return: + :rtype: + + Parameter type definitions are given in rcl_interfaces.msg.ParameterType + + """ + 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, callback: Union[Callable, None] = None + ) -> Future: + """ + + :param names: + :type names: + :param callback: + :type callback: Union[Callable, None] + :return: + :rtype: + """ + 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[Parameter], callback: Union[Callable, None] = None + ) -> Future: + """ + + :param parameters: + :type parameters: Union[Set[Parameter], Parameter] + :param callback: + :type callback: Union[Callable, None] + :return: + :rtype: + """ + request = SetParametersAtomically.Request() + request.parameters = [i.to_parameter_msg() for i in parameters] + # request.parameters = parameters[0].to_parameter_msg() + 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: Union[Callable, None] = None + ) -> Future: + # TODO: `Static parameter cannot be undeclared` + """ Unset parameters with given names. + + :param names: + :type names: List[str] + :param callback: + :type callback: Union[Callable, None] + :return: + :rtype: + """ + 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_parameters( + self, yaml_filename: str, callback: Union[Callable, None] = None + ) -> Future: + """ See: https://github.com/ros2/ros2cli/blob/master/ros2param/ros2param/api/__init__.py. + + :param yaml_filename: Full name of the ``yaml`` file + :type yaml_filename: str + :param callback: Callback function to perform on future completion + :type callback: Union[Callable, None] + :return: Future of set ``set_parameter`` service used to load the parameters + :rtype: Future + """ + raise NotImplementedError diff --git a/rclpy/rclpy/parameter_client_async.py b/rclpy/rclpy/parameter_client_async.py deleted file mode 100644 index e42afab51..000000000 --- a/rclpy/rclpy/parameter_client_async.py +++ /dev/null @@ -1,126 +0,0 @@ -import rclpy -from rclpy import Parameter - - - -from typing import Union -from typing import List -from typing import Callable -from rclpy.task import Future - -from rcl_interfaces.srv import ListParameters -from rcl_interfaces.srv import DescribeParameters -from rcl_interfaces.srv import GetParameters -from rcl_interfaces.srv import GetParameterTypes -from rcl_interfaces.srv import SetParameters -from rcl_interfaces.srv import SetParametersAtomically - -from rcl_interfaces.msg import ListParametersResult - -class AsyncParameterClient: - def __init__(self, target_node_name): - """ - Creates an AsyncParameterClient - - :param target_node_name: - :type target_node_name: - """ - self.target_node = target_node_name - self.node = rclpy.create_node(f'async_param_client__{target_node_name}') - self.list_parameter_client_ = self.node.create_client(ListParameters, f'{target_node_name}/list_parameters') - self.set_parameter_client_ = self.node.create_client(SetParameters, f'{target_node_name}/set_parameters') - self.get_parameter_client_ = self.node.create_client(GetParameters, f'{target_node_name}/get_parameters') - self.get_parameter_types_client_ = self.node.create_client(GetParameterTypes, f'{target_node_name}/get_parameter_types') - - - - def wait_for_service(self, timeout_sec: Union[float, None] = None) -> bool: - """ - Waits for all parameter services to be available. - - :param timeout_sec: Seconds to wait. If ``None``, then wait forever. - :type timeout_sec: Union[float, None] - :return: - :rtype: - - :return: ``True`` if all services are available, False otherwise. - """ - - # TODO(ihasdapie): Wait across all parameter services - return self.list_parameter_client_.wait_for_service(timeout_sec) - - - def list_parameters(self, prefixes: List[str], depth: int, callback: Union[Callable, None] = None) -> Future: - # TODO: add typing/etc to handle listing all - - """ - Lists all parameters with given prefix, - - - :param prefixes: - :type prefixes: List[str] - :param depth: - :type depth: int - :param callback: - :type callback: Union[Callable, None] - :return: - :rtype: ``rclpy.task.Future`` - """ - request = ListParameters.Request() - 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: Union[Callable, None] = None) -> Future: - 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: Union[List[Parameter], Parameter], callback: Union[Callable, None] = None) -> Future: - # NOTE: With list of strs instead - # request = SetParameters.Request() - # request.parameters = parameters - # future = self.set_parameter_client_.call_async(request) - # if callable: - # future.add_done_callback(callback) - # return future - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - From 4d0b44d5b2a80a1fb26048e6dab0325abf8c6f56 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Mon, 23 May 2022 22:11:23 -0700 Subject: [PATCH 04/34] add service_is_ready method Signed-off-by: Brian Chen --- rclpy/rclpy/parameter_client.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/rclpy/rclpy/parameter_client.py b/rclpy/rclpy/parameter_client.py index 3bae2e6c1..549d7602b 100644 --- a/rclpy/rclpy/parameter_client.py +++ b/rclpy/rclpy/parameter_client.py @@ -45,6 +45,20 @@ def __init__(self, node, target_node_name): SetParametersAtomically, f'{target_node_name}/set_parameters_atomically' ) + def service_is_ready(self, ) -> bool: + """ + """ + 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_service(self, timeout_sec: Union[float, None] = None) -> bool: """Wait for all parameter services to be available. From 39dd54dd1bb1c37c1bae2ac0c967f3f84ae70f16 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Mon, 23 May 2022 22:11:51 -0700 Subject: [PATCH 05/34] allow for listing all parameters Signed-off-by: Brian Chen --- rclpy/rclpy/parameter_client.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rclpy/rclpy/parameter_client.py b/rclpy/rclpy/parameter_client.py index 549d7602b..d93e045cd 100644 --- a/rclpy/rclpy/parameter_client.py +++ b/rclpy/rclpy/parameter_client.py @@ -97,7 +97,8 @@ def list_parameters( :rtype: ``rclpy.task.Future`` """ request = ListParameters.Request() - request.prefixes = prefixes + if prefixes: + request.prefixes = prefixes request.depth = depth future = self.list_parameter_client_.call_async(request) if callback: From ebc4481409d67b32441070868a577eefff6b30cb Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Mon, 23 May 2022 22:12:05 -0700 Subject: [PATCH 06/34] synchronus client set/get/list Signed-off-by: Brian Chen --- rclpy/rclpy/parameter_client.py | 47 +++++++++++++++++++++++++++++++++ 1 file changed, 47 insertions(+) diff --git a/rclpy/rclpy/parameter_client.py b/rclpy/rclpy/parameter_client.py index d93e045cd..ba38f153c 100644 --- a/rclpy/rclpy/parameter_client.py +++ b/rclpy/rclpy/parameter_client.py @@ -237,3 +237,50 @@ def load_parameters( :rtype: Future """ raise NotImplementedError + +class SyncParameterClient(AsyncParameterClient): + """ + Synchronous parameters client class. + """ + + # TODO(ihasdapie): configure timeout + def __init__(self, node, target_node_name): + super().__init__(node, target_node_name) + + def set_parameters( + self, + parameters: Sequence[Parameter], + callback: Union[Callable, None] = None): + future = super().set_parameters(parameters, callback) + rclpy.spin_until_future_complete(self.node, future) + response = future.result() + return response + + def get_parameters(self, names: List[str], callback: Union[Callable, None] = None): + future = super().get_parameters(names, callback) + rclpy.spin_until_future_complete(self.node, future) + response = future.result() + return response + + def list_parameters(self, prefixes: Union[List[str], None] = None, depth: int = 1, callback: Union[Callable, None] = None): + future = super().list_parameters(prefixes, depth, callback) + rclpy.spin_until_future_complete(self.node, future) + response = future.result() + return response + + + + + + + + + + + + + + + + + From aec10c597aa699aa91ef3bb7af0c7f504514659e Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Mon, 23 May 2022 22:14:00 -0700 Subject: [PATCH 07/34] use Parameter message type instead of rclpy.Parameter to align with ros2param Signed-off-by: Brian Chen --- rclpy/rclpy/parameter_client.py | 39 ++++++++++++++++----------------- 1 file changed, 19 insertions(+), 20 deletions(-) diff --git a/rclpy/rclpy/parameter_client.py b/rclpy/rclpy/parameter_client.py index ba38f153c..79e5e54cc 100644 --- a/rclpy/rclpy/parameter_client.py +++ b/rclpy/rclpy/parameter_client.py @@ -8,11 +8,13 @@ SetParameters, SetParametersAtomically, ) -from rclpy.parameter import Parameter +import rclpy +from rcl_interfaces.msg import Parameter +from rclpy.parameter import Parameter as RclpyParameter from rclpy.task import Future -class AsyncParameterClient: +class AsyncParameterClient(object): def __init__(self, node, target_node_name): # TODO: qos_profile, callback group """Create an AsyncParameterClient. @@ -26,15 +28,15 @@ def __init__(self, node, target_node_name): # TODO: Link to them??? self.target_node = target_node_name self.node = node + self.get_parameter_client_ = self.node.create_client( + GetParameters, f'{target_node_name}/get_parameters' + ) self.list_parameter_client_ = self.node.create_client( ListParameters, f'{target_node_name}/list_parameters' ) self.set_parameter_client_ = self.node.create_client( SetParameters, f'{target_node_name}/set_parameters' ) - self.get_parameter_client_ = self.node.create_client( - GetParameters, f'{target_node_name}/get_parameters' - ) self.get_parameter_types_client_ = self.node.create_client( GetParameterTypes, f'{target_node_name}/get_parameter_types' ) @@ -69,19 +71,17 @@ def wait_for_service(self, timeout_sec: Union[float, None] = None) -> bool: :return: ``True`` if all services are available, False otherwise. """ - return all( - [ - self.list_parameter_client_.wait_for_service(timeout_sec), - self.set_parameter_client_.wait_for_service(timeout_sec), - self.get_parameter_client_.wait_for_service(timeout_sec), - self.get_parameter_types_client_.wait_for_service(timeout_sec), - self.describe_parameters_client_.wait_for_service(timeout_sec), - self.set_parameters_atomically_client_.wait_for_service(timeout_sec), - ] - ) + return all([ + self.list_parameter_client_.wait_for_service(timeout_sec), + self.set_parameter_client_.wait_for_service(timeout_sec), + self.get_parameter_client_.wait_for_service(timeout_sec), + self.get_parameter_types_client_.wait_for_service(timeout_sec), + self.describe_parameters_client_.wait_for_service(timeout_sec), + self.set_parameters_atomically_client_.wait_for_service(timeout_sec), + ]) def list_parameters( - self, prefixes: List[str], depth: int, callback: Union[Callable, None] = None + self, prefixes: Union[None, List[str]] = None, depth: int = 1, callback: Union[Callable, None] = None ) -> Future: # TODO: add typing/etc to handle listing all @@ -137,7 +137,7 @@ def set_parameters( :rtype: """ request = SetParameters.Request() - request.parameters = [i.to_parameter_msg() for i in parameters] + request.parameters = parameters future = self.set_parameter_client_.call_async(request) if callback: future.add_done_callback(callback) @@ -197,8 +197,7 @@ def set_parameters_atomically( :rtype: """ request = SetParametersAtomically.Request() - request.parameters = [i.to_parameter_msg() for i in parameters] - # request.parameters = parameters[0].to_parameter_msg() + request.parameters = parameters future = self.set_parameters_atomically_client_.call_async(request) if callback: future.add_done_callback(callback) @@ -218,7 +217,7 @@ def delete_parameters( :rtype: """ request = SetParameters.Request() - request.parameters = [Parameter(name=i).to_parameter_msg() for i in names] + request.parameters = [RclpyParameter(name=i).to_parameter_msg() for i in names] future = self.set_parameter_client_.call_async(request) if callback: future.add_done_callback(callback) From e79afbb363bfa7f947ac0b48f5f10b37c32a3af5 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Tue, 24 May 2022 17:07:34 -0700 Subject: [PATCH 08/34] move get_parameter_value from ros2param to rclpy Signed-off-by: Brian Chen --- rclpy/rclpy/parameter.py | 39 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) diff --git a/rclpy/rclpy/parameter.py b/rclpy/rclpy/parameter.py index daad33146..d51c0e80f 100644 --- a/rclpy/rclpy/parameter.py +++ b/rclpy/rclpy/parameter.py @@ -14,6 +14,7 @@ import array from enum import Enum +import yaml from rcl_interfaces.msg import Parameter as ParameterMsg from rcl_interfaces.msg import ParameterType, ParameterValue @@ -177,6 +178,44 @@ def to_parameter_msg(self): return ParameterMsg(name=self.name, value=self.get_parameter_value()) +def get_parameter_value(string_value): + """Guess the desired type of the parameter based on the string value.""" + 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. From 852e3a85d7891aef6996acae16b0cb9a641c98ac Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Tue, 24 May 2022 17:07:50 -0700 Subject: [PATCH 09/34] load parameters from file Signed-off-by: Brian Chen --- rclpy/rclpy/parameter_client.py | 108 ++++++++++++++------------------ 1 file changed, 48 insertions(+), 60 deletions(-) diff --git a/rclpy/rclpy/parameter_client.py b/rclpy/rclpy/parameter_client.py index 79e5e54cc..10f68c2bd 100644 --- a/rclpy/rclpy/parameter_client.py +++ b/rclpy/rclpy/parameter_client.py @@ -8,12 +8,15 @@ SetParameters, SetParametersAtomically, ) -import rclpy +import yaml from rcl_interfaces.msg import Parameter from rclpy.parameter import Parameter as RclpyParameter +from rclpy.parameter import get_parameter_value +from rclpy.parameter import PARAMETER_SEPARATOR_STRING from rclpy.task import Future + class AsyncParameterClient(object): def __init__(self, node, target_node_name): # TODO: qos_profile, callback group @@ -144,12 +147,12 @@ def set_parameters( return future def describe_parameters( - self, names: Union[List[str], str], callback: Union[Callable, None] = None + self, names: List[str], callback: Union[Callable, None] = None ) -> Future: """ :param names: - :type names: Union[List[str], str] + :type names: List[str] :param callback: :type callback: Union[Callable, None] :return: @@ -223,63 +226,48 @@ def delete_parameters( future.add_done_callback(callback) return future - def load_parameters( - self, yaml_filename: str, callback: Union[Callable, None] = None - ) -> Future: - """ See: https://github.com/ros2/ros2cli/blob/master/ros2param/ros2param/api/__init__.py. - - :param yaml_filename: Full name of the ``yaml`` file - :type yaml_filename: str - :param callback: Callback function to perform on future completion - :type callback: Union[Callable, None] - :return: Future of set ``set_parameter`` service used to load the parameters - :rtype: Future - """ - raise NotImplementedError - -class SyncParameterClient(AsyncParameterClient): + def load_parameter_file(self, parameter_file, use_wildcard, return_parameters=False): + with open(parameter_file, 'r') as f: + param_file = yaml.safe_load(f) + param_keys = [] + if use_wildcard and '/**' in param_file: + param_keys.append('/**') + if self.target_node in param_file: + param_keys.append(self.target_node) + + if param_keys == []: + raise RuntimeError('Param file does not contain parameters for {}, ' + ' only for nodes: {}' .format(node_name, param_file.keys())) + param_dict = {} + for k in param_keys: + value = param_file[k] + 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(k)) + param_dict.update(value['ros__parameters']) + parameters = parse_parameter_dict(namespace='', parameter_dict=param_dict) + future = self.set_parameters(parameters) + if return_parameters: + return (future, parameters) + return future + +def parse_parameter_dict(namespace, parameter_dict): """ - Synchronous parameters client class. + Builds a list of parameters from a dictionary. """ - - # TODO(ihasdapie): configure timeout - def __init__(self, node, target_node_name): - super().__init__(node, target_node_name) - - def set_parameters( - self, - parameters: Sequence[Parameter], - callback: Union[Callable, None] = None): - future = super().set_parameters(parameters, callback) - rclpy.spin_until_future_complete(self.node, future) - response = future.result() - return response - - def get_parameters(self, names: List[str], callback: Union[Callable, None] = None): - future = super().get_parameters(names, callback) - rclpy.spin_until_future_complete(self.node, future) - response = future.result() - return response - - def list_parameters(self, prefixes: Union[List[str], None] = None, depth: int = 1, callback: Union[Callable, None] = None): - future = super().list_parameters(prefixes, depth, callback) - rclpy.spin_until_future_complete(self.node, future) - response = future.result() - return response - - - - - - - - - - - - - - - - + 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(str(param_value)) + parameters.append(parameter) + return parameters From f1731e3c7ffcde4b5eead7e85d5e8f8cf686b87b Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 27 May 2022 11:42:16 -0700 Subject: [PATCH 10/34] add parameter_client to docs Signed-off-by: Brian Chen --- rclpy/docs/source/api/parameters.rst | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/rclpy/docs/source/api/parameters.rst b/rclpy/docs/source/api/parameters.rst index c38ee7d9c..5dee741fd 100644 --- a/rclpy/docs/source/api/parameters.rst +++ b/rclpy/docs/source/api/parameters.rst @@ -10,3 +10,8 @@ Parameter Service ----------------- .. automodule:: rclpy.parameter_service + +Parameter Client +----------------- + +.. automodule:: rclpy.parameter_client From 5d56b1f077771499676a89e5935fc78967d27d34 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 27 May 2022 13:58:44 -0700 Subject: [PATCH 11/34] address pr comments, docs, lint Signed-off-by: Brian Chen --- rclpy/rclpy/parameter.py | 85 ++++++++- rclpy/rclpy/parameter_client.py | 321 +++++++++++++++++--------------- 2 files changed, 252 insertions(+), 154 deletions(-) diff --git a/rclpy/rclpy/parameter.py b/rclpy/rclpy/parameter.py index d51c0e80f..e5e9bb232 100644 --- a/rclpy/rclpy/parameter.py +++ b/rclpy/rclpy/parameter.py @@ -14,10 +14,14 @@ import array from enum import Enum -import yaml +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, ParameterValue +from rcl_interfaces.msg import ParameterType +from rcl_interfaces.msg import ParameterValue +import yaml PARAMETER_SEPARATOR_STRING = '.' @@ -178,8 +182,13 @@ def to_parameter_msg(self): return ParameterMsg(name=self.name, value=self.get_parameter_value()) -def get_parameter_value(string_value): - """Guess the desired type of the parameter based on the string 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) @@ -216,6 +225,7 @@ def get_parameter_value(string_value): 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. @@ -250,3 +260,70 @@ 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, + 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 index 10f68c2bd..0d5746aa7 100644 --- a/rclpy/rclpy/parameter_client.py +++ b/rclpy/rclpy/parameter_client.py @@ -1,57 +1,91 @@ -from typing import Callable, List, Sequence, Union - -from rcl_interfaces.srv import ( - DescribeParameters, - GetParameters, - GetParameterTypes, - ListParameters, - SetParameters, - SetParametersAtomically, -) -import yaml +# 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 +from typing import List +from typing import Optional +from typing import Sequence +from typing import Union + from rcl_interfaces.msg import Parameter +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 RclpyParameter -from rclpy.parameter import get_parameter_value -from rclpy.parameter import PARAMETER_SEPARATOR_STRING +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(object): - def __init__(self, node, target_node_name): + def __init__( + self, + node: Node, + target_node_name: str, + qos_profile: QoSProfile = qos_profile_services_default, + callback_group: Optional[CallbackGroup] = None): # TODO: qos_profile, callback group - """Create an AsyncParameterClient. + """ + Create an AsyncParameterClient. + + 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 for which the parameter clients will be added to - :type node: rclpy.Node :param target_node_name: Name of remote node for which the parameters will be managed - :type target_node_name: string """ - # Service names are defined in rclcpp/parameter_service_names.hpp - # TODO: Link to them??? self.target_node = target_node_name self.node = node self.get_parameter_client_ = self.node.create_client( - GetParameters, f'{target_node_name}/get_parameters' + GetParameters, f'{target_node_name}/get_parameters', + qos_profile=qos_profile, callback_group=callback_group ) self.list_parameter_client_ = self.node.create_client( - ListParameters, f'{target_node_name}/list_parameters' + ListParameters, f'{target_node_name}/list_parameters', + qos_profile=qos_profile, callback_group=callback_group ) self.set_parameter_client_ = self.node.create_client( - SetParameters, f'{target_node_name}/set_parameters' + SetParameters, f'{target_node_name}/set_parameters', + qos_profile=qos_profile, callback_group=callback_group ) self.get_parameter_types_client_ = self.node.create_client( - GetParameterTypes, f'{target_node_name}/get_parameter_types' + GetParameterTypes, f'{target_node_name}/get_parameter_types', + qos_profile=qos_profile, callback_group=callback_group ) self.describe_parameters_client_ = self.node.create_client( - DescribeParameters, f'{target_node_name}/describe_parameters' + DescribeParameters, f'{target_node_name}/describe_parameters', + qos_profile=qos_profile, callback_group=callback_group ) self.set_parameters_atomically_client_ = self.node.create_client( - SetParametersAtomically, f'{target_node_name}/set_parameters_atomically' + SetParametersAtomically, f'{target_node_name}/set_parameters_atomically', + qos_profile=qos_profile, callback_group=callback_group ) - def service_is_ready(self, ) -> bool: - """ + def service_is_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(), @@ -62,42 +96,47 @@ def service_is_ready(self, ) -> bool: self.set_parameters_atomically_client_.service_is_ready(), ]) - - - def wait_for_service(self, timeout_sec: Union[float, None] = None) -> bool: - """Wait for all parameter services to be available. + def wait_for_service(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. :type timeout_sec: Union[float, None] - :return: - :rtype: - - :return: ``True`` if all services are available, False otherwise. + :return: ``True`` if all services were waite , ``False`` otherwise. """ - return all([ - self.list_parameter_client_.wait_for_service(timeout_sec), - self.set_parameter_client_.wait_for_service(timeout_sec), - self.get_parameter_client_.wait_for_service(timeout_sec), - self.get_parameter_types_client_.wait_for_service(timeout_sec), - self.describe_parameters_client_.wait_for_service(timeout_sec), - self.set_parameters_atomically_client_.wait_for_service(timeout_sec), - ]) + 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: Union[None, List[str]] = None, depth: int = 1, callback: Union[Callable, None] = None + self, + prefixes: Optional[List[str]] = None, + depth: int = 1, + callback: Optional[Callable] = None ) -> Future: - # TODO: add typing/etc to handle listing all - - """ List all parameters with given prefixs. - - :param prefixes: - :type prefixes: List[str] - :param depth: - :type depth: int - :param callback: - :type callback: Union[Callable, None] - :return: - :rtype: ``rclpy.task.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: @@ -108,17 +147,13 @@ def list_parameters( future.add_done_callback(callback) return future - def get_parameters( - self, names: List[str], callback: Union[Callable, None] = None - ) -> Future: + def get_parameters(self, names: List[str], callback: Optional[Callable] = None) -> Future: """ + Get parameters given names. - :param names: - :type names: List[str] - :param callback: - :type callback: Union[Callable, None] - :return: - :rtype: + :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 @@ -128,16 +163,19 @@ def get_parameters( return future def set_parameters( - self, parameters: Sequence[Parameter], callback: Union[Callable, None] = None + self, + parameters: Sequence[Parameter], + callback: Union[Callable, None] = 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: - :type parameters: Sequence[Parameter] - :param callback: - :type callback: Union[Callable, None] - :return: - :rtype: + :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 = parameters @@ -147,19 +185,19 @@ def set_parameters( return future def describe_parameters( - self, names: List[str], callback: Union[Callable, None] = None + self, + names: List[str], + callback: Optional[Callable] = None ) -> Future: """ + Describe parameters given names. - :param names: - :type names: List[str] - :param callback: - :type callback: Union[Callable, None] - :return: - :rtype: - - Parameter type definitions are given in rcl_interfaces.msg.ParameterType + 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 @@ -169,16 +207,21 @@ def describe_parameters( return future def get_parameter_types( - self, names, callback: Union[Callable, None] = None + 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``. - :param names: - :type names: - :param callback: - :type callback: Union[Callable, None] - :return: - :rtype: + Parameter type definitions are given in rcl_interfaces.msg.ParameterType + + :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 @@ -188,16 +231,19 @@ def get_parameter_types( return future def set_parameters_atomically( - self, parameters: Sequence[Parameter], callback: Union[Callable, None] = None + self, + parameters: Sequence[Parameter], + callback: Optional[Callable] = None ) -> Future: """ + Set parameters atomically. - :param parameters: - :type parameters: Union[Set[Parameter], Parameter] - :param callback: - :type callback: Union[Callable, None] - :return: - :rtype: + 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 = parameters @@ -207,17 +253,17 @@ def set_parameters_atomically( return future def delete_parameters( - self, names: List[str], callback: Union[Callable, None] = None + self, names: List[str], callback: Optional[Callable] = None ) -> Future: - # TODO: `Static parameter cannot be undeclared` - """ Unset parameters with given names. - - :param names: - :type names: List[str] - :param callback: - :type callback: Union[Callable, None] - :return: - :rtype: + """ + Unset parameters with given names. + + The result after the returned future is complete + will be of type ``rcl_interfaces.srv.SetParameters.Response``. + + :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 = [RclpyParameter(name=i).to_parameter_msg() for i in names] @@ -226,48 +272,23 @@ def delete_parameters( future.add_done_callback(callback) return future - def load_parameter_file(self, parameter_file, use_wildcard, return_parameters=False): - with open(parameter_file, 'r') as f: - param_file = yaml.safe_load(f) - param_keys = [] - if use_wildcard and '/**' in param_file: - param_keys.append('/**') - if self.target_node in param_file: - param_keys.append(self.target_node) - - if param_keys == []: - raise RuntimeError('Param file does not contain parameters for {}, ' - ' only for nodes: {}' .format(node_name, param_file.keys())) - param_dict = {} - for k in param_keys: - value = param_file[k] - 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(k)) - param_dict.update(value['ros__parameters']) - parameters = parse_parameter_dict(namespace='', parameter_dict=param_dict) - future = self.set_parameters(parameters) - if return_parameters: - return (future, parameters) - return future - -def parse_parameter_dict(namespace, parameter_dict): - """ - Builds a list of parameters from a dictionary. - """ - 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(str(param_value)) - parameters.append(parameter) - return parameters + def load_parameter_file( + self, + parameter_file: str, + use_wildcard: bool = False, + ) -> Future: + """ + Load parameters from a yaml file. + + Wrapper around `parse_parameter_dict` and `load_dict` + + 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 From 591cffa0bf96a10a9b2faffd5938b49dfec01c8d Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Wed, 1 Jun 2022 15:16:15 -0700 Subject: [PATCH 12/34] add tests for parameter client Signed-off-by: Brian Chen --- rclpy/rclpy/parameter.py | 2 +- rclpy/test/test_parameter.py | 20 +++- rclpy/test/test_parameter_client.py | 136 ++++++++++++++++++++++++++++ 3 files changed, 156 insertions(+), 2 deletions(-) create mode 100644 rclpy/test/test_parameter_client.py diff --git a/rclpy/rclpy/parameter.py b/rclpy/rclpy/parameter.py index e5e9bb232..6b917aaa0 100644 --- a/rclpy/rclpy/parameter.py +++ b/rclpy/rclpy/parameter.py @@ -264,7 +264,7 @@ def parameter_value_to_python(parameter_value: ParameterValue): def parameter_dict_from_yaml_file( parameter_file: str, - use_wildcard: bool, + use_wildcard: bool = False, target_nodes: Optional[List[str]] = None, namespace: str = '' ) -> Dict[str, ParameterMsg]: diff --git a/rclpy/test/test_parameter.py b/rclpy/test/test_parameter.py index 4a4a7f7d3..8080cb8ae 100644 --- a/rclpy/test/test_parameter.py +++ b/rclpy/test/test_parameter.py @@ -13,14 +13,15 @@ # 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 @@ -213,6 +214,23 @@ 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() as f: + f.write(str.encode(yaml_string)) + f.seek(0) + parameter_dict = parameter_dict_from_yaml_file(f.name) + assert parameter_dict == expected + if __name__ == '__main__': unittest.main() diff --git a/rclpy/test/test_parameter_client.py b/rclpy/test/test_parameter_client.py new file mode 100644 index 000000000..51284c5ab --- /dev/null +++ b/rclpy/test/test_parameter_client.py @@ -0,0 +1,136 @@ +# 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 +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').to_parameter_msg(), + ]) + self.executor.spin_until_future_complete(future) + results = future.result() + assert results is not None + 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 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 results.descriptors[0].type == 7 + 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 results.types[0] == 7 # refer to rcl_interfaces.msg.ParameterType + + def test_set_parameters_atomically(self): + future = self.client.set_parameters_atomically([ + Parameter('int_param', Parameter.Type.INTEGER, 888).to_parameter_msg(), + 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 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 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() as f: + f.write(str.encode(yaml_string)) + f.seek(0) + future = self.client.load_parameter_file(f.name) + self.executor.spin_until_future_complete(future) + result = future.result() + assert result is not None + assert all([i.successful for i in result.results]) From 6fab6bfe91001522b0ba99960c8274ae963e5cb1 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Wed, 1 Jun 2022 15:21:04 -0700 Subject: [PATCH 13/34] prepend underscore for private members Signed-off-by: Brian Chen --- rclpy/rclpy/parameter_client.py | 50 ++++++++++++++++----------------- 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/rclpy/rclpy/parameter_client.py b/rclpy/rclpy/parameter_client.py index 0d5746aa7..ee2e65609 100644 --- a/rclpy/rclpy/parameter_client.py +++ b/rclpy/rclpy/parameter_client.py @@ -56,27 +56,27 @@ def __init__( """ self.target_node = target_node_name self.node = node - self.get_parameter_client_ = self.node.create_client( + self._get_parameter_client = self.node.create_client( GetParameters, f'{target_node_name}/get_parameters', qos_profile=qos_profile, callback_group=callback_group ) - self.list_parameter_client_ = self.node.create_client( + self._list_parameter_client = self.node.create_client( ListParameters, f'{target_node_name}/list_parameters', qos_profile=qos_profile, callback_group=callback_group ) - self.set_parameter_client_ = self.node.create_client( + self._set_parameter_client = self.node.create_client( SetParameters, f'{target_node_name}/set_parameters', qos_profile=qos_profile, callback_group=callback_group ) - self.get_parameter_types_client_ = self.node.create_client( + self._get_parameter_types_client = self.node.create_client( GetParameterTypes, f'{target_node_name}/get_parameter_types', qos_profile=qos_profile, callback_group=callback_group ) - self.describe_parameters_client_ = self.node.create_client( + self._describe_parameters_client = self.node.create_client( DescribeParameters, f'{target_node_name}/describe_parameters', qos_profile=qos_profile, callback_group=callback_group ) - self.set_parameters_atomically_client_ = self.node.create_client( + self._set_parameters_atomically_client = self.node.create_client( SetParametersAtomically, f'{target_node_name}/set_parameters_atomically', qos_profile=qos_profile, callback_group=callback_group ) @@ -88,12 +88,12 @@ def service_is_ready(self) -> bool: :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(), + 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_service(self, timeout_sec: Optional[float] = None) -> bool: @@ -105,12 +105,12 @@ def wait_for_service(self, timeout_sec: Optional[float] = None) -> bool: :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, + 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: @@ -142,7 +142,7 @@ def list_parameters( if prefixes: request.prefixes = prefixes request.depth = depth - future = self.list_parameter_client_.call_async(request) + future = self._list_parameter_client.call_async(request) if callback: future.add_done_callback(callback) return future @@ -157,7 +157,7 @@ def get_parameters(self, names: List[str], callback: Optional[Callable] = None) """ request = GetParameters.Request() request.names = names - future = self.get_parameter_client_.call_async(request) + future = self._get_parameter_client.call_async(request) if callback: future.add_done_callback(callback) return future @@ -179,7 +179,7 @@ def set_parameters( """ request = SetParameters.Request() request.parameters = parameters - future = self.set_parameter_client_.call_async(request) + future = self._set_parameter_client.call_async(request) if callback: future.add_done_callback(callback) return future @@ -201,7 +201,7 @@ def describe_parameters( """ request = DescribeParameters.Request() request.names = names - future = self.describe_parameters_client_.call_async(request) + future = self._describe_parameters_client.call_async(request) if callback: future.add_done_callback(callback) return future @@ -225,7 +225,7 @@ def get_parameter_types( """ request = GetParameterTypes.Request() request.names = names - future = self.get_parameter_types_client_.call_async(request) + future = self._get_parameter_types_client.call_async(request) if callback: future.add_done_callback(callback) return future @@ -247,7 +247,7 @@ def set_parameters_atomically( """ request = SetParametersAtomically.Request() request.parameters = parameters - future = self.set_parameters_atomically_client_.call_async(request) + future = self._set_parameters_atomically_client.call_async(request) if callback: future.add_done_callback(callback) return future @@ -267,7 +267,7 @@ def delete_parameters( """ request = SetParameters.Request() request.parameters = [RclpyParameter(name=i).to_parameter_msg() for i in names] - future = self.set_parameter_client_.call_async(request) + future = self._set_parameter_client.call_async(request) if callback: future.add_done_callback(callback) return future From 79ac7febe2f6d537706931c0bd2e1195bdac5543 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Wed, 1 Jun 2022 16:07:31 -0700 Subject: [PATCH 14/34] squash linting complaints Signed-off-by: Brian Chen --- rclpy/rclpy/__init__.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rclpy/rclpy/__init__.py b/rclpy/rclpy/__init__.py index cf034b0e6..e3cae919e 100644 --- a/rclpy/rclpy/__init__.py +++ b/rclpy/rclpy/__init__.py @@ -42,8 +42,8 @@ from typing import List from typing import Optional -from typing import Union from typing import TYPE_CHECKING +from typing import Union from rclpy.context import Context from rclpy.parameter import Parameter @@ -132,6 +132,7 @@ def shutdown(*, context: Context = None, uninstall_handlers: Optional[bool] = No ): uninstall_signal_handlers() + def create_node( node_name: str, *, From be6e43d543fcd0a8875f2726d0456ce3410d240e Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 3 Jun 2022 10:12:25 -0700 Subject: [PATCH 15/34] revert typing changes Signed-off-by: Brian Chen --- rclpy/rclpy/__init__.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rclpy/rclpy/__init__.py b/rclpy/rclpy/__init__.py index e3cae919e..26e0dc519 100644 --- a/rclpy/rclpy/__init__.py +++ b/rclpy/rclpy/__init__.py @@ -136,9 +136,9 @@ def shutdown(*, context: Context = None, uninstall_handlers: Optional[bool] = No def create_node( node_name: str, *, - context: Union[Context, None] = None, - cli_args: Union[List[str], None] = None, - namespace: Union[str, None] = None, + context: Context = None, + cli_args: List[str] = None, + namespace: str = None, use_global_arguments: bool = True, enable_rosout: bool = True, start_parameter_services: bool = True, From 40acf328b54ae08120a4c83f6633e42c131292ee Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 3 Jun 2022 10:12:46 -0700 Subject: [PATCH 16/34] address review comments Signed-off-by: Brian Chen --- rclpy/rclpy/parameter_client.py | 52 +++++++++++++++++++---------- rclpy/test/test_parameter.py | 6 ++-- rclpy/test/test_parameter_client.py | 5 ++- 3 files changed, 41 insertions(+), 22 deletions(-) diff --git a/rclpy/rclpy/parameter_client.py b/rclpy/rclpy/parameter_client.py index ee2e65609..c2952d358 100644 --- a/rclpy/rclpy/parameter_client.py +++ b/rclpy/rclpy/parameter_client.py @@ -13,11 +13,7 @@ # limitations under the License. import time -from typing import Callable -from typing import List -from typing import Optional -from typing import Sequence -from typing import Union +from typing import Callable, List, Optional, Sequence, Union from rcl_interfaces.msg import Parameter from rcl_interfaces.srv import DescribeParameters @@ -26,6 +22,7 @@ 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 RclpyParameter @@ -35,49 +32,68 @@ from rclpy.task import Future -class AsyncParameterClient(object): +class AsyncParameterClient: def __init__( self, node: Node, - target_node_name: str, + remote_node_name: str, qos_profile: QoSProfile = qos_profile_services_default, callback_group: Optional[CallbackGroup] = None): - # TODO: qos_profile, callback group """ Create an AsyncParameterClient. - For more on service names, see: `ROS 2 docs`_. + 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 for which the parameter clients will be added to - :param target_node_name: Name of remote node for which the parameters will be managed + :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.target_node = target_node_name + self.remote_node_name = remote_node_name self.node = node self._get_parameter_client = self.node.create_client( - GetParameters, f'{target_node_name}/get_parameters', + 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'{target_node_name}/list_parameters', + 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'{target_node_name}/set_parameters', + 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'{target_node_name}/get_parameter_types', + 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'{target_node_name}/describe_parameters', + 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'{target_node_name}/set_parameters_atomically', + SetParametersAtomically, f'{remote_node_name}/set_parameters_atomically', qos_profile=qos_profile, callback_group=callback_group ) diff --git a/rclpy/test/test_parameter.py b/rclpy/test/test_parameter.py index 8080cb8ae..187ef9f8e 100644 --- a/rclpy/test/test_parameter.py +++ b/rclpy/test/test_parameter.py @@ -218,15 +218,15 @@ def test_parameter_dict_from_yaml_file(self): yaml_string = """/param_test_target: ros__parameters: param_1: 1 - param_str: "string" + 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() as f: - f.write(str.encode(yaml_string)) + with NamedTemporaryFile(mode='w') as f: + f.write(yaml_string) f.seek(0) parameter_dict = parameter_dict_from_yaml_file(f.name) assert parameter_dict == expected diff --git a/rclpy/test/test_parameter_client.py b/rclpy/test/test_parameter_client.py index 51284c5ab..56ae923a1 100644 --- a/rclpy/test/test_parameter_client.py +++ b/rclpy/test/test_parameter_client.py @@ -16,6 +16,7 @@ import unittest import rcl_interfaces.msg +from rcl_interfaces.msg import ParameterType import rcl_interfaces.srv import rclpy import rclpy.context @@ -58,6 +59,7 @@ def test_set_parameter(self): 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) @@ -66,6 +68,7 @@ def test_get_parameter(self): 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 @@ -90,7 +93,7 @@ def test_get_paramter_types(self): self.executor.spin_until_future_complete(future) results = future.result() assert results is not None - assert results.types[0] == 7 # refer to rcl_interfaces.msg.ParameterType + assert results.types[0] == ParameterType.PARAMETER_INTEGER_ARRAY def test_set_parameters_atomically(self): future = self.client.set_parameters_atomically([ From 83be7d3ad78e263262afd337b2bc99f0bd4d19b1 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 3 Jun 2022 14:46:54 -0700 Subject: [PATCH 17/34] add test to parameter_dict_from_yaml for nonexistent file Signed-off-by: Brian Chen --- rclpy/test/test_parameter.py | 5 ++++- rclpy/test/test_parameter_client.py | 3 +++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/rclpy/test/test_parameter.py b/rclpy/test/test_parameter.py index 187ef9f8e..acf6b9b69 100644 --- a/rclpy/test/test_parameter.py +++ b/rclpy/test/test_parameter.py @@ -227,10 +227,13 @@ def test_parameter_dict_from_yaml_file(self): with NamedTemporaryFile(mode='w') as f: f.write(yaml_string) - f.seek(0) + 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 index 56ae923a1..cbd62dfac 100644 --- a/rclpy/test/test_parameter_client.py +++ b/rclpy/test/test_parameter_client.py @@ -85,6 +85,7 @@ def test_describe_parameters(self): 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 == 7 assert results.descriptors[0].name == 'int_arr_param' @@ -93,6 +94,7 @@ def test_get_paramter_types(self): 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): @@ -114,6 +116,7 @@ def test_delete_parameters(self): 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' From 4870aded45973485bbe46299754c088a64f855a7 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 3 Jun 2022 15:03:11 -0700 Subject: [PATCH 18/34] allow for set_parameter to take both rclpy Parameter and rcl_interfaces parameter msg Signed-off-by: Brian Chen --- rclpy/rclpy/parameter_client.py | 23 +++++++++++++---------- rclpy/test/test_parameter_client.py | 14 ++++++++------ 2 files changed, 21 insertions(+), 16 deletions(-) diff --git a/rclpy/rclpy/parameter_client.py b/rclpy/rclpy/parameter_client.py index c2952d358..2eeccea2b 100644 --- a/rclpy/rclpy/parameter_client.py +++ b/rclpy/rclpy/parameter_client.py @@ -15,7 +15,7 @@ import time from typing import Callable, List, Optional, Sequence, Union -from rcl_interfaces.msg import Parameter +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 @@ -25,7 +25,7 @@ from rclpy.callback_groups import CallbackGroup from rclpy.node import Node -from rclpy.parameter import Parameter as RclpyParameter +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 @@ -117,7 +117,6 @@ def wait_for_service(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. - :type timeout_sec: Union[float, None] :return: ``True`` if all services were waite , ``False`` otherwise. """ client_wait_fns = [ @@ -180,8 +179,8 @@ def get_parameters(self, names: List[str], callback: Optional[Callable] = None) def set_parameters( self, - parameters: Sequence[Parameter], - callback: Union[Callable, None] = None + parameters: Sequence[Union[Parameter, ParameterMsg]], + callback: Optional[Callable] = None ) -> Future: """ Set parameters given a list of parameters. @@ -194,7 +193,9 @@ def set_parameters( :return: ``Future`` with the result of the request. """ request = SetParameters.Request() - request.parameters = parameters + request.parameters = [ + i.to_parameter_msg() if isinstance(i, Parameter) else i for i in parameters + ] future = self._set_parameter_client.call_async(request) if callback: future.add_done_callback(callback) @@ -248,7 +249,7 @@ def get_parameter_types( def set_parameters_atomically( self, - parameters: Sequence[Parameter], + parameters: Sequence[Union[Parameter, ParameterMsg]], callback: Optional[Callable] = None ) -> Future: """ @@ -262,7 +263,9 @@ def set_parameters_atomically( :return: ``Future`` with the result of the request. """ request = SetParametersAtomically.Request() - request.parameters = parameters + request.parameters = [ + i.to_parameter_msg() if isinstance(i, Parameter) else i for i in parameters + ] future = self._set_parameters_atomically_client.call_async(request) if callback: future.add_done_callback(callback) @@ -282,7 +285,7 @@ def delete_parameters( :return: ``Future`` with the result of the request. """ request = SetParameters.Request() - request.parameters = [RclpyParameter(name=i).to_parameter_msg() for i in names] + 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) @@ -296,7 +299,7 @@ def load_parameter_file( """ Load parameters from a yaml file. - Wrapper around `parse_parameter_dict` and `load_dict` + 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``. diff --git a/rclpy/test/test_parameter_client.py b/rclpy/test/test_parameter_client.py index cbd62dfac..49ddc0d40 100644 --- a/rclpy/test/test_parameter_client.py +++ b/rclpy/test/test_parameter_client.py @@ -54,7 +54,7 @@ def tearDown(self): 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').to_parameter_msg(), + Parameter('string/param', Parameter.Type.STRING, 'hello world'), ]) self.executor.spin_until_future_complete(future) results = future.result() @@ -86,7 +86,7 @@ def test_describe_parameters(self): results = future.result() assert results is not None assert len(results.descriptors) == 1 - assert results.descriptors[0].type == 7 + assert results.descriptors[0].type == ParameterType.PARAMETER_INTEGER_ARRAY assert results.descriptors[0].name == 'int_arr_param' def test_get_paramter_types(self): @@ -99,7 +99,7 @@ def test_get_paramter_types(self): def test_set_parameters_atomically(self): future = self.client.set_parameters_atomically([ - Parameter('int_param', Parameter.Type.INTEGER, 888).to_parameter_msg(), + 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) @@ -124,6 +124,7 @@ def test_delete_parameters(self): 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): @@ -132,11 +133,12 @@ def test_load_parameter_file(self): param_1: 1 param_str: "string" """ - with NamedTemporaryFile() as f: - f.write(str.encode(yaml_string)) - f.seek(0) + 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]) From ff8a56eaee5e77045b4b4b172749bbd50b2dee74 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 3 Jun 2022 15:06:47 -0700 Subject: [PATCH 19/34] more consistent naming for functions Signed-off-by: Brian Chen --- rclpy/rclpy/parameter_client.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/rclpy/rclpy/parameter_client.py b/rclpy/rclpy/parameter_client.py index 2eeccea2b..6df140c1c 100644 --- a/rclpy/rclpy/parameter_client.py +++ b/rclpy/rclpy/parameter_client.py @@ -97,7 +97,7 @@ def __init__( qos_profile=qos_profile, callback_group=callback_group ) - def service_is_ready(self) -> bool: + def services_are_ready(self) -> bool: """ Check if all services are ready. @@ -112,7 +112,7 @@ def service_is_ready(self) -> bool: self._set_parameters_atomically_client.service_is_ready(), ]) - def wait_for_service(self, timeout_sec: Optional[float] = None) -> bool: + def wait_for_services(self, timeout_sec: Optional[float] = None) -> bool: """ Wait for all parameter services to be available. @@ -234,7 +234,7 @@ def get_parameter_types( The result after the returned future is complete will be of type ``rcl_interfaces.srv.GetParameterTypes.Response``. - Parameter type definitions are given in rcl_interfaces.msg.ParameterType + 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. @@ -280,6 +280,8 @@ def delete_parameters( 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. From b7556f6f1b54dbb1f5c96e7cb1bec43c077a0180 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 3 Jun 2022 15:52:07 -0700 Subject: [PATCH 20/34] nicer list comprehension Signed-off-by: Brian Chen --- rclpy/rclpy/client.py | 3 +-- rclpy/rclpy/parameter_client.py | 8 ++++++-- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/rclpy/rclpy/client.py b/rclpy/rclpy/client.py index 557b824d4..024b100d9 100644 --- a/rclpy/rclpy/client.py +++ b/rclpy/rclpy/client.py @@ -16,7 +16,6 @@ import time from typing import Dict from typing import TypeVar -from typing import Union from rclpy.callback_groups import CallbackGroup from rclpy.context import Context @@ -160,7 +159,7 @@ def service_is_ready(self) -> bool: with self.handle: return self.__client.service_server_is_available() - def wait_for_service(self, timeout_sec: Union[float, None] = None) -> bool: + def wait_for_service(self, timeout_sec: float = None) -> bool: """ Wait for a service server to become ready. diff --git a/rclpy/rclpy/parameter_client.py b/rclpy/rclpy/parameter_client.py index 6df140c1c..7757c3231 100644 --- a/rclpy/rclpy/parameter_client.py +++ b/rclpy/rclpy/parameter_client.py @@ -194,7 +194,9 @@ def set_parameters( """ request = SetParameters.Request() request.parameters = [ - i.to_parameter_msg() if isinstance(i, Parameter) else i for i in 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: @@ -264,7 +266,9 @@ def set_parameters_atomically( """ request = SetParametersAtomically.Request() request.parameters = [ - i.to_parameter_msg() if isinstance(i, Parameter) else i for i in 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: From b90edf080f3326ef06323f3a41ccccf40b493cee Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Wed, 15 Jun 2022 13:31:00 -0700 Subject: [PATCH 21/34] name and type in descriptor(s) is ignored via declare_parameter(s). (#957) Signed-off-by: Tomoya Fujita Signed-off-by: Brian Chen --- rclpy/rclpy/node.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 004b7a110..5b9c4c634 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -349,6 +349,9 @@ def declare_parameter( This method, if successful, will result in any callback registered with :func:`add_on_set_parameters_callback` to be called. + The name and type in the given descriptor is ignored, and should be specified using + the name argument to this function and the default value's type instead. + :param name: Fully-qualified name of the parameter, including its namespace. :param value: Value of the parameter to declare. :param descriptor: Descriptor for the parameter to declare. @@ -381,6 +384,8 @@ def declare_parameters( The tuples in the given parameter list shall contain the name for each parameter, optionally providing a value and a descriptor. + The name and type in the given descriptors are ignored, and should be specified using + the name argument to this function and the default value's type instead. For each entry in the list, a parameter with a name of "namespace.name" will be declared. The resulting value for each declared parameter will be returned, considering From f815c6cdd497b6d96463762484f1210279689ad8 Mon Sep 17 00:00:00 2001 From: Brian Date: Wed, 15 Jun 2022 14:05:16 -0700 Subject: [PATCH 22/34] 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]) From cf7259504ac624372da9b9fca010254580a91675 Mon Sep 17 00:00:00 2001 From: Brian Date: Wed, 15 Jun 2022 18:20:30 -0400 Subject: [PATCH 23/34] Revert "Revert "implement parameter_client" (#958)" This reverts commit bce7e89c8504b138afa80749f724f6151b1b6784. 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, 609 insertions(+), 3 deletions(-) create mode 100644 rclpy/rclpy/parameter_client.py create 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 c38ee7d9c..5dee741fd 100644 --- a/rclpy/docs/source/api/parameters.rst +++ b/rclpy/docs/source/api/parameters.rst @@ -10,3 +10,8 @@ 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 cea1f9163..26e0dc519 100644 --- a/rclpy/rclpy/__init__.py +++ b/rclpy/rclpy/__init__.py @@ -43,6 +43,7 @@ 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 @@ -141,7 +142,7 @@ def create_node( use_global_arguments: bool = True, enable_rosout: bool = True, start_parameter_services: bool = True, - parameter_overrides: List[Parameter] = None, + parameter_overrides: Union[List[Parameter], None] = 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 daad33146..6b917aaa0 100644 --- a/rclpy/rclpy/parameter.py +++ b/rclpy/rclpy/parameter.py @@ -14,9 +14,14 @@ 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, ParameterValue +from rcl_interfaces.msg import ParameterType +from rcl_interfaces.msg import ParameterValue +import yaml PARAMETER_SEPARATOR_STRING = '.' @@ -177,6 +182,50 @@ 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. @@ -211,3 +260,70 @@ 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 new file mode 100644 index 000000000..7757c3231 --- /dev/null +++ b/rclpy/rclpy/parameter_client.py @@ -0,0 +1,319 @@ +# 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 4a4a7f7d3..acf6b9b69 100644 --- a/rclpy/test/test_parameter.py +++ b/rclpy/test/test_parameter.py @@ -13,14 +13,15 @@ # 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 @@ -213,6 +214,26 @@ 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 new file mode 100644 index 000000000..49ddc0d40 --- /dev/null +++ b/rclpy/test/test_parameter_client.py @@ -0,0 +1,144 @@ +# 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]) From 9e2856bd2c9ef872863dc512fa54305cda824c73 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Wed, 15 Jun 2022 10:35:16 -0700 Subject: [PATCH 24/34] fix load_parameter_dict_from_yaml test failure on windows Signed-off-by: Brian Chen --- rclpy/test/test_parameter.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/rclpy/test/test_parameter.py b/rclpy/test/test_parameter.py index acf6b9b69..3231e5f67 100644 --- a/rclpy/test/test_parameter.py +++ b/rclpy/test/test_parameter.py @@ -13,6 +13,7 @@ # limitations under the License. from array import array +import os from tempfile import NamedTemporaryFile import unittest @@ -225,11 +226,12 @@ def test_parameter_dict_from_yaml_file(self): 'param_str': Parameter('param_str', Parameter.Type.STRING, 'string').to_parameter_msg() } - with NamedTemporaryFile(mode='w') as f: + with NamedTemporaryFile(mode='w', delete=False) as f: f.write(yaml_string) f.flush() + f.close() parameter_dict = parameter_dict_from_yaml_file(f.name) - print(parameter_dict) + os.unlink(f.name) assert parameter_dict == expected self.assertRaises(FileNotFoundError, parameter_dict_from_yaml_file, 'unknown_file') From fa20da3e7cebdb90ca3c7d8cec6b95cee52068bb Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Wed, 15 Jun 2022 10:40:45 -0700 Subject: [PATCH 25/34] make list_parameter default depth unlimited Signed-off-by: Brian Chen --- rclpy/rclpy/parameter_client.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/rclpy/rclpy/parameter_client.py b/rclpy/rclpy/parameter_client.py index 7757c3231..1c980d768 100644 --- a/rclpy/rclpy/parameter_client.py +++ b/rclpy/rclpy/parameter_client.py @@ -22,7 +22,6 @@ 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 @@ -127,7 +126,6 @@ def wait_for_services(self, timeout_sec: Optional[float] = None) -> bool: 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]) @@ -142,21 +140,22 @@ def wait_for_services(self, timeout_sec: Optional[float] = None) -> bool: def list_parameters( self, prefixes: Optional[List[str]] = None, - depth: int = 1, + depth: Optional[int] = None, 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 depth: Depth of the parameter tree to list. ``None`` means unlimited. :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 + if depth: + request.depth = depth future = self._list_parameter_client.call_async(request) if callback: future.add_done_callback(callback) From 00c5f4a686bd4c978c2487ed69c586c7b58d00ba Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Wed, 15 Jun 2022 15:15:56 -0700 Subject: [PATCH 26/34] fix: namespaced parameters take precedence over wildcard Signed-off-by: Brian Chen --- rclpy/rclpy/parameter.py | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/rclpy/rclpy/parameter.py b/rclpy/rclpy/parameter.py index 6b917aaa0..867c74ea0 100644 --- a/rclpy/rclpy/parameter.py +++ b/rclpy/rclpy/parameter.py @@ -281,19 +281,27 @@ def parameter_dict_from_yaml_file( """ with open(parameter_file, 'r') as f: param_file = yaml.safe_load(f) - param_keys = set() + param_keys = [] param_dict = {} if use_wildcard and '/**' in param_file: - param_keys.add('/**') + param_keys.append('/**') + 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) + param_keys.append(n) else: - param_keys = param_file.keys() + # wildcard key must go to the front of param_keys so that + # node-namespaced parameters will override the wildcard parameters + keys = set(param_file.keys()) + keys.discard('/**') + param_keys.extend(keys) + + if len(param_keys) == 0: + raise RuntimeError('Param file does not contain selected parameters') for n in param_keys: value = param_file[n] @@ -317,10 +325,10 @@ def _unpack_parameter_dict(namespace, parameter_dict): 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( + if type(param_value) == dict: + parameters.update(_unpack_parameter_dict( namespace=full_param_name + PARAMETER_SEPARATOR_STRING, - parameter_dict=param_value) + parameter_dict=param_value)) else: parameter = ParameterMsg() parameter.name = full_param_name From c883a07328b1f691a4bf3439108f65e4c82f8b8c Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Thu, 16 Jun 2022 16:27:09 -0700 Subject: [PATCH 27/34] leaner implementation of wait_for_services Signed-off-by: Brian Chen --- rclpy/rclpy/client.py | 1 + rclpy/rclpy/parameter_client.py | 27 +++++++++------------------ 2 files changed, 10 insertions(+), 18 deletions(-) diff --git a/rclpy/rclpy/client.py b/rclpy/rclpy/client.py index 024b100d9..92e1b4782 100644 --- a/rclpy/rclpy/client.py +++ b/rclpy/rclpy/client.py @@ -170,6 +170,7 @@ def wait_for_service(self, timeout_sec: float = None) -> bool: """ # TODO(sloretz) Return as soon as the service is available # This is a temporary implementation. The sleep time is arbitrary. + # https://github.com/ros2/rclpy/issues/58 sleep_time = 0.25 if timeout_sec is None: timeout_sec = float('inf') diff --git a/rclpy/rclpy/parameter_client.py b/rclpy/rclpy/parameter_client.py index 1c980d768..a782c1970 100644 --- a/rclpy/rclpy/parameter_client.py +++ b/rclpy/rclpy/parameter_client.py @@ -116,26 +116,17 @@ 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. + :return: ``True`` if all services becomes avaliable, ``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, - ] + # TODO(ihasdapie) See: rclpy.Client.wait_for_service + sleep_time = 0.25 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 + timeout_sec = float('inf') + while not self.services_are_ready() and timeout_sec > 0.0: + time.sleep(sleep_time) + timeout_sec -= sleep_time + return self.services_are_ready() + def list_parameters( self, From 0c32ba1652a3dd3b5edb8b876e560e80cddb6825 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Thu, 16 Jun 2022 22:04:10 -0700 Subject: [PATCH 28/34] fix linting Signed-off-by: Brian Chen --- rclpy/rclpy/parameter_client.py | 1 - 1 file changed, 1 deletion(-) diff --git a/rclpy/rclpy/parameter_client.py b/rclpy/rclpy/parameter_client.py index a782c1970..4a5d5d122 100644 --- a/rclpy/rclpy/parameter_client.py +++ b/rclpy/rclpy/parameter_client.py @@ -127,7 +127,6 @@ def wait_for_services(self, timeout_sec: Optional[float] = None) -> bool: timeout_sec -= sleep_time return self.services_are_ready() - def list_parameters( self, prefixes: Optional[List[str]] = None, From fb21b6689c61b4aa37c257bd79d600ffa6f3107d Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 17 Jun 2022 11:47:53 -0700 Subject: [PATCH 29/34] address pr comments Signed-off-by: Brian Chen --- rclpy/rclpy/__init__.py | 3 +-- rclpy/rclpy/parameter.py | 9 ++++++--- rclpy/rclpy/parameter_client.py | 4 ++-- 3 files changed, 9 insertions(+), 7 deletions(-) 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 867c74ea0..98e0d0065 100644 --- a/rclpy/rclpy/parameter.py +++ b/rclpy/rclpy/parameter.py @@ -269,15 +269,18 @@ def parameter_dict_from_yaml_file( namespace: str = '' ) -> Dict[str, ParameterMsg]: """ - Build a dict of parameters from a YAML file formatted as per ``ros2 param dump``. + Build a dict of parameters from a YAML file. - Will load all parameters if ``target_nodes`` is None + Will load all parameters if ``target_nodes`` is None or empty. + + :raises RuntimeError: if a target node is not in the file + :raises RuntimeError: if the file is not in the same format as provided by ros2 param dump :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 + :return: A dict of Parameter messages keyed by the parameter names """ with open(parameter_file, 'r') as f: param_file = yaml.safe_load(f) diff --git a/rclpy/rclpy/parameter_client.py b/rclpy/rclpy/parameter_client.py index 4a5d5d122..6b1b64132 100644 --- a/rclpy/rclpy/parameter_client.py +++ b/rclpy/rclpy/parameter_client.py @@ -54,9 +54,9 @@ def __init__( client = AsyncParameterClient(node, 'example_node') - # set parameters + # set parameters on example node future = client.set_parameters([ - Parameter('int_param', Parameter.Type.INTEGER, 88).to_parameter_msg(), + Parameter('int_param', Parameter.Type.INTEGER, 88), Parameter('string/param', Parameter.Type.STRING, 'hello world').to_parameter_msg(), ]) self.executor.spin_until_future_complete(future) From 214ab92ad0a8b673f2c874a6db997cfca7896c44 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 17 Jun 2022 15:30:38 -0700 Subject: [PATCH 30/34] don't reference ros2 param dump Signed-off-by: Brian Chen --- rclpy/rclpy/parameter.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/rclpy/rclpy/parameter.py b/rclpy/rclpy/parameter.py index 98e0d0065..f41b7453b 100644 --- a/rclpy/rclpy/parameter.py +++ b/rclpy/rclpy/parameter.py @@ -274,7 +274,7 @@ def parameter_dict_from_yaml_file( Will load all parameters if ``target_nodes`` is None or empty. :raises RuntimeError: if a target node is not in the file - :raises RuntimeError: if the file is not in the same format as provided by ros2 param dump + :raises RuntimeError: if the is not a valid ROS parameter file :param parameter_file: Path to the YAML file to load parameters from. :param use_wildcard: Use wildcard matching for the target nodes. @@ -309,8 +309,7 @@ def parameter_dict_from_yaml_file( 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' + raise RuntimeError( 'YAML file is not a valid ROS parameter file for node {}' .format(n)) param_dict.update(value['ros__parameters']) return _unpack_parameter_dict(namespace, param_dict) From d6dd1911359ac8cd363716c996d241fe771b2913 Mon Sep 17 00:00:00 2001 From: Brian Date: Mon, 20 Jun 2022 13:15:06 -0400 Subject: [PATCH 31/34] Update rclpy/rclpy/parameter.py Co-authored-by: Jacob Perron Signed-off-by: Brian Chen --- rclpy/rclpy/parameter.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/rclpy/rclpy/parameter.py b/rclpy/rclpy/parameter.py index f41b7453b..9ba476f40 100644 --- a/rclpy/rclpy/parameter.py +++ b/rclpy/rclpy/parameter.py @@ -309,8 +309,7 @@ def parameter_dict_from_yaml_file( for n in param_keys: value = param_file[n] if type(value) != dict or 'ros__parameters' not in value: - raise RuntimeError( 'YAML file is not a valid ROS parameter file for node {}' - .format(n)) + raise RuntimeError( f'YAML file is not a valid ROS parameter file for node {n}') param_dict.update(value['ros__parameters']) return _unpack_parameter_dict(namespace, param_dict) From bbf43c460ce80a2ab312e2207be3de5928b357ad Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Tue, 21 Jun 2022 15:26:01 -0700 Subject: [PATCH 32/34] flake8 Signed-off-by: Brian Chen --- rclpy/rclpy/parameter.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclpy/rclpy/parameter.py b/rclpy/rclpy/parameter.py index 9ba476f40..eede4bcbf 100644 --- a/rclpy/rclpy/parameter.py +++ b/rclpy/rclpy/parameter.py @@ -309,7 +309,7 @@ def parameter_dict_from_yaml_file( for n in param_keys: value = param_file[n] if type(value) != dict or 'ros__parameters' not in value: - raise RuntimeError( f'YAML file is not a valid ROS parameter file for node {n}') + raise RuntimeError(f'YAML file is not a valid ROS parameter file for node {n}') param_dict.update(value['ros__parameters']) return _unpack_parameter_dict(namespace, param_dict) From fbf59592207fe652883b433e4651b9b09bf6afff Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Thu, 23 Jun 2022 14:36:11 -0700 Subject: [PATCH 33/34] add load_parameter_file_atomically Signed-off-by: Brian Chen --- rclpy/rclpy/parameter_client.py | 21 +++++++++++++++++++++ rclpy/test/test_parameter_client.py | 15 +++++++++++++++ 2 files changed, 36 insertions(+) diff --git a/rclpy/rclpy/parameter_client.py b/rclpy/rclpy/parameter_client.py index 6b1b64132..1b538f7d8 100644 --- a/rclpy/rclpy/parameter_client.py +++ b/rclpy/rclpy/parameter_client.py @@ -306,3 +306,24 @@ def load_parameter_file( param_dict = parameter_dict_from_yaml_file(parameter_file, use_wildcard) future = self.set_parameters(list(param_dict.values())) return future + + def load_parameter_file_atomically( + self, + parameter_file: str, + use_wildcard: bool = False, + ) -> Future: + """ + Load parameters from a yaml file atomically. + + 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_atomically(list(param_dict.values())) + return future diff --git a/rclpy/test/test_parameter_client.py b/rclpy/test/test_parameter_client.py index 49ddc0d40..0167d0fa3 100644 --- a/rclpy/test/test_parameter_client.py +++ b/rclpy/test/test_parameter_client.py @@ -142,3 +142,18 @@ def test_load_parameter_file(self): assert result is not None assert len(result.results) == 2 assert all([i.successful for i in result.results]) + + def test_load_parameter_file_atomically(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_atomically(f.name) + self.executor.spin_until_future_complete(future) + result = future.result() + assert result is not None + assert result.result.successful From ddd40f360d59ff5f64282246d9fda83f4940f25e Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Mon, 27 Jun 2022 15:16:04 -0700 Subject: [PATCH 34/34] address PR comments Signed-off-by: Brian Chen --- rclpy/rclpy/parameter_client.py | 12 +++-- rclpy/test/test_parameter.py | 18 ++++---- rclpy/test/test_parameter_client.py | 71 +++++++++++++++++------------ 3 files changed, 59 insertions(+), 42 deletions(-) diff --git a/rclpy/rclpy/parameter_client.py b/rclpy/rclpy/parameter_client.py index 1b538f7d8..48b97ad25 100644 --- a/rclpy/rclpy/parameter_client.py +++ b/rclpy/rclpy/parameter_client.py @@ -134,7 +134,7 @@ def list_parameters( callback: Optional[Callable] = None ) -> Future: """ - List all parameters with given prefixs. + List all parameters with given prefixes. :param prefixes: List of prefixes to filter by. :param depth: Depth of the parameter tree to list. ``None`` means unlimited. @@ -290,6 +290,7 @@ def load_parameter_file( self, parameter_file: str, use_wildcard: bool = False, + callback: Optional[Callable] = None ) -> Future: """ Load parameters from a yaml file. @@ -301,16 +302,17 @@ def load_parameter_file( :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. + :return: Future with the result from the set_parameters call. """ param_dict = parameter_dict_from_yaml_file(parameter_file, use_wildcard) - future = self.set_parameters(list(param_dict.values())) + future = self.set_parameters(list(param_dict.values()), callback=callback) return future def load_parameter_file_atomically( self, parameter_file: str, use_wildcard: bool = False, + callback: Optional[Callable] = None ) -> Future: """ Load parameters from a yaml file atomically. @@ -322,8 +324,8 @@ def load_parameter_file_atomically( :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. + :return: Future with the result from the set_parameters_atomically call. """ param_dict = parameter_dict_from_yaml_file(parameter_file, use_wildcard) - future = self.set_parameters_atomically(list(param_dict.values())) + future = self.set_parameters_atomically(list(param_dict.values()), callback=callback) return future diff --git a/rclpy/test/test_parameter.py b/rclpy/test/test_parameter.py index 3231e5f67..ce89a0497 100644 --- a/rclpy/test/test_parameter.py +++ b/rclpy/test/test_parameter.py @@ -226,14 +226,16 @@ def test_parameter_dict_from_yaml_file(self): 'param_str': Parameter('param_str', Parameter.Type.STRING, 'string').to_parameter_msg() } - with NamedTemporaryFile(mode='w', delete=False) as f: - f.write(yaml_string) - f.flush() - f.close() - parameter_dict = parameter_dict_from_yaml_file(f.name) - os.unlink(f.name) - assert parameter_dict == expected - + try: + with NamedTemporaryFile(mode='w', delete=False) as f: + f.write(yaml_string) + f.flush() + f.close() + parameter_dict = parameter_dict_from_yaml_file(f.name) + assert parameter_dict == expected + finally: + if os.path.exists(f.name): + os.unlink(f.name) self.assertRaises(FileNotFoundError, parameter_dict_from_yaml_file, 'unknown_file') diff --git a/rclpy/test/test_parameter_client.py b/rclpy/test/test_parameter_client.py index 0167d0fa3..7c4882fc7 100644 --- a/rclpy/test/test_parameter_client.py +++ b/rclpy/test/test_parameter_client.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import os from tempfile import NamedTemporaryFile import unittest @@ -30,15 +31,17 @@ 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.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.target_node.declare_parameter('float.param..', 3.14) self.client = AsyncParameterClient(self.client_node, 'test_parameter_client_target') self.executor = SingleThreadedExecutor(context=self.context) @@ -54,7 +57,7 @@ def tearDown(self): 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'), + Parameter('string.param', Parameter.Type.STRING, 'hello world'), ]) self.executor.spin_until_future_complete(future) results = future.result() @@ -64,7 +67,7 @@ def test_set_parameter(self): assert all(res) def test_get_parameter(self): - future = self.client.get_parameters(['int_arr_param', 'float/param//']) + 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 @@ -78,7 +81,7 @@ def test_list_parameters(self): results = future.result() assert results is not None assert 'int_arr_param' in results.result.names - assert 'float/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']) @@ -100,7 +103,7 @@ def test_get_paramter_types(self): 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(), + Parameter('string.param', Parameter.Type.STRING, 'Hello World').to_parameter_msg(), ]) self.executor.spin_until_future_complete(future) results = future.result() @@ -133,15 +136,20 @@ def test_load_parameter_file(self): 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]) + try: + with NamedTemporaryFile(mode='w', delete=False) as f: + f.write(yaml_string) + f.flush() + f.close() + 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]) + finally: + if os.path.exists(f.name): + os.unlink(f.name) def test_load_parameter_file_atomically(self): yaml_string = """/param_test_target: @@ -149,11 +157,16 @@ def test_load_parameter_file_atomically(self): param_1: 1 param_str: "string" """ - with NamedTemporaryFile(mode='w') as f: - f.write(yaml_string) - f.flush() - future = self.client.load_parameter_file_atomically(f.name) - self.executor.spin_until_future_complete(future) - result = future.result() - assert result is not None - assert result.result.successful + try: + with NamedTemporaryFile(mode='w', delete=False) as f: + f.write(yaml_string) + f.flush() + f.close() + future = self.client.load_parameter_file_atomically(f.name) + self.executor.spin_until_future_complete(future) + result = future.result() + assert result is not None + assert result.result.successful + finally: + if os.path.exists(f.name): + os.unlink(f.name)