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