def check_precached(left, right):
# check if precached sample is in the view of the left and right images
# and return its coordinates if it is
xyz = None
left_detect = HaarDetector.detect_precached(left)
right_detect = HaarDetector.detect_precached(right)
if left_detect is not None and right_detect is not None:
xyz = calculate_distance(left_detect[0], right_detect[0], left_detect[1], right_detect[1])
rospy.loginfo("Precached:(x,y,z): {0}".format(xyz))
# Debug functionality
#cv2.imshow('LEFT', HaarDetector.get_image())
#cv2.imshow('RIGHT', HaarDetector.get_image())
return xyz
def scan():
# publishes float lists to the sample_coords topic (consumed by nav)
pub = rospy.Publisher('sample_coords', Coords3D, queue_size=10)
# initialize node and name it scan
rospy.init_node('scan', anonymous=False)
rospy.loginfo("entered scan state")
rospy.Subscriber('grab_success', Bool, grab_handle)
# scan cam images
rospy.Subscriber('stereo/left/image_rect_color', Image, left_scan)
rospy.Subscriber('stereo/right/image_rect_color', Image, right_scan)
#Initialize a HaarDetector precached instance
HaarDetector.initialize_precached()
#rate = rospy.Rate(10) # 10hz, loop will run 10 times/second maximum
while not rospy.is_shutdown():
(coords, precached) = scan_for_samples()
if coords is not None:
rospy.loginfo("sample detected by sample cam at " +
"({0},{1},{2})".format(
coords[0], coords[1], coords[2]))
if precached:
rospy.loginfo('sample type: precached')
else:
rospy.loginfo('sample type: easy')
pub.publish(coords[0], coords[1], coords[2], precached)
global halt_scan
halt_scan = True
rospy.loginfo("scan has been halted")
while(halt_scan):
# do nothing until grab finishes
rate.sleep()
rospy.loginfo("returning to scan state")
def identify_precached(img):
return HaarDetector.detect_precached(img)