Exemplo n.º 1
0
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)
Exemplo n.º 2
0
		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():