detect_target.py 文件源码

def depth_callback(self, msg):

      # process only if target is found
      if self.target_found == True:

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

         # using target (v, u) location, find depth value of point and divide by 1000
         # to change millimeters into meters (for Kinect sensors only)
         self.target_d = depth_image[self.target_v, self.target_u] / 1000.0

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

         # record target location and publish target pose message
         rospy.loginfo("Target depth: x at %d  y at %d  z at %f", int(self.target_u), 
                             int(self.target_v), self.target_d)
         self.update_target_pose (self.target_u, self.target_v, self.target_d)

   # This function builds the target PoseStamped message and publishes it.