text: import logging
  import socket
  import struct
  import time
  import threading
  from copy import deepcopy
  
  import numpy as np
  
  class URRTMonitor(threading.Thread):
  
      # Struct for revision of the UR controller giving 692 bytes
      rtstruct692 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dQ')
  
      # for revision of the UR controller giving 540 byte. Here TCP
      # pose is not included!
      rtstruct540 = struct.Struct('>d6d6d6d6d6d6d6d6d18d')
  
      def __init__(self, urHost):
          threading.Thread.__init__(self)
          self.logger = logging.getLogger(self.__class__.__name__)
          self.daemon = True
          self._stop_event = True
          self._dataEvent = threading.Condition()
          self._dataAccess = threading.Lock()
          self._rtSock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
          self._rtSock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
          self._urHost = urHost
          # Package data variables
          self._timestamp = None
          self._ctrlTimestamp = None
          self._qActual = None
          self._qTarget = None
          self._tcp = None
          self._tcp_force = None
          self.__recvTime = 0
          self._last_ctrl_ts = 0
          # self._last_ts = 0
          self._buffering = False
          self._buffer_lock = threading.Lock()
          self._buffer = []
  
      def __recv_bytes(self, nBytes):
          ''' Facility method for receiving exactly "nBytes" bytes from
          the robot_s connector socket.'''
          # Record the time of arrival of the first of the stream block
          recvTime = 0
          pkg = b''
          while len(pkg) < nBytes:
              pkg += self._rtSock.recv(nBytes - len(pkg))
              if recvTime == 0:
                  recvTime = time.time()
          self.__recvTime = recvTime
          return pkg
  
      def wait(self):
          with self._dataEvent:
              self._dataEvent.wait()
  
      def q_actual(self, wait=False, timestamp=False):
          """ Get the actual joint position vector."""
          if wait:
              self.wait()
          with self._dataAccess:
              if timestamp:
                  return self._timestamp, self._qActual
              else:
                  return self._qActual
      getActual = q_actual
  
      def qd_actual(self, wait=False, timestamp=False):
          """ Get the actual joint velocity vector."""
          if wait:
              self.wait()
          with self._dataAccess:
              if timestamp:
                  return self._timestamp, self._qdActual
              else:
                  return self._qdActual
  
      def q_target(self, wait=False, timestamp=False):
          """ Get the target joint position vector."""
          if wait:
              self.wait()
          with self._dataAccess:
              if timestamp:
                  return self._timestamp, self._qTarget
              else:
                  return self._qTarget
      getTarget = q_target
  
      def tcf_pose(self, wait=False, timestamp=False, ctrlTimestamp=False):
          """ Return the tool pose values."""
          if wait:
              self.wait()
          with self._dataAccess:
              tcf = self._tcp
              if ctrlTimestamp or timestamp:
                  ret = [tcf]
                  if timestamp:
                      ret.insert(-1, self._timestamp)
                  if ctrlTimestamp:
                      ret.insert(-1, self._ctrlTimestamp)
                  return ret
              else:
                  return tcf
      getTCF = tcf_pose
  
      def tcf_force(self, wait=False, timestamp=False):
          """ Get the tool force. The returned tool force is a
          six-vector of three forces and three moments."""
          if wait:
              self.wait()
          with self._dataAccess:
              # tcf = self._fwkin(self._qActual)
              tcf_force = self._tcp_force
              if timestamp:
                  return self._timestamp, tcf_force
              else:
                  return tcf_force
      getTCFForce = tcf_force
  
      def __recv_rt_data(self):
          head = self.__recv_bytes(4)
          # Record the timestamp for this logical package
          timestamp = self.__recvTime
          pkgsize = struct.unpack('>i', head)[0]
          self.logger.debug(
              'Received header telling that package is %s bytes long',
              pkgsize)
          payload = self.__recv_bytes(pkgsize - 4)
          if pkgsize >= 692:
              unp = self.rtstruct692.unpack(payload[:self.rtstruct692.size])
          elif pkgsize >= 540:
              unp = self.rtstruct540.unpack(payload[:self.rtstruct540.size])
          else:
              self.logger.warning(
                  'Error, Received packet of length smaller than 540: %s ',
                  pkgsize)
              return
  
          with self._dataAccess:
              self._timestamp = timestamp
              # it seems that packet often arrives packed as two... maybe TCP_NODELAY is not set on UR controller??
              # if (self._timestamp - self._last_ts) > 0.010:
              # self.logger.warning("Error the we did not receive a packet for {}s ".format( self._timestamp - self._last_ts))
              # self._last_ts = self._timestamp
              self._ctrlTimestamp = np.array(unp[0])
              if self._last_ctrl_ts != 0 and (
                      self._ctrlTimestamp -
                      self._last_ctrl_ts) > 0.010:
                  self.logger.warning(
                      "Error the controller failed to send us a packet: time since last packet %s s ",
                      self._ctrlTimestamp - self._last_ctrl_ts)
              self._last_ctrl_ts = self._ctrlTimestamp
              self._qActual = np.array(unp[31:37])
              self._qdActual = np.array(unp[37:43])
              self._qTarget = np.array(unp[1:7])
              self._tcp_force = np.array(unp[67:73])
              self._tcp = np.array(unp[73:79])
  
          if self._buffering:
              with self._buffer_lock:
                  self._buffer.append(
                      (self._timestamp,
                       self._ctrlTimestamp,
                       self._tcp,
                       self._qActual))  # FIXME use named arrays of allow to configure what data to buffer
  
          with self._dataEvent:
              self._dataEvent.notifyAll()
  
      def start_buffering(self):
          """
          start buffering all data from controller
          """
          self._buffer = []
          self._buffering = True
  
      def stop_buffering(self):
          self._buffering = False
  
      def try_pop_buffer(self):
          """
          return oldest value in buffer
          """
          with self._buffer_lock:
              if len(self._buffer) > 0:
                  return self._buffer.pop(0)
              else:
                  return None
  
      def pop_buffer(self):
          """
          return oldest value in buffer
          """
          while True:
              with self._buffer_lock:
                  if len(self._buffer) > 0:
                      return self._buffer.pop(0)
              time.sleep(0.001)
  
      def get_buffer(self):
          """
          return a copy of the entire buffer
          """
          with self._buffer_lock:
              return deepcopy(self._buffer)
  
      def get_all_data(self, wait=True):
          """
          return all data parsed from robot_s as a dict
          """
          if wait:
              self.wait()
          with self._dataAccess:
              return dict(
                  timestamp=self._timestamp,
                  ctrltimestamp=self._ctrlTimestamp,
                  qActual=self._qActual,
                  qTarget=self._qTarget,
                  tcp=self._tcp,
                  tcp_force=self._tcp_force)
  
      def stop(self):
          # print(self.__class__.__name__+': Stopping')
          self._stop_event = True
  
      def close(self):
          self.stop()
          self.join()
  
      def run(self):
          self._stop_event = False
          self._rtSock.connect((self._urHost, 30003))
          while not self._stop_event:
              self.__recv_rt_data()
          self._rtSock.close()
  