From ddbd6e9ce1081d67e21d84adc53ed7181b369494 Mon Sep 17 00:00:00 2001 From: Olivier R-D Date: Sat, 20 Apr 2013 19:12:55 +0200 Subject: [PATCH] add prototype fake robot, more debug logging --- README | 4 +-- tools/fakerobot.py | 65 +++++++++++++++++++++++++++++++++++++++++++++ tools/get_rob.py | 17 ++++++++++++ tools/packet.bin | Bin 0 -> 510 bytes urx/ursecmon.py | 8 ++++-- urx/urx.py | 8 +++--- 6 files changed, 94 insertions(+), 8 deletions(-) create mode 100644 tools/fakerobot.py create mode 100644 tools/get_rob.py create mode 100644 tools/packet.bin diff --git a/README b/README index 6cbe030..4a49c38 100644 --- a/README +++ b/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) +sleep(0.1) #sleep first since the information may be outdated while True : - sleep(0.1) #sleep first since the information may be outdated if robot.is_program_running(): break diff --git a/tools/fakerobot.py b/tools/fakerobot.py new file mode 100644 index 0000000..0118e99 --- /dev/null +++ b/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() diff --git a/tools/get_rob.py b/tools/get_rob.py new file mode 100644 index 0000000..bc136a9 --- /dev/null +++ b/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() + diff --git a/tools/packet.bin b/tools/packet.bin new file mode 100644 index 0000000000000000000000000000000000000000..7cd79e12f5af161a9f83fb3de186d6d70c9dc9ab GIT binary patch literal 510 zcmZQzVEiY*z`!62#31mL1{H@MqVmX-)e}p4bP* zpLVu;eml*{j)B4PxUsR5Q(F4p15A=@q*~v<0MZ+|?^m3GnPFFDanZ>xEgh`Te&(#d z4$MkA{ss#`w0i7y2??0O{qOeWffa5`OLqb(-2eZBK=onO2QV6@(00WN53toIXU+mE zbYN!`lse(MA4p5iEWgpTznyi#4ZX7~Y`3blgB6|!y3qmTMurt)Q}26ifYC4$Z4w?N zfUQ2kz~JP;!0;C+Vaj5E^v*^(&Mkra_wWTD(K@`u{#Ml#hhV{4`!CPiOSEs<@4wlh zk^H3T!2Z`-#+O=uCIK}$Fhh*A_hg(P2}CdpFlom= 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] diff --git a/urx/urx.py b/urx/urx.py index 6156f66..ed3208e 100644 --- a/urx/urx.py +++ b/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): - URRobot.__init__(self, host, useRTInterface, logLevel=logLevel) + def __init__(self, host, useRTInterface=False, logLevel = logging.WARN, parserLogLevel=logging.WARN): + URRobot.__init__(self, host, useRTInterface, logLevel=logLevel, parserLogLevel=parserLogLevel) self.default_linear_acceleration = 0.01 self.default_linear_velocity = 0.01