def __init__(self, limb, name='Demo_26_Final'):
rospy.init_node(name, anonymous=True)
self._startGesture = '0'
self._limb = limb
self._knhObj = KinectNiteHelp()
self._baeObj = BaxterArmEndpoint(self._limb)
self._dmObj = Demo26Help()
self._lhObj = LeapHelp()
self._handCoordinates = []
self._baxterCoordinates = []
self._posCount = 0
rtMatFile = open("RTMatFile.dat", "r")
self._rotMat = cPickle.load(rtMatFile)
self._transMat = cPickle.load(rtMatFile)
self._gCount = 0
self._gOn = 0
self._gPointCount = 0
self._gPoints = []
self._hmmObjG1 = HMM('G1.hmm')
self._hmmObjG2 = HMM('G2.hmm')
self._hmmObjG3 = HMM('G3.hmm')
self._hmmObjG4 = HMM('G4.hmm')
self._flag = '0'
self._pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
#self._flagPub = rospy.Publisher('flag_topic', String)
self._sub = rospy.Subscriber('/key_tap_topic', String, self._callback)
rtMatFile.close()
img = cv.LoadImage('Welcome.png')
msg = cv_bridge.CvBridge().cv_to_imgmsg(img, encoding="bgr8")
self._pub.publish(msg)
# Sleep to allow for image to be published.
rospy.sleep(3)
def __init__(self, limb, name='Demo_26_Master_Point'):
rospy.init_node(name, anonymous=True)
self._limb = limb
self._knhObj = KinectNiteHelp()
self._baeObj = BaxterArmEndpoint(self._limb)
self._dmObj = Demo26Help()
self._lhObj = LeapHelp()
self._handCoordinates = []
self._baxterCoordinates = []
self._posCount = 0
rtMatFile = open("RTMatFile.dat", "r")
self._rotMat = cPickle.load(rtMatFile)
self._transMat = cPickle.load(rtMatFile)
self._gCount = 0
self._gOn = 0
self._gPointCount = 0
self._gPoints = []
rtMatFile.close()
def __init__(self, limb, name='Demo_26_Master_Leap'):
rospy.init_node(name, anonymous=True)
self._limb = limb
self._knhObj = KinectNiteHelp()
self._baeObj = BaxterArmEndpoint(self._limb)
self._bbhObj = BaxterButtonHelp(self._limb)
self._dmObj = Demo26Help()
self._baeObj.startSubscriber()
self._lhObj = LeapHelp()
self._handCoordinates = []
self._baxterCoordinates = []
self._posCount = 0
rtMatFile = open("RTMatFile.dat", "r")
self._rotMat = cPickle.load(rtMatFile)
self._transMat = cPickle.load(rtMatFile)
self._gCount = 0
self._gOn = 0
self._gPointCount = 0
self._gPoints = []
rtMatFile.close()
self._control_arm = baxter_interface.limb.Limb(ARM)
self._control_arm.set_joint_position_speed(0.9)
class Demo26Master:
def __init__(self, limb, name='Demo_26_Master_Leap'):
rospy.init_node(name, anonymous=True)
self._limb = limb
self._knhObj = KinectNiteHelp()
self._baeObj = BaxterArmEndpoint(self._limb)
self._bbhObj = BaxterButtonHelp(self._limb)
self._dmObj = Demo26Help()
self._baeObj.startSubscriber()
self._lhObj = LeapHelp()
self._handCoordinates = []
self._baxterCoordinates = []
self._posCount = 0
rtMatFile = open("RTMatFile.dat", "r")
self._rotMat = cPickle.load(rtMatFile)
self._transMat = cPickle.load(rtMatFile)
self._gCount = 0
self._gOn = 0
self._gPointCount = 0
self._gPoints = []
rtMatFile.close()
self._control_arm = baxter_interface.limb.Limb(ARM)
self._control_arm.set_joint_position_speed(0.9)
def runMaster(self):
# Get and store kinect coordinates
try:
hand_endpoint = self._knhObj.getLeftHandPos()
#print hand_endpoint
self._handCoordinates = array([[hand_endpoint[0]], [hand_endpoint[1]], [hand_endpoint[2]]])
(rows, cols) = self._rotMat.shape
print
#print self._rotMat
#print self._handCoordinates
#print self._transMat
self._baxterCoordinates = self._rotMat * mat(self._handCoordinates) + self._transMat
print "Hand Position: %s" %(self._baxterCoordinates)
#print "Gold Man"
except:
print "Error in kinect_nite_help part."
## Cd eto set left wo and w1 value
try:
self._dmObj.setHandPoint(self._baxterCoordinates[0], self._baxterCoordinates[1], self._baxterCoordinates[2])
self._dmObj.calcAngleOrientation()
angles = self._dmObj.getAngleOrientation('r')
joint_angles = self._control_arm.joint_angles()
joint_angles["left_w0"] = angles[0]
joint_angles["left_w1"] = angles[1]
self._lhObj.processFrame()
handEmpty = self._lhObj.getIsHandEmpty()
print "Hand Empty: %s" %(handEmpty)
if handEmpty == True:
fCount = 0
else:
fCount = self._lhObj.getNumFingers()
print "Finger Count: %s" %(fCount)
except:
print "Error in joint angle set part"
try:
# Move arm to space bro
#print "Gonna Move"
#print joint_angles
self._control_arm.move_to_joint_positions(joint_angles, 1.0)
except:
print "Error in baxter move part"
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
class Demo26Master:
def _callback(self, data):
self._startGesture = data.data
def __init__(self, limb, name='Demo_26_Final'):
rospy.init_node(name, anonymous=True)
self._startGesture = '0'
self._limb = limb
self._knhObj = KinectNiteHelp()
self._baeObj = BaxterArmEndpoint(self._limb)
self._dmObj = Demo26Help()
self._lhObj = LeapHelp()
self._handCoordinates = []
self._baxterCoordinates = []
self._posCount = 0
rtMatFile = open("RTMatFile.dat", "r")
self._rotMat = cPickle.load(rtMatFile)
self._transMat = cPickle.load(rtMatFile)
self._gCount = 0
self._gOn = 0
self._gPointCount = 0
self._gPoints = []
self._hmmObjG1 = HMM('G1.hmm')
self._hmmObjG2 = HMM('G2.hmm')
self._hmmObjG3 = HMM('G3.hmm')
self._hmmObjG4 = HMM('G4.hmm')
self._flag = '0'
self._pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
#self._flagPub = rospy.Publisher('flag_topic', String)
self._sub = rospy.Subscriber('/key_tap_topic', String, self._callback)
rtMatFile.close()
img = cv.LoadImage('Welcome.png')
msg = cv_bridge.CvBridge().cv_to_imgmsg(img, encoding="bgr8")
self._pub.publish(msg)
# Sleep to allow for image to be published.
rospy.sleep(3)
def runMaster(self):
# Get and store kinect coordinates
try:
hand_endpoint = self._knhObj.getLeftHandPos()
#print hand_endpoint
self._handCoordinates = array([[hand_endpoint[0]], [hand_endpoint[1]], [hand_endpoint[2]]])
(rows, cols) = self._rotMat.shape
print
#print self._rotMat
#print self._handCoordinates
#print self._transMat
self._baxterCoordinates = self._rotMat * mat(self._handCoordinates) + self._transMat
print "Hand Position: %s" %(self._baxterCoordinates)
#print "Gold Man"
except:
print "Error in kinect_nite_help part."
## Cd eto set left wo and w1 value
try:
self._dmObj.setHandPoint(self._baxterCoordinates[0], self._baxterCoordinates[1], self._baxterCoordinates[2])
self._dmObj.calcAngleOrientation()
angles = self._dmObj.getAngleOrientation('r')
self._lhObj.processFrame()
handEmpty = self._lhObj.getIsHandEmpty()
print "Hand Empty: %s" %(handEmpty)
if handEmpty == True:
fCount = 0
else:
fCount = self._lhObj.getNumFingers()
print "Finger Count: %s" %(fCount)
print "GestureState: %s" %(self._startGesture)
except:
print "Error in joint angle set part"
try:
if (self._startGesture == '1'):
if(self._gOn == 0):
self._gCount += 1
self._gOn = 1
img = cv.LoadImage('GestureRecord.png')
msg = cv_bridge.CvBridge().cv_to_imgmsg(img, encoding="bgr8")
self._pub.publish(msg)
# Sleep to allow for image to be published.
rospy.sleep(2)
if (fCount == 0) or (fCount == 2) or (fCount == 4):
fCount = fCount + 1
handPoint = [self._baxterCoordinates[0, 0], self._baxterCoordinates[1, 0], self._baxterCoordinates[2, 0], fCount]
print "Point Appended"
self._gPoints.append(handPoint)
print self._gPoints
print self._gOn
self._gPointCount += 1
elif(self._startGesture == '0'):
print "Else"
if(self._gOn == 1):
self._gPointCount = 0
self._gOn = 0
img = cv.LoadImage('GestureAnalyze.png')
msg = cv_bridge.CvBridge().cv_to_imgmsg(img, encoding="bgr8")
self._pub.publish(msg)
# Sleep to allow for image to be published.
rospy.sleep(3)
print "recorded"
# Process points
print "GPoINTS"
print self._gPoints
symbols = getsym(numpy.matrix(self._gPoints))
log_prob = [self._hmmObjG1.viterbi(symbols), self._hmmObjG2.viterbi(symbols), self._hmmObjG3.viterbi(symbols), self._hmmObjG4.viterbi(symbols)]
print log_prob
max_value = max(log_prob)
max_index = log_prob.index(max_value)
self._gPoints = []
# Do gesture recognised processing
self._flag = str(max_index + 1)
gflag = max_index + 1
if gflag == 1:
#while(not sf.Keyboard.is_key_pressed(sf.Keyboard.R_SHIFT)):
# print max_index + 1
# print log_prob
img = cv.LoadImage('Gesture1.png')
msg = cv_bridge.CvBridge().cv_to_imgmsg(img, encoding="bgr8")
self._pub.publish(msg)
# Sleep to allow for image to be published.
rospy.sleep(3)
elif gflag == 2:
#while(not sf.Keyboard.is_key_pressed(sf.Keyboard.R_SHIFT)):
# print max_index + 1
# print log_prob
img = cv.LoadImage('Gesture2.png')
msg = cv_bridge.CvBridge().cv_to_imgmsg(img, encoding="bgr8")
self._pub.publish(msg)
# Sleep to allow for image to be published.
rospy.sleep(3)
elif gflag == 3:
#while(not sf.Keyboard.is_key_pressed(sf.Keyboard.R_SHIFT)):
# print max_index + 1
# print log_prob
img = cv.LoadImage('Gesture3.png')
msg = cv_bridge.CvBridge().cv_to_imgmsg(img, encoding="bgr8")
self._pub.publish(msg)
# Sleep to allow for image to be published.
rospy.sleep(3)
elif gflag == 4:
#while(not sf.Keyboard.is_key_pressed(sf.Keyboard.R_SHIFT)):
# print max_index + 1
# print log_prob
img = cv.LoadImage('Gesture4.png')
msg = cv_bridge.CvBridge().cv_to_imgmsg(img, encoding="bgr8")
self._pub.publish(msg)
# Sleep to allow for image to be published.
rospy.sleep(3)
else:
#while(not sf.Keyboard.is_key_pressed(sf.Keyboard.R_SHIFT)):
# print "You are welcome. No gesture detected"
# print log_prob
img = cv.LoadImage('Welcome.png')
msg = cv_bridge.CvBridge().cv_to_imgmsg(img, encoding="bgr8")
self._pub.publish(msg)
# Sleep to allow for image to be published.
rospy.sleep(3)
except Exception,e:
print str(e)
print "Error in baxter move part"
class Demo26Master:
def __init__(self, limb, name='Demo_26_Master_Point'):
rospy.init_node(name, anonymous=True)
self._limb = limb
self._knhObj = KinectNiteHelp()
self._baeObj = BaxterArmEndpoint(self._limb)
self._dmObj = Demo26Help()
self._lhObj = LeapHelp()
self._handCoordinates = []
self._baxterCoordinates = []
self._posCount = 0
rtMatFile = open("RTMatFile.dat", "r")
self._rotMat = cPickle.load(rtMatFile)
self._transMat = cPickle.load(rtMatFile)
self._gCount = 0
self._gOn = 0
self._gPointCount = 0
self._gPoints = []
rtMatFile.close()
def runMaster(self):
# Get and store kinect coordinates
try:
hand_endpoint = self._knhObj.getLeftHandPos()
#print hand_endpoint
self._handCoordinates = array([[hand_endpoint[0]], [hand_endpoint[1]], [hand_endpoint[2]]])
(rows, cols) = self._rotMat.shape
print
#print self._rotMat
#print self._handCoordinates
#print self._transMat
self._baxterCoordinates = self._rotMat * mat(self._handCoordinates) + self._transMat
print "Hand Position: %s" %(self._baxterCoordinates)
#print "Gold Man"
except:
print "Error in kinect_nite_help part."
## Cd eto set left wo and w1 value
try:
self._dmObj.setHandPoint(self._baxterCoordinates[0], self._baxterCoordinates[1], self._baxterCoordinates[2])
self._dmObj.calcAngleOrientation()
angles = self._dmObj.getAngleOrientation('r')
self._lhObj.processFrame()
handEmpty = self._lhObj.getIsHandEmpty()
print "Hand Empty: %s" %(handEmpty)
if handEmpty == True:
fCount = 0
else:
fCount = self._lhObj.getNumFingers()
print "Finger Count: %s" %(fCount)
except:
print "Error in joint angle set part"
try:
if (sf.Keyboard.is_key_pressed(sf.Keyboard.L_SHIFT)):
if(self._gOn == 0):
self._gCount += 1
self._gOn = 1
if (fCount == 0) or (fCount == 2) or (fCount == 4):
fCount = fCount + 1
handPoint = [self._baxterCoordinates[0, 0], self._baxterCoordinates[1, 0], self._baxterCoordinates[2, 0], fCount]
print "Point Appended"
self._gPoints.append(handPoint)
print self._gPoints
print self._gOn
self._gPointCount += 1
else:
print "Else"
if(self._gOn == 1):
self._gPointCount = 0
self._gOn = 0
print "recorded"
print "Bitch!"
str = "PointData.csv"
with open(str, "wb") as f:
writer = csv.writer(f)
writer.writerows(self._gPoints)
#a = numpy.mat(self._gPoints)
#str = "PointData" + self._gCount + ".txt"
#numpy.savetxt(str, a)
self._gPoints = []
except:
print "Error in baxter move part"
def main():
rospy.init_node("Demo_Jul_22")
color_num = {'1':'blue', '2':'green', '3':'yellow', '4':'red'}
color = 'black'
place_num = 0
used_nums = [0, 5] # Fingers not to be processed
posFile = open('RestPosFile.dat', "r")
dummy_load = cPickle.load(posFile)
rest_pos = cPickle.load(posFile)
posFile.close()
print "Initial Calibration"
ll = baxter_interface.limb.Limb("left")
rl = baxter_interface.limb.Limb("right")
lg = baxter_interface.gripper.Gripper("left")
rg = baxter_interface.gripper.Gripper("right")
ll.set_joint_position_speed(0.9)
rl.set_joint_position_speed(0.9)
lg.calibrate()
rg.calibrate()
lg.open()
rg.open()
ll.move_to_neutral(5)
rl.move_to_neutral(5)
key_pos = 0
lhObj = LeapHelp()
color = 'black'
place_num = 0
send_image('DemoJuly22Images/DemoJul22Registering.png')
while(place_num <= 3):
lhObj.processFrame()
print "Fingers: %s" % (lhObj.getNumFingers())
if(lhObj.getKeyTapDetected() == True):
send_image('DemoJuly22Images/DemoJul22ColorSelect.png')
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 not in used_nums:
used_nums.append(finger_count)
color = color_num[str(finger_count)]
image_name = 'DemoJuly22Images/DemoJul22' + color + '.png'
send_image(image_name)
print "FingerCount:"
print finger_count
print "Color:"
print color
place_num += 1
print "Place Num:"
print place_num
else:
send_image('DemoJuly22Images/DemoJul22AnotherColor.png')
print"Selected color has alrady been assembled"
time.sleep(1)
if color == 'black':
continue
ll = baxter_interface.limb.Limb("left")
rl = baxter_interface.limb.Limb("right")
lg = baxter_interface.gripper.Gripper("left")
rg = baxter_interface.gripper.Gripper("right")
ll.set_joint_position_speed(0.9)
rl.set_joint_position_speed(0.9)
lg.calibrate()
rg.calibrate()
lg.open()
rg.open()
ll.move_to_neutral(5)
rl.move_to_neutral(5)
send_image('DemoJuly22Images/DemoJul22PickingParts.png')
print "Moving Left Arm"
(j1, j2) = get_positions(color, 'left')
ll.set_joint_position_speed(0.7)
ll.move_to_neutral(timeout = 5.0)
ll.move_to_joint_positions(j1, 5.0)
ll.set_joint_position_speed(0.5)
ll.move_to_joint_positions(j2, 3.0)
lg.close()
ll.move_to_joint_positions(j1, 3.0)
ll.set_joint_position_speed(0.7)
ll.move_to_neutral(timeout = 5.0)
print "Moving Right Arm"
(j1, j2) = get_positions(color, 'right')
rl.set_joint_position_speed(0.5)
rl.move_to_neutral(timeout = 5.0)
rl.move_to_joint_positions(j1, 5.0)
rl.set_joint_position_speed(0.3)
rl.move_to_joint_positions(j2, 3.0)
rg.close()
rl.move_to_joint_positions(j1, 3.0)
rl.set_joint_position_speed(0.5)
rl.move_to_neutral(timeout = 5.0)
send_image('DemoJuly22Images/DemoJul22Assembly.png')
#time.sleep(1)
os.system("rosrun baxter_examples joint_position_file_playback.py -f Assemble ")
time.sleep(1)
rg.open()
time.sleep(1)
rl.move_to_neutral(timeout = 5.0)
send_image('DemoJuly22Images/DemoJul22PlacingProduct.png')
#time.sleep(1)
command = "rosrun baxter_examples joint_position_file_playback.py -f Place" + str(place_num)
os.system(command)
lg.open()
time.sleep(1)
ll.set_joint_position_speed(0.5)
ll.move_to_joint_positions(rest_pos, 5.0)
ll.set_joint_position_speed(0.7)
ll.move_to_neutral(timeout = 5.0)
color = 'black'
if key_pos == 1:
color = 'black'
send_image('DemoJuly22Images/DemoJul22Registering.png')
key_pos = 0
rate = rospy.Rate(10)
print "Demo Complete"
send_image('DemoJuly22Images/DemoJul22Welcome.png')
return 0
