modify io-interface

ur_driver/src/ur_driver/io_interface.py

@@ -10,46 +10,47 @@ FUN_SET_FLAG = 2
 FUN_SET_ANALOG_OUT = 3
 FUN_SET_TOOL_VOLTAGE = 4
 
-Flag_States = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
-Digital_Out_States = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
-Digital_In_States = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
-Analog_Out_States = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
-Analog_In_States = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
+#Flag_States = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
+Digital_Out_States = [0,0,0,0,0,0,0,0,0,0]  #8(controller)+2(tool)
+Digital_In_States = [0,0,0,0,0,0,0,0,0,0]   #8(controller)+2(tool)
+Analog_Out_States = [0,0]  #2(controller)
+Analog_In_States = [0,0]   #2(controller)+0(tool)
 
 ANALOG_TOLERANCE_VALUE = 0.01
-DELAY_TIME = 0.3
-i = 0
-set_io = rospy.ServiceProxy('set_io', SetIO)
 
 def set_io_val(fun, pin, val):
-    rospy.wait_for_service('set_io')
     try:
         set_io(fun, pin, val)
     except rospy.ServiceException, e:
         print "Service call failed: %s"%e
 
 def set_tool_voltage(volts):
-    set_io_val(FUN_SET_TOOL_VOLTAGE, volts, 0)
+    try:
+        set_io(FUN_SET_TOOL_VOLTAGE, volts, 0)
+    except rospy.ServiceException, e:
+        print "Service call failed: %s"%e
 
 def set_digital_out(pin, val):
-    set_io_val(FUN_SET_DIGITAL_OUT, pin, val)
-    if Digital_Out_States[pin] != val:
-        rospy.logwarn("DIGITAL OUT IS NOT A HAPPY CAMPER, its not setting a pin on the first try, it may help to increase IO_SLEEP_TIME in driver.py")
-        set_digital_out(pin, val)
+    try:
+        set_io(FUN_SET_DIGITAL_OUT, pin, val)
+    except rospy.ServiceException, e:
+        print "Service call failed: %s"%e
 
 def set_analog_out(pin, val):
-    set_io_val(FUN_SET_ANALOG_OUT, pin, val)
-    if abs(Analog_Out_States[pin] - val) > ANALOG_TOLERANCE_VALUE:
-        rospy.logwarn("ANALOG OUT IS NOT A HAPPY CAMPER, its not setting a pin on the first try, it may help to increase IO_SLEEP_TIME in driver.py")
-        set_digital_out(pin, val)
+    try:
+        set_io(FUN_SET_ANALOG_OUT, pin, val)
+    except rospy.ServiceException, e:
+        print "Service call failed: %s"%e
 
 def set_flag(pin, val):
-    set_io_val(FUN_SET_FLAG, pin, val)
-    if Flag_States[pin] != val:
-        rospy.logwarn("SETTING A FLAG IS NOT A HAPPY CAMPER, its not setting a pin on the first try, it may help to increase IO_SLEEP_TIME in driver.py")
-        set_digital_out(pin, val)
+    rospy.logerr("SETTING FLAGS IS NOT SUPPORTED!")
+    #try:
+        #set_io(FUN_SET_FLAG, pin, val)
+    #except rospy.ServiceException, e:
+        #print "Service call failed: %s"%e
 
 def callback(data):
+    rospy.logerr("Flag_States are currently not supported")
     #for i in range(0,32):
         #del Flag_States[i]
         #Flag_States.insert(i, data.flag_states[i].state)
@@ -62,11 +63,17 @@ def callback(data):
     for i in range(0,2):
         del Analog_Out_States[i]
         Analog_Out_States.insert(i, data.analog_out_states[i].state)
-    for i in range(0,4):
+    rospy.logerr("ToolInput analog_in[2] and analog_in[3] currently not supported")
+    for i in range(0,2):
         del Analog_In_States[i]
         Analog_In_States.insert(i, data.analog_in_states[i].state)
 
 def get_states():
     rospy.init_node('UR_State_Getter')
     rospy.Subscriber("io_states", IOStates, callback)
+    
+def set_states():
+    rospy.wait_for_service('set_io')
+    global set_io
+    set_io = rospy.ServiceProxy('set_io', SetIO)
 

ur_driver/test_io.py

@@ -3,25 +3,27 @@ import time
 from ur_driver.io_interface import *
 
 if __name__ == "__main__":
-    print "toggling things"
+    print "testing io-interface"
     get_states()
     print "listener has been activated"
+    set_states()
+    print "service-server has been started"
     while(True):
         set_tool_voltage(12)
         set_digital_out(0, True)
         set_analog_out(0, 0.75)
-        print "Flags are currently not supported"
-        #set_flag(0, True)
-        #print(Flag_States[0])
+        #print "Flags are currently not supported"
+        ##set_flag(0, True)
+        ##print(Flag_States[0])
         print(Analog_Out_States[0])
         print(Digital_Out_States[0])
         time.sleep(1)
         set_tool_voltage(24)
         set_digital_out(0, False)
         set_analog_out(0, 0.25)
-        print "Flags are currently not supported"
-        #set_flag(0, False)
-        #print(Flag_States[0])
+        #print "Flags are currently not supported"
+        ##set_flag(0, False)
+        ##print(Flag_States[0])
         print(Analog_Out_States[0])
         print(Digital_Out_States[0])
         time.sleep(1)