detect_target.py 文件源码

def image_callback(self, msg):

      # convert ROS image to OpenCV image
      try:
         image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
      except CvBridgeError as e:
         print(e)

      # create hsv image of scene
      hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

      # find pink objects in the image
      lower_pink = numpy.array([139, 0, 240], numpy.uint8)
      upper_pink = numpy.array([159, 121, 255], numpy.uint8)
      mask = cv2.inRange(hsv, lower_pink, upper_pink)

      # dilate and erode with kernel size 11x11
      cv2.morphologyEx(mask, cv2.MORPH_CLOSE, numpy.ones((11,11))) 

      # find all of the contours in the mask image
      contours, heirarchy = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
      self.contourLength  = len(contours)

      # Check for at least one target found
      if self.contourLength < 1:
         print "No target found"

      else:                       # target found

         ## Loop through all of the contours, and get their areas
         area = [0.0]*len(contours)
         for i in range(self.contourLength):
            area[i] = cv2.contourArea(contours[i])

         #### Target #### the largest "pink" object
         target_image = contours[area.index(max(area))]

         # Using moments find the center of the object and draw a red outline around the object
         target_m = cv2.moments(target_image)
         self.target_u = int(target_m['m10']/target_m['m00'])
         self.target_v = int(target_m['m01']/target_m['m00'])
         points = cv2.minAreaRect(target_image)
         box = cv2.cv.BoxPoints(points)
         box = numpy.int0(box)
         cv2.drawContours(image, [box], 0, (0, 0, 255), 2)
         rospy.loginfo("Center of target is x at %d and y at %d", int(self.target_u), int(self.target_v))

         self.target_found = True               # set flag for depth_callback processing

         # show image with target outlined with a red rectangle
         cv2.imshow ("Target", image)
         cv2.waitKey(3)

   # This callback function handles processing Kinect depth image, looking for the depth value 
   #   at the location of the center of the pink target.