def importIitYarp(**kwargs):
importedDicts = importIitYarpRecursive(**kwargs)
if kwargs.get('zeroTime', kwargs.get('zeroTimestamps', True)):
# Optional: start the timestamps at zero for the first event
# This is done collectively for all the concurrent imports
rezeroTimestampsForImportedDicts(importedDicts)
if len(importedDicts) == 1:
importedDicts = importedDicts[0]
return importedDicts
def importRpgDvsRos(**kwargs):
topics = importRosbag(**kwargs)
template = kwargs.get('template', {})
# The template becomes the data branch of the importedDict
outDict = {'info': kwargs, 'data': {}}
for channelKey in template:
print('Importing for channel : ', channelKey)
channelKeyStripped = str(channelKey).translate(
str.maketrans('', '', string.punctuation))
outDict['data'][channelKeyStripped] = {}
for dataType in template[channelKey]:
topic = template[channelKey][dataType]
topic = topics.pop(topic)
msgs = topic['msgs']
print('Importing for dataType "' + dataType + '"; there are ' +
str(len(msgs)) + ' messages')
if dataType == 'dvs':
# Interpret these messages as EventArray
outDict['data'][channelKeyStripped][
dataType] = interpretMsgsAsDvs(msgs, **kwargs)
elif dataType == 'pose6q':
outDict['data'][channelKeyStripped][
dataType] = interpretMsgsAsPose6q(msgs, **kwargs)
elif dataType == 'frame':
outDict['data'][channelKeyStripped][
dataType] = interpretMsgsAsFrame(msgs, **kwargs)
elif dataType == 'cam':
outDict['data'][channelKeyStripped][
dataType] = interpretMsgsAsCam(msgs, **kwargs)
elif dataType == 'imu':
outDict['data'][channelKeyStripped][
dataType] = interpretMsgsAsImu(msgs, **kwargs)
else:
print('dataType "', dataType, '" not recognised.')
if getOrInsertDefault(kwargs, 'zeroTimestamps', True):
# Optional: start the timestamps at zero for the first event
# This is done collectively for all the concurrent imports
zeroTimestampsForAChannel(outDict['data'][channelKeyStripped])
# jointly rezero for all channels
if kwargs.get('zeroTimestamps', True):
# Optional: start the timestamps at zero for the first event
# This is done collectively for all the concurrent imports
rezeroTimestampsForImportedDicts(outDict)
# report the remaining topics
remainingTopics = topics.keys()
if remainingTopics:
print(
'The following topics are present in the file but were not imported: '
)
for topic in remainingTopics:
print(topic)
# if cam and dvs exist in the same channel, apply height and width to dimY/X
for channelKey in outDict['data'].keys():
utiliseCameraInfoWithinChannel(outDict['data'][channelKey])
return outDict
'''
def cropTime(inDict, **kwargs):
if isinstance(inDict, list):
return [cropTime(inDictInst, **kwargs) for inDictInst in inDict]
elif 'info' in inDict:
outDict = {'info': inDict['info'].copy(), 'data': {}}
for channelName in inDict['data'].keys():
outDict['data'][channelName] = {}
for dataTypeName in inDict['data'][channelName].keys():
outDict['data'][channelName][dataTypeName] = cropTime(
inDict['data'][channelName][dataTypeName], **kwargs)
rezeroTimestampsForImportedDicts(outDict)
return outDict
elif 'ts' in inDict:
ts = inDict['ts']
if not np.any(ts): # the dataset is empty
return inDict
startTime = kwargs.get(
'startTime', kwargs.get('minTime', kwargs.get('beginTime', ts[0])))
stopTime = kwargs.get(
'stopTime', kwargs.get('maxTime', kwargs.get('endTime', ts[-1])))
if startTime == ts[0] and stopTime == ts[-1]:
# No cropping to do - pass out the dict unmodified
return inDict
startIdx = np.searchsorted(ts, startTime)
stopIdx = np.searchsorted(ts, stopTime)
tsNew = ts[startIdx:stopIdx]
if kwargs.get('zeroTime', kwargs.get('zeroTimestamps', True)):
tsNew = tsNew - startTime
outDict = {'ts': tsNew}
for fieldName in inDict.keys():
if fieldName != 'ts':
field = inDict[fieldName]
try:
outDict[fieldName] = field[startIdx:stopIdx]
except IndexError:
outDict[fieldName] = field.copy(
) # This might fail for certain data types
except TypeError:
outDict[
fieldName] = field # This might fail for certain data types
if kwargs.get('zeroTime', kwargs.get('zeroTimestamps', True)):
tsOffsetOriginal = inDict.get('tsOffset', 0)
outDict['tsOffset'] = tsOffsetOriginal - startTime
return outDict
else:
# We assume that this is a datatype which doesn't contain ts,
# so we pass it out unmodified
return inDict
def importRpgDvsRos(filePathOrName, **kwargs):
template = kwargs.get('template')
if template == {}: # Just list contents of bag without importing
topics = importRosbag(filePathOrName=filePathOrName, listTopics=True, **kwargs)
return
topics = importRosbag(filePathOrName=filePathOrName, **kwargs)
outDict = {
'info': kwargs,
'data': {}
}
outDict['info']['filePathOrName'] = filePathOrName
if template is None:
for topicLabel in topics.keys():
rosbagType = topics[topicLabel].pop('rosbagType')
bimveeType = bimveeTypeForRosbagType(rosbagType)
if bimveeType is None:
print('Actually, ' + topicLabel + ' has not been imported, because the rosbag message type ' + rosbagType + ' has not been recognised.')
else:
outDict['data'][topicLabel] = {bimveeType: topics[topicLabel]}
else:
# If we get to here then there is a template to parse
# The template becomes the data branch of the importedDict
for channelKey in template.keys():
channelKeyStripped = str(channelKey).translate(str.maketrans('', '', string.punctuation))
outDict['data'][channelKeyStripped] = {}
for dataType in template[channelKey]:
topicLabel = template[channelKey][dataType]
topic = topics.pop(topicLabel)
rosbagType = topic.pop('rosbagType')
bimveeType = bimveeTypeForRosbagType(rosbagType)
if bimveeType != dataType:
print('dataType "', dataType, '" not correctly defined for topic: "', topicLabel, '"')
else:
outDict['data'][channelKeyStripped][dataType] = topic
# Post processing
if kwargs.get('zeroTime', kwargs.get('zeroTimestamps', True)):
# Optional: start the timestamps at zero for the first event
# This is done collectively for all the concurrent imports
for channelKey in outDict['data'].keys():
zeroTimestampsForAChannel(outDict['data'][channelKey])
# jointly rezero for all channels
rezeroTimestampsForImportedDicts(outDict)
return outDict
def importVicon(**kwargs):
filePathOrName = kwargs['filePathOrName']
pattern = re.compile('(\d+) (\d+\.\d+) \((.*)\)')
# yarpBottleTimes = []
outDict = {'info': {'filePathOrName': filePathOrName}, 'data': {}}
separateBodiesAsChannels = kwargs.get('separateBodiesAsChannels', False)
if separateBodiesAsChannels:
uniqueIds = []
else:
poseDict = {'ts': [], 'point': [], 'rotation': [], 'bodyId': []}
with open(filePathOrName, 'r') as file:
print('Found file to read')
line = file.readline()
while line:
found = pattern.search(line.strip())
# yarpBottleTimes.append(float(found.group(2)))
viconData = found.group(3)
bodies = viconData.split(') (')
for body in bodies:
elements = body.split(" ")
bodyId = elements[1].strip('\"')
ts = elements[2]
point = elements[3:6]
# Note: quaternion order is [w,x,y,z] - this is defined by yarp
# IFrameTransform component, so ignore vicon documentation
rotation = elements[6:]
if separateBodiesAsChannels:
try:
poseDict = outDict['data'][bodyId]['pose6q']
except KeyError:
print('KeyError exception.. Creating new key', bodyId)
uniqueIds.append(bodyId)
outDict['data'][bodyId] = {
'pose6q': {
'ts': [],
'point': [],
'rotation': []
}
}
poseDict = outDict['data'][bodyId]['pose6q']
poseDict['ts'].append(ts)
poseDict['point'].append(point)
poseDict['rotation'].append(rotation)
if not separateBodiesAsChannels:
poseDict['bodyId'].append(bodyId.encode('utf-8'))
line = file.readline()
# converting lists of strings to numpy arrays of objects
if separateBodiesAsChannels:
for id in uniqueIds:
outDict['data'][id]['pose6q']['ts'] = np.array(
outDict['data'][id]['pose6q']['ts'], dtype=np.float64)
outDict['data'][id]['pose6q']['point'] = np.array(
outDict['data'][id]['pose6q']['point'], dtype=np.float64)
outDict['data'][id]['pose6q']['rotation'] = np.array(
outDict['data'][id]['pose6q']['rotation'], dtype=np.float64)
if getOrInsertDefault(kwargs, 'zeroTimestamps', True):
zeroTimestampsForAChannel(outDict['data'][id])
if getOrInsertDefault(kwargs, 'zeroTimestamps', True):
rezeroTimestampsForImportedDicts(outDict)
outDict['info']['uniqueIds'] = uniqueIds
else:
poseDict['ts'] = np.array(poseDict['ts'], dtype=np.float64)
poseDict['point'] = np.array(poseDict['point'], dtype=np.float64)
poseDict['rotation'] = np.array(poseDict['rotation'], dtype=np.float64)
poseDict['bodyId'] = np.array(poseDict['bodyId'])
poseDict['uniqueIds'] = np.unique(poseDict['bodyId'])
if kwargs.get('separateMarkersFromSegments', False):
outDict['data']['vicon'] = separateMarkersFromSegments(poseDict)
else:
outDict['data']['vicon'] = {'pose6q': poseDict}
if getOrInsertDefault(kwargs, 'zeroTimestamps', True):
zeroTimestampsForAChannel(
outDict['data']
['vicon']) # TODO: Zeroing in the separated channel case
return outDict