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

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

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