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