diff --git a/rosbridge_library/CMakeLists.txt b/rosbridge_library/CMakeLists.txt
index c0c2c5680..3b8dbcc50 100644
--- a/rosbridge_library/CMakeLists.txt
+++ b/rosbridge_library/CMakeLists.txt
@@ -50,5 +50,6 @@ catkin_package(
 # Test launch files
 if (CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
-  add_rostest(test/test_all.test)
+  add_rostest(test/capabilities/test_capabilities.test)
+  add_rostest(test/internal/test_internal.test)
 endif()
diff --git a/rosbridge_library/test/capabilities/test_call_service.py b/rosbridge_library/test/capabilities/test_call_service.py
index 13cc2c68b..dff99483c 100755
--- a/rosbridge_library/test/capabilities/test_call_service.py
+++ b/rosbridge_library/test/capabilities/test_call_service.py
@@ -37,23 +37,29 @@ def test_call_service_works(self):
         # First, call the service the 'proper' way
         p = rospy.ServiceProxy("/rosout/get_loggers", GetLoggers)
         p.wait_for_service()
+        time.sleep(1.0)
         ret = p()
 
-
         proto = Protocol("test_call_service_works")
         s = CallService(proto)
         msg = loads(dumps({"op": "call_service", "service": "/rosout/get_loggers"}))
 
-        received = {"msg": None}
+        received = {"msg": None, "arrived": False}
 
         def cb(msg, cid=None):
             received["msg"] = msg
+            received["arrived"] = True
 
         proto.send = cb
 
         s.call_service(msg)
 
-        time.sleep(0.5)
+        timeout = 5.0
+        start = rospy.Time.now()
+        while rospy.Time.now() - start < rospy.Duration(timeout):
+            if received["arrived"]:
+                break
+            time.sleep(0.1)
 
         self.assertTrue(received["msg"]["result"])
         for x, y in zip(ret.loggers, received["msg"]["values"]["loggers"]):
@@ -65,16 +71,21 @@ def test_call_service_fail(self):
         s = CallService(proto)
         send_msg = loads(dumps({"op": "call_service", "service": "/rosout/set_logger_level", "args": '["ros", "invalid"]'}))
 
-        received = {"msg": None}
-
-        def cb(msg, cid=None): 
+        received = {"msg": None, "arrived": False}
+        def cb(msg, cid=None):
             received["msg"] = msg
+            received["arrived"] = True
 
         proto.send = cb
 
         s.call_service(send_msg)
 
-        time.sleep(0.5)
+        timeout = 5.0
+        start = rospy.Time.now()
+        while rospy.Time.now() - start < rospy.Duration(timeout):
+            if received["arrived"]:
+                break
+            time.sleep(0.1)
 
         self.assertFalse(received["msg"]["result"])
 
diff --git a/rosbridge_library/test/capabilities/test_subscribe.py b/rosbridge_library/test/capabilities/test_subscribe.py
index f1a6b602f..c181a3c92 100755
--- a/rosbridge_library/test/capabilities/test_subscribe.py
+++ b/rosbridge_library/test/capabilities/test_subscribe.py
@@ -108,7 +108,7 @@ def send(outgoing):
 
         sub.subscribe(loads(dumps({"op": "subscribe", "topic": topic, "type": msg_type})))
 
-        p = rospy.Publisher(topic, String)
+        p = rospy.Publisher(topic, String, queue_size=5)
         time.sleep(0.25)
         p.publish(msg)