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