Skip to content
Snippets Groups Projects
Commit 5c34345a authored by Stuart Glaser's avatar Stuart Glaser
Browse files

URScript only allows a single socket. Reduced the driver to use a single TCP listener

parent b6ad81dd
Branches
Tags
No related merge requests found
Hide whitespace changes
Inline Side-by-side
ur5_driver/driver.py
+ 19
− 60
View file @ 5c34345a
......@@ -23,27 +23,29 @@ pub_joint_states = rospy.Publisher('joint_states', JointState)
class EOF(Exception): pass
# Only handles JOINT_STATES messages.
class StateTCPHandler(SocketServer.BaseRequestHandler):
dump_state = open('dump_state', 'wb')
# Receives messages from the robot over the socket
class CommanderTCPHandler(SocketServer.BaseRequestHandler):
def recv_more(self):
more = self.request.recv(4096)
if not more:
raise EOF()
return more
def handle(self):
print "Handling a joint states connection"
print "Handling a request"
try:
buf = self.recv_more()
if not buf: return
while True:
print "Buf:", [ord(b) for b in buf]
#print "Buf:", [ord(b) for b in buf]
# Unpacks the message type
mtype = struct.unpack_from("!i", buf, 0)[0]
buf = buf[4:]
print "Message type:", mtype
#print "Message type:", mtype
if mtype == MSG_OUT:
# Unpacks string message, terminated by tilde
......@@ -61,7 +63,6 @@ class StateTCPHandler(SocketServer.BaseRequestHandler):
state_mult = struct.unpack_from("!" + ("i"*3*6), buf, 0)
buf = buf[3*6*4:]
state = [s / MULT_jointstate for s in state_mult]
print "Joint state:", state
msg = JointState()
msg.header.stamp = rospy.get_rostime()
......@@ -70,68 +71,29 @@ class StateTCPHandler(SocketServer.BaseRequestHandler):
msg.velocity = state[6:12]
msg.effort = state[12:18]
pub_joint_states.publish(msg)
else:
raise Exception("Unknown message type: %i" % mtype)
if not buf:
buf = buf + self.recv_more()
except EOF:
print "Connection closed"
class CommanderTCPHandler(SocketServer.BaseRequestHandler):
def recv_more(self):
more = self.request.recv(4096)
if not more:
raise EOF()
return more
def handle(self):
print "Handling a request"
try:
buf = self.recv_more()
if not buf: return
while True:
print "Buf:", [ord(b) for b in buf]
# Unpacks the message type
mtype = struct.unpack_from("!i", buf, 0)[0]
buf = buf[4:]
print "Message type:", mtype
if mtype == MSG_OUT:
# Unpacks string message, terminated by tilde
i = buf.find("~")
while i < 0:
buf = buf + self.recv_more()
i = buf.find("~")
if len(buf) > 2000:
raise Exception("Probably forgot to terminate a string: %s..." % buf[:150])
s, buf = buf[:i], buf[i+1:]
print "Out: %s" % s
elif mtype == MSG_QUIT:
print "Quitting"
sys.exit(0)
elif mtype == MSG_JOINT_STATES:
while len(buf) < 3*(6*4):
buf = buf + self.recv_more()
state_mult = struct.unpack_from("!" + ("i"*3*6), buf, 0)
buf = buf[3*6*4:]
state = [s / MULT_jointstate for s in state_mult]
print "Joint state:", state
else:
raise Exception("Unknown message type: %i" % mtype)
if not buf:
buf = buf + self.recv_more()
except EOF:
print "Connection closed"
print "Connection closed (command)"
class TCPServer(SocketServer.TCPServer):
allow_reuse_address = True
timeout = 5
# Waits until all threads have completed. Allows KeyboardInterrupt to occur
def joinAll(threads):
while any(t.isAlive() for t in threads):
for t in threads:
t.join(0.1)
sock = None
def main():
global sock
......@@ -143,16 +105,13 @@ def main():
sock.sendall(fin.read())
server = TCPServer(("", 50001), CommanderTCPHandler)
server_state = TCPServer(("", 50002), StateTCPHandler)
thread_commander = threading.Thread(name="CommanderHandler", target=server.handle_request)
thread_state = threading.Thread(name="StateHandler", target=server_state.handle_request)
thread_commander.daemon = True
thread_state.daemon = True
thread_commander.start()
thread_state.start()
thread_commander.join()
thread_state.join()
# Waits for the threads to finish
joinAll([thread_commander])
print "Dump"
print "="*70
......
ur5_driver/prog
+ 29
− 9
View file @ 5c34345a
......@@ -4,6 +4,14 @@ def driverProg():
MSG_QUIT = 2
MSG_JOINT_STATES = 3
MULT_jointstate = 1000
def send_out(msg):
enter_critical
socket_send_int(1)
socket_send_string(msg)
socket_send_string("~")
exit_critical
end
thread statePublisherThread():
......@@ -11,6 +19,7 @@ def driverProg():
q = get_joint_positions()
qdot = get_joint_speeds()
tau = get_joint_torques()
enter_critical
socket_send_int(MSG_JOINT_STATES)
socket_send_int(floor(MULT_jointstate * q[0]))
socket_send_int(floor(MULT_jointstate * q[1]))
......@@ -29,9 +38,11 @@ def driverProg():
socket_send_int(floor(MULT_jointstate * tau[2]))
socket_send_int(floor(MULT_jointstate * tau[3]))
socket_send_int(floor(MULT_jointstate * tau[4]))
socket_send_int(floor(MULT_jointstate * tau[5]))
#socket_send_int(floor(MULT_jointstate * tau[5]))
socket_send_int(7895160) # Recognizable ".xxx" or 00787878
exit_critical
end
socket_open(HOSTNAME, 50002)
#socket_open(HOSTNAME, 50002)
send_joint_state()
while True:
send_joint_state()
......@@ -49,14 +60,23 @@ def driverProg():
movej([2.2,0,-1.57,0,0,0],2)
movej([1.5,0,-1.57,0,0,0],2)
#i = 0
#while i < 5:
# movej([2.2,0,-1.57,0,0,0],2)
# movej([1.5,0,-1.57,0,0,0],2)
# i = i + 1
#end
i = 0
while i < 5:
send_out("Listening")
ll = socket_read_binary_integer(1)
mtype = ll[1]
if mtype == MSG_QUIT:
send_out("Received QUIT")
break
else:
send_out("Received unknown message type")
end
#movej([2.2,0,-1.57,0,0,0],2)
#movej([1.5,0,-1.57,0,0,0],2)
i = i + 1
end
sleep(1)
#sleep(1)
kill thread_state
socket_send_int(2)
end
......
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