Add timeout option to call_service messages

ROSBRIDGE_PROTOCOL.md

@@ -436,7 +436,8 @@ Calls a ROS service.
   "service": <string>,
   (optional) "args": <list<json>>,
   (optional) "fragment_size": <int>,
-  (optional) "compression": <string>
+  (optional) "compression": <string>,
+  (optional) "timeout": <float>
 }
 ```
 
@@ -449,6 +450,7 @@ Calls a ROS service.
     before it is fragmented
  * **compression** – an optional string to specify the compression scheme to be
     used on messages. Valid values are "none" and "png"
+ * **timeout** – the time, in seconds, to wait for a response from the server
 
 
 Stops advertising an external ROS service server

rosbridge_library/src/rosbridge_library/capabilities/call_service.py

@@ -81,6 +81,7 @@ def call_service(self, message):
         fragment_size = message.get("fragment_size", None)
         compression = message.get("compression", "none")
         args = message.get("args", [])
+        timeout = message.get("timeout", 5.0)
 
         if CallService.services_glob is not None and CallService.services_glob:
             self.protocol.log(
@@ -112,7 +113,14 @@ def call_service(self, message):
         e_cb = partial(self._failure, cid, service)
 
         # Run service caller in the same thread.
-        ServiceCaller(trim_servicename(service), args, s_cb, e_cb, self.protocol.node_handle).run()
+        ServiceCaller(
+            trim_servicename(service),
+            args,
+            timeout,
+            s_cb,
+            e_cb,
+            self.protocol.node_handle,
+        ).run()
 
     def _success(self, cid, service, fragment_size, compression, message):
         outgoing_message = {

rosbridge_library/src/rosbridge_library/internal/services.py

@@ -30,11 +30,9 @@
 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 
-import time
-from threading import Thread
+from threading import Condition, Thread
 from typing import Any, Callable, Optional
 
-import rclpy
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.expand_topic_name import expand_topic_name
 from rclpy.node import Node
@@ -58,6 +56,7 @@ def __init__(
         self,
         service: str,
         args: dict,
+        timeout: float,
         success_callback: Callable[[dict], None],
         error_callback: Callable[[Exception], None],
         node_handle: Node,
@@ -71,6 +70,8 @@ def __init__(
         ordered list, or a dict of name-value pairs.  Anything else will be
         treated as though no arguments were provided (which is still valid for
         some kinds of service)
+        timeout          -- the time, in seconds, to wait for a response from the server.
+                            A non-positive value means no timeout.
         success_callback -- a callback to call with the JSON result of the
         service call
         error_callback   -- a callback to call if an error occurs.  The
@@ -81,14 +82,22 @@ def __init__(
         self.daemon = True
         self.service = service
         self.args = args
+        self.timeout = timeout
         self.success = success_callback
         self.error = error_callback
         self.node_handle = node_handle
 
     def run(self) -> None:
         try:
             # Call the service and pass the result to the success handler
-            self.success(call_service(self.node_handle, self.service, args=self.args))
+            self.success(
+                call_service(
+                    self.node_handle,
+                    self.service,
+                    args=self.args,
+                    server_response_timeout=self.timeout,
+                )
+            )
         except Exception as e:
             # On error, just pass the exception to the error handler
             self.error(e)
@@ -114,8 +123,8 @@ def call_service(
     node_handle: Node,
     service: str,
     args: Optional[dict] = None,
-    server_timeout_time: float = 1.0,
-    sleep_time: float = 0.001,
+    server_ready_timeout: float = 1.0,
+    server_response_timeout: float = 5.0,
 ) -> dict:
     # Given the service name, fetch the type and class of the service,
     # and a request instance
@@ -141,20 +150,35 @@ def call_service(
         service_class, service, callback_group=ReentrantCallbackGroup()
     )
 
-    if not client.wait_for_service(server_timeout_time):
+    if not client.wait_for_service(server_ready_timeout):
         node_handle.destroy_client(client)
         raise InvalidServiceException(service)
 
     future = client.call_async(inst)
-    while rclpy.ok() and not future.done():
-        time.sleep(sleep_time)
+    condition = Condition()
+
+    def future_done_callback():
+        with condition:
+            condition.notify_all()
+
+    future.add_done_callback(lambda future: future_done_callback())
+
+    with condition:
+        if not condition.wait_for(
+            lambda: future.done(),
+            timeout=(server_response_timeout if server_response_timeout > 0 else None),
+        ):
+            future.cancel()
+            node_handle.destroy_client(client)
+            raise Exception("Timeout exceeded while waiting for service response")
+
     result = future.result()
 
     node_handle.destroy_client(client)
     if result is not None:
         # Turn the response into JSON and pass to the callback
         json_response = extract_values(result)
     else:
-        raise Exception(result)
+        raise Exception("Service call returned None")
 
     return json_response

rosbridge_library/test/internal/services/test_services.py

@@ -52,6 +52,7 @@ def start(self):
         thread = services.ServiceCaller(
             self.name,
             gen,
+            5.0,
             self.success,
             self.error,
             self.node,
@@ -201,6 +202,7 @@ def error():
         services.ServiceCaller(
             self.node.get_name() + "/list_parameters",
             None,
+            5.0,
             success,
             error,
             self.node,