def move_to_object(
self): #, object_name, stop_distance, vel_scaling_factor):
"""
description
"""
# self.object_name = object_name
# self.stop_distance = stop_distance
# self.vel_scaling_factor = vel_scaling_factor
object_name = "waypoint_3"
stop_distance = 0.1
vel_scaling_factor = 0.1
beacon1 = xyz_vicon.xyz_vicon(
object_name) #calling that object class from xyz_vicon
beacon1.talker() #subscribing to the xyzhelmet node
kuka2 = xyz_kuka.xyzkuka() #calling xyzkuka class from xyz_kuka
kuka2.talker() #subscribing to the xyzkuka node
kuka1 = kuka() #calling kuka class
stime = time.time()
rospy.loginfo(stime)
e = 0.2
# print(beacon1.xpos)
rospy.sleep(2)
while (sqrt((beacon1.xpos - kuka2.xpos)**2 +
(beacon1.ypos - kuka2.ypos)**2) >
stop_distance) and not rospy.is_shutdown():
xhelmet = beacon1.xpos
yhelmet = beacon1.ypos
xkuka = kuka2.xpos
ykuka = kuka2.ypos
d = sqrt((beacon1.xpos - kuka2.xpos)**2 +
(beacon1.ypos - kuka2.ypos)**2)
theta = kuka2.yaw
vel_mat = np.matrix([[vel_scaling_factor * (xhelmet - xkuka) / d],
[0.1 * (yhelmet - ykuka) / d]])
rotation_angle = np.matrix([[cos(theta), sin(theta)],
[-sin(theta), cos(theta)]])
newvel_mat = np.dot(rotation_angle, vel_mat)
xvel = newvel_mat[0]
yvel = newvel_mat[1]
constant_matrix = np.matrix([[1, 0], [0, 1 / e]])
angular_matrix = np.dot(constant_matrix, newvel_mat)
vvel = angular_matrix[0]
wvel = angular_matrix[1]
#rospy.loginfo("Xvel %0.4f, Yvel %0.4f",xvel,yvel)
rospy.sleep(0.5)
kuka1.linearvel(xvel, yvel)
kuka1.linearvel(0, 0)
def go_to_position(self, object_name):
initial_position.main()
self.object_name = 'cup2'
beacon1 = xyz_vicon.xyz_vicon('cup2')
beacon1.talker()
kuka2 = xyz_kuka.xyzkuka()
kuka2.talker()
rospy.sleep(1)
put_down = putdown.arm()
xbeacon = beacon1.xpos
ybeacon = beacon1.ypos
zbeacon = beacon1.zpos
xkuka = kuka2.xpos
ykuka = kuka2.ypos
zkuka = kuka2.zpos
theta = kuka2.yaw
deltax = xbeacon - xkuka
deltay = ybeacon - ykuka
deltaz = (zbeacon - zkuka)
# print "initial deltas"
# print deltax
# print deltay
# print deltaz
pos_mat = np.matrix([[deltax], [deltay]])
rotation_angle = np.matrix([[cos(theta), sin(theta)],
[-sin(theta), cos(theta)]])
new_pos_kuka = np.dot(rotation_angle, pos_mat)
newdeltax = new_pos_kuka[0] - 0.18
newdeltay = new_pos_kuka[1]
print "printing newdeltay"
print newdeltay
print "printing newdeltax"
print newdeltax - 0.02
print "printing deltaz"
print -deltaz + 0.1
rospy.sleep(1)
print "now working"
arm_ik_control.go_to_xyz(newdeltax - 0.02, newdeltay + 0.12,
-deltaz + 0.1, self.arm_pub)
rospy.sleep(5)
print "now about to grab the cup"
arm_ik_control.go_to_xyz(newdeltax - 0.02, newdeltay + 0.04,
-deltaz + 0.1, self.arm_pub)
rospy.sleep(2)
gripper_close.main()
rospy.sleep(2)
put_down.main()
rospy.sleep(2)
initial_position.main()
def go_to_position(self, object_name):
self.object_name = object_name
beacon1 = xyz_vicon.xyz_vicon(object_name)
beacon1.talker()
kuka2 = xyz_kuka.xyzkuka()
kuka2.talker()
rospy.sleep(2)
xbeacon = beacon1.xpos
ybeacon = beacon1.ypos
zbeacon = beacon1.zpos
print "these are cup's positions"
print xbeacon
print ybeacon
print zbeacon
xkuka = kuka2.xpos
ykuka = kuka2.ypos
zkuka = kuka2.zpos
theta = kuka2.yaw
rospy.sleep(2)
print "these are kuka's positions"
print xkuka
print ykuka
print zkuka
print theta
pos_mat = np.matrix([[xkuka], [ykuka]])
rotation_angle = np.matrix([[cos(theta), sin(theta)],
[-sin(theta), cos(theta)]])
new_pos_kuka = np.dot(rotation_angle, pos_mat)
newkukax = new_pos_kuka[0] - 0.18
newkukay = new_pos_kuka[1]
deltax = abs(xbeacon - newkukax)
deltay = abs(ybeacon - newkukay)
deltaz = abs((zbeacon - zkuka)) + 0.15
arm_ik_control.go_to_xyz(deltax, deltay, deltaz, self.arm_pub)
def mainfile(self):
waypoint_1 = moveKUKA_waypoint_1.kuka()
waypoint_2 = moveKUKA_waypoint_2.kuka()
waypoint_3 = moveKUKA_waypoint_3.kuka()
# pickupcup_2 = pickupcup2.arm()
pickupcup_3 = pickupcup3.arm()
put_it_back = putitback.arm()
put_down = putdown.arm()
waypoint1 = xyz_vicon.xyz_vicon('waypoint_1')
waypoint1.talker()
waypoint2 = xyz_vicon.xyz_vicon('waypoint_2')
waypoint2.talker()
waypoint3 = xyz_vicon.xyz_vicon('waypoint_3')
waypoint3.talker()
rospy.sleep(1)
print "obtaining waypoints' positions"
waypoint1xpos = waypoint1.xpos
waypoint1ypos = waypoint1.ypos
waypoint2xpos = waypoint2.xpos
waypoint2ypos = waypoint2.ypos
waypoint3xpos = waypoint3.xpos
waypoint3ypos = waypoint3.ypos
iteration = 0
for iteration in range (0,10):
print "in a for loop"
iteration = iteration+1
print "this is iteration # " + str(iteration)
rospy.sleep(1)
print "Now obtaining positions of the KUKA"
rospy.sleep(1)
# cup2 = xyz_vicon.xyz_vicon('cup2')
# cup2.talker()
cup3 = xyz_vicon.xyz_vicon('cup3')
cup3.talker()
waypoint1 = xyz_vicon.xyz_vicon('waypoint_1')
waypoint1.talker()
waypoint2 = xyz_vicon.xyz_vicon('waypoint_2')
waypoint2.talker()
waypoint3 = xyz_vicon.xyz_vicon('waypoint_3')
waypoint3.talker()
kuka = xyz_kuka.xyzkuka()
kuka.talker()
rospy.sleep(1)
# cup2xpos = cup2.xpos
# cup2ypos = cup2.ypos
cup3xpos = cup3.xpos
cup3ypos = cup3.ypos
kukaxpos = kuka.xpos
kukaypos = kuka.ypos
print "Now calculating the distances"
rospy.sleep(1)
distance_to_waypoint_1 = abs(sqrt((kukaxpos - waypoint1xpos)**2)+(kukaypos - waypoint1ypos)**2)
distance_to_waypoint_2 = abs(sqrt((kukaxpos - waypoint2xpos)**2)+(kukaypos - waypoint2ypos)**2)
distance_to_waypoint_3 = abs(sqrt((kukaxpos - waypoint3xpos)**2)+(kukaypos - waypoint3ypos)**2)
# distance_to_cup2 = abs(sqrt((kukaxpos - cup2xpos)**2)+(kukaypos - cup2ypos)**2)
distance_to_cup3 = abs(sqrt((kukaxpos - cup3xpos)**2)+(kukaypos - cup3ypos)**2)
stopping_distance = 2
pickup_distance = 0.8
dropping_distance = 0.3
print "Where is KUKA?"
rospy.sleep(1)
if distance_to_waypoint_1 < distance_to_waypoint_2 and distance_to_waypoint_1 < distance_to_waypoint_3 and distance_to_cup3 > pickup_distance:#distance_to_cup2 > pickup_distance and
print "KUKA is next to waypoint 1 and no cup nearby"
rospy.sleep(1)
actions=[waypoint_3] #, waypoint_2]
perform_action = random.choice(actions)
if waypoint_2 == perform_action or waypoint_3 == perform_action:
print "Now executing " + str(perform_action)
perform_action.move_to_object()
rospy.sleep(1)
print "go back to the for loop"
if distance_to_waypoint_1 < distance_to_waypoint_2 and distance_to_waypoint_1 < distance_to_waypoint_3 and distance_to_cup3 > pickup_distance: #and distance_to_cup2 < pickup_distance
print "KUKA is next to waypoint 1 and cup 2 nearby"
rospy.sleep(1)
actions=[waypoint_2, waypoint_3 ,pickupcup_2]
perform_action = random.choice(actions)
if waypoint_2 == perform_action or waypoint_3 == perform_action:
print "Now executing " + str(perform_action)
perform_action.move_to_object()
else:
print "Now executing " + str(perform_action)
perform_action.go_to_position('cup2')
another_actions = [waypoint_2, waypoint_3]
perform_another_actions = random.choice(actions)
if waypoint_2 == perform_another_actions or waypoint_3 == perform_another_actions:
print "Now executing " + str(perform_another_actions)
perform_action.move_to_object()
rospy.sleep(1)
print "go back to the for loop"
if distance_to_waypoint_1 < distance_to_waypoint_2 and distance_to_waypoint_1 < distance_to_waypoint_3 and distance_to_cup3 < pickup_distance: #and distance_to_cup2 > pickup_distance:
print "KUKA is next to waypoint 1 and cup 3 nearby"
rospy.sleep(1)
actions=[waypoint_2, waypoint_3, pickupcup_3]
perform_action = random.choice(actions)
if waypoint_2 == perform_action or waypoint_3 == perform_action:
print "Now executing " + str(perform_action)
perform_action.move_to_object()
else:
print "Now executing " + str(perform_action)
perform_action.go_to_position('cup3')
another_actions = [waypoint_2, waypoint_3]
perform_another_actions = random.choice(actions)
if waypoint_2 == perform_another_actions or waypoint_3 == perform_another_actions:
print "Now executing " + str(perform_another_actions)
perform_another_actions.move_to_object()
rospy.sleep(1)
print "go back to the for loop"
if distance_to_waypoint_1 < distance_to_waypoint_2 and distance_to_waypoint_1 < distance_to_waypoint_3 and distance_to_cup3 > pickup_distance: #and distance_to_cup2 < dropping_distance
print "KUKA is next to waypoint 1 and cup 2 ontop"
rospy.sleep(1)
actions=[waypoint_2, waypoint_3, put_it_back] #put it on top of the box
perform_action = random.choice(actions)
if waypoint_2 == perform_action or waypoint_3 == perform_action:
print "Now executing " + str(perform_action)
perform_action.move_to_object()
else:
print "Now executing " + str(perform_action)
perform_action.main()
another_actions = [waypoint_2, waypoint_3]
perform_another_actions = random.choice(actions)
if waypoint_2 == perform_another_actions or waypoint_3 == perform_another_actions:
print "Now executing " + str(perform_another_actions)
perform_action.move_to_object()
rospy.sleep(1)
print "go back to the for loop"
if distance_to_waypoint_1 < distance_to_waypoint_2 and distance_to_waypoint_1 < distance_to_waypoint_3 and distance_to_cup3 < dropping_distance:# and distance_to_cup2 > pickup_distance:
print "KUKA is next to waypoint 1 and cup 3 ontop"
rospy.sleep(1)
actions=[waypoint_2, waypoint_3, put_it_back]
perform_action = random.choice(actions)
if waypoint_2 == perform_action or waypoint_3 == perform_action:
print "Now executing " + str(perform_action)
perform_action.move_to_object()
else:
print "Now executing " + str(perform_action)
perform_action.main()
another_actions = [waypoint_2, waypoint_3]
perform_another_actions = random.choice(actions)
if waypoint_2 == perform_another_actions or waypoint_3 == perform_another_actions:
print "Now executing " + str(perform_another_actions)
perform_action.move_to_object()
rospy.sleep(1)
print "go back to the for loop"
if distance_to_waypoint_1 < distance_to_waypoint_2 and distance_to_waypoint_1 < distance_to_waypoint_3 and distance_to_cup3 < dropping_distance: # and distance_to_cup2 < pickup_distance:
rospy.sleep(1)
print "cup 3 is on top of the KUKA and cup 2 is nearby"
actions = [waypoint_2, waypoint_3, put_it_back] #putdown cup 2
perform_action = random.choice(actions)
if waypoint_2 == perform_action or waypoint_3 == perform_action:
print "Now executing " + str(perform_action)
perform_action.go_to_object()
else:
print "Now executing " + str(perform_action)
perform_action.main()
rospy.sleep(1)
print "go back to the for loop"
if distance_to_waypoint_1 < distance_to_waypoint_2 and distance_to_waypoint_1 < distance_to_waypoint_3 and distance_to_cup3 < pickup_distance: #and distance_to_cup2 < dropping_distance
print "cup 2 is on top of the KUKA and cup 3 is nearby"
actions = [waypoint_2, waypoint_3, put_it_back] #putdown cup 3
perform_action = random.choice(actions)
if waypoint_2 == perform_action or waypoint_3 == perform_action:
print "Now executing " + str(perform_action)
perform_action.go_to_object()
else:
print "Now executing " + str(perform_action)
perform_action.main()
rospy.sleep(1)
print "go back to the for loop"
if distance_to_waypoint_2 < distance_to_waypoint_1 and distance_to_waypoint_2 < distance_to_waypoint_3 and distance_to_cup3 > pickup_distance: #and distance_to_cup2 > pickup_distance
print "KUKA is next to waypoint 2 and no cup nearby"
rospy.sleep(1)
actions=[waypoint_1, waypoint_3]
perform_action = random.choice(actions)
if waypoint_1 == perform_action or waypoint_3 == perform_action:
print "Now executing " + str(perform_action)
perform_action.move_to_object()
rospy.sleep(1)
print "go back to the for loop"
if distance_to_waypoint_2 < distance_to_waypoint_1 and distance_to_waypoint_2 < distance_to_waypoint_3 and distance_to_cup3 > pickup_distance: #and distance_to_cup2 < pickup_distance
print "KUKA is next to waypoint 2 and cup 2 nearby"
rospy.sleep(1)
# actions=[waypoint_1, waypoint_3, pickupcup_2]
actions=[pickupcup_2]
perform_action = random.choice(actions)
if waypoint_1 == perform_action or waypoint_3 == perform_action:
print "Now executing " + str(perform_action)
perform_action.move_to_object()
else:
print "Now executing " + str(perform_action)
perform_action.go_to_position('cup2')
another_actions = [waypoint_1, waypoint_3]
perform_another_actions = random.choice(actions)
if waypoint_1 == perform_another_actions or waypoint_3 == perform_another_actions:
print "Now executing " + str(perform_another_actions)
perform_action.move_to_object()
rospy.sleep(1)
print "go back to the for loop"
if distance_to_waypoint_2 < distance_to_waypoint_1 and distance_to_waypoint_2 < distance_to_waypoint_3 and distance_to_cup3 < pickup_distance: #and distance_to_cup2 > pickup_distance:
print "KUKA is next to waypoint 2 and cup 3 nearby"
rospy.sleep(1)
actions=[waypoint_2, waypoint_3, pickupcup_3]
perform_action = random.choice(actions)
if waypoint_1 == perform_action or waypoint_3 == perform_action:
print "Now executing " + str(perform_action)
perform_action.move_to_object()
else:
print "Now executing " + str(perform_action)
perform_action.go_to_position('cup3')
another_actions = [waypoint_1, waypoint_3]
perform_another_actions = random.choice(actions)
if waypoint_1 == perform_another_actions or waypoint_3 == perform_another_actions:
print "Now executing " + str(perform_another_actions)
perform_another_actions.move_to_object()
rospy.sleep(1)
print "go back to the for loop"
if distance_to_waypoint_2 < distance_to_waypoint_1 and distance_to_waypoint_2 < distance_to_waypoint_3 and distance_to_cup3 > pickup_distance: # and distance_to_cup2 < dropping_distance :
print "KUKA is next to waypoint 2 and cup 2 ontop"
rospy.sleep(1)
actions=[waypoint_1, waypoint_3, put_it_back] #put it on top of the box
perform_action = random.choice(actions)
if waypoint_1 == perform_action or waypoint_3 == perform_action:
print "Now executing " + str(perform_action)
perform_action.move_to_object()
else:
print "Now executing " + str(perform_action)
perform_action.main()
another_actions = [waypoint_1, waypoint_3]
perform_another_actions = random.choice(actions)
if waypoint_1 == perform_another_actions or waypoint_3 == perform_another_actions:
print "Now executing " + str(perform_another_actions)
perform_action.move_to_object()
rospy.sleep(1)
print "go back to the for loop"
if distance_to_waypoint_2 < distance_to_waypoint_1 and distance_to_waypoint_2 < distance_to_waypoint_3 and distance_to_cup2 > pickup_distance: #and distance_to_cup3 < dropping_distance
print "KUKA is next to waypoint 2 and cup 3 ontop"
rospy.sleep(1)
actions=[waypoint_1, waypoint_3, put_it_back]
perform_action = random.choice(actions)
if waypoint_1 == perform_action or waypoint_3 == perform_action:
print "Now executing " + str(perform_action)
perform_action.move_to_object()
else:
print "Now executing " + str(perform_action)
perform_action.main()
another_actions = [waypoint_1, waypoint_3]
perform_another_actions = random.choice(actions)
if waypoint_1 == perform_another_actions or waypoint_3 == perform_another_actions:
print "Now executing " + str(perform_another_actions)
perform_action.move_to_object()
rospy.sleep(1)
print "go back to the for loop"
if distance_to_waypoint_2 < distance_to_waypoint_1 and distance_to_waypoint_2 < distance_to_waypoint_3 and distance_to_cup3 < dropping_distance: #and distance_to_cup2 < pickup_distance:
rospy.sleep(1)
print "cup 3 is on top of the KUKA and cup 2 is nearby"
actions = [waypoint_1, waypoint_3, put_it_back] #putdown cup 2
perform_action = random.choice(actions)
if waypoint_1 == perform_action or waypoint_3 == perform_action:
print "Now executing " + str(perform_action)
perform_action.go_to_object()
else:
print "Now executing " + str(perform_action)
perform_action.main()
rospy.sleep(1)
print "go back to the for loop"
if distance_to_waypoint_2 < distance_to_waypoint_1 and distance_to_waypoint_2 < distance_to_waypoint_3 and distance_to_cup3 < pickup_distance: #and distance_to_cup2 < dropping_distance
print "cup 3 is on top of the KUKA and cup 2 is nearby"
actions = [waypoint_1, waypoint_3, put_it_back] #putdown cup 3
perform_action = random.choice(actions)
if waypoint_1 == perform_action or waypoint_3 == perform_action:
print "Now executing " + str(perform_action)
perform_action.go_to_object()
else:
print "Now executing " + str(perform_action)
perform_action.main()
rospy.sleep(1)
print "go back to the for loop"
if distance_to_waypoint_3 < distance_to_waypoint_2 and distance_to_waypoint_3 < distance_to_waypoint_1 and distance_to_cup3 > pickup_distance: #and distance_to_cup2 > pickup_distance
print "KUKA is next to waypoint 3 and no cup nearby"
rospy.sleep(1)
actions=[waypoint_2, waypoint_1]
perform_action = random.choice(actions)
if waypoint_2 == perform_action or waypoint_1 == perform_action:
print "Now executing " + str(perform_action)
perform_action.move_to_object()
rospy.sleep(1)
print "go back to the for loop"
if distance_to_waypoint_3 < distance_to_waypoint_2 and distance_to_waypoint_3 < distance_to_waypoint_1 and distance_to_cup3 > pickup_distance: #and distance_to_cup2 < pickup_distance
print "KUKA is next to waypoint 3 and cup 2 nearby"
rospy.sleep(1)
actions=[waypoint_2, waypoint_1, pickupcup_2]
perform_action = random.choice(actions)
if waypoint_2 == perform_action or waypoint_1 == perform_action:
print "Now executing " + str(perform_action)
perform_action.move_to_object()
else:
print "Now executing " + str(perform_action)
perform_action.go_to_position('cup2')
another_actions = [waypoint_2, waypoint_1]
perform_another_actions = random.choice(actions)
if waypoint_2 == perform_another_actions or waypoint_1 == perform_another_actions:
print "Now executing " + str(perform_another_actions)
perform_action.move_to_object()
rospy.sleep(1)
print "go back to the for loop"
if distance_to_waypoint_3 < distance_to_waypoint_2 and distance_to_waypoint_3 < distance_to_waypoint_1 and distance_to_cup3 < pickup_distance: # and distance_to_cup2 > pickup_distance:
print "KUKA is next to waypoint 3 and cup 3 nearby"
rospy.sleep(1)
actions=[waypoint_2, waypoint_1, pickupcup_3]
perform_action = random.choice(actions)
if pickupcup_3 == perform_action:
print "Now executing " + str(perform_action)
perform_action.go_to_position('cup3')
print "cup has been picked up"
another_actions = [waypoint_2, waypoint_1]
perform_another_actions = random.choice(another_actions)
if waypoint_2 == perform_another_actions or waypoint_1 == perform_another_actions:
print "Now executing " + str(perform_another_actions)
perform_another_actions.move_to_object()
else:
print "Now executing " + str(perform_action)
perform_action.move_to_object()
rospy.sleep(1)
print "go back to the for loop"
if distance_to_waypoint_3 < distance_to_waypoint_2 and distance_to_waypoint_3 < distance_to_waypoint_1 and distance_to_cup3 > pickup_distance: #and distance_to_cup2 < dropping_distance
print "KUKA is next to waypoint 3 and cup 2 ontop"
rospy.sleep(1)
actions=[waypoint_2, waypoint_1, put_it_back] #put it on top of the box
perform_action = random.choice(actions)
if waypoint_2 == perform_action or waypoint_1 == perform_action:
print "Now executing " + str(perform_action)
perform_action.move_to_object()
else:
print "Now executing " + str(perform_action)
perform_action.main()
another_actions = [waypoint_2, waypoint_1]
perform_another_actions = random.choice(actions)
if waypoint_2 == perform_another_actions or waypoint_1 == perform_another_actions:
print "Now executing " + str(perform_another_actions)
perform_action.move_to_object()
rospy.sleep(1)
print "go back to the for loop"
if distance_to_waypoint_3 < distance_to_waypoint_2 and distance_to_waypoint_3 < distance_to_waypoint_1 and distance_to_cup3 < dropping_distance: #and distance_to_cup2 > pickup_distance:
print "KUKA is next to waypoint 3 and cup 3 ontop"
rospy.sleep(1)
actions=[waypoint_2, waypoint_1, put_it_back]
perform_action = random.choice(actions)
if waypoint_2 == perform_action or waypoint_1 == perform_action:
print "Now executing " + str(perform_action)
perform_action.move_to_object()
else:
print "Now executing " + str(perform_action)
perform_action.main()
another_actions = [waypoint_2, waypoint_1]
perform_another_actions = random.choice(actions)
if waypoint_2 == perform_another_actions or waypoint_1 == perform_another_actions:
print "Now executing " + str(perform_another_actions)
perform_action.move_to_object()
rospy.sleep(1)
print "go back to the for loop"
if distance_to_waypoint_3 < distance_to_waypoint_2 and distance_to_waypoint_3 < distance_to_waypoint_1 and distance_to_cup3 < dropping_distance : #and distance_to_cup2 < pickup_distance:
rospy.sleep(1)
print "cup 3 is on top of the KUKA and cup 2 is nearby"
actions = [waypoint_2, waypoint_1, put_it_back] #putdown cup 2
perform_action = random.choice(actions)
if waypoint_2 == perform_action or waypoint_1 == perform_action:
print "Now executing " + str(perform_action)
perform_action.go_to_object()
else:
print "Now executing " + str(perform_action)
perform_action.main()
rospy.sleep(1)
print "go back to the for loop"
if distance_to_waypoint_3 < distance_to_waypoint_2 and distance_to_waypoint_3 < distance_to_waypoint_1 and distance_to_cup3 < pickup_distance: #and distance_to_cup2 < dropping_distance
print "cup 3 is on top of the KUKA and cup 2 is nearby"
actions = [waypoint_2, waypoint_1, put_it_back] #putdown cup 3
perform_action = random.choice(actions)
if waypoint_2 == perform_action or waypoint_1 == perform_action:
print "Now executing " + str(perform_action)
perform_action.go_to_object()
else:
print "Now executing " + str(perform_action)
perform_action.main()
rospy.sleep(1)
print "go back to the for loop"