class PickObjective(State):
def __init__(self):
State.__init__(self, outcomes=['succeeded','aborted','preempted','action1','action2','action3','action4','action5','action6','action7'], output_keys=['waypoint_out'])
self.get_objective = GetObjective()
def execute(self, userdata):
self.get_objective.send_request(GetObjectiveRequest())
finished_within_time = self.get_objective.wait_for_response(rospy.Duration(10))
response = self.get_objective.get_response()
waypoint_out = response.goal
waypoint_type = response.type
#rospy.loginfo("Going to waypoint " + str(waypoint_out))
if(waypoint_type.data == 1):
return 'action1'
if(waypoint_type.data == 2):
return 'action2'
if(waypoint_type.data == 3):
return 'action3'
if(waypoint_type.data == 4):
return 'action4'
if(waypoint_type.data == 5):
return 'action5'
if(waypoint_type.data == 6):
return 'action6'
if(waypoint_type.data == 7):
return 'action7'
return 'aborted'
