Change speedx method to RT data stream EXAMPLE

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):
         """

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