def turn_to_sample(u, coords, precached):
print "turning"
rospy.loginfo("COORDS[0]: " + str(coords[0]))
while coords[0] > 40 or coords[0] < -40:
if coords[0] > 0:
rospy.loginfo("turn right")
u.motor_left(0.22)
u.motor_right(-0.22)
time.sleep(1)
u.motor_left(0.0)
u.motor_right(0.0)
if precached:
coords = scan.check_precached(scan_img_left, scan_img_right)
else:
coords = scan.check_easy_sample(scan_img_left, scan_img_right)
elif coords[0] <= 0:
rospy.loginfo("turn left")
u.motor_left(-0.22)
u.motor_right(0.22)
time.sleep(1)
u.motor_right(0.0)
u.motor_left(0.0)
if precached:
coords = scan.check_precached(scan_img_left, scan_img_right)
else:
coords = scan.check_easy_sample(scan_img_left, scan_img_right)
while coords is None:
if precached:
coords = scan.check_precached(scan_img_left, scan_img_right)
else:
coords = scan.check_easy_sample(scan_img_left, scan_img_right)
def turn_to_sample(u, coords, precached):
print "turning"
rospy.loginfo("COORDS[0]: " + str(coords[0]))
while coords[0] > 40 or coords[0] < -40:
if coords[0] > 0:
rospy.loginfo("turn right")
u.motor_left(0.22)
u.motor_right(-0.22)
time.sleep(1)
u.motor_left(0.0)
u.motor_right(0.0)
if precached:
coords = scan.check_precached(scan_img_left, scan_img_right)
else:
coords = scan.check_easy_sample(scan_img_left, scan_img_right)
elif coords[0] <= 0:
rospy.loginfo("turn left")
u.motor_left(-0.22)
u.motor_right(0.22)
time.sleep(1)
u.motor_right(0.0)
u.motor_left(0.0)
if precached:
coords = scan.check_precached(scan_img_left, scan_img_right)
else:
coords = scan.check_easy_sample(scan_img_left, scan_img_right)
while coords is None:
if precached:
coords = scan.check_precached(scan_img_left, scan_img_right)
else:
coords = scan.check_easy_sample(scan_img_left, 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 = scan.check_precached(scan_img_left, scan_img_right)
if coords is None:
coords = scan.check_easy_sample(scan_img_left, scan_img_right)
else:
precached = True
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, precached)
def test_scan_precached():
while not rospy.is_shutdown():
coords = scan.check_precached(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_left', 2, '0.1', rospy.Time.now())
u('motor_right', 2, '0.1', rospy.Time.now())
while coords is None:
coords = scan.check_precached(scan_img_left, scan_img_right)
if coords is None:
coords = scan.check_easy_sample(scan_img_left, scan_img_right)
else:
precached = True
print "saw at thing"
# once sample is seen, stop and move arm back
u('motor_left', 2, '0.0', rospy.Time.now())
u('motor_right', 2, '0.0', rospy.Time.now())
time.sleep(1)
rospy.loginfo("coords: " + str(coords))
u('arm_target', 2, 'X 0', rospy.Time.now())
params = 'Y ' + str(u('arm_max', 2, 'Y', rospy.Time.now()))
u('arm_target', 2, params, rospy.Time.now())
while u('arm_should_be_moving', 2, 'X', rospy.Time.now()) or u('arm_should_be_moving', 2, 'Y', rospy.Time.now()): pass
return (coords, precached)
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_left', 2, '0.1', rospy.Time.now())
u('motor_right', 2, '0.1', rospy.Time.now())
while coords is None:
coords = scan.check_precached(scan_img_left, scan_img_right)
if coords is None:
coords = scan.check_easy_sample(scan_img_left, scan_img_right)
else:
precached = True
print "saw at thing"
# once sample is seen, stop and move arm back
u('motor_left', 2, '0.0', rospy.Time.now())
u('motor_right', 2, '0.0', rospy.Time.now())
time.sleep(1)
rospy.loginfo("coords: " + str(coords))
u('arm_target', 2, 'X 0', rospy.Time.now())
params = 'Y ' + str(u('arm_max', 2, 'Y', rospy.Time.now()))
u('arm_target', 2, params, rospy.Time.now())
while u('arm_should_be_moving', 2, 'X', rospy.Time.now()) or u(
'arm_should_be_moving', 2, 'Y', rospy.Time.now()):
pass
return (coords, precached)
def turn_to_sample(u, coords, precached):
print "turning"
rospy.loginfo("COORDS[0]: " + str(coords[0]))
time_turn = 1.2
while coords[0] > 40 or coords[0] < -40:
if coords[0] > 0:
rospy.loginfo("turn right")
u('motor_left', 2, '0.22', rospy.Time.now())
u('motor_right', 2, '-0.22', rospy.Time.now())
time.sleep(time_turn)
u('motor_left', 2, '0.0', rospy.Time.now())
u('motor_right', 2, '0.0', rospy.Time.now())
if precached:
coords = scan.check_precached(scan_img_left, scan_img_right)
else:
coords = scan.check_easy_sample(scan_img_left, scan_img_right)
elif coords[0] <= 0:
rospy.loginfo("turn left")
u('motor_left', 2, '-0.22', rospy.Time.now())
u('motor_right', 2, '0.22', rospy.Time.now())
time.sleep(time_turn)
u('motor_left', 2, '0.0', rospy.Time.now())
u('motor_right', 2, '0.0', rospy.Time.now())
if precached:
coords = scan.check_precached(scan_img_left, scan_img_right)
else:
coords = scan.check_easy_sample(scan_img_left, scan_img_right)
# go forward a bit after each turn
u('motor_left', 2, '0.1', rospy.Time.now())
u('motor_right', 2, '0.1', rospy.Time.now())
time.sleep(2)
u('motor_left', 2, '0.0', rospy.Time.now())
u('motor_right', 2, '0.0', rospy.Time.now())
while coords is None:
if precached:
coords = scan.check_precached(scan_img_left, scan_img_right)
else:
coords = scan.check_easy_sample(scan_img_left, scan_img_right)
if time_turn > 0.4:
time_turn -= 0.1
def turn_to_sample(u, coords, precached):
print "turning"
rospy.loginfo("COORDS[0]: " + str(coords[0]))
time_turn = 1.2
while coords[0] > 40 or coords[0] < -40:
if coords[0] > 0:
rospy.loginfo("turn right")
u('motor_left', 2, '0.22', rospy.Time.now())
u('motor_right', 2, '-0.22', rospy.Time.now())
time.sleep(time_turn)
u('motor_left', 2, '0.0', rospy.Time.now())
u('motor_right', 2, '0.0', rospy.Time.now())
if precached:
coords = scan.check_precached(scan_img_left, scan_img_right)
else:
coords = scan.check_easy_sample(scan_img_left, scan_img_right)
elif coords[0] <= 0:
rospy.loginfo("turn left")
u('motor_left', 2, '-0.22', rospy.Time.now())
u('motor_right', 2, '0.22', rospy.Time.now())
time.sleep(time_turn)
u('motor_left', 2, '0.0', rospy.Time.now())
u('motor_right', 2, '0.0', rospy.Time.now())
if precached:
coords = scan.check_precached(scan_img_left, scan_img_right)
else:
coords = scan.check_easy_sample(scan_img_left, scan_img_right)
# go forward a bit after each turn
u('motor_left', 2, '0.1', rospy.Time.now())
u('motor_right', 2, '0.1', rospy.Time.now())
time.sleep(2)
u('motor_left', 2, '0.0', rospy.Time.now())
u('motor_right', 2, '0.0', rospy.Time.now())
while coords is None:
if precached:
coords = scan.check_precached(scan_img_left, scan_img_right)
else:
coords = scan.check_easy_sample(scan_img_left, scan_img_right)
if time_turn > 0.4:
time_turn -= 0.1