def __init__(self, options):
self.options = options
# Set requested GPU
os.environ["CUDA_DEVICE_ORDER"] = "PCI_BUS_ID"
os.environ["CUDA_VISIBLE_DEVICES"] = str(options.gpu_id)
# Initialize YARP network
yarp.Network.init()
# Initialize RF module
yarp.RFModule.__init__(self)
# Initialize inference
self.inference = Inference(self.options.model, self.options.refine_model, self.options.width, self.options.height, self.options.cam_fx, self.options.cam_fy, self.options.cam_cx, self.options.cam_cy)
# Initialize YARP ports
self.rgb_in = yarp.BufferedPortImageRgb()
self.rgb_in.open("/dense-fusion/camera/rgb:i")
self.depth_in = yarp.BufferedPortImageFloat()
self.depth_in.open("/dense-fusion/camera/depth:i")
self.mask_in = yarp.BufferedPortImageMono()
self.mask_in.open("/dense-fusion/camera/mask:i")
self.camera_pose_in = yarp.BufferedPortVector()
self.camera_pose_in.open("/dense-fusion/camera/pose:i")
self.prediction_out = yarp.Port()
self.prediction_out.open("/dense-fusion/pose:o")
# Input buffers initialization
self.rgb_buffer = bytearray(numpy.zeros((self.options.height, self.options.width, 3), dtype = numpy.uint8))
self.rgb_image = yarp.ImageRgb()
self.rgb_image.resize(self.options.width, self.options.height)
self.rgb_image.setExternal(self.rgb_buffer, self.options.width, self.options.height)
self.depth_buffer = bytearray(numpy.zeros((self.options.height, self.options.width, 1), dtype = numpy.float32))
self.depth_image = yarp.ImageFloat()
self.depth_image.resize(self.options.width, self.options.height)
self.depth_image.setExternal(self.depth_buffer, self.options.width, self.options.height)
self.mask_buffer = bytearray(numpy.zeros((self.options.height, self.options.width, 1), dtype = numpy.uint8))
self.mask_image = yarp.ImageMono()
self.mask_image.resize(self.options.width, self.options.height)
self.mask_image.setExternal(self.mask_buffer, self.options.width, self.options.height)
# Stamp initialization for finite differences
self.last_stamp = -1
self.last_total_position = None
if (portNameList[j][0] == 'rpcbase'):
portsList.append(yarp.RpcServer())
portsList[j].open(portNameList[j][1] + ':i')
svPort = j
elif (portNameList[j][0] == 'callsign'):
callSignList = portNameList[j][1].split(',')
else:
parts = portNameList[j][1].split(' ')
print parts
if (parts[1].lower() == 'imagergb'):
portsList.append(yarp.BufferedPortImageRgb())
portsList[j].open(parts[0])
elif (parts[1].lower() == 'imagemono'):
portsList.append(yarp.BufferedPortImageMono())
portsList[j].open(parts[0])
elif (parts[1].lower() == 'bottle'):
portsList.append(yarp.BufferedPortBottle())
portsList[j].open(parts[0])
#mrd models with label/instance training will always have:
#1 an input data line which is used when a label is requested
#2 an output data line which is used when a generated instance is required
if (parts[0][-1] == 'i'):
labelPort = j
elif (parts[0][-1] == 'o'):
instancePort = j
#making sure all ports are connected
out = 0
def configure(self, rf):
print sys.argv
stringCommand = 'from SAM.SAM_Drivers import ' + sys.argv[
4] + ' as Driver'
print stringCommand
exec stringCommand
self.mm = [Driver()]
self.dataPath = sys.argv[1]
self.modelPath = sys.argv[2]
self.driverName = sys.argv[4]
self.configPath = sys.argv[3]
off = 17
print '-------------------'
print 'Interaction Settings:'
print
print 'Data Path: '.ljust(off), self.dataPath
print 'Model Path: '.ljust(off), self.modelPath
print 'Config Path: '.ljust(off), self.configPath
print 'Driver:'.ljust(off), self.driverName
print '-------------------'
print 'Configuring Interaction...'
print
self.mm = SAM_utils.initialiseModels(
[self.dataPath, self.modelPath, self.driverName], 'update',
'interaction')
# parse settings from config file
parser2 = SafeConfigParser()
parser2.read(self.configPath)
self.portNameList = parser2.items(self.dataPath.split('/')[-1])
print self.portNameList
self.portsList = []
for j in range(len(self.portNameList)):
if self.portNameList[j][0] == 'rpcbase':
self.portsList.append(yarp.Port())
self.portsList[j].open(self.portNameList[j][1] + ":i")
self.svPort = j
self.attach(self.portsList[j])
elif self.portNameList[j][0] == 'callsign':
# should check for repeated call signs by getting list from samSupervisor
self.callSignList = self.portNameList[j][1].split(',')
elif self.portNameList[j][0] == 'collectionmethod':
self.collectionMethod = self.portNameList[j][1].split(' ')[0]
try:
if self.mm[0].model_mode != 'temporal':
self.bufferSize = int(
self.portNameList[j][1].split(' ')[1])
elif self.mm[0].model_mode == 'temporal':
self.bufferSize = self.mm[0].windowSize
except ValueError:
print 'collectionMethod bufferSize is not an integer'
print 'Should be e.g: collectionMethod = buffered 3'
return False
if self.collectionMethod not in [
'buffered', 'continuous', 'future_buffered'
]:
print 'collectionMethod should be set to buffered / continuous / future_buffered'
return False
else:
parts = self.portNameList[j][1].split(' ')
print parts
if parts[1].lower() == 'imagergb':
self.portsList.append(yarp.BufferedPortImageRgb())
self.portsList[j].open(parts[0])
elif parts[1].lower() == 'imagemono':
self.portsList.append(yarp.BufferedPortImageMono())
self.portsList[j].open(parts[0])
elif parts[1].lower() == 'bottle':
self.portsList.append(yarp.BufferedPortBottle())
self.portsList[j].open(parts[0])
else:
print 'Data type ', parts[1], 'for ', self.portNameList[j][
0], ' unsupported'
return False
# mrd models with label/instance training will always have:
# 1 an input data line which is used when a label is requested
# 2 an output data line which is used when a generated instance is required
if parts[0][-1] == 'i':
self.labelPort = j
elif parts[0][-1] == 'o':
self.instancePort = j
if self.svPort is None or self.labelPort is None or self.instancePort is None:
print 'Config file properties incorrect. Should look like this:'
print '[Actions]'
print 'dataIn = /sam/actions/actionData:i Bottle'
print 'dataOut = /sam/actions/actionData:o Bottle'
print 'rpcBase = /sam/actions/rpc'
print 'callSign = ask_action_label, ask_action_instance'
print 'collectionMethod = buffered 3'
# self.mm[0].configInteraction(self)
self.inputType = self.portNameList[self.labelPort][1].split(
' ')[1].lower()
self.outputType = self.portNameList[self.labelPort][1].split(
' ')[1].lower()
self.dataList = []
self.classificationList = []
yarp.Network.init()
self.test()
return True
def configure(self, rf):
"""
Configure interactionSAMModel yarp module
Args:
rf: Yarp RF context input
argv_1 : String containing data path.
argv_2 : String containing model path.
argv_3 : String containing config file path (from `config_path` parameter of samSupervisor config file).
argv_4 : String driver name corresponding to a valid driver present in SAM_Drivers folder.
argv_5 : String `'True'` or `'False'` to switch formatting of logging depending on whether interaction is logging to a separate window or to the stdout of another process.
Returns:
Boolean indicating success or no success in initialising the yarp module
"""
stringCommand = 'from SAM.SAM_Drivers import ' + sys.argv[
4] + ' as Driver'
exec stringCommand
self.mm = [Driver()]
self.dataPath = sys.argv[1]
self.modelPath = sys.argv[2]
self.driverName = sys.argv[4]
self.configPath = sys.argv[3]
self.windowedMode = sys.argv[5] == 'True'
self.modelRoot = self.dataPath.split('/')[-1]
file_i = 0
loggerFName = join(self.dataPath,
self.baseLogFileName + '_' + str(file_i) + '.log')
# check if file exists
while os.path.isfile(loggerFName) and os.path.getsize(loggerFName) > 0:
loggerFName = join(
self.dataPath,
self.baseLogFileName + '_' + str(file_i) + '.log')
file_i += 1
if self.windowedMode:
logFormatter = logging.Formatter("[%(levelname)s] %(message)s")
else:
logFormatter = logging.Formatter(
"\033[31m%(asctime)s [%(name)-33s] [%(levelname)8s] %(message)s\033[0m"
)
rootLogger = logging.getLogger('interaction ' + self.driverName)
rootLogger.setLevel(logging.DEBUG)
fileHandler = logging.FileHandler(loggerFName)
fileHandler.setFormatter(logFormatter)
rootLogger.addHandler(fileHandler)
consoleHandler = logging.StreamHandler()
consoleHandler.setFormatter(logFormatter)
rootLogger.addHandler(consoleHandler)
logging.root = rootLogger
off = 17
logging.info('Arguments: ' + str(sys.argv))
logging.info(stringCommand)
logging.info('Using log' + str(loggerFName))
logging.info('-------------------')
logging.info('Interaction Settings:')
logging.info('Data Path: '.ljust(off) + str(self.dataPath))
logging.info('Model Path: '.ljust(off) + str(self.modelPath))
logging.info('Config Path: '.ljust(off) + str(self.configPath))
logging.info('Driver: '.ljust(off) + str(self.driverName))
logging.info('-------------------')
logging.info('Configuring Interaction...')
logging.info('')
# parse settings from config file
parser2 = SafeConfigParser()
parser2.read(self.configPath)
proposedBuffer = 5
if self.modelRoot in parser2.sections():
self.portNameList = parser2.items(self.dataPath.split('/')[-1])
logging.info(str(self.portNameList))
self.portsList = []
for j in range(len(self.portNameList)):
if self.portNameList[j][0] == 'rpcbase':
self.portsList.append(yarp.Port())
self.portsList[j].open(self.portNameList[j][1] + ":i")
self.svPort = j
self.attach(self.portsList[j])
elif self.portNameList[j][0] == 'visualise':
if self.portNameList[j][1] == "True":
self.drawLatent = True
elif self.portNameList[j][0] == 'callsign':
# should check for repeated call signs by getting list from samSupervisor
self.callSignList = self.portNameList[j][1].split(',')
elif self.portNameList[j][0] == 'latentmodelport':
self.portsList.append(yarp.BufferedPortImageRgb())
self.latentPort = j
ports = self.portNameList[j][1].split(',')
self.portsList[j].open(ports[0])
yarp.Network.connect(ports[0], ports[1])
elif self.portNameList[j][0] == 'collectionmethod':
self.collectionMethod = self.portNameList[j][1].split(
' ')[0]
try:
proposedBuffer = int(
self.portNameList[j][1].split(' ')[1])
except ValueError:
logging.error(
'collectionMethod bufferSize is not an integer')
logging.error(
'Should be e.g: collectionMethod = buffered 3')
return False
if self.collectionMethod not in [
'buffered', 'continuous', 'future_buffered'
]:
logging.error(
'collectionMethod should be set to buffered / continuous / future_buffered'
)
return False
elif self.portNameList[j][0] == 'recency':
try:
self.recency = int(self.portNameList[j][1])
except ValueError:
logging.error('Recency value for ' +
str(self.driverName) +
' is not an integer')
self.recency = 5
else:
parts = self.portNameList[j][1].split(' ')
logging.info(parts)
if parts[1].lower() == 'imagergb':
self.portsList.append(yarp.BufferedPortImageRgb())
# self.portsList[j].open(parts[0])
elif parts[1].lower() == 'imagemono':
self.portsList.append(yarp.BufferedPortImageMono())
# self.portsList[j].open(parts[0])
elif parts[1].lower() == 'bottle':
self.portsList.append(yarp.BufferedPortBottle())
# self.portsList[j].open(parts[0])
else:
logging.error('Data type ' + str(parts[1]) + ' for ' +
str(self.portNameList[j][0]) +
' unsupported')
return False
# mrd models with label/instance training will always have:
# 1 an input data line which is used when a label is requested
# 2 an output data line which is used when a generated instance is required
if parts[0][-1] == 'i':
self.labelPort = j
self.labelPortName = parts[0]
elif parts[0][-1] == 'o':
self.instancePort = j
self.instancePortName = parts[0]
if self.collectionMethod == 'continuous':
self.portsList.append(yarp.BufferedPortBottle())
self.eventPort = len(self.portsList) - 1
self.eventPortName = '/'.join(
self.labelPortName.split('/')[:3]) + '/event'
self.portsList[self.eventPort].open(self.eventPortName)
self.classTimestamps = []
if self.recency is None:
logging.warning('No recency value specified for ' +
self.driverName)
logging.warning('Setting value to default of 5 seconds')
self.recency = 5
if self.svPort is None or self.labelPort is None or self.instancePort is None:
logging.warning(
'Config file properties incorrect. Should look like this:')
logging.warning('[Actions]')
logging.warning('dataIn = /sam/actions/actionData:i Bottle')
logging.warning('dataOut = /sam/actions/actionData:o Bottle')
logging.warning('rpcBase = /sam/actions/rpc')
logging.warning(
'callSign = ask_action_label, ask_action_instance')
logging.warning('collectionMethod = buffered 3')
# self.mm[0].configInteraction(self)
self.inputType = self.portNameList[self.labelPort][1].split(
' ')[1].lower()
self.outputType = self.portNameList[self.labelPort][1].split(
' ')[1].lower()
self.dataList = []
self.classificationList = []
self.probClassList = []
self.classTimestamps = []
yarp.Network.init()
self.mm = initialiseModels(
[self.dataPath, self.modelPath, self.driverName],
'update',
'interaction',
drawLatent=self.drawLatent)
self.modelLoaded = True
if self.drawLatent:
self.latentPlots = dict()
self.latentPlots['ax'], self.latentPlots['dims'] = self.mm[
0].SAMObject.visualise(plot_scales=True)
self.sendLatent(self.latentPlots['ax'])
if self.mm[0].model_mode != 'temporal':
self.bufferSize = proposedBuffer
elif self.mm[0].model_mode == 'temporal':
self.bufferSize = self.mm[0].temporalModelWindowSize
self.portsList[self.labelPort].open(self.labelPortName)
self.portsList[self.instancePort].open(self.instancePortName)
# self.test()
return True
else:
logging.error('Section ' + str(self.modelRoot) + ' not found in ' +
str(self.configPath))
return False