Add call service timeout tests to rosbridge_library

Author: bjsowa

rosbridge_library/test/capabilities/test_call_service.py

@@ -24,6 +24,13 @@ def trigger_cb(self, request, response):
         response.message = "called trigger service successfully"
         return response
 
+    def trigger_long_cb(self, request, response):
+        """Helper callback function for a long running test service with no arguments."""
+        time.sleep(0.5)
+        response.success = True
+        response.message = "called trigger service successfully"
+        return response
+
     def set_bool_cb(self, request, response):
         """Helper callback function for a test service with arguments."""
         response.success = request.data
@@ -51,6 +58,12 @@ def setUp(self):
             self.trigger_cb,
             callback_group=self.cb_group,
         )
+        self.trigger_long_srv = self.node.create_service(
+            Trigger,
+            self.node.get_name() + "/trigger_long",
+            self.trigger_long_cb,
+            callback_group=self.cb_group,
+        )
         self.set_bool_srv = self.node.create_service(
             SetBool,
             self.node.get_name() + "/set_bool",
@@ -63,8 +76,9 @@ def setUp(self):
 
     def tearDown(self):
         self.executor.remove_node(self.node)
-        self.node.destroy_node()
         self.executor.shutdown()
+        self.exec_thread.join()
+        self.node.destroy_node()
         rclpy.shutdown()
 
     def test_missing_arguments(self):
@@ -99,12 +113,6 @@ def cb(msg, cid=None, compression="none"):
 
         s.call_service(send_msg)
 
-        timeout = 1.0
-        start = time.time()
-        while time.time() - start < timeout:
-            if received["arrived"]:
-                break
-
         self.assertTrue(received["arrived"])
         values = received["msg"]["values"]
         self.assertEqual(values["success"], True)
@@ -136,12 +144,6 @@ def cb(msg, cid=None, compression="none"):
 
         s.call_service(send_msg)
 
-        timeout = 1.0
-        start = time.time()
-        while time.time() - start < timeout:
-            if received["arrived"]:
-                break
-
         self.assertTrue(received["arrived"])
         values = received["msg"]["values"]
         self.assertEqual(values["success"], True)
@@ -175,11 +177,45 @@ def cb(msg, cid=None, compression="none"):
 
         s.call_service(send_msg)
 
-        timeout = 1.0
-        start = time.time()
-        while time.time() - start < timeout:
-            if received["arrived"]:
-                break
+        self.assertTrue(received["arrived"])
+        self.assertFalse(received["msg"]["result"])
+
+    def test_call_service_timeout(self):
+
+        client = self.node.create_client(Trigger, self.trigger_long_srv.srv_name)
+        assert client.wait_for_service(1.0)
+
+        proto = Protocol("test_call_service_timeout", self.node)
+        s = CallService(proto)
+        send_msg = loads(
+            dumps({"op": "call_service", "service": self.trigger_long_srv.srv_name, "timeout": 2.0})
+        )
+
+        received = {"msg": None, "arrived": False}
+
+        def cb(msg, cid=None, compression="none"):
+            print("Received message")
+            received["msg"] = msg
+            received["arrived"] = True
+
+        proto.send = cb
+
+        s.call_service(send_msg)
+
+        self.assertTrue(received["arrived"])
+        self.assertTrue(received["msg"]["result"])
+        values = received["msg"]["values"]
+        self.assertEqual(values["success"], True)
+        self.assertEqual(values["message"], "called trigger service successfully")
+
+        send_msg = loads(
+            dumps({"op": "call_service", "service": self.trigger_long_srv.srv_name, "timeout": 0.1})
+        )
+        received = {"msg": None, "arrived": False}
+
+        s.call_service(send_msg)
 
         self.assertTrue(received["arrived"])
         self.assertFalse(received["msg"]["result"])
+        values = received["msg"]["values"]
+        self.assertEqual(values, "Timeout exceeded while waiting for service response")

rosbridge_library/test/capabilities/test_service_capabilities.py

@@ -150,6 +150,62 @@ def test_call_advertised_service(self):
         self.assertEqual(self.received_message["op"], "service_response")
         self.assertTrue(self.received_message["result"])
 
+    def test_call_advertised_service_with_timeout(self):
+        # Advertise the service
+        service_path = "/set_bool_3"
+        advertise_msg = loads(
+            dumps(
+                {
+                    "op": "advertise_service",
+                    "type": "std_srvs/SetBool",
+                    "service": service_path,
+                }
+            )
+        )
+        self.received_message = None
+        self.advertise.advertise_service(advertise_msg)
+
+        # Call the advertised service using rosbridge
+        self.received_message = None
+        call_msg = loads(
+            dumps(
+                {
+                    "op": "call_service",
+                    "id": "foo",
+                    "service": service_path,
+                    "args": {"data": True},
+                    "timeout": 0.5,
+                }
+            )
+        )
+        Thread(target=self.call_service.call_service, args=(call_msg,)).start()
+
+        start_time = time.monotonic()
+        while self.received_message is None:
+            rclpy.spin_once(self.node, timeout_sec=0.1)
+            if time.monotonic() - start_time > 0.3:
+                self.fail("Timed out waiting for service call message.")
+
+        self.assertFalse(self.received_message is None)
+        self.assertTrue("op" in self.received_message)
+        self.assertEqual(self.received_message["op"], "call_service")
+        self.assertTrue("id" in self.received_message)
+
+        self.received_message = None
+
+        start_time = time.monotonic()
+        while self.received_message is None:
+            rclpy.spin_once(self.node, timeout_sec=0.1)
+            if time.monotonic() - start_time > 1.0:
+                self.fail("Timed out waiting for service response message.")
+
+        self.assertFalse(self.received_message is None)
+        self.assertEqual(self.received_message["op"], "service_response")
+        self.assertFalse(self.received_message["result"])
+        self.assertEqual(
+            self.received_message["values"], "Timeout exceeded while waiting for service response"
+        )
+
     def test_unadvertise_with_live_request(self):
         # Advertise the service
         service_path = "/set_bool_3"