diff --git a/rosapi/scripts/rosapi_node b/rosapi/scripts/rosapi_node index 01dde9e5..3ef910b7 100755 --- a/rosapi/scripts/rosapi_node +++ b/rosapi/scripts/rosapi_node @@ -78,6 +78,7 @@ class Rosapi(Node): self.declare_parameter("topics_glob", "[*]") self.declare_parameter("services_glob", "[*]") self.declare_parameter("params_glob", "[*]") + self.declare_parameter("params_timeout", 5.0) self.globs = self.get_globs() self.register_services() @@ -88,7 +89,9 @@ class Rosapi(Node): full_name = self.get_namespace() + self.get_name() else: full_name = self.get_namespace() + "/" + self.get_name() - params.init(full_name) + + timeout_sec = self.get_parameter("params_timeout").value + params.init(full_name, timeout_sec) self.create_service(Topics, "~/topics", self.get_topics) self.create_service(Interfaces, "~/interfaces", self.get_interfaces) diff --git a/rosapi/src/rosapi/params.py b/rosapi/src/rosapi/params.py index 36af6913..51513f14 100644 --- a/rosapi/src/rosapi/params.py +++ b/rosapi/src/rosapi/params.py @@ -45,10 +45,14 @@ """ Methods to interact with the param server. Values have to be passed as JSON in order to facilitate dynamically typed SRV messages """ +# Constants +DEFAULT_PARAM_TIMEOUT_SEC = 5.0 + # Ensure thread safety for setting / getting parameters. param_server_lock = threading.RLock() _node = None _parent_node_name = "" +_timeout_sec = DEFAULT_PARAM_TIMEOUT_SEC _parameter_type_mapping = [ "", @@ -64,12 +68,12 @@ ] -def init(parent_node_name): +def init(parent_node_name, timeout_sec=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, _parent_node_name + global _node, _parent_node_name, _timeout_sec # TODO(@jubeira): remove this node; use rosapi node with MultiThreadedExecutor or # async / await to prevent the service calls from blocking. parent_node_basename = parent_node_name.split("/")[-1] @@ -81,6 +85,10 @@ def init(parent_node_name): ) _parent_node_name = get_absolute_node_name(parent_node_name) + 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 set_param(node_name, name, value, params_glob): """Sets a parameter in a given node""" @@ -226,21 +234,20 @@ def _get_param_names(node_name): # This method is called in a service callback; calling a service of the same node # will cause a deadlock. global _parent_node_name - if node_name == _parent_node_name: + if node_name == _parent_node_name or node_name == _node.get_fully_qualified_name(): return [] client = _node.create_client(ListParameters, f"{node_name}/list_parameters") - ready = client.wait_for_service(timeout_sec=5.0) - if not ready: - raise RuntimeError("Wait for list_parameters service timed out") + if not client.service_is_ready(): + return [] request = ListParameters.Request() future = client.call_async(request) if _node.executor: - _node.executor.spin_until_future_complete(future) + _node.executor.spin_until_future_complete(future, timeout_sec=_timeout_sec) else: - rclpy.spin_until_future_complete(_node, future) + rclpy.spin_until_future_complete(_node, future, timeout_sec=_timeout_sec) response = future.result() if response is not None: diff --git a/rosbridge_server/launch/rosbridge_websocket_launch.xml b/rosbridge_server/launch/rosbridge_websocket_launch.xml index 8f8c6bd4..62d0411d 100644 --- a/rosbridge_server/launch/rosbridge_websocket_launch.xml +++ b/rosbridge_server/launch/rosbridge_websocket_launch.xml @@ -23,6 +23,7 @@ + @@ -78,5 +79,6 @@ +