class UR::Rtde

Constants

JOINTMODE
PROGRAMSTATE
PROTOCOL_VERSION
ROBOTMODE
SAFETYMODE
TOOLMODE

Public Class Methods

new(host, logger=Logger.new(STDOUT,level: :INFO)) click to toggle source
# File lib/rtde.rb, line 89
def initialize(host, logger=Logger.new(STDOUT,level: :INFO)) #{{{
  host = '//' + host if host !~ /\/\//
  uri = URI::parse(host)
  @logger = logger
  @hostname = uri.host
  @port = uri.port.nil? ? 30004 : uri.port
  @conn_state = ConnectionState::DISCONNECTED
  @sock = nil
  @output_config = nil
  @input_config = {}
end

Public Instance Methods

connect() click to toggle source
# File lib/rtde.rb, line 101
def connect #{{{
  return if @sock

  @buf = ''.b # buffer data in binary format
  begin
    @sock = Socket.new Socket::AF_INET, Socket::SOCK_STREAM
    @sock.setsockopt Socket::SOL_SOCKET, Socket::SO_REUSEADDR, 1
    @sock = TCPSocket.new(@hostname, @port)
    @conn_state = ConnectionState::CONNECTED
  rescue
    @sock = nil
    raise
  end
  if not negotiate_protocol_version
    raise RuntimeError.new 'Unable to negotiate protocol version'
  end
  self
end
connected?() click to toggle source
# File lib/rtde.rb, line 132
def connected? #{{{
  @conn_state != ConnectionState::DISCONNECTED
end
controller_version() click to toggle source
# File lib/rtde.rb, line 136
def controller_version #{{{
  cmd = Command::RTDE_GET_URCONTROL_VERSION
  version = send_and_receive cmd
  @logger.debug 'Controller Version: ' + version.major.to_s + '.' + version.minor.to_s + '.' + version.bugfix.to_s + '.' + version.build.to_s
  if version
    if version.major == 3 && version.minor <=2 && version.bugfix < 19171
      @logger.error 'Upgrade your controller to version higher than 3.2.19171'
      exit
    end
    [version.major, version.minor, version.bugfix, version.build]
  else
    [nil, nil, nil, nil]
  end
end
disconnect() click to toggle source
# File lib/rtde.rb, line 120
def disconnect #{{{
  if @sock
    @sock.close
    @sock = nil
    @conn_state = ConnectionState::DISCONNECTED
    @logger.info "Connection closed " + @hostname +":" + @port.to_s
    true
  else
    false
  end
end
has_data?() click to toggle source
# File lib/rtde.rb, line 338
def has_data? #{{{
  timeout = 0
  readable, _, _ = IO.select([@sock], [], [], timeout)
  readable.length != 0
end
negotiate_protocol_version() click to toggle source
# File lib/rtde.rb, line 151
def negotiate_protocol_version #{{{
  cmd = Command::RTDE_REQUEST_PROTOCOL_VERSION
  payload = [PROTOCOL_VERSION].pack 'S>'
  send_and_receive cmd, payload
end
on_packet(cmd, payload) click to toggle source
# File lib/rtde.rb, line 326
def on_packet(cmd, payload) #{{{
  return unpack_protocol_version_package(payload)     if cmd == Command::RTDE_REQUEST_PROTOCOL_VERSION
  return unpack_urcontrol_version_package(payload)    if cmd == Command::RTDE_GET_URCONTROL_VERSION
  return unpack_text_message(payload)                 if cmd == Command::RTDE_TEXT_MESSAGE
  return unpack_setup_outputs_package(payload)        if cmd == Command::RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS
  return unpack_setup_inputs_package(payload)         if cmd == Command::RTDE_CONTROL_PACKAGE_SETUP_INPUTS
  return unpack_start_package(payload)                if cmd == Command::RTDE_CONTROL_PACKAGE_START
  return unpack_pause_package(payload)                if cmd == Command::RTDE_CONTROL_PACKAGE_PAUSE
  return unpack_data_package(payload, @output_config) if cmd == Command::RTDE_DATA_PACKAGE
  @logger.error 'Unknown package command' + cmd.to_s
end
receive(binary=false) click to toggle source
# File lib/rtde.rb, line 264
def receive(binary=false) #{{{
  @logger.debug 'Start receive'
  if !@output_config
    @logger.error 'Output configuration not initialized'
    nil
  end
  return nil if @conn_state != ConnectionState::STARTED
  recv Command::RTDE_DATA_PACKAGE, binary
