image_conv.py

#!/usr/bin/env python
from __future__ import print_function
import roslib
roslib.load_manifest('camera')
import sys
import rospy
import cv2
from std_msgs.msg import Int16
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import numpy as np
import cv2.cv as cv
from camera.msg import ball

kernel = np.ones((5,5),np.uint8)

STATE = 1 #PICK -- initial state

bridge = CvBridge()

CB_COUNT = 0
RUN_COUNT = 0

# Initial values(of ball)
hmn = 21 #3
hmx = 37 #28
smn = 118 #77
smx = 255
vmn = 107 #186
vmx = 255

def nothing(x):
    pass

def state(data):
  if data == 1:
    print ("Find ball")# red
    hmn = 3 #39
    hmx = 28 #80
    smn = 77 #77
    smx = 255
    vmn = 186 #186
    vmx = 255
  elif data == 0:
    print ("Find target") #green
    hmn = 39 #3
    hmx = 80 #28
    smn = 77 #80
    smx = 255
    vmn = 186 #154
    vmx = 255
  else:
    pass

def run():
  global cv_image
  pub = rospy.Publisher('ball_pos', ball, queue_size=1)
  msg = ball()
  msg.x = 50
  msg.y = 50
  msg.radius = 50

  # (rows,cols,channels) = cv_image.shape
  # if cols > 60 and rows > 60 :
  #   cv2.circle(cv_image, (50,50), 10, 255)

  ######################################################################
  #converting to HSV
  hsv = cv2.cvtColor(cv_image,cv2.COLOR_BGR2HSV)
  hue,sat,val = cv2.split(hsv)

  # get info from track bar and appy to result
  # hmn = cv2.getTrackbarPos('hmin','HueComp')
  # hmx = cv2.getTrackbarPos('hmax','HueComp')

  # smn = cv2.getTrackbarPos('smin','SatComp')
  # smx = cv2.getTrackbarPos('smax','SatComp')

  # vmn = cv2.getTrackbarPos('vmin','ValComp')
  # vmx = cv2.getTrackbarPos('vmax','ValComp')

  # Apply thresholding
  hthresh = cv2.inRange(np.array(hue),np.array(hmn),np.array(hmx))
  sthresh = cv2.inRange(np.array(sat),np.array(smn),np.array(smx))
  vthresh = cv2.inRange(np.array(val),np.array(vmn),np.array(vmx))

  # AND h s and v
  tracking = cv2.bitwise_and(hthresh,cv2.bitwise_and(sthresh,vthresh))

  # Some morpholigical filtering
  dilation = cv2.dilate(tracking,kernel,iterations = 1)
  closing = cv2.morphologyEx(dilation, cv2.MORPH_CLOSE, kernel)
  closing = cv2.GaussianBlur(closing,(5,5),0)

  # Detect circles using HoughCircles
  circles = cv2.HoughCircles(closing,cv.CV_HOUGH_GRADIENT,2,120,param1=120,param2=50,minRadius=10,maxRadius=0)
  # circles = np.uint16(np.around(circles))

  #Draw Circles
  if circles is not None:
    for i in circles[0,:]:
      # If the ball is far, draw it in green
      if int(round(i[2])) < 30:
          cv2.circle(cv_image,(int(round(i[0])),int(round(i[1]))),int(round(i[2])),(0,255,0),5) # image, center, radius, colour, thickness
          cv2.circle(cv_image,(int(round(i[0])),int(round(i[1]))),2,(0,255,0),10)
      # else draw it in red
      elif int(round(i[2])) > 35:
          cv2.circle(cv_image,(int(round(i[0])),int(round(i[1]))),int(round(i[2])),(0,0,255),5)
          cv2.circle(cv_image,(int(round(i[0])),int(round(i[1]))),2,(0,0,255),10)
          # buzz = 1
      print ("center: %d, %d" % (int(round(i[0])), int(round(i[1]))))

      # publish msg
      msg.x = int(round(i[0]))
      msg.y = int(round(i[1]) )
      msg.radius = int(round(i[2]))
      pub.publish(msg)
  global RUN_COUNT
  RUN_COUNT += 1
  #print("run=%d" %RUN_COUNT)
  #you can use the 'buzz' variable as a trigger to switch some GPIO lines on Rpi :)
  # print buzz
  # if buzz:
      # put your GPIO line here


  #Show the result in frames
  # cv2.imshow('HueComp',hthresh)
  # cv2.imshow('SatComp',sthresh)
  # cv2.imshow('ValComp',vthresh)
  # cv2.imshow('closing',closing)
  # cv2.imshow('tracking',cv_image)

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

  # try:
  #   self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
  # except CvBridgeError as e:
  #   print(e)

def callback(data):
  print ("callback OKAY!")
  global bridge
  global cv_image
  try:
    cv_image = bridge.imgmsg_to_cv2(data, "bgr8")
  except CvBridgeError as e:
    print(e)
  global CB_COUNT
  CB_COUNT += 1
  #print("CB=%d" %CB_COUNT)
  run()

def image_process():
  # bridge = CvBridge()
  print ("1")
  image_sub = rospy.Subscriber("/usb_cam/image_raw", Image, callback)
  state_sub = rospy.Subscriber("/robot_state", Int16 , state)
  print("\nImage_converter started")

  ###################################################################
  # cv2.namedWindow('HueComp')
  # cv2.namedWindow('SatComp')
  # cv2.namedWindow('ValComp')
  # cv2.namedWindow('closing')
  # cv2.namedWindow('tracking')

  # Creating track bar for min and max for hue, saturation and value
  # You can adjust the defaults as you like
  # cv2.createTrackbar('hmin', 'HueComp',3,179,nothing)
  # cv2.createTrackbar('hmax', 'HueComp',28,179,nothing)

  # cv2.createTrackbar('smin', 'SatComp',77,255,nothing)
  # cv2.createTrackbar('smax', 'SatComp',255,255,nothing)

  # cv2.createTrackbar('vmin', 'ValComp',186,255,nothing)
  # cv2.createTrackbar('vmax', 'ValComp',255,255,nothing)
  print ("windows created")

  #run()
  ###################################################################
  # spin() simply keeps python from exiting until this node is stopped
  rospy.spin()

if __name__ == '__main__':
  rospy.init_node('image_converter', anonymous=True)
  image_process()