diff --git a/urx/urrobot.py b/urx/urrobot.py index 94c87f5..8d3579b 100644 --- a/urx/urrobot.py +++ b/urx/urrobot.py @@ -86,6 +86,15 @@ def send_program(self, prog): self.logger.info("Sending program: " + prog) self.secmon.send_program(prog) + def send_custom_program(self, prog): + """ + send a complete program using urscript to the robot + the program is executed immediatly and any runnning + program is interrupted + """ + self.logger.info("Sending program: " + prog) + self.rtmon.send_program(prog) + def get_tcp_force(self, wait=True): """ return measured force in TCP @@ -263,8 +272,8 @@ def speedx(self, command, velocities, acc, min_time): vels = [round(i, self.max_float_length) for i in velocities] vels.append(acc) vels.append(min_time) - prog = "{}([{},{},{},{},{},{}], a={}, t_min={})".format(command, *vels) - self.send_program(prog) + prog = "{}([{},{},{},{},{},{}],{},{})".format(command, *vels) + self.send_custom_program(prog) def movej(self, joints, acc=0.1, vel=0.05, wait=True, relative=False, threshold=None): """ diff --git a/urx/urrtmon.py b/urx/urrtmon.py index 9c08133..d9141fe 100644 --- a/urx/urrtmon.py +++ b/urx/urrtmon.py @@ -10,6 +10,7 @@ import time import threading from copy import deepcopy +from urx.ursecmon import Program import numpy as np @@ -55,6 +56,8 @@ def __init__(self, urHost): self._buffer = [] self._csys = None self._csys_lock = threading.Lock() + self._prog_queue = [] + self._prog_queue_lock = threading.Lock() def set_csys(self, csys): with self._csys_lock: @@ -195,6 +198,28 @@ def __recv_rt_data(self): with self._dataEvent: self._dataEvent.notifyAll() + def __send_rt_data(self): + with self._prog_queue_lock: + while len(self._prog_queue) > 0: + data = self._prog_queue.pop() + self._rtSock.send(data.program) + with data.condition: + data.condition.notify_all() + + def send_program(self, prog): + """ + send program to robot in URRobot format + If another program is send while a program is running the first program is aborded. + """ + prog.strip() + if not isinstance(prog, bytes): + prog = prog.encode() + data = Program(prog + b"\n") + with data.condition: + with self._prog_queue_lock: + self._prog_queue.append(data) + data.condition.wait() + def start_buffering(self): """ start buffering all data from controller @@ -260,4 +285,5 @@ def run(self): self._rtSock.connect((self._urHost, 30003)) while not self._stop_event: self.__recv_rt_data() + self.__send_rt_data() self._rtSock.close()