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 
00032 import rospy
00033 
00034 import threading
00035 import traceback
00036 
00037 from actionlib_msgs.msg import *
00038 
00039 from actionlib import ActionServer
00040 from actionlib.server_goal_handle import ServerGoalHandle;
00041 
00042 def nop_cb(goal_handle):
00043     pass
00044 
00045 
00046 ## @class SimpleActionServer
00047 ## @brief The SimpleActionServer
00048 ## implements a singe goal policy on top of the ActionServer class. The
00049 ## specification of the policy is as follows: only one goal can have an
00050 ## active status at a time, new goals preempt previous goals based on the
00051 ## stamp in their GoalID field (later goals preempt earlier ones), an
00052 ## explicit preempt goal preempts all goals with timestamps that are less
00053 ## than or equal to the stamp associated with the preempt, accepting a new
00054 ## goal implies successful preemption of any old goal and the status of the
00055 ## old goal will be change automatically to reflect this.
00056 class SimpleActionServer:
00057     ## @brief Constructor for a SimpleActionServer
00058     ## @param name A name for the action server
00059     ## @param execute_cb Optional callback that gets called in a separate thread whenever
00060     ## a new goal is received, allowing users to have blocking callbacks.
00061     ## Adding an execute callback also deactivates the goalCallback.
00062     ## @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.
00063     def __init__(self, name, ActionSpec, execute_cb = None, auto_start = True):
00064 
00065         self.new_goal = False
00066         self.preempt_request = False
00067         self.new_goal_preempt_request = False
00068 
00069         self.execute_callback = execute_cb;
00070         self.goal_callback = None;
00071         self.preempt_callback = None;
00072 
00073         self.need_to_terminate = False
00074         self.terminate_mutex = threading.RLock();
00075 
00076         # since the internal_goal/preempt_callbacks are invoked from the
00077         # ActionServer while holding the self.action_server.lock
00078         # self.lock must always be locked after the action server lock
00079         # to avoid an inconsistent lock acquisition order
00080         self.lock = threading.RLock();
00081 
00082         self.execute_condition = threading.Condition(self.lock);
00083 
00084         self.current_goal = ServerGoalHandle();
00085         self.next_goal = ServerGoalHandle();
00086 
00087         if self.execute_callback:
00088             self.execute_thread = threading.Thread(None, self.executeLoop);
00089             self.execute_thread.start();
00090         else:
00091             self.execute_thread = None
00092 
00093         #create the action server
00094         self.action_server = ActionServer(name, ActionSpec, self.internal_goal_callback,self.internal_preempt_callback,auto_start);
00095 
00096 
00097     def __del__(self):
00098         if hasattr(self, 'execute_callback') and self.execute_callback:
00099             with self.terminate_mutex:
00100                 self.need_to_terminate = True;
00101 
00102             assert(self.execute_thread);
00103             self.execute_thread.join();
00104 
00105 
00106     ## @brief Accepts a new goal when one is available The status of this
00107     ## goal is set to active upon acceptance, and the status of any
00108     ## previously active goal is set to preempted. Preempts received for the
00109     ## new goal between checking if isNewGoalAvailable or invokation of a
00110     ## goal callback and the acceptNewGoal call will not trigger a preempt
00111     ## callback.  This means, isPreemptReqauested should be called after
00112     ## accepting the goal even for callback-based implementations to make
00113     ## sure the new goal does not have a pending preempt request.
00114     ## @return A shared_ptr to the new goal.
00115     def accept_new_goal(self):
00116         with self.action_server.lock, self.lock:
00117             if not self.new_goal or not self.next_goal.get_goal():
00118                 rospy.logerr("Attempting to accept the next goal when a new goal is not available");
00119                 return None;
00120 
00121             #check if we need to send a preempted message for the goal that we're currently pursuing
00122             if self.is_active() and self.current_goal.get_goal() and self.current_goal != self.next_goal:
00123                 self.current_goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server");
00124 
00125             rospy.logdebug("Accepting a new goal");
00126 
00127             #accept the next goal
00128             self.current_goal = self.next_goal;
00129             self.new_goal = False;
00130 
00131             #set preempt to request to equal the preempt state of the new goal
00132             self.preempt_request = self.new_goal_preempt_request;
00133             self.new_goal_preempt_request = False;
00134 
00135             #set the status of the current goal to be active
00136             self.current_goal.set_accepted("This goal has been accepted by the simple action server");
00137 
00138             return self.current_goal.get_goal();
00139 
00140 
00141     ## @brief Allows  polling implementations to query about the availability of a new goal
00142     ## @return True if a new goal is available, false otherwise
00143     def is_new_goal_available(self):
00144         return self.new_goal;
00145 
00146 
00147     ## @brief Allows  polling implementations to query about preempt requests
00148     ## @return True if a preempt is requested, false otherwise
00149     def is_preempt_requested(self):
00150         return self.preempt_request;
00151 
00152     ## @brief Allows  polling implementations to query about the status of the current goal
00153     ## @return True if a goal is active, false otherwise
00154     def is_active(self):
00155        if not self.current_goal.get_goal():
00156            return False;
00157 
00158        status = self.current_goal.get_goal_status().status;
00159        return status == actionlib_msgs.msg.GoalStatus.ACTIVE or status == actionlib_msgs.msg.GoalStatus.PREEMPTING;
00160 
00161     ## @brief Sets the status of the active goal to succeeded
00162     ## @param  result An optional result to send back to any clients of the goal
00163     def set_succeeded(self,result=None, text=""):
00164       with self.action_server.lock, self.lock:
00165           if not result:
00166               result=self.get_default_result();
00167           self.current_goal.set_succeeded(result, text);
00168 
00169     ## @brief Sets the status of the active goal to aborted
00170     ## @param  result An optional result to send back to any clients of the goal
00171     def set_aborted(self, result = None, text=""):
00172         with self.action_server.lock, self.lock:
00173             if not result:
00174                 result=self.get_default_result();
00175             self.current_goal.set_aborted(result, text);
00176 
00177     ## @brief Publishes feedback for a given goal
00178     ## @param  feedback Shared pointer to the feedback to publish
00179     def publish_feedback(self,feedback):
00180         self.current_goal.publish_feedback(feedback);
00181 
00182 
00183     def get_default_result(self):
00184         return self.action_server.ActionResultType();
00185 
00186     ## @brief Sets the status of the active goal to preempted
00187     ## @param  result An optional result to send back to any clients of the goal
00188     def set_preempted(self,result=None, text=""):
00189         if not result:
00190             result=self.get_default_result();
00191         with self.action_server.lock, self.lock:
00192             rospy.logdebug("Setting the current goal as canceled");
00193             self.current_goal.set_canceled(result, text);
00194 
00195     ## @brief Allows users to register a callback to be invoked when a new goal is available
00196     ## @param cb The callback to be invoked
00197     def register_goal_callback(self,cb):
00198         if self.execute_callback:
00199             rospy.logwarn("Cannot call SimpleActionServer.register_goal_callback() because an executeCallback exists. Not going to register it.");
00200         else:
00201             self.goal_callback = cb;
00202 
00203     ## @brief Allows users to register a callback to be invoked when a new preempt request is available
00204     ## @param cb The callback to be invoked
00205     def register_preempt_callback(self, cb):
00206         self.preempt_callback = cb;
00207 
00208 
00209     ## @brief Explicitly start the action server, used it auto_start is set to false
00210     def start(self):
00211         self.action_server.start();
00212 
00213 
00214     ## @brief Callback for when the ActionServer receives a new goal and passes it on
00215     def internal_goal_callback(self, goal):
00216           self.execute_condition.acquire();
00217 
00218           try:
00219               rospy.logdebug("A new goal %shas been recieved by the single goal action server",goal.get_goal_id().id);
00220 
00221               #check that the timestamp is past that of the current goal and the next goal
00222               if((not self.current_goal.get_goal() or goal.get_goal_id().stamp >= self.current_goal.get_goal_id().stamp)
00223                  and (not self.next_goal.get_goal() or goal.get_goal_id().stamp >= self.next_goal.get_goal_id().stamp)):
00224                   #if next_goal has not been accepted already... its going to get bumped, but we need to let the client know we're preempting
00225                   if(self.next_goal.get_goal() and (not self.current_goal.get_goal() or self.next_goal != self.current_goal)):
00226                       self.next_goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server");
00227 
00228                   self.next_goal = goal;
00229                   self.new_goal = True;
00230                   self.new_goal_preempt_request = False;
00231 
00232                   #if the server is active, we'll want to call the preempt callback for the current goal
00233                   if(self.is_active()):
00234                       self.preempt_request = True;
00235                       #if the user has registered a preempt callback, we'll call it now
00236                       if(self.preempt_callback):
00237                           self.preempt_callback();
00238 
00239                   #if the user has defined a goal callback, we'll call it now
00240                   if self.goal_callback:
00241                       self.goal_callback();
00242 
00243                   #Trigger runLoop to call execute()
00244                   self.execute_condition.notify();
00245                   self.execute_condition.release();
00246               else:
00247                   #the goal requested has already been preempted by a different goal, so we're not going to execute it
00248                   goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server");
00249                   self.execute_condition.release();
00250           except Exception, e:
00251               rospy.logerr("SimpleActionServer.internal_goal_callback - exception %s",str(e))
00252               self.execute_condition.release();
00253 
00254     ## @brief Callback for when the ActionServer receives a new preempt and passes it on
00255     def internal_preempt_callback(self,preempt):
00256           with self.lock:
00257               rospy.logdebug("A preempt has been received by the SimpleActionServer");
00258 
00259               #if the preempt is for the current goal, then we'll set the preemptRequest flag and call the user's preempt callback
00260               if(preempt == self.current_goal):
00261                   rospy.logdebug("Setting preempt_request bit for the current goal to TRUE and invoking callback");
00262                   self.preempt_request = True;
00263 
00264                   #if the user has registered a preempt callback, we'll call it now
00265                   if(self.preempt_callback):
00266                       self.preempt_callback();
00267               #if the preempt applies to the next goal, we'll set the preempt bit for that
00268               elif(preempt == self.next_goal):
00269                   rospy.logdebug("Setting preempt request bit for the next goal to TRUE");
00270                   self.new_goal_preempt_request = True;
00271 
00272     ## @brief Called from a separate thread to call blocking execute calls
00273     def executeLoop(self):
00274           loop_duration = rospy.Duration.from_sec(.1);
00275 
00276           while (not rospy.is_shutdown()):
00277               rospy.logdebug("SAS: execute");
00278 
00279               with self.terminate_mutex:
00280                   if (self.need_to_terminate):
00281                       break;
00282 
00283               # the following checks (is_active, is_new_goal_available)
00284               # are performed without locking
00285               # the worst thing that might happen in case of a race
00286               # condition is a warning/error message on the console
00287               if (self.is_active()):
00288                   rospy.logerr("Should never reach this code with an active goal");
00289                   return
00290 
00291               if (self.is_new_goal_available()):
00292                   # accept_new_goal() is performing its own locking
00293                   goal = self.accept_new_goal();
00294                   if not self.execute_callback:
00295                       rospy.logerr("execute_callback_ must exist. This is a bug in SimpleActionServer");
00296                       return
00297 
00298                   try:
00299                       self.execute_callback(goal)
00300 
00301                       if self.is_active():
00302                           rospy.logwarn("Your executeCallback did not set the goal to a terminal status.  " +
00303                                         "This is a bug in your ActionServer implementation. Fix your code!  "+
00304                                         "For now, the ActionServer will set this goal to aborted");
00305                           self.set_aborted(None, "No terminal state was set.");
00306                   except Exception, ex:
00307                       rospy.logerr("Exception in your execute callback: %s\n%s", str(ex),
00308                                    traceback.format_exc())
00309                       self.set_aborted(None, "Exception in execute callback: %s" % str(ex))
00310 
00311               with self.execute_condition:
00312                   self.execute_condition.wait(loop_duration.to_sec());
00313 
00314 
00315 


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep
autogenerated on Fri Aug 28 2015 10:04:41