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'
def __init__(self):
State.__init__(self,
outcomes=[
'succeeded', 'aborted', 'preempted', 'action1',
'action2', 'action3', 'action4', 'action5',
'action6'
],
output_keys=['waypoint_out'])
self.get_objective = GetObjective()
def execute(self, userdata):
rospy.loginfo("Side : " + str(userdata.robot_side))
tesssst = GetObjective()
tesssst.send_request(GetObjectiveRequest())
blabla = GetObjectiveRequest()
request = UpdatePriorityRequest()
request.goal.pose.position.x = userdata.robot_side*(1.500 - 1.300)
request.goal.pose.position.y = 1.000
request.prio = 100
#self.update_objective.send_request(request)
#finished_within_time = self.update_objective.wait_for_response(rospy.Duration(10))
#response = self.update_objective.get_response()
rospy.loginfo("Objective DropCubes priority updated")
return 'succeeded'
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()
