add prototype fake robot, more debug logging

README

@@ -15,9 +15,9 @@ rob.translate((0.1, 0, 0), a, v) #move tool and keep orientation
 rob.orient((0.1, 0, 0), a, v) #orient tool and keep tool tip position
 rob.stopj(a)
 
-robot.movel(x,y,z,a,b,c), wait=False)
+robot.movel(x,y,z,rx,ry,rz), wait=False)
-while True :
 sleep(0.1) #sleep first since the information may be outdated
+while True :
     if robot.is_program_running():
         break
 

tools/fakerobot.py

@@ -0,0 +1,65 @@
+import socket
+import threading
+import socketserver
+import time
+
+class RequestHandler(socketserver.BaseRequestHandler):
+    #def __init__(self, *args, **kwargs):
+        #print(self, *args, **kwargs)
+        #print("Got connection from {}".format( self.client_address[0]) )
+        #socketserver.BaseRequestHandler.__init__(self, *args, **kwargs)
+
+    def handle(self):
+        try:
+            data = str(self.request.recv(1024), 'ascii')
+        except Exception as ex:
+            print("Got exception", ex)
+        else:
+            cur_thread = threading.current_thread()
+            print(cur_thread.name, "received: ", data, )
+
+    def setup(self):
+        print("Got new connection from {}".format( self.client_address) )
+        self.server.handlers.append(self)
+
+    def finish(self):
+        print("Connection from {} lost".format( self.client_address) )
+        self.server.handlers.remove(self)
+
+class Server(socketserver.ThreadingMixIn, socketserver.TCPServer):
+    def init(self):
+        """
+        __init__ should not be overriden
+        """
+        self.handlers = []
+
+    def cleanup(self):
+        for handler in self.handlers:
+            handler.request.shutdown(socket.SHUT_RDWR)
+            handler.request.close()
+        self.shutdown()
+
+
+
+
+if __name__ == "__main__":
+    # Port 0 means to select an arbitrary unused port
+    host, port = "localhost", 30002 # this is the standard secondary port for a UR robot
+
+    server = Server((host, port), RequestHandler)
+    server.init()
+    server_thread = threading.Thread(target=server.serve_forever)
+    server_thread.daemon = True
+    server_thread.start()
+    print("Fake Universal robot running at ", host, port)
+    try:
+        f = open("packet.bin", "rb")
+        packet = f.read()
+        f.close()
+        while True:
+            time.sleep(0.09) #The real robot published data 10 times a second
+            for handler in server.handlers:
+                handler.request.sendall(packet)
+    finally:
+        print("Shutting down server")
+        server.cleanup()

tools/get_rob.py

@@ -0,0 +1,17 @@
+#!/usr/bin/env python
+from urx import Robot
+import math3d
+from math import pi
+import logging
+
+if __name__ == "__main__":
+    try:
+        robot = Robot( 'localhost')#, logLevel=logging.DEBUG, parserLogLevel=logging.DEBUG)
+        r = robot
+        from IPython.frontend.terminal.embed import InteractiveShellEmbed
+        ipshell = InteractiveShellEmbed( banner1="\nStarting IPython shell, robot object is available\n")
+        ipshell(local_ns=locals())
+    finally:
+        if "robot" in dir():
+            robot.cleanup()
+

urx/ursecmon.py

@@ -11,8 +11,6 @@ __author__ = "Olivier Roulet-Dubonnet"
 __copyright__ = "Copyright 2011-2012, Olivier Roulet-Dubonnet"
 __credits__ = ["Olivier Roulet-Dubonnet"]
 __license__ = "GPLv3"
-__version__ = "0.4"
-__status__ = "Development"
 
 from threading import Thread, Condition, Lock
 import logging
@@ -159,12 +157,17 @@ class ParserUtils(object):
         returns None if none found
         """
         counter = 0
+        limit = 10
         while True:
             if len(data) >= 5:
                 psize, ptype = self.get_header(data)
                 if psize < 5 or psize > 2000 or ptype != 16:
                     data = data[1:]
                     counter += 1
+                    if counter > limit:
+                        self.logger.warn("tried {} times to find a packet in data, advertised packet size: {}, type: {}".format(counter, psize, ptype))
+                        self.logger.warn("Data length: {}".format(len(data)))
+                        limit = limit * 10
                 elif len(data) >= psize:
                     self.logger.debug("Got packet with size {0} and type {1}".format(psize, ptype))
                     if counter:
@@ -265,6 +268,7 @@ class SecondaryMonitor(Thread):
         returns something that looks like a packet, nothing is guaranted
         """
         while True:
+            #self.logger.debug("data queue size is: {}".format(len(self._dataqueue)))
             ans = self._parser.find_first_packet(self._dataqueue[:])
             if ans:
                 self._dataqueue = ans[1]

urx/urx.py

@@ -43,7 +43,7 @@ class URRobot(object):
     The RT interfaces is only used for the get_force related methods
     Rmq: A program sent to the robot i executed immendiatly and any running program is stopped
     """
-    def __init__(self, host, useRTInterface=False, logLevel=logging.WARN):
+    def __init__(self, host, useRTInterface=False, logLevel=logging.WARN, parserLogLevel=logging.WARN):
         self.logger = logging.getLogger(self.__class__.__name__)
         if len(logging.root.handlers) == 0: #dirty hack
             logging.basicConfig()
@@ -51,7 +51,7 @@ class URRobot(object):
         self.host = host
         
         self.logger.info("Opening secondary monitor socket")
-        self.secmon = ursecmon.SecondaryMonitor(self.host, logLevel=logLevel, parserLogLevel=logging.WARN) #data from robot at 10Hz
+        self.secmon = ursecmon.SecondaryMonitor(self.host, logLevel=logLevel, parserLogLevel=parserLogLevel) #data from robot at 10Hz
         
         if useRTInterface:
             self.logger.info("Opening real-time monitor socket")
@@ -381,8 +381,8 @@ class Robot(URRobot):
     Compare to the URRobot class, this class adds the possibilty to work directly with matrices
     and includes support for calibrating the robot coordinate system
     """
-    def __init__(self, host, useRTInterface=False, logLevel = logging.WARN):
+    def __init__(self, host, useRTInterface=False, logLevel = logging.WARN, parserLogLevel=logging.WARN):
-        URRobot.__init__(self, host, useRTInterface, logLevel=logLevel)
+        URRobot.__init__(self, host, useRTInterface, logLevel=logLevel, parserLogLevel=parserLogLevel)
         self.default_linear_acceleration = 0.01
         self.default_linear_velocity = 0.01