diff --git a/ur5_driver/driver.py b/ur5_driver/driver.py
index 602f3301a961cf00ccd125adac7052258d77f6e2..8e070c53befbb1d4f108580d4ba7c701b810f555 100755
--- a/ur5_driver/driver.py
+++ b/ur5_driver/driver.py
@@ -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
diff --git a/ur5_driver/prog b/ur5_driver/prog
index 428ff093fa051c5dfe1d657b3da6294596a059d1..b93b30dc53240a032cc6bc132bf0d50ce8b0aa7d 100644
--- a/ur5_driver/prog
+++ b/ur5_driver/prog
@@ -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