Successfully printing joint states of the robot

ur5_driver/driver.py

 #!/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()

ur5_driver/prog

 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()