simple_action_server.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 # Copyright (c) 2009, Willow Garage, Inc.
00003 # All rights reserved.
00004 #
00005 # Redistribution and use in source and binary forms, with or without
00006 # modification, are permitted provided that the following conditions are met:
00007 #
00008 #     * Redistributions of source code must retain the above copyright
00009 #       notice, this list of conditions and the following disclaimer.
00010 #     * Redistributions in binary form must reproduce the above copyright
00011 #       notice, this list of conditions and the following disclaimer in the
00012 #       documentation and/or other materials provided with the distribution.
00013 #     * Neither the name of the Willow Garage, Inc. nor the names of its
00014 #       contributors may be used to endorse or promote products derived from
00015 #       this software without specific prior written permission.
00016 #
00017 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027 # POSSIBILITY OF SUCH DAMAGE.
00028 #
00029 # Author: Alexander Sorokin.
00030 # Based on C++ simple_action_server.h by Eitan Marder-Eppstein
00031 import rospy
00032 import threading
00033 import traceback
00034 
00035 from actionlib_msgs.msg import GoalStatus
00036 
00037 from actionlib import ActionServer
00038 from actionlib.server_goal_handle import ServerGoalHandle
00039 
00040 
00041 def nop_cb(goal_handle):
00042     pass
00043 
00044 
00045 ## @class SimpleActionServer
00046 ## @brief The SimpleActionServer
00047 ## implements a singe goal policy on top of the ActionServer class. The
00048 ## specification of the policy is as follows: only one goal can have an
00049 ## active status at a time, new goals preempt previous goals based on the
00050 ## stamp in their GoalID field (later goals preempt earlier ones), an
00051 ## explicit preempt goal preempts all goals with timestamps that are less
00052 ## than or equal to the stamp associated with the preempt, accepting a new
00053 ## goal implies successful preemption of any old goal and the status of the
00054 ## old goal will be change automatically to reflect this.
00055 class SimpleActionServer:
00056     ## @brief Constructor for a SimpleActionServer
00057     ## @param name A name for the action server
00058     ## @param execute_cb Optional callback that gets called in a separate thread whenever
00059     ## a new goal is received, allowing users to have blocking callbacks.
00060     ## Adding an execute callback also deactivates the goalCallback.
00061     ## @param  auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server.
00062     def __init__(self, name, ActionSpec, execute_cb=None, auto_start=True):
00063 
00064         self.new_goal = False
00065         self.preempt_request = False
00066         self.new_goal_preempt_request = False
00067 
00068         self.execute_callback = execute_cb
00069         self.goal_callback = None
00070         self.preempt_callback = None
00071 
00072         self.need_to_terminate = False
00073         self.terminate_mutex = threading.RLock()
00074 
00075         # since the internal_goal/preempt_callbacks are invoked from the
00076         # ActionServer while holding the self.action_server.lock
00077         # self.lock must always be locked after the action server lock
00078         # to avoid an inconsistent lock acquisition order
00079         self.lock = threading.RLock()
00080 
00081         self.execute_condition = threading.Condition(self.lock)
00082 
00083         self.current_goal = ServerGoalHandle()
00084         self.next_goal = ServerGoalHandle()
00085 
00086         if self.execute_callback:
00087             self.execute_thread = threading.Thread(None, self.executeLoop)
00088             self.execute_thread.start()
00089         else:
00090             self.execute_thread = None
00091 
00092         # create the action server
00093         self.action_server = ActionServer(name, ActionSpec, self.internal_goal_callback, self.internal_preempt_callback, auto_start)
00094 
00095     def __del__(self):
00096         if hasattr(self, 'execute_callback') and self.execute_callback:
00097             with self.terminate_mutex:
00098                 self.need_to_terminate = True
00099 
00100             assert(self.execute_thread)
00101             self.execute_thread.join()
00102 
00103     ## @brief Accepts a new goal when one is available The status of this
00104     ## goal is set to active upon acceptance, and the status of any
00105     ## previously active goal is set to preempted. Preempts received for the
00106     ## new goal between checking if isNewGoalAvailable or invokation of a
00107     ## goal callback and the acceptNewGoal call will not trigger a preempt
00108     ## callback.  This means, isPreemptReqauested should be called after
00109     ## accepting the goal even for callback-based implementations to make
00110     ## sure the new goal does not have a pending preempt request.
00111     ## @return A shared_ptr to the new goal.
00112     def accept_new_goal(self):
00113         with self.action_server.lock, self.lock:
00114             if not self.new_goal or not self.next_goal.get_goal():
00115                 rospy.logerr("Attempting to accept the next goal when a new goal is not available")
00116                 return None
00117 
00118             # check if we need to send a preempted message for the goal that we're currently pursuing
00119             if self.is_active() and self.current_goal.get_goal() and self.current_goal != self.next_goal:
00120                 self.current_goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server")
00121 
00122             rospy.logdebug("Accepting a new goal")
00123 
00124             # accept the next goal
00125             self.current_goal = self.next_goal
00126             self.new_goal = False
00127 
00128             # set preempt to request to equal the preempt state of the new goal
00129             self.preempt_request = self.new_goal_preempt_request
00130             self.new_goal_preempt_request = False
00131 
00132             # set the status of the current goal to be active
00133             self.current_goal.set_accepted("This goal has been accepted by the simple action server")
00134 
00135             return self.current_goal.get_goal()
00136 
00137     ## @brief Allows  polling implementations to query about the availability of a new goal
00138     ## @return True if a new goal is available, false otherwise
00139     def is_new_goal_available(self):
00140         return self.new_goal
00141 
00142     ## @brief Allows  polling implementations to query about preempt requests
00143     ## @return True if a preempt is requested, false otherwise
00144     def is_preempt_requested(self):
00145         return self.preempt_request
00146 
00147     ## @brief Allows  polling implementations to query about the status of the current goal
00148     ## @return True if a goal is active, false otherwise
00149     def is_active(self):
00150         if not self.current_goal.get_goal():
00151             return False
00152 
00153         status = self.current_goal.get_goal_status().status
00154         return status == GoalStatus.ACTIVE or status == GoalStatus.PREEMPTING
00155 
00156     ## @brief Sets the status of the active goal to succeeded
00157     ## @param  result An optional result to send back to any clients of the goal
00158     def set_succeeded(self, result=None, text=""):
00159         with self.action_server.lock, self.lock:
00160             if not result:
00161                 result = self.get_default_result()
00162             self.current_goal.set_succeeded(result, text)
00163 
00164     ## @brief Sets the status of the active goal to aborted
00165     ## @param  result An optional result to send back to any clients of the goal
00166     def set_aborted(self, result=None, text=""):
00167         with self.action_server.lock, self.lock:
00168             if not result:
00169                 result = self.get_default_result()
00170             self.current_goal.set_aborted(result, text)
00171 
00172     ## @brief Publishes feedback for a given goal
00173     ## @param  feedback Shared pointer to the feedback to publish
00174     def publish_feedback(self, feedback):
00175         self.current_goal.publish_feedback(feedback)
00176 
00177     def get_default_result(self):
00178         return self.action_server.ActionResultType()
00179 
00180     ## @brief Sets the status of the active goal to preempted
00181     ## @param  result An optional result to send back to any clients of the goal
00182     def set_preempted(self, result=None, text=""):
00183         if not result:
00184             result = self.get_default_result()
00185         with self.action_server.lock, self.lock:
00186             rospy.logdebug("Setting the current goal as canceled")
00187             self.current_goal.set_canceled(result, text)
00188 
00189     ## @brief Allows users to register a callback to be invoked when a new goal is available
00190     ## @param cb The callback to be invoked
00191     def register_goal_callback(self, cb):
00192         if self.execute_callback:
00193             rospy.logwarn("Cannot call SimpleActionServer.register_goal_callback() because an executeCallback exists. Not going to register it.")
00194         else:
00195             self.goal_callback = cb
00196 
00197     ## @brief Allows users to register a callback to be invoked when a new preempt request is available
00198     ## @param cb The callback to be invoked
00199     def register_preempt_callback(self, cb):
00200         self.preempt_callback = cb
00201 
00202     ## @brief Explicitly start the action server, used it auto_start is set to false
00203     def start(self):
00204         self.action_server.start()
00205 
00206     ## @brief Callback for when the ActionServer receives a new goal and passes it on
00207     def internal_goal_callback(self, goal):
00208         self.execute_condition.acquire()
00209 
00210         try:
00211             rospy.logdebug("A new goal %shas been recieved by the single goal action server", goal.get_goal_id().id)
00212 
00213             # check that the timestamp is past that of the current goal and the next goal
00214             if((not self.current_goal.get_goal() or goal.get_goal_id().stamp >= self.current_goal.get_goal_id().stamp)
00215                and (not self.next_goal.get_goal() or goal.get_goal_id().stamp >= self.next_goal.get_goal_id().stamp)):
00216                 # if next_goal has not been accepted already... its going to get bumped, but we need to let the client know we're preempting
00217                 if(self.next_goal.get_goal() and (not self.current_goal.get_goal() or self.next_goal != self.current_goal)):
00218                     self.next_goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server")
00219 
00220                 self.next_goal = goal
00221                 self.new_goal = True
00222                 self.new_goal_preempt_request = False
00223 
00224                 # if the server is active, we'll want to call the preempt callback for the current goal
00225                 if(self.is_active()):
00226                     self.preempt_request = True
00227                     # if the user has registered a preempt callback, we'll call it now
00228                     if(self.preempt_callback):
00229                         self.preempt_callback()
00230 
00231                 # if the user has defined a goal callback, we'll call it now
00232                 if self.goal_callback:
00233                     self.goal_callback()
00234 
00235                 # Trigger runLoop to call execute()
00236                 self.execute_condition.notify()
00237                 self.execute_condition.release()
00238             else:
00239                 # the goal requested has already been preempted by a different goal, so we're not going to execute it
00240                 goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server")
00241                 self.execute_condition.release()
00242         except Exception as e:
00243             rospy.logerr("SimpleActionServer.internal_goal_callback - exception %s", str(e))
00244             self.execute_condition.release()
00245 
00246     ## @brief Callback for when the ActionServer receives a new preempt and passes it on
00247     def internal_preempt_callback(self, preempt):
00248         with self.lock:
00249             rospy.logdebug("A preempt has been received by the SimpleActionServer")
00250 
00251             # if the preempt is for the current goal, then we'll set the preemptRequest flag and call the user's preempt callback
00252             if(preempt == self.current_goal):
00253                 rospy.logdebug("Setting preempt_request bit for the current goal to TRUE and invoking callback")
00254                 self.preempt_request = True
00255 
00256                 # if the user has registered a preempt callback, we'll call it now
00257                 if(self.preempt_callback):
00258                     self.preempt_callback()
00259             # if the preempt applies to the next goal, we'll set the preempt bit for that
00260             elif(preempt == self.next_goal):
00261                 rospy.logdebug("Setting preempt request bit for the next goal to TRUE")
00262                 self.new_goal_preempt_request = True
00263 
00264     ## @brief Called from a separate thread to call blocking execute calls
00265     def executeLoop(self):
00266         loop_duration = rospy.Duration.from_sec(.1)
00267 
00268         while (not rospy.is_shutdown()):
00269             with self.terminate_mutex:
00270                 if (self.need_to_terminate):
00271                     break
00272 
00273             # the following checks (is_active, is_new_goal_available)
00274             # are performed without locking
00275             # the worst thing that might happen in case of a race
00276             # condition is a warning/error message on the console
00277             if (self.is_active()):
00278                 rospy.logerr("Should never reach this code with an active goal")
00279                 return
00280 
00281             if (self.is_new_goal_available()):
00282                 # accept_new_goal() is performing its own locking
00283                 goal = self.accept_new_goal()
00284                 if not self.execute_callback:
00285                     rospy.logerr("execute_callback_ must exist. This is a bug in SimpleActionServer")
00286                     return
00287 
00288                 try:
00289                     self.execute_callback(goal)
00290 
00291                     if self.is_active():
00292                         rospy.logwarn("Your executeCallback did not set the goal to a terminal status.  " +
00293                                       "This is a bug in your ActionServer implementation. Fix your code!  " +
00294                                       "For now, the ActionServer will set this goal to aborted")
00295                         self.set_aborted(None, "No terminal state was set.")
00296                 except Exception as ex:
00297                     rospy.logerr("Exception in your execute callback: %s\n%s", str(ex),
00298                                  traceback.format_exc())
00299                     self.set_aborted(None, "Exception in execute callback: %s" % str(ex))
00300 
00301             with self.execute_condition:
00302                 self.execute_condition.wait(loop_duration.to_sec())


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep
autogenerated on Thu Sep 28 2017 02:51:16