#! /usr/bin/env python

from __future__ import print_function
import sys
import cv2
import numpy as np
import rospy
from rospy.numpy_msg import numpy_msg
from sensor_msgs.msg import CompressedImage

class Calibrate:

    def __init__(self):
        pass

    def run(self):
        rospy.init_node('calibrate_distance')

        rospy.Subscriber('image/compressed', numpy_msg(CompressedImage),
                         self.on_image)
        rospy.spin()

    def on_image(self, msg):
        data = np.fromstring(msg.data, np.uint8)
        frame = cv2.imdecode(data, cv2.IMREAD_COLOR)
        rospy.loginfo('Got frame with data size {0}'.format(frame.shape))

        h, w = frame.shape[:2]
        cv2.line(frame, (0,h/2), (w-1,h/2), (0,255,0), 2, cv2.LINE_AA)
        cv2.line(frame, (w/2,0), (w/2,h-1), (0,255,0), 2, cv2.LINE_AA)
        cv2.imshow('image', frame)
        cv2.waitKey(1)

if __name__ == "__main__":
    try:
        Calibrate().run()

    except rospy.ROSInterruptException:
        pass
