text: # Copyright (c) 2018, The SenseAct Authors.
  # All rights reserved.
  #
  # This source code is licensed under the BSD-style license found in the
  # LICENSE file in the root directory of this source tree.
  import socket
  import time
  import sys
  import logging
  import numpy as np
  from senseact.communicator import Communicator
  from senseact.devices.ur import ur_utils
  from senseact.sharedbuffer import SharedBuffer
  from threading import Lock
  
  # Setup logging
  logging.basicConfig(format='%(asctime)s %(levelname)s %(message)s',
                      level=logging.DEBUG,
                      stream=sys.stdout)
  
  class URCommunicator(Communicator):
      """Communicator class for UR5 robot.
  
      This class implements UR-robot specific packet communicator.
      The packets it receives from UR-robot is of type
      `ur_utils.REALTIME_COMM_PACKET`. The actuator commands it allows
      sending currently are:
      - servoj
      - speedj
      - movej
      - movel
      - stopj
      - unlock protective stop.
      """
  
      def __init__(self, host,
                   actuation_sync_period=True,
                   disable_nagle_algorithm=True,
                   speedj_timeout = 0.5,
                   buffer_len=None
                   ):
          """Inits URCommunicator class with device- and task-specific parameters.
  
          Args:
              host: a string specifying UR5 Controller IP address
              actuation_sync_period: a boolean specifying whther to sync sending
                  actuations to UR5 with receiving sensory packets from UR5
              disable_nagle_algorithm: a boolean specifying a parameter in
                  socket class
              speedj_timeout: a float specifying time-out on a speedj command in s
              buffer_len: an integer length of sensor and actuation buffers
          """
          self._host = host
          self._disable_nagle_algorithm = disable_nagle_algorithm
          self._actuation_sync_period = actuation_sync_period
          self._speedj_timeout = speedj_timeout
  
          # The number of sensor reads since the last actuator write
          self._num_reads = 0
          self._num_read_lock = Lock()
  
          self._buffer_len = buffer_len
          if buffer_len is None:
              self._buffer_len = SharedBuffer.DEFAULT_BUFFER_LEN
  
          sensor_args = {'buffer_len': self._buffer_len,
                         'array_len': ur_utils.REALTIME_COMM_PACKET_SIZE,
                         'array_type': 'b',
                         'np_array_type': ur_utils.REALTIME_COMM_PACKET,
                         }
  
          actuator_args = {'buffer_len': self._buffer_len,
                           'array_len': 1 + max([x['size'] for x in ur_utils.COMMANDS.values()]),
                           'array_type': 'd',
                           'np_array_type': 'd',
                           }
  
          super(URCommunicator, self).__init__(use_sensor=True,
                                               use_actuator=True,
                                               sensor_args=sensor_args,
                                               actuator_args=actuator_args)
  
          # make connections
          self._sock = URCommunicator.make_connection(
              self._host,
              port=ur_utils.REALTIME_COMM_CLIENT_INTERFACE_PORT,
              disable_nagle_algorithm=self._disable_nagle_algorithm
          )
          logging.debug('Socket Info: {}'.format(self._sock))
          self._sock.settimeout(0.2)
  
          self._dashboard_sock = URCommunicator.make_connection(
              host=self._host,
              port=ur_utils.DASHBOARD_SERVER_PORT,
              disable_nagle_algorithm = self._disable_nagle_algorithm
          )
          time.sleep(0.5)
  
          self._start_time = time.time()
          self._recv_time = self._start_time
          self._prev_recv_time = self._start_time
  
          # DEBUG:
          logging.debug('Start time: {}'.format(self._start_time))
  
          # This flag is specific to the `actuator_handle`, used to stop it from sending
          # actuation command in the event of a lost connection, which is detected and
          # re-established from `_sensor_handler`. This is the only way the sensor and
          # the actuator threads communicate.
          self._stop = False
  
      def _sensor_handler(self):
          """Receives and stores sensory packets from UR5.
  
          Waits for packets to arrive from UR-robot through `socket.recv` call,
          checks for any delay in transmission, and stores the packet in `sensor_buffer`.
          This method also handles re-establishing of a lost connection.
  
          Raises:
              IOError, ValueError - data convertion errors.
          """
  
          try:
              data = self._sock.recv(ur_utils.REALTIME_COMM_PACKET_SIZE)
              self._recv_time = time.time()  # time after receiving packet
  
              # To fix CB2 issue
              data = data.ljust(ur_utils.REALTIME_COMM_PACKET.itemsize, b'\0')
  
              # check and parse received packet
              self.pre_check(data)
              
              parsed = np.frombuffer(data, dtype=ur_utils.REALTIME_COMM_PACKET)
  
              self.sensor_buffer.write(parsed)
  
              self._prev_recv_time = self._recv_time
              self._num_read_lock.acquire()
              self._num_reads += 1
              self._num_read_lock.release()
          except (IOError, ValueError) as e:
              if isinstance(e, ValueError):
                  # May happen with weak wireless connection
                  logging.debug('', e, "Could not convert data.")
              else:
                  logging.debug('', e, ': Lost socket to UR, going into reconnect loop')
              self._stop = True
              time.sleep(ur_utils.ACTUATOR_DT*2)  # to let _actuator_handler thread return
              self._sock.close()
              self._dashboard_sock.close()
              self._sock = self.make_connection(self._host,
                                                ur_utils.REALTIME_COMM_CLIENT_INTERFACE_PORT,
                                                disable_nagle_algorithm=self._disable_nagle_algorithm
                                                )
              self._dashboard_sock = self.make_connection(self._host,
                                                          ur_utils.DASHBOARD_SERVER_PORT,
                                                          disable_nagle_algorithm = self._disable_nagle_algorithm
                                                          )
              self._stop = False
          except Exception as e:
              import sys
              logging.debug("Unexpected error:", e, sys.exc_info()[0])
              raise
  
      def _actuator_handler(self):
          """Sends actuation commands to UR5.
  
          Waits for `actuator_buffer` being updated.
          If the connection is lost, does not do anything.
          Otherwise, proceeds with sending the command corresponding to
          the content of `actuator_buffer`.
          """
  
          # sleep until ready to send an actuation command
          # the ready condition depends on the `actuator_sync_period`
          while self._actuator_running:
              if self._actuation_sync_period > 0:
                  # Synchronized to sensor readings, so check to see if the correct
                  # number of readings have been detected to break out of the loop
                  self._num_read_lock.acquire()
                  if self._num_reads >= self._actuation_sync_period:
                      self._num_reads = 0
                      self._num_read_lock.release()
                      break
                  self._num_read_lock.release()
              else:
                  # Not synchronized to sensor readings, so break out of the loop
                  # and actuate a command as soon as the buffer is updated
                  if self.actuator_buffer.updated():
                      break
              time.sleep(1e-4)
  
          # Do not perform an actuation if the socket connection is broken
          # or the actuator should no longer be running
          if self._stop or not self._actuator_running:
              return
  
          # obtain actuation command if it is ready
          # or a read was just performed and an action needs to be sent
          if self.actuator_buffer.updated() or self._actuation_sync_period > 0:
              # keep track of the updated flag as it will be set to 0 below
              updated = self.actuator_buffer.updated()
              self._num_read_lock.acquire()
              self._num_reads = 0
              self._num_read_lock.release()
              recent_actuation, time_stamp, _ = self.actuator_buffer.read_update()
              recent_actuation = recent_actuation[0]
              if recent_actuation[0] == ur_utils.COMMANDS['SERVOJ']['id']:
                  servoj_values = ur_utils.COMMANDS['SERVOJ']
                  cmd = ur_utils.ServoJ(
                      q=recent_actuation[1:1 + servoj_values['size'] - 3],
                      t=servoj_values['default']['t']
                      if recent_actuation[-3] == ur_utils.USE_DEFAULT else recent_actuation[-3],
                      lookahead_time=servoj_values['default']['lookahead_time']
                      if recent_actuation[-2] == ur_utils.USE_DEFAULT else recent_actuation[-2],
                      gain=servoj_values['default']['gain']
                      if recent_actuation[-1] == ur_utils.USE_DEFAULT else recent_actuation[-1]
                  )
              elif recent_actuation[0] == ur_utils.COMMANDS['SPEEDJ']['id']:
                  if time.time() - time_stamp[-1] > self._speedj_timeout:
                      return
                  speedj_values = ur_utils.COMMANDS['SPEEDJ']
                  cmd = ur_utils.SpeedJ(
                      qd=recent_actuation[1:1 + speedj_values['size'] - 2],
                      a=speedj_values['default']['a']
                      if recent_actuation[-2] == ur_utils.USE_DEFAULT else recent_actuation[-2],
                      t_min=speedj_values['default']['t_min']
                      if recent_actuation[-1] == ur_utils.USE_DEFAULT else recent_actuation[-1],
                  )
              elif not updated:
                  # The commands below this point should only be executed
                  # if they are new actuations
                  return
              elif recent_actuation[0] == ur_utils.COMMANDS['MOVEL']['id']:
                  movel_values = ur_utils.COMMANDS['MOVEL']
                  cmd = ur_utils.MoveL(
                      pose=recent_actuation[1:1 + movel_values['size'] - 4],
                      a=movel_values['default']['a']
                      if recent_actuation[-4] == ur_utils.USE_DEFAULT else recent_actuation[-4],
                      v=movel_values['default']['v']
                      if recent_actuation[-3] == ur_utils.USE_DEFAULT else recent_actuation[-3],
                      t=movel_values['default']['t']
                      if recent_actuation[-2] == ur_utils.USE_DEFAULT else recent_actuation[-2],
                      r=movel_values['default']['r']
                      if recent_actuation[-1] == ur_utils.USE_DEFAULT else recent_actuation[-1],
                  )
              elif recent_actuation[0] == ur_utils.COMMANDS['MOVEJ']['id']:
                  movej_values = ur_utils.COMMANDS['MOVEJ']
                  cmd = ur_utils.MoveJ(
                      q=recent_actuation[1:1 + movej_values['size'] - 4],
                      a=movej_values['default']['a']
                      if recent_actuation[-4] == ur_utils.USE_DEFAULT else recent_actuation[-4],
                      v=movej_values['default']['v']
                      if recent_actuation[-3] == ur_utils.USE_DEFAULT else recent_actuation[-3],
                      t=movej_values['default']['t']
                      if recent_actuation[-2] == ur_utils.USE_DEFAULT else recent_actuation[-2],
                      r=movej_values['default']['r']
                      if recent_actuation[-1] == ur_utils.USE_DEFAULT else recent_actuation[-1],
                  )
              elif recent_actuation[0] == ur_utils.COMMANDS['STOPJ']['id']:
                  cmd = ur_utils.StopJ(
                      a=recent_actuation[1]
                  )
              elif recent_actuation[0] == ur_utils.COMMANDS['UNLOCK_PSTOP']['id']:
                  logging.debug("Unlocking p-stop")
                  self._dashboard_sock.send('unlock protective stop\n'.encode('ascii'))
                  return
              elif recent_actuation[0] == ur_utils.COMMANDS['NOTHING']['id']:
                  return
              else:
                  raise NotImplementedError
              cmd_str = '{}\n'.format(cmd)
              self._sock.send(cmd_str.encode('ascii'))
  
      def pre_check(self, data):
          """Checks time and completeness of packet reception.
  
          Args:
              data: a numpy array with sensory information received from UR5
          """
          if self._recv_time > self._prev_recv_time + 1.1 / 125:
              logging.debug(
                  '{}: Hiccup of {:.2f}ms overhead between UR packets)'.format(
                      self._recv_time - self._start_time,
                      (self._recv_time - self._prev_recv_time - 0.008) * 1000,
                  ))
              # DEBUG:
              logging.debug('Start time: {}'.format(self._start_time))
          if len(data) != ur_utils.REALTIME_COMM_PACKET.itemsize:
              logging.debug('Warning: incomplete packet from UR')
              return
  
      @staticmethod
      def make_connection(host, port, disable_nagle_algorithm):
          """Establishes a TCP/IP socket connection with a UR5 controller.
  
          Args:
              host: a string specifying UR5 Controller IP address
              port: a string specifying UR5 Controller port
              disable_nagle_algorithm: a boolean specifying whether to
                  disable nagle algorithm
  
          Returns:
               None or TCP socket connected to UR5 device.
          """
          sock = None
          for res in socket.getaddrinfo(host,
                                        port,
                                        socket.AF_UNSPEC,
                                        socket.SOCK_STREAM):
              afam, socktype, proto, canonname, sock_addr = res
              del canonname
              try:
                  sock = socket.socket(afam, socktype, proto)
                  if disable_nagle_algorithm:
                      sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, True)
              except OSError as msg:
                  logging.debug(msg)
                  sock = None
                  continue
              try:
                  sock.connect(sock_addr)
              except OSError as msg:
                  logging.debug(msg)
                  sock.close()
                  sock = None
                  continue
              break
          return sock
  