From fda0422a46750fcba5b2e4d0ccab2071ba88dbfa Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Thu, 26 May 2022 16:06:06 -0700 Subject: [PATCH 01/33] fix: allow for executor shutdown call from callback Signed-off-by: Brian Chen --- rclpy/rclpy/executors.py | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index 25ade8970..d7f7982b5 100644 --- a/rclpy/rclpy/executors.py +++ b/rclpy/rclpy/executors.py @@ -79,20 +79,21 @@ def __exit__(self, t, v, tb): self._num_work_executing -= 1 self._work_condition.notify_all() - def wait(self, timeout_sec=None): + def wait(self, timeout_sec: Optional[float] = None, is_shutdown: bool = False): """ Wait until all work completes. :param timeout_sec: Seconds to wait. Block forever if None or negative. Don't wait if 0 :type timeout_sec: float or None + :param: is_shutdown: if this is a shutdown call :rtype: bool True if all work completed """ - if timeout_sec is not None and timeout_sec < 0: + if timeout_sec is not None and timeout_sec < 0.0: timeout_sec = None # Wait for all work to complete with self._work_condition: if not self._work_condition.wait_for( - lambda: self._num_work_executing == 0, timeout_sec): + lambda: self._num_work_executing == 0 or is_shutdown, timeout_sec): return False return True @@ -205,7 +206,7 @@ def shutdown(self, timeout_sec: float = None) -> bool: # Tell executor it's been shut down self._guard.trigger() - if not self._work_tracker.wait(timeout_sec): + if not self._work_tracker.wait(timeout_sec, is_shutdown=self._is_shutdown): return False # Clean up stuff that won't be used anymore @@ -425,7 +426,13 @@ async def handler(entity, gc, is_shutdown, work_tracker): entity.callback_group.ending_execution(entity) # Signal that work has been done so the next callback in a mutually exclusive # callback group can get executed - gc.trigger() + + # Catch expected error where calling executor.shutdown() + # from callback causes the GuardCondition to be destroyed + try: + gc.trigger() + except InvalidHandle: + pass task = Task( handler, (entity, self._guard, self._is_shutdown, self._work_tracker), executor=self) From c9367dfdfaf010c0ed74effa809b78b97f205dfe Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Thu, 26 May 2022 17:14:07 -0700 Subject: [PATCH 02/33] feat: tests for executor shutdown, linting Signed-off-by: Brian Chen --- rclpy/rclpy/executors.py | 2 +- rclpy/test/test_executor.py | 24 ++++++++++++++++++++++++ 2 files changed, 25 insertions(+), 1 deletion(-) diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index d7f7982b5..e3b808ded 100644 --- a/rclpy/rclpy/executors.py +++ b/rclpy/rclpy/executors.py @@ -428,7 +428,7 @@ async def handler(entity, gc, is_shutdown, work_tracker): # callback group can get executed # Catch expected error where calling executor.shutdown() - # from callback causes the GuardCondition to be destroyed + # from callback causes the GuardCondition to be destroyed try: gc.trigger() except InvalidHandle: diff --git a/rclpy/test/test_executor.py b/rclpy/test/test_executor.py index 9a10c6e4f..5710d9b49 100644 --- a/rclpy/test/test_executor.py +++ b/rclpy/test/test_executor.py @@ -13,6 +13,7 @@ # limitations under the License. import asyncio +import multiprocessing as mp import threading import time import unittest @@ -456,6 +457,29 @@ def timer_callback(): executor.shutdown() self.node.destroy_timer(tmr) + def shutdown_executor_from_callback(self): + """Test regression of rclpy#944: executor shutdown from callback hangs indefinitely.""" + self.assertIsNotNone(self.node.handle) + timer_period = 0.1 + hung_on_shutdown = True + executor = SingleThreadedExecutor(context=self.context) + + def timer_callback(): + nonlocal hung_on_shutdown, executor + executor.shutdown() + hung_on_shutdown = False + + tmr = self.node.create_timer(timer_period, timer_callback) + executor.add_node(self.node) + + # Use process so we can terminate it if it hangs + p = mp.Process(target=executor.spin_once, daemon=True) + p.start() + time.sleep(1 + timer_period) + self.assertFalse(hung_on_shutdown) + p.terminate() + self.node.destroy_timer(tmr) + if __name__ == '__main__': unittest.main() From 097e9976b5bd6d06f3f28d9231b4c6d72e6219e4 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Thu, 19 May 2022 17:05:19 -0700 Subject: [PATCH 03/33] 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 4e5528f15..a23e6ada8 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 b851df615eb9b9f64018b2ac7fae767a67fbbb88 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Thu, 19 May 2022 17:05:33 -0700 Subject: [PATCH 04/33] 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 86d9d205a8716df2beea59acd36db2d6db7265be Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 20 May 2022 16:47:11 -0700 Subject: [PATCH 05/33] 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 3bc9513061044fdc8135aaee9ef0c8d7afbe8ab0 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Mon, 23 May 2022 22:11:23 -0700 Subject: [PATCH 06/33] 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 a56b1ae46540cbd522a63e5f21cce0ffa40ef050 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Mon, 23 May 2022 22:11:51 -0700 Subject: [PATCH 07/33] 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 64e074f3a737077ff8044ee3da2da792b9bdbab7 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Mon, 23 May 2022 22:12:05 -0700 Subject: [PATCH 08/33] 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 a3b4fa3ef45cf175bcc4dbf6f0a24505a69f6789 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Mon, 23 May 2022 22:14:00 -0700 Subject: [PATCH 09/33] 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 e2d26b7cf1a81a03a5a4b106fbc69aa6e5ae7ad2 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Tue, 24 May 2022 17:07:34 -0700 Subject: [PATCH 10/33] 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 5d27400b254938d52ecfacfb12b56609687b2522 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Tue, 24 May 2022 17:07:50 -0700 Subject: [PATCH 11/33] 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 3de00bda1cb9c3d3ffcf69ebf75ca67aa8bbd541 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 27 May 2022 11:42:16 -0700 Subject: [PATCH 12/33] 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 7be64ba56ef9c5d375a16b9f305007ba50bcb8fb Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 27 May 2022 13:58:44 -0700 Subject: [PATCH 13/33] 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 ff9b0e4ceba33b09a83c09767b54167d34988f93 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Wed, 1 Jun 2022 15:16:15 -0700 Subject: [PATCH 14/33] 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 2b3f677fa8f160729e69192f8352371c31cafa53 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Wed, 1 Jun 2022 15:21:04 -0700 Subject: [PATCH 15/33] 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 7894e96f4ad484b9cb026bc336897b520066a342 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Wed, 1 Jun 2022 16:07:31 -0700 Subject: [PATCH 16/33] 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 95ad20adbb1cd78559e7f31ce35b7379ad7386af Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 3 Jun 2022 10:12:25 -0700 Subject: [PATCH 17/33] 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 75a9b1f7801dcffe8036674ce99fe26deb4b9827 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 3 Jun 2022 10:12:46 -0700 Subject: [PATCH 18/33] 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 7d459d10908de892df87a2e1b994706f81220fc3 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 3 Jun 2022 14:29:54 -0700 Subject: [PATCH 19/33] use threading for executor shutdown test Signed-off-by: Brian Chen --- rclpy/test/test_executor.py | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/rclpy/test/test_executor.py b/rclpy/test/test_executor.py index 5710d9b49..b083bb6dc 100644 --- a/rclpy/test/test_executor.py +++ b/rclpy/test/test_executor.py @@ -13,7 +13,6 @@ # limitations under the License. import asyncio -import multiprocessing as mp import threading import time import unittest @@ -461,23 +460,19 @@ def shutdown_executor_from_callback(self): """Test regression of rclpy#944: executor shutdown from callback hangs indefinitely.""" self.assertIsNotNone(self.node.handle) timer_period = 0.1 - hung_on_shutdown = True executor = SingleThreadedExecutor(context=self.context) + shutdown_event = threading.Event() def timer_callback(): - nonlocal hung_on_shutdown, executor + nonlocal shutdown_event, executor executor.shutdown() - hung_on_shutdown = False + shutdown_event.set() tmr = self.node.create_timer(timer_period, timer_callback) executor.add_node(self.node) - - # Use process so we can terminate it if it hangs - p = mp.Process(target=executor.spin_once, daemon=True) - p.start() - time.sleep(1 + timer_period) - self.assertFalse(hung_on_shutdown) - p.terminate() + t = threading.Thread(target=executor.spin_once, daemon=True) + t.start() + self.assertTrue(shutdown_event.wait(120)) self.node.destroy_timer(tmr) From 2c0502bc4524fe4fe14d4c3a2fd6b5b4bf2ed015 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 3 Jun 2022 14:30:13 -0700 Subject: [PATCH 20/33] make default for shutdown to not wait Signed-off-by: Brian Chen --- rclpy/rclpy/executors.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index e3b808ded..5ab6a07e7 100644 --- a/rclpy/rclpy/executors.py +++ b/rclpy/rclpy/executors.py @@ -79,7 +79,7 @@ def __exit__(self, t, v, tb): self._num_work_executing -= 1 self._work_condition.notify_all() - def wait(self, timeout_sec: Optional[float] = None, is_shutdown: bool = False): + def wait(self, timeout_sec: Optional[float] = None): """ Wait until all work completes. @@ -93,7 +93,7 @@ def wait(self, timeout_sec: Optional[float] = None, is_shutdown: bool = False): # Wait for all work to complete with self._work_condition: if not self._work_condition.wait_for( - lambda: self._num_work_executing == 0 or is_shutdown, timeout_sec): + lambda: self._num_work_executing == 0, timeout_sec): return False return True @@ -191,7 +191,7 @@ def create_task(self, callback: Union[Callable, Coroutine], *args, **kwargs) -> # Task inherits from Future return task - def shutdown(self, timeout_sec: float = None) -> bool: + def shutdown(self, timeout_sec: float = 0) -> bool: """ Stop executing callbacks and wait for their completion. @@ -205,9 +205,9 @@ def shutdown(self, timeout_sec: float = None) -> bool: self._is_shutdown = True # Tell executor it's been shut down self._guard.trigger() - - if not self._work_tracker.wait(timeout_sec, is_shutdown=self._is_shutdown): - return False + if not self._is_shutdown: + if not self._work_tracker.wait(timeout_sec): + return False # Clean up stuff that won't be used anymore with self._nodes_lock: From 008c340164d89b52f65aab95536eede818f1f28c Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 3 Jun 2022 14:36:11 -0700 Subject: [PATCH 21/33] use url instead of issue # Signed-off-by: Brian Chen --- rclpy/test/test_executor.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclpy/test/test_executor.py b/rclpy/test/test_executor.py index b083bb6dc..1681a9821 100644 --- a/rclpy/test/test_executor.py +++ b/rclpy/test/test_executor.py @@ -457,7 +457,7 @@ def timer_callback(): self.node.destroy_timer(tmr) def shutdown_executor_from_callback(self): - """Test regression of rclpy#944: executor shutdown from callback hangs indefinitely.""" + """https://github.com/ros2/rclpy/issues/944: allow for executor shutdown from callback.""" self.assertIsNotNone(self.node.handle) timer_period = 0.1 executor = SingleThreadedExecutor(context=self.context) From efc8b4d271999f4ea394a36283faef60e9db85d7 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 3 Jun 2022 14:46:54 -0700 Subject: [PATCH 22/33] 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 8c27cf509f04724e67c90a35ed36134fedc5820c Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 3 Jun 2022 15:03:11 -0700 Subject: [PATCH 23/33] 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 56a2c21b178d9b8a5ba1c0da66f820771ecdbcb4 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 3 Jun 2022 15:06:47 -0700 Subject: [PATCH 24/33] 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 8f04ce63e6aa14884b1c6708b7dd5336aef2fbe1 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 3 Jun 2022 16:02:08 -0700 Subject: [PATCH 25/33] address pr comments Signed-off-by: Brian Chen --- rclpy/rclpy/executors.py | 1 - rclpy/test/test_executor.py | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index 5ab6a07e7..c81f00087 100644 --- a/rclpy/rclpy/executors.py +++ b/rclpy/rclpy/executors.py @@ -85,7 +85,6 @@ def wait(self, timeout_sec: Optional[float] = None): :param timeout_sec: Seconds to wait. Block forever if None or negative. Don't wait if 0 :type timeout_sec: float or None - :param: is_shutdown: if this is a shutdown call :rtype: bool True if all work completed """ if timeout_sec is not None and timeout_sec < 0.0: diff --git a/rclpy/test/test_executor.py b/rclpy/test/test_executor.py index 1681a9821..c0c3cc810 100644 --- a/rclpy/test/test_executor.py +++ b/rclpy/test/test_executor.py @@ -470,7 +470,7 @@ def timer_callback(): tmr = self.node.create_timer(timer_period, timer_callback) executor.add_node(self.node) - t = threading.Thread(target=executor.spin_once, daemon=True) + t = threading.Thread(target=executor.spin, daemon=True) t.start() self.assertTrue(shutdown_event.wait(120)) self.node.destroy_timer(tmr) From 7a2c47d83d127b1f046acf24df67a06f1e0f0bdd Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 3 Jun 2022 15:52:07 -0700 Subject: [PATCH 26/33] 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 a23e6ada8..4e5528f15 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 d274d970d41ad5b64bbac329b09e83c43a8cee43 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Wed, 8 Jun 2022 23:04:40 -0400 Subject: [PATCH 27/33] try waiting for completion Signed-off-by: Brian Chen --- rclpy/rclpy/executors.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index c81f00087..3c5bea476 100644 --- a/rclpy/rclpy/executors.py +++ b/rclpy/rclpy/executors.py @@ -190,7 +190,7 @@ def create_task(self, callback: Union[Callable, Coroutine], *args, **kwargs) -> # Task inherits from Future return task - def shutdown(self, timeout_sec: float = 0) -> bool: + def shutdown(self, timeout_sec: float = None) -> bool: """ Stop executing callbacks and wait for their completion. From 0e603f3ddeebeb223a2f90bc5a0b5b18ac90b9bc Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Sat, 11 Jun 2022 05:29:53 -0700 Subject: [PATCH 28/33] typo fix. (#951) Signed-off-by: Tomoya Fujita --- rclpy/rclpy/client.py | 2 +- rclpy/rclpy/node.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rclpy/rclpy/client.py b/rclpy/rclpy/client.py index 4e5528f15..024b100d9 100644 --- a/rclpy/rclpy/client.py +++ b/rclpy/rclpy/client.py @@ -101,7 +101,7 @@ def unblock(future): def call_async(self, request: SrvTypeRequest) -> Future: """ - Make a service request and asyncronously get the result. + Make a service request and asynchronously get the result. :param request: The service request. :return: A future that completes when the request does. diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 983c4c49c..004b7a110 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -750,7 +750,7 @@ def set_parameters_atomically(self, parameter_list: List[Parameter]) -> SetParam allowed for the node, this method will raise a ParameterNotDeclaredException exception. Parameters are set all at once. - If setting a parameter fails due to not being declared, then no parameter will be set set. + If setting a parameter fails due to not being declared, then no parameter will be set. Either all of the parameters are set or none of them are set. If undeclared parameters are allowed for the node, then all the parameters will be From 7985411881aa39dfad7444b3b4e5360c85fcf021 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Wed, 15 Jun 2022 13:31:00 -0700 Subject: [PATCH 29/33] name and type in descriptor(s) is ignored via declare_parameter(s). (#957) Signed-off-by: Tomoya Fujita --- 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 bce7e89c8504b138afa80749f724f6151b1b6784 Mon Sep 17 00:00:00 2001 From: Brian Date: Wed, 15 Jun 2022 14:05:16 -0700 Subject: [PATCH 30/33] Revert "implement parameter_client" (#958) See discussion @ https://github.com/ros2/rclpy/pull/956 --- 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 e3b683e8ab2ac18523c41633df92c28894d23434 Mon Sep 17 00:00:00 2001 From: Brian Date: Mon, 27 Jun 2022 15:32:34 -0700 Subject: [PATCH 31/33] Change sphinx theme to readthedocs (#950) --- rclpy/docs/source/conf.py | 3 ++- rclpy/package.xml | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/rclpy/docs/source/conf.py b/rclpy/docs/source/conf.py index ac689dd2c..b218279b1 100644 --- a/rclpy/docs/source/conf.py +++ b/rclpy/docs/source/conf.py @@ -58,6 +58,7 @@ 'sphinx.ext.autosummary', 'sphinx.ext.doctest', 'sphinx.ext.coverage', + 'sphinx_rtd_theme', ] # Add any paths that contain templates here, relative to this directory. @@ -93,7 +94,7 @@ # The theme to use for HTML and HTML Help pages. See the documentation for # a list of builtin themes. # -html_theme = 'alabaster' +html_theme = 'sphinx_rtd_theme' # Theme options are theme-specific and customize the look and feel of a theme # further. For a list of options available for each theme, see the diff --git a/rclpy/package.xml b/rclpy/package.xml index cb419d2a5..9e2f0b4fd 100644 --- a/rclpy/package.xml +++ b/rclpy/package.xml @@ -48,6 +48,7 @@ test_msgs python3-sphinx + python3-sphinx-rtd-theme ament_cmake From c3a679666cce40379dadd96f9b8b4c7c237e0b5f Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Tue, 28 Jun 2022 09:21:31 -0500 Subject: [PATCH 32/33] Mirror rolling to master --- .github/workflows/mirror-rolling-to-master.yaml | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 .github/workflows/mirror-rolling-to-master.yaml diff --git a/.github/workflows/mirror-rolling-to-master.yaml b/.github/workflows/mirror-rolling-to-master.yaml new file mode 100644 index 000000000..2885eb4a4 --- /dev/null +++ b/.github/workflows/mirror-rolling-to-master.yaml @@ -0,0 +1,13 @@ +name: Mirror rolling to master + +on: + push: + branches: [ rolling ] + +jobs: + mirror-to-master: + runs-on: ubuntu-latest + steps: + - uses: zofrex/mirror-branch@v1 + with: + target-branch: master From 3053a8ad9cc266300c7c8e3070bb6552652cf88d Mon Sep 17 00:00:00 2001 From: Brian Date: Wed, 29 Jun 2022 11:40:48 -0700 Subject: [PATCH 33/33] implement parameter_client (#959) Signed-off-by: Brian Chen Co-authored-by: Tomoya Fujita Co-authored-by: Jacob Perron --- rclpy/docs/source/api/parameters.rst | 5 + rclpy/rclpy/client.py | 1 + rclpy/rclpy/parameter.py | 127 +++++++++- rclpy/rclpy/parameter_client.py | 331 +++++++++++++++++++++++++++ rclpy/test/test_parameter.py | 27 ++- rclpy/test/test_parameter_client.py | 172 ++++++++++++++ 6 files changed, 661 insertions(+), 2 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/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.py b/rclpy/rclpy/parameter.py index daad33146..eede4bcbf 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,79 @@ 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. + + 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 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. + :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 messages keyed by the parameter names + """ + with open(parameter_file, 'r') as f: + param_file = yaml.safe_load(f) + param_keys = [] + param_dict = {} + + if use_wildcard and '/**' in param_file: + 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.append(n) + else: + # 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] + 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}') + 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.update(_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..48b97ad25 --- /dev/null +++ b/rclpy/rclpy/parameter_client.py @@ -0,0 +1,331 @@ +# 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 on example node + future = client.set_parameters([ + 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) + 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 becomes avaliable, ``False`` otherwise. + """ + # TODO(ihasdapie) See: rclpy.Client.wait_for_service + sleep_time = 0.25 + if timeout_sec is None: + 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, + prefixes: Optional[List[str]] = None, + depth: Optional[int] = None, + callback: Optional[Callable] = None + ) -> Future: + """ + 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. + :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 + if depth: + 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, + callback: Optional[Callable] = None + ) -> 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_parameters call. + """ + param_dict = parameter_dict_from_yaml_file(parameter_file, use_wildcard) + 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. + + 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_parameters_atomically call. + """ + param_dict = parameter_dict_from_yaml_file(parameter_file, use_wildcard) + 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 4a4a7f7d3..ce89a0497 100644 --- a/rclpy/test/test_parameter.py +++ b/rclpy/test/test_parameter.py @@ -13,14 +13,16 @@ # limitations under the License. from array import array +import os +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 +215,29 @@ 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() + } + + 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') + 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..7c4882fc7 --- /dev/null +++ b/rclpy/test/test_parameter_client.py @@ -0,0 +1,172 @@ +# 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 os +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" + """ + 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: + ros__parameters: + param_1: 1 + param_str: "string" + """ + 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)