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)