class DataCollectorAndLabeler:
def __init__(self, output_folder, server, menu_handler, marker_size, calibration_file):
if not os.path.exists(output_folder):
os.mkdir(output_folder) # Create the new folder
else:
while True:
msg = Fore.YELLOW + "To continue, the directory '{}' will be delete.\n"
msg = msg + "Do you wish to continue? [y/N] " + Style.RESET_ALL
answer = raw_input(msg.format(output_folder))
if len(answer) > 0 and answer[0].lower() in ('y', 'n'):
if answer[0].lower() == 'n':
sys.exit(1)
else:
break
else:
sys.exit(1) # defaults to N
shutil.rmtree(output_folder) # Delete old folder
os.mkdir(output_folder) # Recreate the folder
self.output_folder = output_folder
self.listener = TransformListener()
self.sensors = {}
self.sensor_labelers = {}
self.server = server
self.menu_handler = menu_handler
self.data_stamp = 0
self.collections = {}
self.bridge = CvBridge()
self.config = loadJSONConfig(calibration_file)
if self.config is None:
sys.exit(1) # loadJSON should tell you why.
self.world_link = self.config['world_link']
# Add sensors
print(Fore.BLUE + 'Sensors:' + Style.RESET_ALL)
print('Number of sensors: ' + str(len(self.config['sensors'])))
# Go through the sensors in the calib config.
for sensor_key, value in self.config['sensors'].items():
# Create a dictionary that describes this sensor
sensor_dict = {'_name': sensor_key, 'parent': value['link'],
'calibration_parent': value['parent_link'],
'calibration_child': value['child_link']}
# TODO replace by utils function
print("Waiting for message")
msg = rospy.wait_for_message(value['topic_name'], rospy.AnyMsg)
connection_header = msg._connection_header['type'].split('/')
ros_pkg = connection_header[0] + '.msg'
msg_type = connection_header[1]
print('Topic ' + value['topic_name'] + ' has type ' + msg_type)
sensor_dict['topic'] = value['topic_name']
sensor_dict['msg_type'] = msg_type
# If topic contains a message type then get a camera_info message to store along with the sensor data
if sensor_dict['msg_type'] == 'Image': # if it is an image must get camera_info
sensor_dict['camera_info_topic'] = os.path.dirname(sensor_dict['topic']) + '/camera_info'
from sensor_msgs.msg import CameraInfo
camera_info_msg = rospy.wait_for_message(sensor_dict['camera_info_topic'], CameraInfo)
from rospy_message_converter import message_converter
sensor_dict['camera_info'] = message_converter.convert_ros_message_to_dictionary(camera_info_msg)
# Get the kinematic chain form world_link to this sensor's parent link
now = rospy.Time()
self.listener.waitForTransform(value['link'], self.world_link, now, rospy.Duration(5))
chain = self.listener.chain(value['link'], now, self.world_link, now, self.world_link)
chain_list = []
for parent, child in zip(chain[0::], chain[1::]):
key = self.generateKey(parent, child)
chain_list.append({'key': key, 'parent': parent, 'child': child})
sensor_dict['chain'] = chain_list # Add to sensor dictionary
self.sensors[sensor_key] = sensor_dict
sensor_labeler = InteractiveDataLabeler(self.server, self.menu_handler, sensor_dict,
marker_size, self.config['calibration_pattern'])
self.sensor_labelers[sensor_key] = sensor_labeler
print('finished visiting sensor ' + sensor_key)
print(Fore.BLUE + sensor_key + Style.RESET_ALL + ':\n' + str(sensor_dict))
# print('sensor_labelers:')
# print(self.sensor_labelers)
self.abstract_transforms = self.getAllAbstractTransforms()
# print("abstract_transforms = " + str(self.abstract_transforms))
def getTransforms(self, abstract_transforms, time=None):
transforms_dict = {} # Initialize an empty dictionary that will store all the transforms for this data-stamp
if time is None:
time = rospy.Time.now()
for ab in abstract_transforms: # Update all transformations
self.listener.waitForTransform(ab['parent'], ab['child'], time, rospy.Duration(1.0))
(trans, quat) = self.listener.lookupTransform(ab['parent'], ab['child'], time)
key = self.generateKey(ab['parent'], ab['child'])
transforms_dict[key] = {'trans': trans, 'quat': quat, 'parent': ab['parent'], 'child': ab['child']}
return transforms_dict
def lockAllLabelers(self):
for sensor_name, sensor in self.sensors.iteritems():
self.sensor_labelers[sensor_name].lock.acquire()
print("Locked all labelers")
def unlockAllLabelers(self):
for sensor_name, sensor in self.sensors.iteritems():
self.sensor_labelers[sensor_name].lock.release()
print("Unlocked all labelers")
def getLabelersTimeStatistics(self):
stamps = [] # a list of the several time stamps of the stored messages
for sensor_name, sensor in self.sensors.iteritems():
stamps.append(copy.deepcopy(self.sensor_labelers[sensor_name].msg.header.stamp))
max_delta = getMaxTimeDelta(stamps)
average_time = getAverageTime(stamps) # For looking up transforms use average time of all sensor msgs
print('Times:')
for stamp in stamps:
printRosTime(stamp)
return stamps, average_time, max_delta
def saveCollection(self):
# --------------------------------------
# Collect sensor data and labels (images, laser scans, etc)
# --------------------------------------
# Lock the semaphore for all labelers
self.lockAllLabelers()
# Analyse message time stamps and decide if collection can be stored
stamps, average_time, max_delta = self.getLabelersTimeStatistics()
if max_delta.to_sec() > float(self.config['max_duration_between_msgs']): # times are close enough?
rospy.logwarn('Max duration between msgs in collection is ' + str(max_delta.to_sec()) +
'. Not saving collection.')
self.unlockAllLabelers()
return None
else: # test passed
rospy.loginfo('Max duration between msgs in collection is ' + str(max_delta.to_sec()))
# Collect all the transforms
transforms = self.getTransforms(self.abstract_transforms, average_time) # use average time of sensor msgs
printRosTime(average_time, "Collected transforms for time ")
all_sensor_data_dict = {}
all_sensor_labels_dict = {}
for sensor_name, sensor in self.sensors.iteritems():
print('collect sensor: ' + sensor_name)
msg = copy.deepcopy(self.sensor_labelers[sensor_name].msg)
labels = copy.deepcopy(self.sensor_labelers[sensor_name].labels)
# TODO add exception also for point cloud and depth image
# Update sensor data ---------------------------------------------
if sensor['msg_type'] == 'Image': # Special case of requires saving image data as png separate files
cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8") # Convert to opencv image and save image to disk
filename = self.output_folder + '/' + sensor['_name'] + '_' + str(self.data_stamp) + '.jpg'
filename_relative = sensor['_name'] + '_' + str(self.data_stamp) + '.jpg'
cv2.imwrite(filename, cv_image)
image_dict = message_converter.convert_ros_message_to_dictionary(msg) # Convert sensor data to dictionary
del image_dict['data'] # Remove data field (which contains the image), and replace by "data_file"
image_dict['data_file'] = filename_relative # Contains full path to where the image was saved
# Update the data dictionary for this data stamp
all_sensor_data_dict[sensor['_name']] = image_dict
else:
# Update the data dictionary for this data stamp
all_sensor_data_dict[sensor['_name']] = message_converter.convert_ros_message_to_dictionary(msg)
# Update sensor labels ---------------------------------------------
if sensor['msg_type'] in ['Image', 'LaserScan']:
all_sensor_labels_dict[sensor_name] = labels
else:
raise ValueError('Unknown message type.')
collection_dict = {'data': all_sensor_data_dict, 'labels': all_sensor_labels_dict, 'transforms': transforms}
self.collections[self.data_stamp] = collection_dict
self.data_stamp += 1
# Save to json file
D = {'sensors': self.sensors, 'collections': self.collections, 'calibration_config': self.config}
self.createJSONFile(self.output_folder + '/data_collected.json', D)
self.unlockAllLabelers()
def getAllAbstractTransforms(self):
rospy.sleep(0.5) # wait for transformations
# Get a list of all transforms to collect
transforms_list = []
now = rospy.Time.now()
all_frames = self.listener.getFrameStrings()
for frame in all_frames:
chain = self.listener.chain(frame, now, self.world_link, now, self.world_link)
for idx in range(0, len(chain) - 1):
parent = chain[idx]
child = chain[idx + 1]
transforms_list.append({'parent': parent, 'child': child, 'key': self.generateKey(parent, child)})
# https://stackoverflow.com/questions/31792680/how-to-make-values-in-list-of-dictionary-unique
uniq_l = list(map(dict, frozenset(frozenset(i.items()) for i in transforms_list)))
return uniq_l # get unique values
def createJSONFile(self, output_file, D):
print("Saving the json output file to " + str(output_file) + ", please wait, it could take a while ...")
f = open(output_file, 'w')
json.encoder.FLOAT_REPR = lambda f: ("%.6f" % f) # to get only four decimal places on the json file
print >> f, json.dumps(D, indent=2, sort_keys=True)
f.close()
print("Completed.")
@staticmethod
def generateKey(parent, child, suffix=''):
return parent + '-' + child + suffix
class DataCollectorAndLabeler:
def __init__(self, args, server, menu_handler):
self.output_folder = utilities.resolvePath(args['output_folder'])
if os.path.exists(
self.output_folder
) and not args['overwrite']: # dataset path exists, abort
print(
'\n' + Fore.RED + 'Error: Dataset ' + self.output_folder +
' exists.\nIf you want to replace it add a "--overwrite" flag.'
+ Style.RESET_ALL + '\n')
rospy.signal_shutdown()
elif os.path.exists(
self.output_folder
) and args['overwrite']: # move existing path to a backup location
now = datetime.now()
dt_string = now.strftime("%Y-%m-%d-%H-%M-%S")
basename = os.path.basename(self.output_folder)
backup_folder = '/tmp/' + basename + '_' + dt_string
time.sleep(2)
print('\n\nWarning: Dataset ' + Fore.YELLOW + self.output_folder +
Style.RESET_ALL + ' exists.\nMoving it to a new folder: ' +
Fore.YELLOW + backup_folder +
'\nThis will be deleted after a system reboot!' +
Style.RESET_ALL + '\n\n')
time.sleep(2)
execute('mv ' + self.output_folder + ' ' + backup_folder,
verbose=True)
os.mkdir(self.output_folder) # Recreate the folder
self.listener = TransformListener()
self.sensors = {}
self.sensor_labelers = {}
self.server = server
self.menu_handler = menu_handler
self.data_stamp = 0
self.collections = {}
self.bridge = CvBridge()
self.config = loadConfig(args['calibration_file'])
if self.config is None:
sys.exit(1) # loadJSON should tell you why.
self.world_link = self.config['world_link']
# Add sensors
print(Fore.BLUE + 'Sensors:' + Style.RESET_ALL)
print('Number of sensors: ' + str(len(self.config['sensors'])))
# Go through the sensors in the calib config.
for sensor_key, value in self.config['sensors'].items():
# Create a dictionary that describes this sensor
sensor_dict = {
'_name': sensor_key,
'parent': value['link'],
'calibration_parent': value['parent_link'],
'calibration_child': value['child_link']
}
# TODO replace by utils function
print("Waiting for message " + value['topic_name'] + ' ...')
msg = rospy.wait_for_message(value['topic_name'], rospy.AnyMsg)
print('... received!')
connection_header = msg._connection_header['type'].split('/')
ros_pkg = connection_header[0] + '.msg'
msg_type = connection_header[1]
print('Topic ' + value['topic_name'] + ' has type ' + msg_type)
sensor_dict['topic'] = value['topic_name']
sensor_dict['msg_type'] = msg_type
# If topic contains a message type then get a camera_info message to store along with the sensor data
if sensor_dict[
'msg_type'] == 'Image': # if it is an image must get camera_info
sensor_dict['camera_info_topic'] = os.path.dirname(
sensor_dict['topic']) + '/camera_info'
from sensor_msgs.msg import CameraInfo
print('Waiting for camera_info message on topic ' +
sensor_dict['camera_info_topic'] + ' ...')
camera_info_msg = rospy.wait_for_message(
sensor_dict['camera_info_topic'], CameraInfo)
print('... received!')
from rospy_message_converter import message_converter
sensor_dict[
'camera_info'] = message_converter.convert_ros_message_to_dictionary(
camera_info_msg)
# Get the kinematic chain form world_link to this sensor's parent link
now = rospy.Time()
print('Waiting for transformation from ' + value['link'] + ' to ' +
self.world_link)
self.listener.waitForTransform(value['link'], self.world_link, now,
rospy.Duration(5))
print('... received!')
chain = self.listener.chain(value['link'], now, self.world_link,
now, self.world_link)
chain_list = []
for parent, child in zip(chain[0::], chain[1::]):
key = self.generateKey(parent, child)
chain_list.append({
'key': key,
'parent': parent,
'child': child
})
sensor_dict['chain'] = chain_list # Add to sensor dictionary
self.sensors[sensor_key] = sensor_dict
sensor_labeler = InteractiveDataLabeler(
self.server, self.menu_handler, sensor_dict,
args['marker_size'], self.config['calibration_pattern'])
self.sensor_labelers[sensor_key] = sensor_labeler
print('Setup for sensor ' + sensor_key + ' is complete.')
print(Fore.BLUE + sensor_key + Style.RESET_ALL + ':\n' +
str(sensor_dict))
# print('sensor_labelers:')
# print(self.sensor_labelers)
self.abstract_transforms = self.getAllAbstractTransforms()
# print("abstract_transforms = " + str(self.abstract_transforms))
def getTransforms(self, abstract_transforms, time=None):
transforms_dict = {
} # Initialize an empty dictionary that will store all the transforms for this data-stamp
if time is None:
time = rospy.Time.now()
for ab in abstract_transforms: # Update all transformations
self.listener.waitForTransform(ab['parent'], ab['child'], time,
rospy.Duration(1.0))
(trans,
quat) = self.listener.lookupTransform(ab['parent'], ab['child'],
time)
key = self.generateKey(ab['parent'], ab['child'])
transforms_dict[key] = {
'trans': trans,
'quat': quat,
'parent': ab['parent'],
'child': ab['child']
}
return transforms_dict
def lockAllLabelers(self):
for sensor_name, sensor in self.sensors.iteritems():
self.sensor_labelers[sensor_name].lock.acquire()
print("Locked all labelers")
def unlockAllLabelers(self):
for sensor_name, sensor in self.sensors.iteritems():
self.sensor_labelers[sensor_name].lock.release()
print("Unlocked all labelers")
def getLabelersTimeStatistics(self):
stamps = [] # a list of the several time stamps of the stored messages
for sensor_name, sensor in self.sensors.iteritems():
stamps.append(
copy.deepcopy(
self.sensor_labelers[sensor_name].msg.header.stamp))
max_delta = getMaxTimeDelta(stamps)
# TODO : this is because of Andre's bag file problem. We should go back to the getAverageTime
# average_time = getAverageTime(stamps) # For looking up transforms use average time of all sensor msgs
average_time = getMaxTime(
stamps
) # For looking up transforms use average time of all sensor msgs
print('Times:')
for stamp, sensor_name in zip(stamps, self.sensors):
printRosTime(stamp, prefix=sensor_name + ': ')
return stamps, average_time, max_delta
def saveCollection(self):
# --------------------------------------
# Collect sensor data and labels (images, laser scans, etc)
# --------------------------------------
# Lock the semaphore for all labelers
self.lockAllLabelers()
# Analyse message time stamps and decide if collection can be stored
stamps, average_time, max_delta = self.getLabelersTimeStatistics()
if max_delta is not None: # if max_delta is None (only one sensor), continue
if max_delta.to_sec() > float(
self.config['max_duration_between_msgs']
): # times are close enough?
rospy.logwarn('Max duration between msgs in collection is ' +
str(max_delta.to_sec()) +
'. Not saving collection.')
self.unlockAllLabelers()
return None
else: # test passed
rospy.loginfo('Max duration between msgs in collection is ' +
str(max_delta.to_sec()))
# Collect all the transforms
transforms = self.getTransforms(
self.abstract_transforms,
average_time) # use average time of sensor msgs
printRosTime(average_time, "Collected transforms for time ")
all_sensor_data_dict = {}
all_sensor_labels_dict = {}
for sensor_key, sensor in self.sensors.iteritems():
print('collect sensor: ' + sensor_key)
msg = copy.deepcopy(self.sensor_labelers[sensor_key].msg)
labels = copy.deepcopy(self.sensor_labelers[sensor_key].labels)
print('sensor' + sensor_key)
# TODO add exception also for point cloud and depth image
# Update sensor data ---------------------------------------------
if sensor[
'msg_type'] == 'Image': # Special case of requires saving image data as png separate files
cv_image = self.bridge.imgmsg_to_cv2(
msg,
"bgr8") # Convert to opencv image and save image to disk
filename = self.output_folder + '/' + sensor[
'_name'] + '_' + str(self.data_stamp) + '.jpg'
filename_relative = sensor['_name'] + '_' + str(
self.data_stamp) + '.jpg'
cv2.imwrite(filename, cv_image)
image_dict = message_converter.convert_ros_message_to_dictionary(
msg) # Convert sensor data to dictionary
del image_dict[
'data'] # Remove data field (which contains the image), and replace by "data_file"
image_dict[
'data_file'] = filename_relative # Contains full path to where the image was saved
# Update the data dictionary for this data stamp
all_sensor_data_dict[sensor['_name']] = image_dict
else:
# Update the data dictionary for this data stamp
all_sensor_data_dict[sensor[
'_name']] = message_converter.convert_ros_message_to_dictionary(
msg)
# Update sensor labels ---------------------------------------------
if sensor['msg_type'] in ['Image', 'LaserScan', 'PointCloud2']:
all_sensor_labels_dict[sensor_key] = labels
else:
raise ValueError('Unknown message type.')
collection_dict = {
'data': all_sensor_data_dict,
'labels': all_sensor_labels_dict,
'transforms': transforms
}
self.collections[self.data_stamp] = collection_dict
self.data_stamp += 1
# Save to json file
D = {
'sensors': self.sensors,
'collections': self.collections,
'calibration_config': self.config
}
print('Saving file ' + self.output_folder + '/data_collected.json')
self.createJSONFile(self.output_folder + '/data_collected.json', D)
self.unlockAllLabelers()
def getAllAbstractTransforms(self):
# Get a list of all transforms to collect
transforms_list = []
now = rospy.Time.now()
all_frames = self.listener.getFrameStrings()
for frame in all_frames:
# print('Waiting for transformation from ' + frame + ' to ' + self.world_link + '(max 3 secs)')
try:
self.listener.waitForTransform(frame, self.world_link, now,
rospy.Duration(3))
chain = self.listener.chain(frame, now, self.world_link, now,
self.world_link)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logerr('Could not get transform from ' + frame + ' to ' +
self.world_link + '(max 3 secs)')
continue
for idx in range(0, len(chain) - 1):
parent = chain[idx]
child = chain[idx + 1]
transforms_list.append({
'parent': parent,
'child': child,
'key': self.generateKey(parent, child)
})
# https://stackoverflow.com/questions/31792680/how-to-make-values-in-list-of-dictionary-unique
uniq_l = list(
map(dict,
frozenset(frozenset(i.items()) for i in transforms_list)))
return uniq_l # get unique values
def createJSONFile(self, output_file, D):
print("Saving the json output file to " + str(output_file) +
", please wait, it could take a while ...")
f = open(output_file, 'w')
json.encoder.FLOAT_REPR = lambda f: (
"%.6f" % f) # to get only four decimal places on the json file
print >> f, json.dumps(D, indent=2, sort_keys=True)
f.close()
print("Completed.")
@staticmethod
def generateKey(parent, child, suffix=''):
return parent + '-' + child + suffix
