cam_info.P[0 * 4 + 2] = yaml_data[0, 2]
cam_info.P[1 * 4 + 1] = yaml_data[1, 1]
cam_info.P[1 * 4 + 2] = yaml_data[1, 2]
cam_info.P[2 * 4 + 2] = 1
for i in xrange(len(image_list_rgb)):
print(str(i).zfill(4) + " / " + str(len(image_list_rgb)))
now = rospy.get_rostime()
rgb_frame = cv2.imread(image_list_rgb[i], cv2.IMREAD_COLOR)
depth_frame = cv2.imread(image_list_depth[i], cv2.IMREAD_ANYDEPTH)
# depth_frame = depth_frame[:, 154:]
# depth_frame = depth_frame[:, :-154]
# depth_frame = cv2.resize(depth_frame,(424, 512), interpolation = cv2.INTER_CUBIC)
msg_frame_rgb = CvBridge().cv2_to_imgmsg(rgb_frame, encoding="bgr8")
msg_frame_depth = CvBridge().cv2_to_imgmsg(depth_frame,
encoding="16UC1")
msg_frame_depth.width = depth_frame.shape[1]
msg_frame_depth.height = depth_frame.shape[0]
# msg_frame_depth.step =
msg_frame_rgb.header.stamp = now
msg_frame_rgb.header.frame_id = "world"
msg_frame_depth.header = cam_info.header = msg_frame_rgb.header
while (rospy.get_rostime() - now).to_sec() < seconds_for_image:
rgb_pub.publish(msg_frame_rgb)
depth_pub.publish(msg_frame_depth)
camera_info_pub.publish(cam_info)
prefix = "/".join(image_list_depth[i].split("/")[:-1])
name = image_list_depth[i].split("/")[-1][:-4]
names_pub.publish(prefix + "/" + name)
if rospy.is_shutdown():
sys.exit(1)
rospy.sleep(0.01)
