class CustomActionClass(object): _feedback = CustomActionMsgFeedback() _result = CustomActionMsgResult() _pubTakoff = rospy.Publisher("/drone/takeoff", Empty, queue_size=10) _pubLand = rospy.Publisher("/drone/land", Empty, queue_size=10) def __init__(self): self._as = actionlib.SimpleActionServer("/action_custom_msg_as", CustomActionMsgAction, self.goal_callback, False) self._as.start() def goal_callback(self, goal): success = True # publish info to the console for the user if (goal.goal == 'TAKEOFF'): rospy.loginfo("Taking off!") self._pubTakoff.publish(Empty()) self._feedback.feedback = "TAKEOFF" elif (goal.goal == 'LAND'): rospy.loginfo("Landing!") self._pubLand.publish(Empty()) self._feedback.feedback = "LAND" else: rospy.loginfo('The goal has been cancelled/preempted') self._as.set_preempted() success = False self._as.publish_feedback(self._feedback) if success: rospy.loginfo('Success!') self._as.set_succeeded(self._feedback)
def goal_callback(self, goal): # this callback is called when the action server is called. # helper variable success = True action = goal.goal # publish info to the console for the user rospy.loginfo('Goal: ' + action) # check that preempt (cancelation) has not been requested by the action client if self._as.is_preempt_requested(): rospy.loginfo('The goal has been cancelled/preempted') # the following line, sets the client in preempted state (goal cancelled) self._as.set_preempted() success = False elif action == 'TAKEOFF': self.take_off() elif action == 'LAND': self.land() else: rospy.loginfo('Invalid goal') success = False # at this point, either the goal has been achieved (success==true) # or the client preempted the goal (success==false) if success: rospy.loginfo('Action succeeded') self._as.set_succeeded(CustomActionMsgResult())
class Take_Off_Land_Class(object): # create messages that are used to publish feedback/result _feedback = CustomActionMsgFeedback() _result = CustomActionMsgResult() def __init__(self): # creates the action server self._as = actionlib.SimpleActionServer("/action_custom_msg_as", CustomActionMsgAction, self.goal_callback, False) self._as.start() def goal_callback(self, goal): # this callback is called when the action server is called. # this is the function that computes the Fibonacci sequence # and returns the sequence to the node that called the action server # helper variables r = rospy.Rate(1) success = True launchPub = rospy.Publisher('/drone/takeoff', Empty, queue_size=1) landPub = rospy.Publisher('/drone/land', Empty, queue_size=1) # publish info to the console for the user rospy.loginfo("Checking For Instruction") goalCommand = goal.goal time.sleep(1) if self._as.is_preempt_requested(): rospy.loginfo('The goal has been cancelled/preempted') # the following line, sets the client in preempted state (goal cancelled) self._as.set_preempted() success = False # we end the calculation of the Fibonacci sequence if (goalCommand == "TAKEOFF"): self._feedback.feedback = "TAKEOFF" launchPub.publish(Empty()) if (goalCommand == "LAND"): self._feedback.feedback = "LAND" landPub.publish(Empty()) self._as.publish_feedback(self._feedback) # at this point, either the goal has been achieved (success==true) # or the client preempted the goal (success==false) # If success, then we publish the final result # If not success, we do not publish anything in the result #if success: # self._result.sequence = self._feedback.sequence # rospy.loginfo('Succeeded calculating the Fibonacci of order %i' % fibonacciOrder ) self._as.set_succeeded(self._result)
class CustomActionMsgClass(object): _feedback = CustomActionMsgFeedback() _result = CustomActionMsgResult() def __init__(self): self._as = actionlib.SimpleActionServer("action_custom_msg_as", CustomActionMsgAction, self.goal_callback, False) self._as.start() def goal_callback(self, goal): success = True r = rospy.Rate(1) self._pub_takeoff = rospy.Publisher('/drone/takeoff', Empty, queue_size=1) self._takeoff_msg = Empty() self._pub_land = rospy.Publisher('/drone/land', Empty, queue_size=1) self._land_msg = Empty() takeoff_or_land = goal.goal i = 0 for i in xrange(0, 4): if self._as.is_preempt_requested(): rospy.loginfo('The goal has been cancelled/preempted') self._as.set_preempted() success = False break if takeoff_or_land == 'TAKEOFF': self._pub_takeoff.publish(self._takeoff_msg) self._feedback.feedback = 'Taking Off...' self._as.publish_feedback(self._feedback) if takeoff_or_land == 'LAND': self._pub_land.publish(self._land_msg) self._feedback.feedback = 'Landing...' self._as.publish_feedback(self._feedback) r.sleep() if success: self._result = Empty() self._as.set_succeeded(self._result)
class Actions_quiz(object): # create messages that are used to publish feedback/result _feedback = CustomActionMsgFeedback() _result = CustomActionMsgResult() def __init__(self): # creates the action server self._as = actionlib.SimpleActionServer("/action_custom_msg_as", CustomActionMsgAction, self.goal_callback, False) self._as.start() def goal_callback(self, goal): # this callback is called when the action server is called. # this is the function that computes the Fibonacci sequence # and returns the sequence to the node that called the action server # helper variables success = True _goal = goal.goal success = True r = rospy.Rate(1) launch = rospy.Publisher('/drone/takeoff', Empty, queue_size=1) land = rospy.Publisher('/drone/land', Empty, queue_size=1) rospy.loginfo('Instruction : %s' % _goal) time.sleep(1) # check that preempt (cancelation) has not been requested by the action client if self._as.is_preempt_requested(): rospy.loginfo('The goal has been cancelled/preempted') # the following line, sets the client in preempted state (goal cancelled) self._as.set_preempted() success = False if (_goal == "TAKEOFF"): launch.publish(Empty()) self._feedback.feedback = "TAKEOFF" elif (_goal == "LAND"): land.publish(Empty()) self._feedback.feedback = "LAND" self._as.publish_feedback(self._feedback) # at this point, either the goal has been achieved (success==true) # or the client preempted the goal (success==false) # If success, then we publish the final result # If not success, we do not publish anything in the result #if success: # rospy.loginfo('Successfully done command : %s' % goal.goal ) self._as.set_succeeded(self._result)
class CustomActionMsgClass(object): # Message assignment feedback = CustomActionMsgFeedback() result = CustomActionMsgResult() def __init__(self): # Constructor for action server self.action_server = actionlib.SimpleActionServer("action_custom_msg_as", CustomActionMsgAction, self.goal_callback, False) self.action_server.start() def goal_callback(self, goal): # Publishers Assignment self.takeoff_pub = rospy.Publisher('/drone/takeoff', Empty, queue_size=1) self.land_pub = rospy.Publisher('/drone/land', Empty, queue_size=1) goal_word = goal.goal r = rospy.Rate(1) success = True for i in range(4): # Check for preempt and set if required if self.action_server.is_preempt_requested(): rospy.loginfo('The goal has been preempted') self.action_server.set_preempted() success = False break # Check for TAKEOFF goal word if goal_word == 'TAKEOFF': self.takeoff_pub.publish(Empty()) self.feedback.feedback = 'Taking Off...' rospy.loginfo('Taking Off...') self.action_server.publish_feedback(self.feedback) # Check for LAND goal word if goal_word == 'LAND': self.land_pub.publish(Empty()) self.feedback.feedback = 'Landing...' rospy.loginfo('Landing...') self.action_server.publish_feedback(self.feedback) # Send feedback once per second r.sleep() # Returning Empty result if success == True: self.result = Empty() self.action_server.set_succeeded(self.result)
def goal_callback(self, _goal): feedback = CustomActionMsgFeedback() feedback.feedback = _goal.goal if _goal.goal == 'TAKEOFF ': activity = self.takeoff_drone elif _goal.goal == 'LAND': activity = self.land_drone else: assert 0 action = self._as_ rate = rospy.Rate(1) activity() while not rospy.is_shutdown(): action.publish_feedback(feedback) rate.sleep() result = CustomActionMsgResult() action.set_succeeded(result)
class CustomActionMsgClass(object): # create messages that are used to publish feedback/result _feedback = CustomActionMsgFeedback() _result = CustomActionMsgResult() def __init__(self): # creates the action server self._as = actionlib.SimpleActionServer("action_custom_msg_as", CustomActionMsgAction, self.goal_callback, False) self._as.start() def goal_callback(self, goal): # this callback is called when the action server is called. # helper variables success = True r = rospy.Rate(1) # define the different publishers and messages that will be used self._pub_takeoff = rospy.Publisher('/drone/takeoff', Empty, queue_size=1) self._takeoff_msg = Empty() self._pub_land = rospy.Publisher('/drone/land', Empty, queue_size=1) self._land_msg = Empty() # Get the goal word: UP or DOWN takeoff_or_land = goal.goal i = 0 for i in xrange(0, 4): # check that preempt (cancelation) has not been requested by the action client if self._as.is_preempt_requested(): rospy.loginfo('The goal has been cancelled/preempted') # the following line, sets the client in preempted state (goal cancelled) self._as.set_preempted() success = False break # Logic that makes the robot move UP or DOWN if takeoff_or_land == 'TAKEOFF': self._pub_takeoff.publish(self._takeoff_msg) self._feedback.feedback = 'Taking Off...' self._as.publish_feedback(self._feedback) if takeoff_or_land == 'LAND': self._pub_land.publish(self._land_msg) self._feedback.feedback = 'Landing...' self._as.publish_feedback(self._feedback) # the sequence is computed at 1 Hz frequency r.sleep() # at this point, either the goal has been achieved (success==true) # or the client preempted the goal (success==false) # If success, then we publish the final result # If not success, we do not publish anything in the result if success: self._result = Empty() self._as.set_succeeded(self._result)
class MoveSquareClass(object): # create messages that are used to publish feedback/result _feedback = CustomActionMsgFeedback() _result = CustomActionMsgResult() def __init__(self): # creates the action server self._as = actionlib.SimpleActionServer("action_custom_msg_as", CustomActionMsgAction, self.goal_callback, False) self._as.start() self.ctrl_c = False self.rate = rospy.Rate(10) def publish_once_in_cmd_vel(self, cmd): """ This is because publishing in topics sometimes fails teh first time you publish. In continuos publishing systems there is no big deal but in systems that publish only once it IS very important. """ while not self.ctrl_c: connections = self._pub_cmd_vel.get_num_connections() if connections > 0: self._pub_cmd_vel.publish(cmd) rospy.loginfo("Publish in cmd_vel...") break else: self.rate.sleep() def take_off(self): # make the drone takeoff i = 0 while not i == 5: self._pub_takeoff.publish(self._takeoff_msg) rospy.loginfo('Taking off...') time.sleep(1) i += 1 rospy.loginfo('Finished Takeoff') self._as.set_succeeded(self._result) def land(self): # make the drone takeoff i = 0 while not i == 5: self._pub_land.publish(self._land_msg) rospy.loginfo('Landing...') time.sleep(1) i += 1 rospy.loginfo('Finished Landing') self._as.set_succeeded(self._result) def goal_callback(self, goal): # this callback is called when the action server is called. # this is the function that computes the Fibonacci sequence # and returns the sequence to the node that called the action server # helper variables r = rospy.Rate(1) success = True # define the different publishers and messages that will be used self._pub_cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1) self._move_msg = Twist() self._pub_takeoff = rospy.Publisher('/drone/takeoff', Empty, queue_size=1) self._takeoff_msg = Empty() self._pub_land = rospy.Publisher('/drone/land', Empty, queue_size=1) self._land_msg = Empty() # get the command from goal object command = goal.goal # check that preempt (cancelation) has not been requested by the action client if self._as.is_preempt_requested(): rospy.loginfo('The goal has been cancelled/preempted') # the following line, sets the client in preempted state (goal cancelled) self._as.set_preempted() success = False elif command == "TAKEOFF": self.take_off() self._feedback.feedback = "TAKEOFF" elif command == "LAND": self.land() self._feedback.feedback = "LAND" # build and publish the feedback message self._as.publish_feedback(self._feedback) # the sequence is computed at 1 Hz frequency r.sleep() # at this point, either the goal has been achieved (success==true) # or the client preempted the goal (success==false) # If success, then we publish the final result # If not success, we do not publish anything in the result if success: rospy.loginfo('Finished the mission, you may type another command')