detect_crazyflie.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 green objects in the image
      lower_green = numpy.array([50, 50, 177], numpy.uint8)      # fluffy green ball
      upper_green = numpy.array([84, 150, 255], numpy.uint8)
      mask = cv2.inRange(hsv, lower_green, upper_green)

      # 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 ball found
      if self.contourLength < 1:
         print "No objects found"
         sys.exit("No objects found")        # if no Crazyflie in image, exit process

      ## 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])

      #### Ball #### the largest "green" object
      ball_image = contours[area.index(max(area))]

      # Find the circumcircle of the green ball and draw a blue outline around it
      (self.cf_u,self.cf_v),radius = cv2.minEnclosingCircle(ball_image)
      ball_center = (int(self.cf_u),int(self.cf_v))
      ball_radius = int(radius)
      cv2.circle(image, ball_center, ball_radius, (255,0,0), 2)

      # show image with green ball outlined with a blue circle
      cv2.imshow ("KinectV2", 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 green ball on top of Crazyflie.