detect_crazyflie.py 文件源码

def depth_callback(self, msg):

      # create OpenCV depth image using defalut passthrough encoding
      try:
         depth_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough')
      except CvBridgeError as e:
         print(e)

      # using green ball (u, v) position, find depth value of Crazyflie point and divide by 1000
      # to change millimeters into meters (for Kinect sensors only)
      self.cf_d = depth_image[self.cf_v, self.cf_u] / 1000.0
      rospy.loginfo("Depth: x at %d  y at %d  z at %f", int(self.cf_u), int(self.cf_v), self.cf_d)

      # if depth value is zero, use the last non-zero depth value
      if self.cf_d == 0:
         self.cf_d = self.last_d
      else:
         self.last_d = self.cf_d

      # publish Crazyflie tf transform
      self.update_cf_transform (self.cf_u, self.cf_v, self.cf_d)


   # This function builds the Crazyflie base_link tf transform and publishes it.