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 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'