def csv2bag(ground_truth_file,odometry_file,laser_file,output_folder):
output_folder = sys.argv[4]
# infer name from ground truth
dataset_name = sys.argv[1].split('/')[-1]
savename = join(output_folder,dataset_name.split('.')[0])+'.bag'
print savename
# we need to read these 3 data elements together
# idea: we build a dictionary were keys are timestamps
dataset = []
dataset_items = {'ground_truth': sys.argv[1],'odometry': sys.argv[2], 'laser': sys.argv[3]}
for key,value in dataset_items.iteritems():
dataset_item = open(value)
# skip the csv header
first = True
for line in dataset_item.readlines():
if first:
first = False
else:
line = line.strip()
tokens = line.split(',')
timestamp = float(tokens[0])
tokens = [elem.strip() for elem in tokens]
data = (key, ','.join(tokens[1:]))
dataset.append((timestamp, data))
dataset_item.close()
# probably useful to sort here
dataset = sorted(dataset, key=lambda x: x[0])
bag = rosbag.Bag(savename, 'w')
iteration = 0
odom_x, odom_y, odom_theta = 0,0,0
for timestamp, data in dataset:
# we need to decide how to interpret this data
if True:#iteration%20==0:
if data[0] == 'ground_truth':
line = data[1].strip()
tokens = line.split(',')
msg = Odometry()
msg.header.frame_id = 'odom'
t = rospy.Time(timestamp)
msg.header.stamp = t
msg.pose.pose.position.x = float(tokens[0])
msg.pose.pose.position.y = float(tokens[1])
msg.pose.pose.position.z = 0.0
odom_quat = tf.transformations.quaternion_from_euler(0, 0, float(tokens[2]))
msg.pose.pose.orientation = Quaternion(*odom_quat)
msg.child_frame_id = "base_link"
# I have to figure out if this data is available
msg.twist.twist.linear.x = 0.0
msg.twist.twist.linear.y = 0.0
msg.twist.twist.angular.z = 0.0
bag.write('base_pose_ground_truth', msg, t)
elif data[0] == 'odometry':
line = data[1].strip()
tokens = line.split(',')
msg = Odometry()
msg.header.frame_id = 'odom'
t = rospy.Time(timestamp)
msg.header.stamp = t
msg.pose.pose.position.x = float(tokens[3])
msg.pose.pose.position.y = float(tokens[4])
msg.pose.pose.position.z = 0.0
odom_quat = tf.transformations.quaternion_from_euler(0, 0, float(tokens[5]))
msg.pose.pose.orientation = Quaternion(*odom_quat)
msg.child_frame_id = "base_link"
# I have to figure out if this data is available
msg.twist.twist.linear.x = 0.0
msg.twist.twist.linear.y = 0.0
msg.twist.twist.angular.z = 0.0
bag.write('odom', msg, t)
odom_trans = TransformStamped()
odom_trans.header.stamp = rospy.Time(timestamp)
odom_trans.header.frame_id = "/odom"
odom_trans.child_frame_id = "/base_link"
odom_trans.transform.translation.x = float(tokens[3])
odom_trans.transform.translation.y = float(tokens[4])
odom_trans.transform.translation.z = 0.0
odom_trans.transform.rotation = Quaternion(*odom_quat)
tf_msg = TFMessage()
tf_msg.transforms.append(odom_trans)
bag.write('tf', tf_msg, t)
odom_x, odom_y, odom_theta = [float(t) for t in tokens[3:6]]
t = rospy.Time(timestamp)
tf_msg = make_tf_msg(0.08, 0.0, 0.0, t)
bag.write('tf', tf_msg, t)
elif data[0] == 'laser':
line = data[1].strip()
tokens = line.split(',')
msg = LaserScan()
num_scans = len(tokens)-2
msg.header.frame_id = 'base_laser_link'
t = rospy.Time(timestamp)
msg.header.stamp = t
msg.angle_min = -90.0 / 180.0 * pi
msg.angle_max = 90.0 / 180.0 * pi
msg.angle_increment = pi / num_scans
msg.time_increment = 0.2 / 360.0
msg.scan_time = 0.2
msg.range_min = 0.001
msg.range_max = 50.0
msg.ranges = [float(r) for r in tokens[2:(num_scans + 1)]]
bag.write('base_scan', msg, t)
# experimental: reuse immediately preceding odometry data as pose
#t = rospy.Time(timestamp)
#tf_msg = make_tf_msg(odom_x, odom_y, odom_theta, t)
#bag.write('tf', tf_msg, t)
iteration+=1
