diff --git a/rosapi/src/rosapi/params.py b/rosapi/src/rosapi/params.py index 443e60b14..17fed9124 100644 --- a/rosapi/src/rosapi/params.py +++ b/rosapi/src/rosapi/params.py @@ -48,10 +48,11 @@ as JSON in order to facilitate dynamically typed SRV messages """ # Constants -DEFAULT_PARAM_TIMEOUT_SEC = 5.0 +DEFAULT_PARAM_TIMEOUT_SEC = 1.0 _node = None _timeout_sec = DEFAULT_PARAM_TIMEOUT_SEC +_client_cache = {} _parameter_type_mapping = [ "", @@ -72,14 +73,51 @@ def init(node: Node, timeout_sec: float | int = DEFAULT_PARAM_TIMEOUT_SEC): Initializes params module with a rclpy.node.Node for further use. This function has to be called before any other for the module to work. """ - global _node, _timeout_sec + global _node, _timeout_sec, _client_cache _node = node + _client_cache = {} if not isinstance(timeout_sec, (int, float)) or timeout_sec <= 0: raise ValueError("Parameter timeout must be a positive number") _timeout_sec = timeout_sec +def _get_or_create_client(service_type, service_name): + """Get existing client from cache or create new one""" + global _client_cache + + cache_key = (service_type, service_name) + + if cache_key in _client_cache: + client = _client_cache[cache_key] + if client.service_is_ready(): + return client + else: + _node.destroy_client(client) + del _client_cache[cache_key] + + client = _node.create_client( + service_type, + service_name, + callback_group=MutuallyExclusiveCallbackGroup(), + ) + + if client.service_is_ready(): + _client_cache[cache_key] = client + return client + else: + _node.destroy_client(client) + raise Exception(f"Service {service_name} is not available") + + +def clear_client_cache(): + """Clear all cached clients""" + global _client_cache + for client in _client_cache.values(): + _node.destroy_client(client) + _client_cache = {} + + async def set_param(node_name: str, name: str, value: str, params_glob: list[str]): """Sets a parameter in a given node""" @@ -120,15 +158,8 @@ async def _set_param(node_name: str, name: str, value: str, parameter_type=None) setattr(parameter.value, _parameter_type_mapping[parameter_type], loads(value)) assert _node is not None - client = _node.create_client( - SetParameters, - f"{node_name}/set_parameters", - callback_group=MutuallyExclusiveCallbackGroup(), - ) - if not client.service_is_ready(): - _node.destroy_client(client) - raise Exception(f"Service {client.srv_name} is not available") + client = _get_or_create_client(SetParameters, f"{node_name}/set_parameters") request = SetParameters.Request() request.parameters = [parameter] @@ -137,8 +168,6 @@ async def _set_param(node_name: str, name: str, value: str, parameter_type=None) await futures_wait_for(_node, [future], _timeout_sec) - _node.destroy_client(client) - if not future.done(): future.cancel() raise Exception("Timeout occurred") @@ -171,15 +200,8 @@ async def _get_param(node_name: str, name: str) -> ParameterValue: """Internal helper function for get_param""" assert _node is not None - client = _node.create_client( - GetParameters, - f"{node_name}/get_parameters", - callback_group=MutuallyExclusiveCallbackGroup(), - ) - if not client.service_is_ready(): - _node.destroy_client(client) - raise Exception(f"Service {client.srv_name} is not available") + client = _get_or_create_client(GetParameters, f"{node_name}/get_parameters") request = GetParameters.Request() request.names = [name] @@ -188,8 +210,6 @@ async def _get_param(node_name: str, name: str) -> ParameterValue: await futures_wait_for(_node, [future], _timeout_sec) - _node.destroy_client(client) - if not future.done(): future.cancel() raise Exception("Timeout occurred")