def commandReceived(cmd):
global count
global enable
global pub
global grippub
cmd1 = 'ok'
if cmd1 == 'ok':
print("rfh_zoo got ok call from topic.")
enable = True
cmd = None
if enable:
stemname = 'candy'
count = count % 3
print(count)
filename = stemname + str(count + 1)
enable = False
print("Getting candy {0}".format(str(count + 1)))
yfp.followPointsInFile(filename, pub=pub, grippub=grippub)
count += 1
print("Delivering candy...")
yfp.followPointsInFile('deliver', pub=pub, grippub=grippub)
stemname = 'jointpoints'
filename = stemname + '0'
n = 0
while os.path.exists(filename):
n += 1
filename = stemname + str(n)
pointsfile = wptool.WpTool(filename, 'w')
pub = rospy.Publisher('/arm_1/arm_controller/position_command', JointPositions, queue_size=10)
grippub = rospy.Publisher('/arm_1/gripper_controller/position_command', JointPositions, queue_size=10)
rospy.init_node('teleop_arm_keyboard')
rospy.Subscriber("joint_states", JointState, jointStatesReceivedHandler)
print("Enter initializer file name:")
initfilename = raw_input()
if not initfilename == '':
yfp.followPointsInFile(str(initfilename), pub=pub, grippub=grippub)
lcount = 0
try:
print msg
while(1):
desiredPos = copy.deepcopy(crntJointPositions)
desGripPos = copy.deepcopy(gripperPositions)
key = getKey()
if key in jointBindings.keys():
#print ("Current States:",crntJointStates.position)
if (lcount == 14):
print msg
lcount = (lcount + 1) % 15
elif key in stepBindings.keys():
