From 8024b51868b9ac49d96f2627e2a895df247edb73 Mon Sep 17 00:00:00 2001
From: ipa-fxm <felix.messmer@ipa.fhg.de>
Date: Tue, 30 Sep 2014 12:27:30 +0000
Subject: [PATCH] modify io-interface
---
ur_driver/src/ur_driver/io_interface.py | 53 ++++++++++++++-----------
ur_driver/test_io.py | 16 ++++----
2 files changed, 39 insertions(+), 30 deletions(-)
diff --git a/ur_driver/src/ur_driver/io_interface.py b/ur_driver/src/ur_driver/io_interface.py
index 3e0a44c..e78c0b4 100644
--- a/ur_driver/src/ur_driver/io_interface.py
+++ b/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)
diff --git a/ur_driver/test_io.py b/ur_driver/test_io.py
index a38be3c..7b3c2a8 100755
--- a/ur_driver/test_io.py
+++ b/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)
--
GitLab