diff --git a/ur5_driver/driver.py b/ur5_driver/driver.py
index ee35862d1e3f68b8e6252d39d0cfc093f6600540..9473d6be2e30ce5b1df80d2233d3f8e4dae21c71 100755
--- a/ur5_driver/driver.py
+++ b/ur5_driver/driver.py
@@ -1,6 +1,6 @@
#!/usr/bin/env python
import roslib; roslib.load_manifest('ur5_driver')
-import time, sys
+import time, sys, threading
import socket
import struct
import SocketServer
@@ -16,6 +16,53 @@ MULT_jointstate = 1000.0
class EOF(Exception): pass
+# Only handles JOINT_STATES messages.
+class StateTCPHandler(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"
+ 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_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"
+
class CommanderTCPHandler(SocketServer.BaseRequestHandler):
def recv_more(self):
more = self.request.recv(4096)
@@ -80,7 +127,24 @@ def main():
sock.sendall(fin.read())
server = TCPServer(("", 50001), CommanderTCPHandler)
- server.handle_request()
+ server2 = TCPServer(("", 50002), StateTCPHandler)
+
+ thread_commander = threading.Thread(name="CommanderHandler", target=server.handle_request)
+ thread_state = threading.Thread(name="StateHandler", target=server2.handle_request)
+ thread_state.daemon = True
+ thread_commander.start()
+ thread_state.start()
+ thread_commander.join()
+ thread_state.join()
+
+ print "Dump"
+ print "="*70
+ o = ""
+ while len(o) < 4096:
+ r = sock.recv(1024)
+ if not r: break
+ o = o + r
+ print o
if __name__ == '__main__': main()
diff --git a/ur5_driver/prog b/ur5_driver/prog
index 46e1da36c205c2bc7ea11399db24249a1d7e1fee..428ff093fa051c5dfe1d657b3da6294596a059d1 100644
--- a/ur5_driver/prog
+++ b/ur5_driver/prog
@@ -1,41 +1,51 @@
def driverProg():
+ HOSTNAME = "10.0.2.97"
MSG_OUT = 1
MSG_QUIT = 2
MSG_JOINT_STATES = 3
MULT_jointstate = 1000
-
- def send_joint_state():
- q = get_joint_positions()
- qdot = get_joint_speeds()
- tau = get_joint_torques()
- socket_send_int(MSG_JOINT_STATES)
- socket_send_int(floor(MULT_jointstate * q[0]))
- socket_send_int(floor(MULT_jointstate * q[1]))
- socket_send_int(floor(MULT_jointstate * q[2]))
- socket_send_int(floor(MULT_jointstate * q[3]))
- socket_send_int(floor(MULT_jointstate * q[4]))
- socket_send_int(floor(MULT_jointstate * q[5]))
- socket_send_int(floor(MULT_jointstate * qdot[0]))
- socket_send_int(floor(MULT_jointstate * qdot[1]))
- socket_send_int(floor(MULT_jointstate * qdot[2]))
- socket_send_int(floor(MULT_jointstate * qdot[3]))
- socket_send_int(floor(MULT_jointstate * qdot[4]))
- socket_send_int(floor(MULT_jointstate * qdot[5]))
- socket_send_int(floor(MULT_jointstate * tau[0]))
- socket_send_int(floor(MULT_jointstate * tau[1]))
- 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]))
- end
+
- socket_open("10.0.2.97", 50001)
+ thread statePublisherThread():
+ def send_joint_state():
+ q = get_joint_positions()
+ qdot = get_joint_speeds()
+ tau = get_joint_torques()
+ socket_send_int(MSG_JOINT_STATES)
+ socket_send_int(floor(MULT_jointstate * q[0]))
+ socket_send_int(floor(MULT_jointstate * q[1]))
+ socket_send_int(floor(MULT_jointstate * q[2]))
+ socket_send_int(floor(MULT_jointstate * q[3]))
+ socket_send_int(floor(MULT_jointstate * q[4]))
+ socket_send_int(floor(MULT_jointstate * q[5]))
+ socket_send_int(floor(MULT_jointstate * qdot[0]))
+ socket_send_int(floor(MULT_jointstate * qdot[1]))
+ socket_send_int(floor(MULT_jointstate * qdot[2]))
+ socket_send_int(floor(MULT_jointstate * qdot[3]))
+ socket_send_int(floor(MULT_jointstate * qdot[4]))
+ socket_send_int(floor(MULT_jointstate * qdot[5]))
+ socket_send_int(floor(MULT_jointstate * tau[0]))
+ socket_send_int(floor(MULT_jointstate * tau[1]))
+ 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]))
+ end
+ socket_open(HOSTNAME, 50002)
+ send_joint_state()
+ while True:
+ send_joint_state()
+ sync()
+ end
+ sync()
+ end
+
+ socket_open(HOSTNAME, 50001)
socket_send_int(1)
socket_send_string("hello~")
- send_joint_state()
- #socket_send_int(q[0]
- #M = 1000
+ thread_state = run statePublisherThread()
+
movej([2.2,0,-1.57,0,0,0],2)
movej([1.5,0,-1.57,0,0,0],2)
@@ -46,6 +56,8 @@ def driverProg():
# i = i + 1
#end
+ sleep(1)
+ kill thread_state
socket_send_int(2)
end
driverProg()