end
recv(command, binary=false) click to toggle source
# File lib/rtde.rb, line 273
def recv(command, binary=false) #{{{
  @logger.debug 'Start recv' + @buf.to_s
  while connected?
    readable, _, xlist = IO.select([@sock], [], [@sock])
    @logger.debug 'Readable: ' + readable.to_s
    if readable.length > 0
      @logger.debug 'readable.length >0: ' + readable.length.to_s
      more = @sock.recv(4096)
      if more.length == 0
        trigger_disconnected
        return nil
      end
      @buf += more
    end

    if xlist.length > 0 || readable.length == 0
      @logger.info 'lost connection with controller'
      trigger_disconnected
      return nil
    end
    while @buf.length >= 3
      @logger.debug '@buf>=3'
      packet_header = Serialize::ControlHeader.unpack(@buf)

      if @buf.length >= packet_header.size
        @logger.debug '@buf.length >= packet_header.size' + @buf.length.to_s + ">=" + packet_header.size.to_s
        packet, @buf = @buf[3..packet_header.size], @buf[packet_header.size..-1]
        #@logger.debug 'Packet:' + packet.to_s
        @logger.debug 'Packet_Header_Command: ' + packet_header.command.to_s + "\n"
        data = on_packet(packet_header.command, packet)
        @logger.debug 'DATA:' + data.to_s
        if @buf.length >= 3 && command == Command::RTDE_DATA_PACKAGE
          @logger.debug '@buf.length >= 3 && command == Command::RTDE_DATA_PACKAGE'
          next_packet_header = Serialize::ControlHeader.unpack(@buf)
          if next_packet_header.command == command
            @logger.info 'skipping package(1)'
            next
          end
        end
        if packet_header.command == command
          @logger.debug 'returning becuase of packet_header.command == command'
          return  binary ? packet[1..-1] : data
        else
          @logger.info 'skipping package(2)'
        end
      else
        break
      end
    end
  end
  nil
end
send(input_data) click to toggle source
# File lib/rtde.rb, line 157
def send(input_data) #{{{
  if @conn_state != ConnectionState::STARTED
    @logger.error 'Cannot send when RTDE synchroinization is inactive'
    return
  end
  if not @input_config.include? input_data.recipe_id
    @logger.error 'Input configuration id not found: ' + @input_data.recipe_id
    return
  end
  config = @input_config[input_data.recipe_id]
  send_all Command::RTDE_DATA_PACKAGE, config.pack(input_data)
end
send_all(command, payload = ''.b) click to toggle source
# File lib/rtde.rb, line 173
def send_all(command, payload = ''.b) #{{{
  fmt = 'S>C'
  size = ([0,0].pack fmt).length + payload.length
  buf = [size, command].pack(fmt) + payload
  @logger.debug 'send_all.size: ' +size.to_s
  @logger.debug 'send_all.buf: ' + buf.to_s + "\n"
  if !@sock
    @logger.error 'Unable to send: not connected to Robot'
    return false
  end

  _, writable, _ = IO.select([], [@sock], [])
  if writable.length > 0
    #@logger.debug 'buffer: ' + buf
    @sock.sendmsg(buf)
    @logger.debug 'sending ok'
    true
  else
    trigger_disconnected
    false
  end
end
send_and_receive(cmd, payload = ''.b) click to toggle source
# File lib/rtde.rb, line 169
def send_and_receive(cmd, payload = ''.b) #{{{
  @logger.debug 'Start send_and_receive'
  send_all(cmd, payload) ? recv(cmd) : nil
end
send_input_setup(variables, types=[]) click to toggle source
# File lib/rtde.rb, line 225
def send_input_setup(variables, types=[]) #{{{
  cmd = Command::RTDE_CONTROL_PACKAGE_SETUP_INPUTS
  payload = variables.join ','
  result = send_and_receive cmd, payload
  if types.length != 0 && result.types != types
    @logger.error(
      'Data type inconsistency for input setup: ' +
      types.to_s + ' - ' +
      result.types.to_s
    )
    return nil
  end

  result.names = variables
  @input_config[result.id] = result
  Serialize::DataObject.create_empty variables, result.id
end
send_message(message, source = 'Ruby Client', type = Serialize::Message::INFO_MESSAGE) click to toggle source
# File lib/rtde.rb, line 195
def send_message(message, source = 'Ruby Client', type = Serialize::Message::INFO_MESSAGE) #{{{
  cmd = Command::RTDE_TEXT_MESSAGE
  fmt = 'Ca%dCa%dC' % [message.length, source.length]
  payload = struct.pack(fmt, message.length, message, source.length, source, type)
  send_all(cmd, payload)
