From 57ddbb3178dcb3266e035dd4771a0e1860b9e252 Mon Sep 17 00:00:00 2001
From: Andrew Glusiec <airplanesrule@gmail.com>
Date: Wed, 9 Jul 2014 10:34:34 -0400
Subject: [PATCH] testing
---
ur_driver/src/ur_driver/driver.py | 151 +++++++++++++++++++++++++++++-
1 file changed, 150 insertions(+), 1 deletion(-)
diff --git a/ur_driver/src/ur_driver/driver.py b/ur_driver/src/ur_driver/driver.py
index 1a63b55..d73ec98 100755
--- a/ur_driver/src/ur_driver/driver.py
+++ b/ur_driver/src/ur_driver/driver.py
@@ -19,6 +19,9 @@ from geometry_msgs.msg import WrenchStamped
from ur_driver.deserialize import RobotState, RobotMode
+from ur_driver.srv import SetIO
+from ur_driver.msg import *
+
prevent_programming = False
# Joint offsets, pulled from calibration information stored in the URDF
@@ -39,11 +42,23 @@ MSG_WAYPOINT_FINISHED = 5
MSG_STOPJ = 6
MSG_SERVOJ = 7
MSG_WRENCH = 9
+MSG_SET_DIGITAL_OUT = 10
+MSG_GET_IO = 11
+MSG_SET_FLAG = 12
+MSG_SET_TOOL_VOLTAGE = 13
+MSG_SET_ANALOG_OUT = 14
MULT_wrench = 10000.0
MULT_jointstate = 10000.0
MULT_time = 1000000.0
MULT_blend = 1000.0
+FUN_SET_DIGITAL_OUT = 1
+FUN_SET_FLAG = 2
+FUN_SET_ANALOG_OUT = 3
+FUN_SET_TOOL_VOLTAGE = 4
+
+IO_SLEEP_TIME = 0.05
+
JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
@@ -57,6 +72,7 @@ connected_robot_lock = threading.Lock()
connected_robot_cond = threading.Condition(connected_robot_lock)
pub_joint_states = rospy.Publisher('joint_states', JointState)
pub_wrench = rospy.Publisher('wrench', WrenchStamped)
+pub_io_states = rospy.Publisher('io_states', IOStates)
#dump_state = open('dump_state', 'wb')
class EOF(Exception): pass
@@ -321,6 +337,75 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
wrench_msg.wrench.torque.z = state[5]
pub_wrench.publish(wrench_msg)
+#gets all IO States and publishes them into a message
+ elif mtype == MSG_GET_IO:
+
+ #gets digital in states
+ while len(buf) < 4:
+ buf = buf + self.recv_more()
+ inp = struct.unpack_from("!i", buf, 0)[0]
+ buf = buf[4:]
+ msg = IOStates()
+ for i in range(0, 10):
+ msg.digital_in_states.append(DigitalIn(i, (inp & (1<<i))>>i))
+ #gets digital out states
+ while len(buf) < 4:
+ buf = buf + self.recv_more()
+ inp = struct.unpack_from("!i", buf, 0)[0]
+ buf = buf[4:]
+ for i in range(0, 10):
+ msg.digital_out_states.append(DigitalOut(i, (inp & (1<<i))>>i))
+ #gets flag states
+ while len(buf) < 4:
+ buf = buf + self.recv_more()
+ inp = struct.unpack_from("!i", buf, 0)[0]
+ buf = buf[4:]
+ for i in range(0, 32):
+ msg.flag_states.append(Flag(i, (inp & (1<<i))>>i))
+ #gets analog_out[0] state
+ while len(buf) < 4:
+ buf = buf + self.recv_more()
+ inp = struct.unpack_from("!i", buf, 0)[0]
+ inp /= 1000000.0
+ buf = buf[4:]
+ msg.analog_out_states.append(Analog(0, inp))
+ #gets analog_out[1] state
+ while len(buf) < 4:
+ buf = buf + self.recv_more()
+ inp = struct.unpack_from("!i", buf, 0)[0]
+ inp /= 1000000.0
+ buf = buf[4:]
+ msg.analog_out_states.append(Analog(1, inp))
+ #gets analog_in[0] state
+ while len(buf) < 4:
+ buf = buf + self.recv_more()
+ inp = struct.unpack_from("!i", buf, 0)[0]
+ inp /= 1000000.0
+ buf = buf[4:]
+ msg.analog_in_states.append(Analog(0, inp))
+ #gets analog_in[1] state
+ while len(buf) < 4:
+ buf = buf + self.recv_more()
+ inp = struct.unpack_from("!i", buf, 0)[0]
+ inp /= 1000000.0
+ buf = buf[4:]
+ msg.analog_in_states.append(Analog(1, inp))
+ #gets analog_in[2] state
+ while len(buf) < 4:
+ buf = buf + self.recv_more()
+ inp = struct.unpack_from("!i", buf, 0)[0]
+ inp /= 1000000.0
+ buf = buf[4:]
+ msg.analog_in_states.append(Analog(2, inp))
+ #gets analog_in[3] state
+ while len(buf) < 4:
+ buf = buf + self.recv_more()
+ inp = struct.unpack_from("!i", buf, 0)[0]
+ inp /= 1000000.0
+ buf = buf[4:]
+ msg.analog_in_states.append(Analog(3, inp))
+ pub_io_states.publish(msg)
+
elif mtype == MSG_QUIT:
print "Quitting"
raise EOF("Received quit")
@@ -354,7 +439,48 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
buf = struct.pack("!%ii" % len(params), *params)
with self.socket_lock:
self.request.send(buf)
-
+
+ #Experimental set_digital_output implementation
+ def set_digital_out(self, pinnum, value):
+ params = [MSG_SET_DIGITAL_OUT] + \
+ [pinnum] + \
+ [value]
+ buf = struct.pack("!%ii" % len(params), *params)
+ #print params
+ with self.socket_lock:
+ self.request.send(buf)
+ time.sleep(IO_SLEEP_TIME)
+
+ def set_analog_out(self, pinnum, value):
+ params = [MSG_SET_ANALOG_OUT] + \
+ [pinnum] + \
+ [value * 1000000]
+ buf = struct.pack("!%ii" % len(params), *params)
+ #print params
+ with self.socket_lock:
+ self.request.send(buf)
+ time.sleep(IO_SLEEP_TIME)
+
+ def set_tool_voltage(self, value):
+ params = [MSG_SET_TOOL_VOLTAGE] + \
+ [value] + \
+ [0]
+ buf = struct.pack("!%ii" % len(params), *params)
+ #print params
+ with self.socket_lock:
+ self.request.send(buf)
+ time.sleep(IO_SLEEP_TIME+.5)
+
+ def set_flag(self, pin, val):
+ params = [MSG_SET_FLAG] + \
+ [pin] + \
+ [val]
+ buf = struct.pack("!%ii" % len(params), *params)
+ #print params
+ with self.socket_lock:
+ self.request.send(buf)
+ #set_flag will fail if called too closely together--added delay
+ time.sleep(IO_SLEEP_TIME)
def send_stopj(self):
with self.socket_lock:
@@ -673,6 +799,27 @@ def get_my_ip(robot_ip, port):
s.close()
return tmp
+def handle_set_io(req):
+ r = getConnectedRobot(wait=False)
+ if r:
+ if req.fun == FUN_SET_DIGITAL_OUT:
+ r.set_digital_out(req.pin, req.state)
+ return True
+ elif req.fun == FUN_SET_FLAG:
+ r.set_flag(req.pin, req.state)
+ return True
+ elif req.fun == FUN_SET_ANALOG_OUT:
+ r.set_analog_out(req.pin, req.state)
+ return True
+ elif req.fun == FUN_SET_TOOL_VOLTAGE:
+ r.set_tool_voltage(req.pin)
+ return True
+ else:
+ raise ROSServiceException("Robot not connected")
+
+def set_io_server():
+ s= rospy.Service('set_io', SetIO, handle_set_io)
+
def main():
rospy.init_node('ur_driver', disable_signals=True)
if rospy.get_param("use_sim_time", False):
@@ -721,6 +868,8 @@ def main():
connection.connect()
connection.send_reset_program()
+ set_io_server()
+
action_server = None
try:
while not rospy.is_shutdown():
--
GitLab