import yaml
from sensor_msgs.msg import CameraInfo


def yaml_to_CameraInfo(yaml_fname):
    # Load data from file
    with open(yaml_fname, "r") as file_handle:
        calib_data = yaml.load(file_handle)

    # try to load the parameters
    camera_info_msg = CameraInfo()
    try: 
        camera_info_msg.width = calib_data["image_width"]
    except:
        pass

    try: 
        camera_info_msg.height = calib_data["image_height"]
    except:
        pass

    try: 
        camera_info_msg.k = calib_data["camera_matrix"]["data"]
    except:
        pass

    try: 
        camera_info_msg.d = calib_data["distortion_coefficients"]["data"]
    except:
        pass

    try: 
        camera_info_msg.r = calib_data["rectification_matrix"]["data"]
    except:
        pass

    try: 
        camera_info_msg.p = calib_data["projection_matrix"]["data"]
    except:
        pass

    try: 
        camera_info_msg.distortion_model = calib_data["distortion_model"]
    except:
        pass

    return camera_info_msg