#!/usr/bin/env python

# Python libs
import sys, time
import numpy as np
# OpenCV
import cv2 as cv
# Ros libraries
import roslib
import rospy
# Ros Messages
from sensor_msgs.msg import CompressedImage


class Image:
    def __init__(self):
        # self.bridge = CvBridge()

        # subscribed Topic
        self.subscriber = rospy.Subscriber("team1/camera/rgb/compressed",
                                           CompressedImage,
                                           self.callback,
                                           queue_size=1)

    def callback(self, ros_data):
        #### direct conversion to CV2 ####
        np_arr = np.fromstring(ros_data.data, np.uint8)
        self.image_np = cv.imdecode(np_arr, cv.IMREAD_COLOR)
        # print "Callback"

        cv.imshow('RGB', self.image_np)
        k = cv.waitKey(1)

def main(args):
    '''Initializes and cleanup ros node'''
    rospy.init_node('drive_car', anonymous=True)
    # define frame object and detect
    frame = Image()

    try:
        rospy.spin()
    except KeyboardInterrupt:
        print "Shutting down ROS Image feature detector module"
    cv.destroyAllWindows()

if __name__ == '__main__':
    main(sys.argv)