def get_ball(self, camera): # Creating track bar if USE_TRACKBARS: callback = lambda x: 0 cv.createTrackbar('hmin', 'result', 0, 179, callback) cv.createTrackbar('hmax', 'result',179,179, callback) cv.createTrackbar('smin', 'result',0,255, callback) cv.createTrackbar('smax', 'result',255,255, callback) cv.createTrackbar('vmin', 'result',0,255, callback) cv.createTrackbar('vmax', 'result',255,255, callback) rate = rospy.Rate(10) #_, self.frame = self.capture.read() frame = cv.blur(camera.get_frame(), (3,3)) #converting to HSV hsv = cv.cvtColor(frame,cv.COLOR_BGR2HSV) # get info from track bar and appy to result if USE_TRACKBARS: hmin = cv.getTrackbarPos('hmin','result') hmax = cv.getTrackbarPos('hmax','result') smin = cv.getTrackbarPos('smin','result') smax = cv.getTrackbarPos('smax','result') vmin = cv.getTrackbarPos('vmin','result') vmax = cv.getTrackbarPos('vmax','result') lower_hsv = np.array([hmin, smin, vmin]) upper_hsv = np.array([hmax, smax, vmax]) mask_trackbar = cv.inRange(hsv,lower_hsv, upper_hsv) track_x, track_y, mask3, _ = vl.track_object(mask_trackbar) cv.imshow('Thresh trackbar',mask3) #orng_x, orng_y, mask1, current_area = vl.locate_orange_ball(frame) pink_x, pink_y, mask2, current_area = vl.locate_pink_ball(frame) display = frame.copy() cv.circle(display, (320, 70), 10, 255, -1) #cv.circle(display, (orng_x, orng_y), 10, (0,0,255), -1) cv.circle(display, (pink_x, pink_y), 10, (0,255,0), -1) #cv.imshow('Thresh orange',mask1) #cv.imshow('Thresh pink',mask2) #cv.imshow('result', display) #ball_gripped = self.visual_servo(self.tl, camera, pink_x, pink_y, current_area) ball_gripped = self.attack_visual_servo() # rate.sleep() if ball_gripped: print 'THROW' return True else: return False
def attack_visual_servo(self): u, v, mask2, current_area = vl.locate_pink_ball(self.frame) #u, v, mask2, current_area = vl.locate_orange_ball(self.frame) #print current_area current_z = self.motion_controller.get_gripper_coords()[2][0] cv.imshow('MASK',mask2) # No object is being tracked if u == -1 or v == -1: xi = np.zeros(2) # xy vel to zero desired_depth_vel = 0.0 else: xi = camera.calc_desired_feat_velocities(u, v, XY_K0) desired_depth_vel = camera.calc_desired_depth_velocity_z(DEPTH_K0, current_area) if current_z <= Planner.BALL_Z: baxter.grip_ball() rospy.sleep(0.2) print "I found the ball!" motion_controller.command_velocity(np.zeros(6)) return True #print 'Desired depth vel:', desired_depth_vel xi = -np.append(xi, desired_depth_vel) print 'xi', xi vect = Vector3Stamped() vect.header.frame_id = '/right_hand_camera' vect.header.stamp = rospy.Time(0) vect.vector = Vector3(*xi[0:3]) try: trans_vect = self.tl.transformVector3('/base', vect) squiggle = np.array([trans_vect.vector.x,trans_vect.vector.y,trans_vect.vector.z,0,0,0]) print 'squiggle', squiggle motion_controller.command_velocity(squiggle) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print "you suck" return False