detect_target.py 文件源码

def __init__(self):

      # initialize ROS node
      rospy.init_node('target_detector', anonymous=True)

      # initialize publisher for target pose, PoseStamped message, and set initial sequence number
      self.pub = rospy.Publisher('target_pose', PoseStamped, queue_size=1)
      self.pub_pose = PoseStamped()
      self.pub_pose.header.seq = 0

      self.rate = rospy.Rate(1.0)                      # publish message at 1 Hz

      # initialize values for locating target on Kinect v2 image
      self.target_u = 0                        # u is pixels left(0) to right(+)
      self.target_v = 0                        # v is pixels top(0) to bottom(+)
      self.target_d = 0                        # d is distance camera(0) to target(+) from depth image
      self.target_found = False                # flag initialized to False
      self.last_d = 0                          # last non-zero depth measurement

      # target orientation to Kinect v2 image (Euler)
      self.r = 0
      self.p = 0
      self.y = 0

      # Convert image from a ROS image message to a CV image
      self.bridge = CvBridge()

      # Wait for the camera_info topic to become available
      rospy.wait_for_message('/kinect2/qhd/image_color_rect', Image)

      # Subscribe to registered color and depth images
      rospy.Subscriber('/kinect2/qhd/image_color_rect', Image, self.image_callback, queue_size=1)
      rospy.Subscriber('/kinect2/qhd/image_depth_rect', Image, self.depth_callback, queue_size=1)

      self.rate.sleep()                        # suspend until next cycle

   # This callback function handles processing Kinect color image, looking for the pink target.