From 06284c20876021c8a5e123c81daa8b025917421c Mon Sep 17 00:00:00 2001
From: Tristan Schnell <schnell@fzi.de>
Date: Thu, 26 Sep 2019 15:47:24 +0200
Subject: [PATCH] added basic action node for an IO integration test

---
 ur_rtde_driver/scripts/io_test.py | 40 +++++++++++++++++++++++++++++++
 1 file changed, 40 insertions(+)
 create mode 100755 ur_rtde_driver/scripts/io_test.py

diff --git a/ur_rtde_driver/scripts/io_test.py b/ur_rtde_driver/scripts/io_test.py
new file mode 100755
index 0000000..0d6d861
--- /dev/null
+++ b/ur_rtde_driver/scripts/io_test.py
@@ -0,0 +1,40 @@
+#!/usr/bin/env python
+
+import rospy
+from ur_msgs.srv import SetIO
+from ur_msgs.msg import IOStates
+
+def main():
+    rospy.init_node('io_testing_client')
+    service_client = rospy.ServiceProxy('/ur_hardware_interface/set_io', SetIO)
+    service_client.wait_for_service()
+
+    maximum_messages = 5
+    pin = 0
+
+    service_client(1, pin, 0)
+    messages = 0
+    pin_state = True
+
+    while(pin_state):
+        if messages >= 5:
+            return False
+        io_state = rospy.wait_for_message('/ur_hardware_interface/io_states', IOStates)
+        pin_state = io_state.digital_out_states[pin].state
+        messages += 1
+
+    service_client(1, pin, 1)
+    messages = 0
+    pin_state = False
+
+    while(not pin_state):
+        if messages >= 5:
+            return False
+        io_state = rospy.wait_for_message('/ur_hardware_interface/io_states', IOStates)
+        pin_state = io_state.digital_out_states[pin].state
+        messages += 1
+
+    return True
+
+if __name__ == '__main__':
+    print(main())