def __init__(self):
    'Default constructor'
    self.rgb_image_topic = rospy.get_param('~rgb_image_topic', '/camera/rgb/image_raw')
    self.camera_publisher = rospy.Publisher(self.rgb_image_topic, sensor_msgs.msg.Image, queue_size=1)
    self.camera_pub_frequency = rospy.get_param('~camera_pub_frequency', 20)
    self.bridge = CvBridge()
    self.camera_info_topic = rospy.get_param('~camera_info_topic', '/camera/rgb/camera_info')
    self.camera_info = sensor_msgs.msg.CameraInfo()
    self.camera_info_publisher = rospy.Publisher(self.camera_info_topic, sensor_msgs.msg.CameraInfo, queue_size=1)
    self.camera_frame_id = rospy.get_param('~camera_frame_id', 'camera_link')
    self.camera_info.header.frame_id = self.camera_frame_id
    self.capture = cv2.VideoCapture(0)
    (ok, frame) = self.capture.read()
    (width, height, _) = frame.shape
    focal_length = height
    center = ((height / 2), (width / 2))
    camera_matrix = np.array([[focal_length, 0, center[0]], [0, focal_length, center[1]], [0, 0, 1]], dtype='double')
    P_matrix = np.array([[focal_length, 0, center[0], 0], [0, focal_length, center[1], 0], [0, 0, 1, 0]], dtype='double')
    dist_coeffs = np.zeros((4, 1))
    self.camera_info.D = list(dist_coeffs)
    self.camera_info.K = list(camera_matrix.flatten())
    self.camera_info.P = list(P_matrix.flatten())
    self.timer = rospy.Timer(rospy.Duration((1.0 / self.camera_pub_frequency)), self.timer_callback)
    rospy.loginfo('Camera publisher ready !')
    while (not rospy.is_shutdown()):
        rospy.spin()
    self.capture.release()
Default constructor
def __init__(self):
    
    self.rgb_image_topic = rospy.get_param('~rgb_image_topic', '/camera/rgb/image_raw')
    self.camera_publisher = rospy.Publisher(self.rgb_image_topic, sensor_msgs.msg.Image, queue_size=1)
    self.camera_pub_frequency = rospy.get_param('~camera_pub_frequency', 20)
    self.bridge = CvBridge()
    self.camera_info_topic = rospy.get_param('~camera_info_topic', '/camera/rgb/camera_info')
    self.camera_info = sensor_msgs.msg.CameraInfo()
    self.camera_info_publisher = rospy.Publisher(self.camera_info_topic, sensor_msgs.msg.CameraInfo, queue_size=1)
    self.camera_frame_id = rospy.get_param('~camera_frame_id', 'camera_link')
    self.camera_info.header.frame_id = self.camera_frame_id
    self.capture = cv2.VideoCapture(0)
    (ok, frame) = self.capture.read()
    (width, height, _) = frame.shape
    focal_length = height
    center = ((height / 2), (width / 2))
    camera_matrix = np.array([[focal_length, 0, center[0]], [0, focal_length, center[1]], [0, 0, 1]], dtype='double')
    P_matrix = np.array([[focal_length, 0, center[0], 0], [0, focal_length, center[1], 0], [0, 0, 1, 0]], dtype='double')
    dist_coeffs = np.zeros((4, 1))
    self.camera_info.D = list(dist_coeffs)
    self.camera_info.K = list(camera_matrix.flatten())
    self.camera_info.P = list(P_matrix.flatten())
    self.timer = rospy.Timer(rospy.Duration((1.0 / self.camera_pub_frequency)), self.timer_callback)
    rospy.loginfo('Camera publisher ready !')
    while (not rospy.is_shutdown()):
        rospy.spin()
    self.capture.release()