def test_forward_until_scanned(u, precached):
coords = None
# MOVE FORWARD until sample is seen by scan cam
u.motor_right(0.1)
u.motor_left(0.1)
while coords is None:
if precached:
coords = scan2.check_precached(scan_img_left, scan_img_right)
else:
coords = scan2.check_easy_sample(scan_img_left, scan_img_right)
print "saw at thing"
# once sample is seen, stop and move arm back
u.motor_left(0.0)
u.motor_right(0.0)
time.sleep(1)
rospy.loginfo("coords: " + str(coords))
u.arm_target("X", 0)
u.arm_target("Y", u.arm_max("Y"))
while u.arm_should_be_moving("X") or u.arm_should_be_moving("Y"):
pass
return coords
def test_scan_easy():
time.sleep(3)
while not rospy.is_shutdown():
coords = scan2.check_easy_sample(scan_img_left, scan_img_right)
rospy.loginfo("coords: " + str(coords))
cv2.imshow('LEFT', scan_img_left)
cv2.imshow('RIGHT', scan_img_right)
def test_forward_until_scanned_both(u):
coords = None
precached = False # is the scanned sample precached or easy?
# MOVE FORWARD until sample is seen by scan cam
u.motor_right(0.1)
u.motor_left(0.1)
while coords is None:
coords = scan2.check_easy_sample(scan_img_left, scan_img_right)
if coords[0] is "Left": print "Turn Left"
elif coords[0] is "Right": print "Turn Right"
if coords is None:
coords = scan2.check_precached(scan_img_left, scan_img_right)
if coords is not None:
precached = True
# once sample is seen, stop and move arm back
u.motor_left(0.0)
u.motor_right(0.0)
time.sleep(1)
rospy.loginfo("coords: " + str(coords))
u.arm_target("X", 0)
u.arm_target("Y", u.arm_max("Y"))
while u.arm_should_be_moving("X") or u.arm_should_be_moving("Y"):
pass
return (coords, precached)
def test_scan_easy():
time.sleep(3)
while not rospy.is_shutdown():
coords = scan2.check_easy_sample(scan_img_left, scan_img_right)
rospy.loginfo("coords: " + str(coords))
cv2.imshow('LEFT', scan_img_left)
cv2.imshow('RIGHT', scan_img_right)
def test_forward_until_scanned_both(u):
coords = None
precached = False # is the scanned sample precached or easy?
# MOVE FORWARD until sample is seen by scan cam
u.motor_right(0.1)
u.motor_left(0.1)
while coords is None:
coords = scan2.check_easy_sample(scan_img_left, scan_img_right)
if coords[0] is "Left": print "Turn Left"
elif coords[0] is "Right": print "Turn Right"
if coords is None:
coords = scan2.check_precached(scan_img_left, scan_img_right)
if coords is not None:
precached = True
# once sample is seen, stop and move arm back
u.motor_left(0.0)
u.motor_right(0.0)
time.sleep(1)
rospy.loginfo("coords: " + str(coords))
u.arm_target("X", 0)
u.arm_target("Y", u.arm_max("Y"))
while u.arm_should_be_moving("X") or u.arm_should_be_moving("Y"): pass
return (coords, precached)
def test_forward_until_scanned(u, precached):
coords = None
# MOVE FORWARD until sample is seen by scan cam
u.motor_right(0.1)
u.motor_left(0.1)
while coords is None:
if precached:
coords = scan2.check_precached(scan_img_left, scan_img_right)
else:
coords = scan2.check_easy_sample(scan_img_left, scan_img_right)
print "saw at thing"
# once sample is seen, stop and move arm back
u.motor_left(0.0)
u.motor_right(0.0)
time.sleep(1)
rospy.loginfo("coords: " + str(coords))
u.arm_target("X", 0)
u.arm_target("Y", u.arm_max("Y"))
while u.arm_should_be_moving("X") or u.arm_should_be_moving("Y"): pass
return coords
