Esempio n. 1
0
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)
Esempio n. 2
0
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)
Esempio n. 3
0
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)
Esempio n. 4
0
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")
Esempio n. 5
0
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")
Esempio n. 6
0
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
Esempio n. 7
0
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)
Esempio n. 8
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
Esempio n. 9
0
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)    
Esempio n. 10
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
Esempio n. 11
0
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))