from sensor_msgs.msg import CameraInfo, Image
from cv_bridge import CvBridge, CvBridgeError
import cv2

if __name__ == '__main__':
    rospy.init_node('node_name')
    while ~rospy.is_shutdown():
        sub_cam_info = rospy.Subscriber('/camera/rgb/raw_camera_info', CameraInfo)
        sub_rgb = rospy.Subscriber('/camera/rgb/raw_image_color', Image)

camera_info_K = sub_cam_info.K
camera_info_dist_model = sub_cam_info.distortion_model
rgb_image = CvBridge().imgmsg_to_cv2(sub_rgb, encoding="rgb8")

rgb_undist = cv2.undistort(rgb_img.data, camera_info_K, camera_info_dist_model)