Moved everything to rostests

ur_rtde_driver/test/io_test.py

@@ -0,0 +1,68 @@
+#!/usr/bin/env python
+import sys
+import time
+import unittest
+
+
+import rospy
+import rostopic
+
+
+PKG = 'ur_rtde_driver'
+NAME = 'io_test'
+
+
+from ur_msgs.srv import SetIO
+from ur_msgs.msg import IOStates
+
+
+class IOTest(unittest.TestCase):
+    def __init__(self, *args):
+        super(IOTest, self).__init__(*args)
+        rospy.init_node('io_test')
+
+        timeout = 10
+
+        self.service_client = rospy.ServiceProxy('/ur_hardware_interface/set_io', SetIO)
+        try:
+            self.service_client.wait_for_service(timeout)
+        except rospy.exceptions.ROSException as err:
+            self.fail(
+                "Could not reach SetIO service. Make sure that the driver is actually running."
+                " Msg: {}".format(err))
+
+    def test_set_io(self):
+        """Test to set an IO and check whether it has been set."""
+
+        maximum_messages = 5
+        pin = 0
+        self.assertEqual(maximum_messages, 5)
+
+        self.service_client(1, pin, 0)
+        messages = 0
+        pin_state = True
+
+        while(pin_state):
+            if messages >= maximum_messages:
+                self.fail("Could not read desired state after {} messages.".format(maximum_messages))
+            io_state = rospy.wait_for_message('/ur_hardware_interface/io_states', IOStates)
+            pin_state = io_state.digital_out_states[pin].state
+            messages += 1
+        self.assertEqual(pin_state, 0)
+
+        self.service_client(1, pin, 1)
+        messages = 0
+        pin_state = False
+
+        while(not pin_state):
+            if messages >= maximum_messages:
+                self.fail("Could not read desired state after {} messages.".format(maximum_messages))
+            io_state = rospy.wait_for_message('/ur_hardware_interface/io_states', IOStates)
+            pin_state = io_state.digital_out_states[pin].state
+            messages += 1
+        self.assertEqual(pin_state, 1)
+
+
+if __name__ == '__main__':
+    import rostest
+    rostest.run(PKG, NAME, IOTest, sys.argv)