#!/usr/bin/env python

import roslib
import rospy
import cv2

import numpy as np
import sys
from std_msgs.msg import Header
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

class image_openCV:
	def __init__(self):
		self.bridge=CvBridge()
		self.image_sub=rospy.Subscriber("bebop/image_raw",Image,self.callback)

	def callback(self, data):
		try:
			cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
		except CvBridgeError as e:
			print(e)

		(rows,cols,channels) = cv_image.shape
		img_gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
		circles = cv2.HoughCircles(img_gray, cv2.HOUGH_GRADIENT, 1.3,100 )
		if circles is not None:
			#circles = np.round(circles[0, :]).astype("int")
			#cv2.circle(cv_image, (x, y), r, (0, 255, 0), 4)
			#circles = np.uint16(np.around(circles))
			for i in circles[0,:]:
    				cv2.circle(cv_image,(i[0],i[1]),i[2],(0,255,0),2)
    				cv2.circle(cv_image,(i[0],i[1]),2,(0,0,255),3)

				

		cv2.imshow("Image window", cv_image)
		cv2.waitKey(3)

def main(args):
	ic = image_openCV()
	rospy.init_node('image_openCV', anonymous=True)
	try:
		rospy.spin()
	except KeyboardInterrupt:
		print("Shutting down")
	cv2.destroyAllWindows()
 
if __name__ == '__main__':
     main(sys.argv)
