def main(): rospy.init_node("Surf_2014_Retrieve") tobj = moveGetCamArmImage(limb = 'left', file_name = "RetrieveBaxterCamArmJointPosFile.dat") tobj.moveCamArm() img = tobj.getCamArmImage() icObj = ImageClipper('RetrieveImageClipPosFile.dat') img = icObj.clipImage(img) back_img = cv2.imread('RetrieveImage.png') per_white = 100.0 while (per_white >= 10.5): img = tobj.getCamArmImage() img = icObj.clipImage(img) per_white, binary_img = getForeBack(back_img, img) cv2.imshow('Current_Image', img) cv2.imshow('Back_Image', back_img) cv2.imshow('Binary_Image', binary_img) print per_white print if per_white >= 1.0: print "TOOL PRESENT" else: print "NO TOOL" cv2.waitKey(0)
def main(): rospy.init_node("Surf_2014_Final") fo = open('left_del_cam_pos.dat', "r") del_limb = cPickle.load(fo) #del_limb = 'left' left_del_pos = cPickle.load(fo) cam_right_pos = cPickle.load(fo) fo.close() posFile = open("BaxterCamArmJointPosFile.dat", "r") cam_arm_joint_pos = cPickle.load(posFile) posFile.close() mvObj = moveGetCamArmImage('right') mvObj.moveCamArm() img = mvObj.getCamArmImage() ppObj = PixelToBaxterPosTransformer() ppObj.setOffsets(0.0, 0.017, -0.035) timgObj = ToolImageProcessHelp() timgObj.setImage(img, 'Original') timgObj.processImage_BackProject() cv2.imshow('Contours', timgObj.getContourImage()) cv2.waitKey(0) cv2.destroyAllWindows() num = timgObj.getNumTools() h = timgObj.getHuMomentsList() c = timgObj.getCentroidsList() ToolList = [] for i in range(num): tool = ToolIdentify() tool.setHuMoments([ h[0,i], h[1,i], h[2,i], h[3,i], h[4,i], h[5,i], h[6,i], h[7,i] ]) tool.setCentroid(c[0, i], c[1, i]) tool.identifyTool() ToolList.append(tool) print "Tool #%s: %s" % (i+1, tool.getToolName()) index = raw_input("Enter index of tool to deliver:") index = int(index) - 1 # Because tools are printed from 1 onwards but indexing starts from 0 ppObj.calcBaxterPos(ToolList[index]._centroidX, ToolList[index]._centroidY) (joints_loc, inter_joints_loc1, inter_joints_loc2) = ppObj.getBaxterJointSpace(del_limb) print "Delivering %s..." % (ToolList[index].getToolName()) #############Chaneg right to left in cam_arm_joint_pos################ #dummy = [] #for i in cam_arm_joint_pos.keys(): # new_key = i.replace('right', 'left') # dummy.append(new_key) #cam_right_pos = dict(zip(dummy, cam_arm_joint_pos.values())) ######################################################################## # Move Delivery Arm control_arm = baxter_interface.limb.Limb(del_limb) control_arm.set_joint_position_speed(0.5) # Joint movement speed control_arm.move_to_neutral(5) control_arm.move_to_joint_positions(inter_joints_loc1, 7.0) time.sleep(1) control_arm.move_to_joint_positions(joints_loc, 7.0) time.sleep(1) control_arm.move_to_joint_positions(inter_joints_loc2, 7.0) time.sleep(1) control_arm.move_to_joint_positions(left_del_pos, 7.0) time.sleep(3) control_arm.move_to_neutral(5) return 0
def main(): rospy.init_node("Surf_2014_Final") lhObj = LeapHelp() initial_num_tools = int(raw_input("Enter number of tools on table:")) left_untuck = {"left_w0":-0.08, "left_w1":-1.0, "left_w2":-1.19, "left_e0":1.94, "left_e1":0.67, "left_s0":1.03, "left_s1":-0.50} fo = open('left_del_cam_pos.dat', "r") del_limb = cPickle.load(fo) #del_limb = 'left' left_del_pos = cPickle.load(fo) cam_right_pos = cPickle.load(fo) fo.close() fo = open('left_high_pos.dat', "r") del_limb = cPickle.load(fo) left_high_pos = cPickle.load(fo) fo.close() fo = open('left_ret_pos.dat', "r") ret_limb = cPickle.load(fo) left_ret_del_pos = cPickle.load(fo) fo.close() posFile = open("BaxterCamArmJointPosFile.dat", "r") cam_arm_joint_pos = cPickle.load(posFile) posFile.close() posFile = open("BaxterRetArmJointPosFile.dat", "r") ret_arm_joint_pos = cPickle.load(posFile) # Ontable, moveCamArm above table, left_ret_del_pos:retrieval dleivery position posFile.close() ret_back_img = cv2.imread("RetrieveImage.png") ret_arm_cam = moveGetCamArmImage(limb = 'left', file_name = "RetrieveBaxterCamArmJointPosFile.dat") mvObj = moveGetCamArmImage('right') mvObj.moveCamArm() img = mvObj.getCamArmImage() ppObj = PixelToBaxterPosTransformer() ppObj.setOffsets(0.0, 0.019, -0.025) # Figuring out smin smin = getSMin(img.copy(), initial_num_tools, 10) timgObj = ToolImageProcessHelp() timgObj.setImage(img, 'Original') timgObj.setSMin(smin) #timgObj.processImage_BackProject() timgObj.processImage() num = timgObj.getNumTools() h = timgObj.getHuMomentsList() c = timgObj.getCentroidsList() baxter_img = timgObj.getContourImage() baxter_img = cv2.resize(baxter_img, (1024, 600)) send_image(baxter_img) print "SMIN:%s" % (smin) ToolList = [] for i in range(num): tool = ToolIdentify() tool.setHuMoments([ h[0,i], h[1,i], h[2,i], h[3,i], h[4,i], h[5,i], h[6,i], h[7,i] ]) tool.setCentroid(c[0, i], c[1, i]) tool.identifyTool() ToolList.append(tool) #print "Tool #%s: %s" % (i+1, tool.getToolName()) last_ret_time = time.time() key_pos = 0 just_delivered = False while(num > 0): just_delivered = False for i in range(num): print "Tool #%s: %s" % (i+1, ToolList[i].getToolName()) ##############LEAP STUFF######################################## index = -1 lhObj.processFrame() print "Fingers: %s" % (lhObj.getNumFingers()) if(lhObj.getKeyTapDetected() == True): print "Extend Fingers:" key_pos = key_pos + 1 key_pos = key_pos % 2 time.sleep(2) lhObj.processFrame() finger_count = lhObj.getNumFingers() if (finger_count) in range(1, num+1): print "Tool Available" print finger_count index = int(finger_count) - 1 else: print"Selected tool unavaialable" time.sleep(1) ################################################################ #index = raw_input("Enter index of tool to deliver:") #index = int(index) - 1 # Because tools are printed from 1 onwards but indexing starts from 0 if(index != -1): ppObj.calcBaxterPos(ToolList[index]._centroidX, ToolList[index]._centroidY) (joints_loc, inter_joints_loc1, inter_joints_loc2) = ppObj.getBaxterJointSpace(del_limb) print "Delivering %s..." % (ToolList[index].getToolName()) just_delivered = True del(ToolList[index]) num = num - 1 #############Chaneg right to left in cam_arm_joint_pos################ #dummy = [] #for i in cam_arm_joint_pos.keys(): # new_key = i.replace('right', 'left') # dummy.append(new_key) #cam_right_pos = dict(zip(dummy, cam_arm_joint_pos.values())) ######################################################################## #Move Delivery Arm control_arm = baxter_interface.limb.Limb(del_limb) control_arm.set_joint_position_speed(0.5) # Joint movement speed control_arm.move_to_joint_positions(left_high_pos, 7.0) control_arm.move_to_joint_positions(inter_joints_loc1, 7.0) time.sleep(1) control_arm.move_to_joint_positions(joints_loc, 7.0) time.sleep(1) control_arm.move_to_joint_positions(inter_joints_loc2, 7.0) time.sleep(1) control_arm.move_to_joint_positions(left_del_pos, 7.0) time.sleep(3) control_arm.move_to_neutral(timeout = 7) ret_arm_cam.moveCamArm() ############################################################################################ tool_detected = False if(time.time() > 15 + last_ret_time) and (just_delivered == False): print "Checking retrieval tray" ret_arm_cam.moveCamArm() icObj = ImageClipper('RetrieveImageClipPosFile.dat') per_white = 100.0 count = 0 while (per_white >= 18.0 or per_white <= 2.0) and count <= 3: img = ret_arm_cam.getCamArmImage() img = icObj.clipImage(img) per_white, binary_img = getForeBack(ret_back_img, img) print "Waiting for next iteration" print count += 1 if per_white >= 2.0 and per_white <= 15.0: print "TOOL PRESENT" tool_detected = True else: print "NO TOOL" tool_detected = False last_ret_time = time.time() ########## ########### if tool_detected == True: # Move Retrieval Arm control_arm = baxter_interface.limb.Limb(ret_limb) control_arm.set_joint_position_speed(0.5) # Joint movement speed ret_arm_cam.moveCamArm() time.sleep(1) control_arm.move_to_joint_positions(ret_arm_joint_pos, 7.0) time.sleep(1) ret_arm_cam.moveCamArm() time.sleep(1) control_arm.move_to_joint_positions(left_ret_del_pos, 7.0) time.sleep(1) ret_arm_cam.moveCamArm() ############################################################################################ return 0
def main(): rospy.init_node('Calib_HSV') print 1 cv2.namedWindow('Binary', cv2.WINDOW_NORMAL) cv2.namedWindow('Contour', cv2.WINDOW_NORMAL) cv2.namedWindow('Trackbars', cv2.WINDOW_NORMAL) print 1 kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) font = cv2.FONT_HERSHEY_SIMPLEX print 1 cv2.createTrackbar('HueMin','Trackbars', 42, 179, nothing) cv2.createTrackbar('HueMax','Trackbars', 179, 179, nothing) cv2.createTrackbar('SatMin','Trackbars', 94, 255, nothing) cv2.createTrackbar('SatMax','Trackbars', 255, 255, nothing) cv2.createTrackbar('ValMin','Trackbars', 0, 255, nothing) cv2.createTrackbar('ValMax','Trackbars', 255, 255, nothing) print 1 tobj = moveGetCamArmImage(limb = 'right', file_name = "BaxterCamArmJointPosFile.dat") tobj.moveCamArm() img = tobj.getCamArmImage() icObj = ImageClipper('ImageClipPosFile.dat') img = icObj.clipImage(img) blur = cv2.GaussianBlur(img,(5,5),0) blur_copy = img.copy() print "Set hsv values and press c to store in HSV file." print "Press C to Start Calibration" while(not sf.Keyboard.is_key_pressed(sf.Keyboard.C)): pass time.sleep(1) while(not sf.Keyboard.is_key_pressed(sf.Keyboard.C)): hmin = cv2.getTrackbarPos('HueMin', 'Trackbars') smin = cv2.getTrackbarPos('SatMin', 'Trackbars') vmin = cv2.getTrackbarPos('ValMin', 'Trackbars') hmax = cv2.getTrackbarPos('HueMax', 'Trackbars') smax = cv2.getTrackbarPos('SatMax', 'Trackbars') vmax = cv2.getTrackbarPos('ValMax', 'Trackbars') hsv = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV) lower_thresh = numpy.array([hmin, smin, vmin]) upper_thresh = numpy.array([hmax, smax, vmax]) mask = cv2.inRange(hsv, lower_thresh, upper_thresh) mask = cv2.bitwise_not(mask) median = cv2.medianBlur(mask,5) er = cv2.erode(median, kernel, iterations = 0) dl = cv2.dilate(er, kernel, iterations = 1) er = cv2.erode(dl, kernel, iterations = 1) dl = er cv2.imshow('Binary', dl) dl_copy = dl.copy() try: tc = 0 contours,hierarchy = cv2.findContours(dl, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) HuMomentsList = numpy.array([[0.0], [0.0], [0.0], [0.0], [0.0], [0.0], [0.0], [0.0]]) delta = 400 blur_copy = img.copy() blur_copy2 = dl_copy.copy() CentroidsList = numpy.array([[0.0], [0.0]]) for i in range(len(contours)): if (cv2.arcLength(contours[i], True) > delta) and (hierarchy[0, i, 3] == -1): cv2.drawContours(blur_copy, contours, i, (255,255,255), 1) m = cv2.moments(contours[i]) cen_x = int(m["m10"] / m["m00"]) cen_y = int(m["m01"] / m["m00"]) centroid = numpy.array([[cen_x], [cen_y]]) CentroidsList = numpy.hstack((CentroidsList, centroid)) cv2.putText(blur_copy, str(tc + 1), (cen_x, cen_y), font, 1,(0,255,100), 3, 8) HuMoments = cv2.HuMoments(m) I8 = m["nu11"] * ((m["nu30"]+m["nu12"])**2 - (m["nu03"]+m["nu21"])**2) - (m["nu20"]-m["nu02"])*(m["nu30"]+m["nu12"])*(m["nu03"]+m["nu21"]) HuMoments[6] = abs(HuMoments[6]) # Make erflection invariant HuMoments = numpy.vstack((HuMoments, I8)) # Modified Humoments HuMomentsList = numpy.hstack((HuMomentsList, HuMoments)) tc += 1 print "Number of instruments:", tc cv2.imshow('Contour', blur_copy) cv2.waitKey(1) except: pass hsvcutoffs = [hmin, hmax, smin, smax, vmin, vmax] CentroidsList = CentroidsList[:, 1:(tc + 1)] HuMomentsList = HuMomentsList[:, 1:(tc + 1)] HuMomentsList = -numpy.sign(HuMomentsList) * numpy.log10(numpy.abs(HuMomentsList)) print # For 5th HuMoment value of 2nd instrument (Total 8 HuMoment Values for each) # HuMomentsList[4,1] # For X value value of 5th instrument (X:0 Y:1) # CentroidsList[0,4] print print"Centroids:" print CentroidsList print print "HuMoments:" print HuMomentsList print print "HSV Cutoffs:" print hsvcutoffs print HSVValFile = open('HSVValFile.dat', "w"); cPickle.dump(hsvcutoffs, HSVValFile); HSVValFile.close() print "HSV Cutoffs Added to HSVValFile" print cv2.destroyAllWindows()