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()
