'''
Python 3.x library to control an UR robot through its TCP/IP interfaces
Copyright (C) 2017 Martin Huus Bjerge, Rope Robotics ApS, Denmark
Permission is hereby granted, free of charge, to any person obtaining a copy of this software
and associated documentation files (the "Software"), to deal in the Software without restriction,
including without limitation the rights to use, copy, modify, merge, publish, distribute,
sublicense, and/or sell copies of the Software, and to permit persons to whom the Software
is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies
or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL "Rope Robotics ApS" BE LIABLE FOR ANY CLAIM,
DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
Except as contained in this notice, the name of "Rope Robotics ApS" shall not be used
in advertising or otherwise to promote the sale, use or other dealings in this Software
without prior written authorization from "Rope Robotics ApS".
'''
__author__ = "Martin Huus Bjerge"
__copyright__ = "Copyright 2017, Rope Robotics ApS, Denmark"
__license__ = "MIT License"
from pkg_resources import resource_filename
import URBasic
import threading
import socket
import struct
import select
import numpy as np
import xml.etree.ElementTree as ET
import time
import os.path
DEFAULT_TIMEOUT = 1.0
class Command:
RTDE_REQUEST_PROTOCOL_VERSION = 86 # ascii V
RTDE_GET_URCONTROL_VERSION = 118 # ascii v
RTDE_TEXT_MESSAGE = 77 # ascii M
RTDE_DATA_PACKAGE = 85 # ascii U
RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS = 79 # ascii O
RTDE_CONTROL_PACKAGE_SETUP_INPUTS = 73 # ascii I
RTDE_CONTROL_PACKAGE_START = 83 # ascii S
RTDE_CONTROL_PACKAGE_PAUSE = 80 # ascii P
class ConnectionState:
ERROR = 0
DISCONNECTED = 1
CONNECTED = 2
PAUSED = 3
STARTED = 4
class RTDE(threading.Thread): #, metaclass=Singleton
'''
Interface to UR robot Real Time Data Exchange interface.
See this site for more detail:
http://www.universal-robots.com/how-tos-and-faqs/how-to/ur-how-tos/real-time-data-exchange-rtde-guide-22229/
The constructor takes a UR robot hostname as input and a path to a RTDE configuration file.
Input parameters:
host (string): Hostname or IP of UR Robot (RT CLient server)
conf_filename (string): Path to xml file describing what channels to activate
logger (URBasis_DataLogging obj): A instance if a logger object if common logging is needed.
Example:
rob = URBasic.rtde.RTDE('192.168.56.101', 'rtde_configuration.xml')
rob.close_rtde()
'''
def __init__(self, robotModel, conf_filename=None):
'''
Constructor see class description for more info.
'''
if(False):
assert isinstance(robotModel, URBasic.robotModel.RobotModel) ### This line is to get code completion for RobotModel
self.__robotModel = robotModel
logger = URBasic.dataLogging.DataLogging()
name = logger.AddEventLogging(__name__,log2Consol=False)
self._logger = logger.__dict__[name]
self.__reconnectTimeout = 600 #Seconds (while in run)
self.__dataSend = RTDEDataObject()
# always use the rtdeCOnfiguration in the packages folder
conf_filename = resource_filename(__name__, 'rtdeConfigurationDefault.xml')
self.__conf_filename = conf_filename
self.__stop_event = True
threading.Thread.__init__(self)
self.__dataEvent = threading.Condition()
self.__conn_state = ConnectionState.DISCONNECTED
self.__sock = None
self.__rtde_output_names = None
self.__rtde_output_config = None
self.__rtde_input_names = None
self.__rtde_input_initValues = None
self.__rtde_input_config = None
self.__controllerVersion = None
self.__protocol_version = None
self.__packageCounter = 0
self.start()
self._logger.info('RTDE constructor done')
def __connect(self):
'''
Initialize RTDE connection to host and set up data interfaces based on configuration XML.
Return value:
success (boolean)
'''
if self.__sock:
return True
try:
self.__sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.__sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
self.__sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self.__sock.settimeout(DEFAULT_TIMEOUT)
self.__sock.connect((self.__robotModel.ipAddress, 30004))
self.__conn_state = ConnectionState.CONNECTED
except (socket.timeout, socket.error):
if self.__sock:
self.sock.close()
self.__sock = None
return False
return True
def __disconnect(self):
'''
Close the RTDE connection.
'''
if self.__sock:
self.__sock.close()
self.__sock = None
self.__conn_state = ConnectionState.DISCONNECTED
return True
def __isConnected(self):
'''
Returns True if the connection is open.
Return value:
open (boolean)
'''
return self.__conn_state > ConnectionState.DISCONNECTED
def isRunning(self):
'''
Return True if RTDE interface is running
'''
return self.__conn_state >= ConnectionState.STARTED
def __getControllerVersion(self):
'''
Returns the software version of the robot controller running the RTDE server.
Return values:
major (int)
minor (int)
bugfix (int)
'''
cmd = Command.RTDE_GET_URCONTROL_VERSION
self.__send(cmd)
def __negotiateProtocolVersion(self, protocol):
'''
Negotiate the protocol version with the server.
Returns True if the controller supports the specified protocol version.
We recommend that you use this to ensure full compatibility between your
application and future versions of the robot controller.
Input parameters:
protocol (int): protocol version number
Return value:
success (boolean)
'''
cmd = Command.RTDE_REQUEST_PROTOCOL_VERSION
payload = struct.pack('>H',protocol)
self.__send(cmd, payload)
def __setupInput(self, input_variables=None, types=[], initValues=None):
'''
Configure an input package that the external(this) application will send to the robot controller.
An input package is a collection of input input_variables that the external application will provide
to the robot controller in a single update. Variables is a list of variable names and should be
a subset of the names supported as input by the RTDE interface.The list of types is optional,
but if any types are provided it should have the same length as the input_variables list.
The provided types will be matched with the types that the RTDE interface expects and the
function returns None if they are not equal. Multiple input packages can be configured.
The returned InputObject has a reference to the recipe id which is used to identify the
specific input format when sending an update.
If input_variables is empty, xml configuration file is used.
Input parameters:
input_variables (list<string> or Str): [Optional] Variable names from the list of possible RTDE inputs
types (list<string> or str): [Optional] Types matching the input_variables
Return value:
success (boolean)
'''
if input_variables is None:
tree = ET.parse(self.__conf_filename)
root = tree.getroot()
#setup data that can be send
recive = root.find('send')
input_variables = []
initValues = []
for child in recive:
input_variables.append(child.attrib['name'])
initValues.append(float(child.attrib['initValue']))
cmd = Command.RTDE_CONTROL_PACKAGE_SETUP_INPUTS
if type(input_variables) is list:
payload = ','.join(input_variables)
elif type(input_variables) is str:
payload = input_variables
else:
self._logger.error('Variables must be list of stings or a single string, input_variables is: ' + str(type(input_variables)))
return None
self.__rtde_input_names = input_variables
self.__rtde_input_initValues = initValues
payload = payload.encode('utf-8')
self.__send(cmd, payload)
return True
def __setupOutput(self, output_variables=None, types=[]):
'''
Configure an output package that the robot controller will send to the
external(this) application at the control frequency. Variables is a list of
variable names and should be a subset of the names supported as output by the
RTDE interface. The list of types is optional, but if any types are provided
it should have the same length as the output_variables list. The provided types will
be matched with the types that the RTDE interface expects and the function
returns False if they are not equal. Only one output package format can be
specified and hence no recipe id is used for output.
If output_variables is empty, xml configuration file is used.
Input parameters:
output_variables (list<string> or str): [Optional] Variable names from the list of possible RTDE outputs
types (list<string> or str): [Optional] Types matching the output_variables
Return value:
success (boolean)
'''
if output_variables is None:
if not os.path.isfile(self.__conf_filename):
self._logger.error("Configuration file don't exist : " + self.__conf_filename)
return False
tree = ET.parse(self.__conf_filename)
root = tree.getroot()
#Setup data to be recived
recive = root.find('receive')
output_variables = ['timestamp']
for child in recive:
output_variables.append(child.attrib['name'])
cmd = Command.RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS
if type(output_variables) is list:
payload = ','.join(output_variables)
elif type(output_variables) is str:
payload = output_variables
else:
self._logger.error('Variables must be list of stings or a single string, output_variables is: ' + str(type(output_variables)))
return None
self.__rtde_output_names = output_variables
payload = payload.encode('utf-8')
self.__send(cmd, payload)
return True
def __sendStart(self):
'''
Sends a start command to the RTDE server.
Setup of all inputs and outputs must be done before starting the RTDE interface
Return value:
success (boolean)
'''
cmd = Command.RTDE_CONTROL_PACKAGE_START
self.__send(cmd)
return True
def __sendPause(self):
'''
Sends a pause command to the RTDE server
When paused it is possible to change the input and output configurations
Return value:
success (boolean)
'''
cmd = Command.RTDE_CONTROL_PACKAGE_PAUSE
self.__send(cmd)
return True
def sendData(self):
'''
Send the contents of a RTDEDataObject as input to the RTDE server.
Returns True if successful.
Return value:
success (boolean)
'''
if self.__conn_state != ConnectionState.STARTED:
self._logger.error('Cannot send when RTDE is inactive')
return
#if not (self.__rtde_input_config.names.has_key(self.__dataSend.recipe_id)):
# self._logger.error('Input configuration id not found: ' + str(self.__dataSend.recipe_id))
# return
if self.__robotModel.StopRunningFlag():
self._logger.info('"sendData" send ignored due to "stopRunningFlag" True')
return
#config = self.__rtde_input_config[self.__dataSend.recipe_id]
config = self.__rtde_input_config
return self.__send(Command.RTDE_DATA_PACKAGE, config.pack(self.__dataSend))
def setData(self, variable_name, value):
'''
Set data to be send to the robot
Object is locked while updating to avoid sending half updated values,
hence send all values as two lists of equal lengths
Input parameters:
variable_name (List/str): Variable name from the list of possible RTDE inputs
value (list/int/double)
Return value:
Status (Bool): True=Data sucesfull updated, False=Data not updated
'''
#check if input is list of equal length
if type(variable_name) is list:
if type(variable_name) != type(value):
raise ValueError("RTDE " + str(variable_name) + " is not type of " + str(value))
#return False
if len(variable_name) != len(value):
raise ValueError("List of RTDE Output values does not have same length as list of variable names")
#return False
for ii in range(len(value)):
if self.hasattr(self.__rtde_input_config.names, variable_name[ii]):
self.__dataSend.__dict__[variable_name[ii]] = value[ii]
else:
raise ValueError(str(variable_name[ii]) + " not found in RTDE OUTPUT config")
#return False
else:
if variable_name in self.__rtde_input_config.names:
self.__dataSend.__dict__[variable_name] = value
else:
raise ValueError(str(variable_name) + " not found in RTDE OUTPUT config")
def __send(self, command, payload=bytes()):
'''
Send command and data (payload) to Robot Controller
and receive the respond from the Robot Controller.
Input parameters:
cmd (int)
payload (bytes)
Return value:
success (boolean)
'''
fmt = '>HB'
size = struct.calcsize(fmt) + len(payload)
buf = struct.pack(fmt, size, command) + payload
if self.__sock is None:
self._logger.debug('Unable to send: not connected to Robot')
return False
(_, writable, _) = select.select([], [self.__sock], [], DEFAULT_TIMEOUT)
if len(writable):
self.__sock.sendall(buf)
return True
else:
self._logger.info("RTDE disconnected")
self.__disconnect()
return False
def __receive(self):
byte_buffer = bytes()
(readable, _, _) = select.select([self.__sock], [], [], DEFAULT_TIMEOUT)
if (len(readable)):
more = self.__sock.recv(4096) # TODO: MAKE THIS 4096 instead of 16384
if len(more) == 0:
self._logger.info("RTDE disconnected")
self.__disconnect()
return None
byte_buffer += more
while len(byte_buffer) >= 3:
(packet_size, packet_command) = struct.unpack_from('>HB', byte_buffer)
buffer_length = len(byte_buffer)
if ((buffer_length) >= packet_size):
packet, byte_buffer = byte_buffer[3:packet_size], byte_buffer[packet_size:]
data = self.__decodePayload(packet_command, packet)
if(packet_command == Command.RTDE_GET_URCONTROL_VERSION):
self.__verifyControllerVersion(data)
elif(packet_command == Command.RTDE_REQUEST_PROTOCOL_VERSION):
self.__verifyProtocolVersion(data)
elif(packet_command == Command.RTDE_CONTROL_PACKAGE_SETUP_INPUTS):
self.__rtde_input_config = data
self.__rtde_input_config.names = self.__rtde_input_names
#self.__rtde_input_config[self.__rtde_input_config.id] = self.__rtde_input_config
self.__dataSend = RTDEDataObject.create_empty(self.__rtde_input_names, self.__rtde_input_config.id)
if self.__rtde_input_initValues is not None:
for ii in range(len(self.__rtde_input_config.names)):
if 'UINT8' == self.__rtde_input_config.types[ii]:
self.setData(self.__rtde_input_config.names[ii], int(self.__rtde_input_initValues[ii]))
elif 'UINT32' == self.__rtde_input_config.types[ii]:
self.setData(self.__rtde_input_config.names[ii], int(self.__rtde_input_initValues[ii]))
elif 'INT32' == self.__rtde_input_config.types[ii]:
self.setData(self.__rtde_input_config.names[ii], int(self.__rtde_input_initValues[ii]))
elif 'DOUBLE' == self.__rtde_input_config.types[ii]:
self.setData(self.__rtde_input_config.names[ii], (self.__rtde_input_initValues[ii]))
else:
self._logger.error('Unknown data type')
elif(packet_command == Command.RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS):
self.__rtde_output_config = data
self.__rtde_output_config.names = self.__rtde_output_names
elif(packet_command == Command.RTDE_CONTROL_PACKAGE_START):
self._logger.info('RTDE started')
self.__conn_state = ConnectionState.STARTED
elif(packet_command == Command.RTDE_CONTROL_PACKAGE_PAUSE):
self._logger.info('RTDE paused')
self.__conn_state = ConnectionState.PAUSED
elif(packet_command == Command.RTDE_DATA_PACKAGE):
self.__updateModel(data)
elif(packet_command == 0):
byte_buffer = bytes()
else: # TODO: make this so that it still checks the packet content
print("skipping package - unexpected packet_size - length: " , str(len(byte_buffer)), str(packet_size), str(packet_command))
byte_buffer = bytes()
if len(byte_buffer) != 0:
self._logger.warning('skipping package - not a package but buffer was not empty')
byte_buffer = bytes()
def __updateModel(self, rtde_data_package):
self.__packageCounter = self.__packageCounter + 1
#print("got a rtde package nr " + str(self.__packageCounter))
#print("RTDe Package keys:", rtde_data_package.keys())
if(self.__packageCounter % 1000 == 0):
self._logger.info("Total packages: " + str(self.__packageCounter))
if(self.__robotModel.dataDir['timestamp'] != None):
delta = rtde_data_package['timestamp'] - self.__robotModel.dataDir['timestamp']
if(delta > 0.00800001):
self._logger.error("Lost some RTDE at " + str(rtde_data_package['timestamp']) + " - " + str(delta*1000) + " milliseconds since last package")
for tagname in rtde_data_package.keys():
self.__robotModel.dataDir[tagname] = rtde_data_package[tagname]
def __verifyControllerVersion(self, data):
self.__controllerVersion = data
(major, minor, bugfix, build) = self.__controllerVersion
if major and minor and bugfix:
self._logger.info('Controller version: ' + str(major) + '.' + str(minor) + '.' + str(bugfix) + '-' + str(build))
if major <= 3 and minor <= 2 and bugfix < 19171:
self._logger.error("Please upgrade your controller to minimum version 3.2.19171")
raise ValueError("Please upgrade your controller to minimum version 3.2.19171")
def __verifyProtocolVersion(self, data):
self.__protocol_version = data
if(self.__protocol_version != 1):
raise ValueError("We only support protocol version 1 at the moment")
def __decodePayload(self, cmd, payload):
#print(cmd)
'''
Decode the package received from the Robot
payload (bytes)
Return value(s):
Output from Robot controller (type is depended on the cmd value)
'''
if cmd == Command.RTDE_REQUEST_PROTOCOL_VERSION:
if len(payload) != 1:
self._logger.error('RTDE_REQUEST_PROTOCOL_VERSION: Wrong payload size')
return None
return struct.unpack_from('>B', payload)[0]
elif cmd == Command.RTDE_GET_URCONTROL_VERSION:
if 12 == len(payload):
return np.append(np.array(struct.unpack_from('>III', payload)), 0)
elif 16 == len(payload):
return np.array(struct.unpack_from('>IIII', payload))
else:
self._logger.error('RTDE_GET_URCONTROL_VERSION: Wrong payload size')
return None
elif cmd == Command.RTDE_TEXT_MESSAGE:
if len(payload) < 1:
self._logger.error('RTDE_TEXT_MESSAGE: No payload')
return None
EXCEPTION_MESSAGE = 0
ERROR_MESSAGE = 1
WARNING_MESSAGE = 2
INFO_MESSAGE = 3
fmt = ">" + str(len(payload)) + "B"
out = struct.unpack_from(fmt, payload)
level = out[0]
message = ''.join(map(chr,out[1:]))
if(level == EXCEPTION_MESSAGE or
level == ERROR_MESSAGE):
self._logger.error('Server message: ' + message)
elif level == WARNING_MESSAGE:
self._logger.warning('Server message: ' + message)
elif level == INFO_MESSAGE:
self._logger.info('Server message: ' + message)
elif cmd == Command.RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS:
if len(payload) < 1:
self._logger.error('RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS: No payload')
return None
has_recipe_id = False
output_config = RTDE_IO_Config.unpack_recipe(payload, has_recipe_id)
return output_config
elif cmd == Command.RTDE_CONTROL_PACKAGE_SETUP_INPUTS:
if len(payload) < 1:
self._logger.error('RTDE_CONTROL_PACKAGE_SETUP_INPUTS: No payload')
return None
has_recipe_id = True
input_config = RTDE_IO_Config.unpack_recipe(payload, has_recipe_id)
return input_config
elif cmd == Command.RTDE_CONTROL_PACKAGE_START:
if len(payload) != 1:
self._logger.error('RTDE_CONTROL_PACKAGE_START: Wrong payload size')
return None
return bool(struct.unpack_from('>B', payload)[0])
elif cmd == Command.RTDE_CONTROL_PACKAGE_PAUSE:
if len(payload) != 1:
self._logger.error('RTDE_CONTROL_PACKAGE_PAUSE: Wrong payload size')
return None
return bool(struct.unpack_from('>B', payload)[0])
elif cmd == Command.RTDE_DATA_PACKAGE:
if self.__rtde_output_config is None:
self._logger.error('RTDE_DATA_PACKAGE: Missing output configuration')
return None
output = self.__rtde_output_config.unpack(payload)
return output
else:
self._logger.error('Unknown RTDE command type: ' + chr(cmd) + str(cmd))
def __listEquals(self, l1, l2):
if len(l1) != len(l2):
return False
for i in range(len((l1))):
if l1[i] != l2[i]:
return False
return True
def __wait(self):
'''Wait while the data receiving thread is receiving a new data set.'''
cnt = 0
while self.__conn_state < ConnectionState.STARTED:
time.sleep(1)
cnt +=1
if cnt>5:
self._logger.warning('wait_rtde timed out while RTDE interface not running')
return False
with self.__dataEvent:
self.__dataEvent.wait()
return True
'''Threading Data receive'''
def close(self):
if self.__stop_event is False:
self.__stop_event = True
self.__wait()
self.join()
self.__disconnect()
def run(self):
self.__stop_event = False
t0 = time.time()
while (time.time()-t0<self.__reconnectTimeout) and self.__conn_state != ConnectionState.STARTED:
self.__connect()
self.__disconnect()
self.__connect()
self.__getControllerVersion()
self.__receive()
self.__negotiateProtocolVersion(1)
self.__receive()
self.__setupOutput()
self.__receive()
self.__setupInput()
self.__receive()
self.__sendStart()
self.__receive()
#time.sleep(0.5)
if self.__conn_state != ConnectionState.STARTED:
self._logger.error("RTDE interface not able to connect and timed out!")
return
while (not self.__stop_event) and (time.time()-t0<self.__reconnectTimeout):
try:
#self.__receive(Command.RTDE_DATA_PACKAGE)
#startTime = time.time()
self.__receive()
t0 = time.time()
#delta = t0-startTime
#print("Time to recieve: " + str(delta))
except Exception:
if self.__conn_state >= ConnectionState.STARTED:
self.__conn_state = ConnectionState.ERROR
self._logger.error("RTDE interface stopped running")
self.__sendPause()
if not self.__sendStart():
self.__disconnect()
time.sleep(1)
self.__connect()
self.__setupOutput()
self.__setupInput()
self.__sendStart()
if self.__conn_state == ConnectionState.STARTED:
self._logger.info("RTDE interface restarted")
else:
self._logger.warning("RTDE reconnection failed!")
self.__sendPause()
with self.__dataEvent:
self.__dataEvent.notifyAll()
self._logger.info("RTDE interface is stopped")
class RTDE_IO_Config(object):
__slots__ = ['id', 'names', 'types', 'fmt']
@staticmethod
def unpack_recipe(buf, has_recipe_id):
rmd = RTDE_IO_Config();
if has_recipe_id:
rmd.id = struct.unpack_from('>B', buf)[0]
fmt = ">" + str(len(buf)) + "B"
buf = struct.unpack_from(fmt, buf)
buf = ''.join(map(chr,buf[1:]))
rmd.types = buf.split(',')
rmd.fmt = '>B'
else:
fmt = ">" + str(len(buf)) + "B"
buf = struct.unpack_from(fmt, buf)
buf = ''.join(map(chr,buf[:]))
rmd.types = buf.split(',')
rmd.fmt = '>'
for i in rmd.types:
if i=='INT32':
rmd.fmt += 'i'
elif i=='UINT32':
rmd.fmt += 'I'
elif i=='VECTOR6D':
rmd.fmt += 'd'*6
elif i=='VECTOR3D':
rmd.fmt += 'd'*3
elif i=='VECTOR6INT32':
rmd.fmt += 'i'*6
elif i=='VECTOR6UINT32':
rmd.fmt += 'I'*6
elif i=='DOUBLE':
rmd.fmt += 'd'
elif i=='UINT64':
rmd.fmt += 'Q'
elif i=='UINT8':
rmd.fmt += 'B'
elif i=='IN_USE':
raise ValueError('An input parameter is already in use.')
else:
raise ValueError('Unknown data type: ' + i)
return rmd
def pack(self, state):
l = state.pack(self.names, self.types)
return struct.pack(self.fmt, *l)
def unpack(self, data):
li = struct.unpack_from(self.fmt, data)
return RTDEDataObject.unpack(li, self.names, self.types)
class RTDEDataObject(object):
'''
Data container for data send to or received from the Robot Controller.
The Object will have attributes for each of that data tags received or send.
e.g. obj.actual_digital_output_bits
'''
recipe_id = None
def pack(self, names, types):
if len(names) != len(types):
raise ValueError('List sizes are not identical.')
l = []
if(self.recipe_id is not None):
l.append(self.recipe_id)
for i in range(len(names)):
if self.__dict__[names[i]] is None:
raise ValueError('Uninitialized parameter: ' + names[i])
if types[i].startswith('VECTOR'):
l.extend(self.__dict__[names[i]])
else:
l.append(self.__dict__[names[i]])
return l
@staticmethod
def unpack(data, names, types):
if len(names) != len(types):
raise ValueError('List sizes are not identical.')
obj = dict()
offset = 0
for i in range(len(names)):
obj[names[i]] = RTDEDataObject.unpack_field(data, offset, types[i])
offset += RTDEDataObject.get_item_size(types[i])
return obj
@staticmethod
def create_empty(names, recipe_id):
obj = RTDEDataObject()
for i in range(len(names)):
obj.__dict__[names[i]] = None
obj.recipe_id = recipe_id
return obj
@staticmethod
def get_item_size(data_type):
if data_type.startswith('VECTOR6'):
return 6
elif data_type.startswith('VECTOR3'):
return 3
return 1
@staticmethod
def unpack_field(data, offset, data_type):
size = RTDEDataObject.get_item_size(data_type)
if(data_type == 'VECTOR6D' or
data_type == 'VECTOR3D'):
return np.array([float(data[offset+i]) for i in range(size)])
elif(data_type == 'VECTOR6UINT32'):
return np.array([int(data[offset+i]) for i in range(size)])
elif(data_type == 'DOUBLE'):
return float(data[offset])
elif(data_type == 'UINT32' or
data_type == 'UINT64'):
return int(data[offset])
elif(data_type == 'VECTOR6INT32'):
return np.array([int(data[offset+i]) for i in range(size)])
elif(data_type == 'INT32' or
data_type == 'UINT8'):
return int(data[offset])
raise ValueError('unpack_field: unknown data type: ' + data_type)
