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
