outbag.write("truth/pose", msg, msg.header.stamp)
# Add the Images
for f in tqdm(sorted(listdir(images_dir))):
if '.png' not in f: continue
img = scipy.misc.imread(images_dir + "/" + f)
time_ns = int(f.split('.')[0])
if (time_ns - t0) > duration * 1e9: break
msg = Image()
msg.header.frame_id = "body"
msg.header.stamp = rospy.Time(int(math.modf(time_ns / 1e9)[1]),
int(math.modf(time_ns / 1e9)[0] * 1e9))
msg.height = img.shape[0]
msg.width = img.shape[1]
msg.encoding = "mono8"
msg.step = img.shape[1]
msg.data = img.tobytes()
outbag.write("color", msg, msg.header.stamp)
sys.stdout.flush()
outbag.close()
# imu = np.array(imu)
# plt.figure(1)
# plt.subplot(211)
# plt.title("gyro")
# plt.plot(imu[:,:3])
# plt.subplot(212)
# plt.title("acc")
# plt.plot(imu[:,3:])
