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.
detect_target.py 文件源码
python
阅读 23
收藏 0
点赞 0
评论 0
评论列表
文章目录