add graceful_shutdown() method to advertise_service capability

This gives the service a bit of time to cancel any in-flight service requests (which should fix #265).
This is important because we busy-wait for a rosbridge response for service calls and those threads do not get stopped otherwise.
Also, rospy service clients do not currently support timeouts, so any clients would be stuck too.

A new test case in test_service_capabilities.py verifies the fix works

rosbridge_library/src/rosbridge_library/capabilities/advertise_service.py

@@ -1,18 +1,23 @@
+import fnmatch
+from threading import Lock
+import time
+
 from rosbridge_library.internal.ros_loader import get_service_class
 from rosbridge_library.internal import message_conversion
 from rosbridge_library.capability import Capability
 from rosbridge_library.util import string_types
-import fnmatch
-import rospy
-import time
 
+import rospy
 
 class AdvertisedServiceHandler():
 
     id_counter = 1
     responses = {}
 
     def __init__(self, service_name, service_type, protocol):
+        self.active_requests = 0
+        self.shutdown_requested = False
+        self.lock = Lock()
         self.service_name = service_name
         self.service_type = service_type
         self.protocol = protocol
@@ -25,6 +30,8 @@ def next_id(self):
         return id
 
     def handle_request(self, req):
+        with self.lock:
+            self.active_requests += 1
         # generate a unique ID
         request_id = "service_request:" + self.service_name + ":" + str(self.next_id())
 
@@ -39,12 +46,38 @@ def handle_request(self, req):
 
         # wait for a response
         while request_id not in self.responses.keys():
+            with self.lock:
+                if self.shutdown_requested:
+                    break
             time.sleep(0)
 
+        with self.lock:
+            self.active_requests -= 1
+
+            if self.shutdown_requested:
+                self.protocol.log(
+                    "warning",
+                    "Service %s was unadvertised with a service call in progress, "
+                    "aborting service call with request ID %s" % (self.service_name, request_id))
+                return None
+
         resp = self.responses[request_id]
         del self.responses[request_id]
         return resp
 
+    def graceful_shutdown(self, timeout):
+        """
+        Signal the AdvertisedServiceHandler to shutdown
+
+        Using this, rather than just rospy.Service.shutdown(), allows us
+        time to stop any active service requests, ending their busy wait
+        loops.
+        """
+        with self.lock:
+            self.shutdown_requested = True
+        start_time = time.clock()
+        while time.clock() - start_time < timeout:
+            time.sleep(0)
 
 class AdvertiseService(Capability):
     services_glob = None

rosbridge_library/src/rosbridge_library/capabilities/unadvertise_service.py

@@ -35,6 +35,7 @@ def unadvertise_service(self, message):
 
         # unregister service in ROS
         if service_name in self.protocol.external_service_list.keys():
+            self.protocol.external_service_list[service_name].graceful_shutdown(timeout=1.0)
             self.protocol.external_service_list[service_name].service_handle.shutdown("Unadvertise request.")
             del self.protocol.external_service_list[service_name]
             self.protocol.log("info", "Unadvertised service %s." % service_name)

rosbridge_library/test/capabilities/test_service_capabilities.py

@@ -128,6 +128,54 @@ def test_call_advertised_service(self):
         self.assertEqual(self.received_message["op"], "service_response")
         self.assertTrue(self.received_message["result"])
 
+    def test_unadvertise_with_live_request(self):
+        service_path = "/set_bool_3"
+        advertise_msg = loads(dumps({"op": "advertise_service",
+                                     "type": "std_srvs/SetBool",
+                                     "service": service_path}))
+        self.advertise.advertise_service(advertise_msg)
+
+        # Call the service via rosbridge because rospy.ServiceProxy.call() is
+        # blocking
+        call_service = CallService(self.proto)
+        call_service.call_service(loads(dumps({"op": "call_service",
+                                               "id": "foo",
+                                               "service": service_path,
+                                               "args": [True]})))
+
+        loop_iterations = 0
+        while self.received_message is None:
+            rospy.sleep(rospy.Duration(0.5))
+            loop_iterations += 1
+            if loop_iterations > 3:
+                self.fail("did not receive service call rosbridge message "
+                          "after waiting 2 seconds")
+
+        self.assertFalse(self.received_message is None)
+        self.assertTrue("op" in self.received_message)
+        self.assertTrue(self.received_message["op"] == "call_service")
+        self.assertTrue("id" in self.received_message)
+
+        # Now send the response
+        response_msg = loads(dumps({"op": "unadvertise_service",
+                                    "service": service_path}))
+        self.received_message = None
+        self.unadvertise.unadvertise_service(response_msg)
+
+        loop_iterations = 0
+        while self.received_message is None:
+            rospy.sleep(rospy.Duration(0.5))
+            loop_iterations += 1
+            if loop_iterations > 3:
+                self.fail("did not receive service response rosbridge message "
+                          "after waiting 2 seconds")
+
+        self.assertFalse(self.received_message is None)
+        # Rosbridge should abort the existing service call with an error
+        # (i.e. "result" should be False)
+        self.assertEqual(self.received_message["op"], "service_response")
+        self.assertFalse(self.received_message["result"])
+
 
 PKG = 'rosbridge_library'
 NAME = 'test_service_capabilities'