end
send_output_setup(variables, types=[], frequency = 125) click to toggle source
# File lib/rtde.rb, line 242
def send_output_setup(variables, types=[], frequency = 125) #{{{
  @logger.debug 'Start send_output_setup'
  @logger.debug 'variables: ' + variables.to_s
  @logger.debug 'types: ' + types.to_s + "\n"
  cmd = Command::RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS
  payload = [frequency].pack 'G'
  payload = payload + variables.join(',')
  result = send_and_receive cmd, payload
  if types.length != 0 && result.types != types
    @logger.error(
      'Data type inconsistency for output setup: ' +
      types.to_s + ' - ' +
      result.types.to_s
    )
    return false
  end
  result.names = variables
  @output_config = result
  @logger.debug 'result:' + @output_config.to_s
  return true
end
send_pause() click to toggle source

}}}

# File lib/rtde.rb, line 214
def send_pause #{{{
  cmd = Command::RTDE_CONTROL_PACKAGE_PAUSE
  success = send_and_receive(cmd)
  if success
    @logger.info 'RTDE synchronization paused'
    @conn_state = ConnectionState::PAUSED
  else
    @logger.error('RTDE synchronization failed to pause')
  end
  success
end
send_start() click to toggle source
# File lib/rtde.rb, line 201
def send_start #{{{
  @logger.debug 'Start send_start'
  cmd = Command::RTDE_CONTROL_PACKAGE_START
  if send_and_receive cmd
    @logger.info 'RTDE synchronization started'
    @conn_state = ConnectionState::STARTED
    true
  else
    @logger.error 'RTDE synchronization failed to start'
    false
  end
end
trigger_disconnected() click to toggle source
# File lib/rtde.rb, line 344
def trigger_disconnected #{{{
  @logger.info 'RTDE disconnected'
  disconnect
end
unpack_data_package(payload, output_config) click to toggle source
# File lib/rtde.rb, line 397
def unpack_data_package(payload, output_config) #{{{
  if !output_config
    @logger.error 'RTDE_DATA_PACKAGE: Missing output configuration'
    return nil
  end
  @logger.debug "outputconfig: " + output_config.to_s
  @logger.debug "payload: " + payload.to_s
  output_config.unpack payload
end
unpack_pause_package(payload) click to toggle source
# File lib/rtde.rb, line 390
def unpack_pause_package(payload) #{{{
  if payload.length != 1
    @logger.error 'RTDE_CONTROL_PACKAGE_PAUSE: Wrong payload size'
    return nil
  end
  Serialize::ReturnValue.unpack(payload).success
end
unpack_protocol_version_package(payload) click to toggle source
# File lib/rtde.rb, line 349
def unpack_protocol_version_package(payload) #{{{
  @logger.debug 'unpaking protocol version package'
  return nil if payload.length != 1
  Serialize::ReturnValue.unpack(payload).success
end
unpack_setup_inputs_package(payload) click to toggle source
# File lib/rtde.rb, line 376
def unpack_setup_inputs_package(payload) #{{{
  if payload.length < 1
    @logger.error 'RTDE_CONTROL_PACKAGE_SETUP_INPUTS: No payload'
    return nil
  end
  Serialize::DataConfig.unpack_recipe payload
end
unpack_setup_outputs_package(payload) click to toggle source
# File lib/rtde.rb, line 367
def unpack_setup_outputs_package(payload) #{{{
  @logger.debug 'Start unpack_setup_outputs_package'
  if payload.length < 1
    @logger.error 'RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS: No payload'
    return nil
  end
  @logger.debug 'Payload for unpack: ' + payload.to_s
  Serialize::DataConfig.unpack_recipe payload
end
unpack_start_package(payload) click to toggle source
# File lib/rtde.rb, line 383
def unpack_start_package(payload) #{{{
  if payload.length != 1
    @logger.error 'RTDE_CONTROL_PACKAGE_START: Wrong payload size'
    return nil
  end
  Serialize::ReturnValue.unpack(payload).success
end
unpack_text_message(payload) click to toggle source
# File lib/rtde.rb, line 360
def unpack_text_message(payload) #{{{
  return nil if payload.length < 1
  msg = Serialize::Message.unpack payload
  @logger.error  (msg.source + ':' + msg.message) if msg.level == Serialize::Message::EXCEPTION_MESSAGE || msg.level == Serialize::Message::ERROR_MESSAGE
  @logger.warning(msg.source + ':' + msg.message) if msg.level == Serialize::Message::WARNING_MESSAGE
  @logger.info   (msg.source + ':' + msg.message) if msg.level == Serialize::Message::INFO_MESSAGE
end
unpack_urcontrol_version_package(payload) click to toggle source
# File lib/rtde.rb, line 354
def unpack_urcontrol_version_package(payload) #{{{
  @logger.debug 'unpack urcontrol_version'
  return nil if payload.length != 16
  @logger.debug 'packet lenght ok'
  Serialize::ControlVersion.unpack payload
end