Skip to content
Snippets Groups Projects
Commit 26d35e97 authored by Andrew Glusiec's avatar Andrew Glusiec Committed by ipa-fxm
Browse files

testing

parent 9a160fc5
Branches
Tags
No related merge requests found
......@@ -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, queue_size=1)
pub_wrench = rospy.Publisher('wrench', WrenchStamped, queue_size=1)
pub_io_states = rospy.Publisher('io_states', IOStates, queue_size=1)
#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():
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment