def test_pickup_easy(u):
mask_img = cv2.imread('mask.pbm')
# move arm out of the way of the camera
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
time.sleep(1)
if new_crotch_img2 is not None:
curr_crotch_img = new_crotch_img2
# Do the nasty, should apply mask to image.
mask_img = cv2.resize(mask_img, (curr_crotch_img.shape[1], curr_crotch_img.shape[0]))
#curr_crotch_img = np.concatenate((curr_crotch_img, mask_img), axis=1)
#curr_crotch_img = Image.blend(curr_crotch_img, mask_img, 0.5)
#curr_crotch_img = cv2.addWeighted(curr_crotch_img, 0.5, mask_img, 0.5, 0)
curr_crotch_img = np.bitwise_and(curr_crotch_img, mask_img)
#---------------------------
cv2.imshow('nipple', curr_crotch_img)
rospy.loginfo("image success")
xy = grab.identify_easy_sample(curr_crotch_img)
rospy.loginfo("coordinates: " + str(xy))
if xy:
rospy.loginfo("a sample has been detected")
while xy is not None:
grab.pick_up_at(u,xy)
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
xy = grab.identify_easy_sample(new_crotch_img2)
else:
rospy.loginfo("no sample detected")
grab.place_sample(u,1)
def test_pickup_easy(u):
mask_img = cv2.imread('mask.pbm')
# move arm out of the way of the camera
u('arm_target', 2, 'X 0', rospy.Time.now())
y_max = str(u('arm_max', 2, 'Y', rospy.Time.now()))
params = 'Y ' + y_max
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
time.sleep(1)
if new_crotch_img2 is not None:
curr_crotch_img = new_crotch_img2
mask_img = cv2.resize(mask_img, (curr_crotch_img.shape[1], curr_crotch_img.shape[0]))
curr_crotch_img = np.bitwise_and(curr_crotch_img, mask_img)
rospy.loginfo("image success")
xy = grab.identify_easy_sample(curr_crotch_img)
rospy.loginfo("coordinates: " + str(xy))
if xy:
rospy.loginfo("a sample has been detected")
while xy is not None:
grab.pick_up_at_us(u,xy)
# move arm out of the way of the camera
u('arm_target', 2, 'X 0', rospy.Time.now())
y_max = str(u('arm_max', 2, 'Y', rospy.Time.now()))
params = 'Y ' + y_max
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
xy = grab.identify_easy_sample(new_crotch_img2)
else:
rospy.loginfo("no sample detected")
grab.place_sample_us(u,1)
def test_pickup_easy(u):
mask_img = cv2.imread('mask.pbm')
# move arm out of the way of the camera
u('arm_target', 2, 'X 0', rospy.Time.now())
y_max = str(u('arm_max', 2, 'Y', rospy.Time.now()))
params = 'Y ' + y_max
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
time.sleep(1)
if new_crotch_img2 is not None:
curr_crotch_img = new_crotch_img2
mask_img = cv2.resize(
mask_img, (curr_crotch_img.shape[1], curr_crotch_img.shape[0]))
curr_crotch_img = np.bitwise_and(curr_crotch_img, mask_img)
rospy.loginfo("image success")
xy = grab.identify_easy_sample(curr_crotch_img)
rospy.loginfo("coordinates: " + str(xy))
if xy:
rospy.loginfo("a sample has been detected")
while xy is not None:
grab.pick_up_at_us(u, xy)
# move arm out of the way of the camera
u('arm_target', 2, 'X 0', rospy.Time.now())
y_max = str(u('arm_max', 2, 'Y', rospy.Time.now()))
params = 'Y ' + y_max
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
xy = grab.identify_easy_sample(new_crotch_img2)
else:
rospy.loginfo("no sample detected")
grab.place_sample_us(u, 1)
def test_pickup_easy(u):
# move arm out of the way of the camera
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
time.sleep(1)
if new_crotch_img2 is not None:
curr_crotch_img = new_crotch_img2
rospy.loginfo("image success")
xy = grab.identify_easy_sample(curr_crotch_img)
rospy.loginfo("coordinates: " + str(xy))
if xy:
rospy.loginfo("a sample has been detected")
grab.pick_up_at(u,xy)
grab.place_sample(u,1)
else:
rospy.loginfo("no sample detected")
def test_pickup_easy(u):
# move arm out of the way of the camera
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
time.sleep(1)
if new_crotch_img2 is not None:
curr_crotch_img = new_crotch_img2
rospy.loginfo("image success")
xy = grab.identify_easy_sample(curr_crotch_img)
rospy.loginfo("coordinates: " + str(xy))
if xy:
rospy.loginfo("a sample has been detected")
grab.pick_up_at(u, xy)
grab.place_sample(u, 1)
else:
rospy.loginfo("no sample detected")
def move_til_sample(u, precached):
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
u.motor_right(0.1)
u.motor_left(0.1)
while not rospy.is_shutdown():
curr_crotch_img = new_crotch_img2
xy = None
while xy is None:
curr_crotch_img = new_crotch_img2
if precached:
xy = grab.identify_precached(curr_crotch_img)
else:
xy = grab.identify_easy_sample(curr_crotch_img)
time.sleep(1)
u.motor_right(0.0)
u.motor_left(0.0)
break
def vid():
rospy.init_node('vid', anonymous=False)
rospy.loginfo('node initialized')
rospy.Subscriber('crotch/image/image_raw', Image, handle_img)
rate = rospy.Rate(10)
u = uniboard.Uniboard("/dev/ttyUSB1")
u.arm_home()
# MOVE FORWARD X meters
u.motor_left(0.1)
u.motor_right(0.1)
time.sleep(9)
u.motor_left(0)
u.motor_right(0)
# PICK UP SAMPLE
if TEST_PIT:
# move arm out of the way of the camera
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
time.sleep(1)
if new_crotch_img2 is not None:
curr_crotch_img = new_crotch_img2
rospy.loginfo("image success")
xy = grab.identify_easy_sample(curr_crotch_img)
rospy.loginfo("coordinates: " + str(xy))
if xy:
rospy.loginfo("a sample has been detected")
grab.pick_up_at(u, xy)
else:
rospy.loginfo("no sample detected")
else:
pick_up_center(u)
# MOVE BACKWARD X meters
u.motor_left(-0.1)
u.motor_right(-0.1)
time.sleep(8.9)
u.motor_left(0)
u.motor_right(0)
def move_til_sample(u, precached):
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
u('motor_left', 2, '0.1', rospy.Time.now())
u('motor_right', 2, '0.1', rospy.Time.now())
while not rospy.is_shutdown():
curr_crotch_img = new_crotch_img2
xy = None
while xy is None:
curr_crotch_img = new_crotch_img2
if precached:
xy = grab.identify_precached(curr_crotch_img)
else:
xy = grab.identify_easy_sample(curr_crotch_img)
time.sleep(1)
u('motor_left', 2, '0.0', rospy.Time.now())
u('motor_right', 2, '0.0', rospy.Time.now())
break
def vid():
rospy.init_node('vid', anonymous=False)
rospy.loginfo('node initialized')
rospy.Subscriber('crotch/image/image_raw', Image, handle_img)
rate = rospy.Rate(10)
u = uniboard.Uniboard("/dev/ttyUSB1")
u.arm_home()
# MOVE FORWARD X meters
u.motor_left(0.1)
u.motor_right(0.1)
time.sleep(9)
u.motor_left(0)
u.motor_right(0)
# PICK UP SAMPLE
if TEST_PIT:
# move arm out of the way of the camera
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
time.sleep(1)
if new_crotch_img2 is not None:
curr_crotch_img = new_crotch_img2
rospy.loginfo("image success")
xy = grab.identify_easy_sample(curr_crotch_img)
rospy.loginfo("coordinates: " + str(xy))
if xy:
rospy.loginfo("a sample has been detected")
grab.pick_up_at(u,xy)
else:
rospy.loginfo("no sample detected")
else:
pick_up_center(u)
# MOVE BACKWARD X meters
u.motor_left(-0.1)
u.motor_right(-0.1)
time.sleep(8.9)
u.motor_left(0)
u.motor_right(0)
def move_til_sample(u, precached):
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
u('motor_left', 2, '0.1', rospy.Time.now())
u('motor_right', 2, '0.1', rospy.Time.now())
while not rospy.is_shutdown():
curr_crotch_img = new_crotch_img2
xy = None
while xy is None:
curr_crotch_img = new_crotch_img2
if precached:
xy = grab.identify_precached(curr_crotch_img)
else:
xy = grab.identify_easy_sample(curr_crotch_img)
time.sleep(1)
u('motor_left', 2, '0.0', rospy.Time.now())
u('motor_right', 2, '0.0', rospy.Time.now())
break
def test_identify_easy():
while not rospy.is_shutdown():
if new_crotch_img2 is not None:
curr_crotch_img = new_crotch_img2
xy = grab.identify_easy_sample(curr_crotch_img)
rospy.loginfo("coordinates: " + str(xy))
