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++ action_server.h by Eitan Marder-Eppstein
00031 
00032 import rospy
00033 
00034 import threading
00035 
00036 from actionlib_msgs.msg import *
00037 
00038 from goal_id_generator import GoalIDGenerator
00039 from status_tracker import StatusTracker
00040 
00041 from handle_tracker_deleter import HandleTrackerDeleter
00042 from server_goal_handle import ServerGoalHandle
00043 
00044 from actionlib.exceptions import *
00045 
00046 
00047 def nop_cb(goal_handle):
00048     pass
00049 
00050 def ros_timer(callable,frequency):
00051     rate = rospy.Rate(frequency)
00052 
00053     rospy.logdebug("Starting timer");
00054     while not rospy.is_shutdown():
00055         try:
00056             rate.sleep()
00057             callable()
00058         except rospy.exceptions.ROSInterruptException:
00059             rospy.logdebug("Sleep interrupted");
00060 
00061 ## @class ActionServer
00062 ## @brief The ActionServer is a helpful tool for managing goal requests to a
00063 ## node. It allows the user to specify callbacks that are invoked when goal
00064 ## or cancel requests come over the wire, and passes back GoalHandles that
00065 ## can be used to track the state of a given goal request. The ActionServer
00066 ## makes no assumptions about the policy used to service these goals, and
00067 ## sends status for each goal over the wire until the last GoalHandle
00068 ## associated with a goal request is destroyed.
00069 class ActionServer:
00070     ## @brief  Constructor for an ActionServer
00071     ## @param  ns/name A namespace for the action server
00072     ## @param  actionspec An explicit specification of the action
00073     ## @param  goal_cb A goal callback to be called when the ActionServer receives a new goal over the wire
00074     ## @param  cancel_cb A cancel callback to be called when the ActionServer receives a new cancel request over the wire
00075     ## @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.
00076     def __init__(self, ns,  ActionSpec, goal_cb, cancel_cb = nop_cb, auto_start = True):
00077         self.ns=ns;
00078 
00079         try:
00080             a = ActionSpec()
00081 
00082             self.ActionSpec = ActionSpec
00083             self.ActionGoal = type(a.action_goal)
00084             self.ActionResult = type(a.action_result)
00085             self.ActionResultType = type(a.action_result.result)
00086             self.ActionFeedback = type(a.action_feedback)
00087         except AttributeError:
00088             raise ActionException("Type is not an action spec: %s" % str(ActionSpec))
00089 
00090 
00091         self.goal_sub = None
00092         self.cancel_sub = None
00093         self.status_pub = None
00094         self.result_pub = None
00095         self.feedback_pub = None
00096 
00097         self.lock = threading.RLock()
00098 
00099         self.status_timer = None;
00100 
00101         self.status_list = [];
00102 
00103         self.last_cancel = rospy.Time();
00104         self.status_list_timeout = rospy.Duration();
00105 
00106         self.id_generator = GoalIDGenerator();
00107 
00108         self.goal_callback = goal_cb;
00109         assert(self.goal_callback);
00110 
00111         self.cancel_callback = cancel_cb;
00112         self.auto_start = auto_start;
00113 
00114         self.started = False;
00115 
00116         if self.auto_start:
00117             rospy.logwarn("You've passed in true for auto_start to the python action server, you should always pass in false to avoid race conditions.")
00118             self.start()
00119 
00120 
00121     ## @brief  Register a callback to be invoked when a new goal is received, this will replace any  previously registered callback
00122     ## @param  cb The callback to invoke
00123     def register_goal_callback(self, cb):
00124         self.goal_callback = cb
00125 
00126     ## @brief  Register a callback to be invoked when a new cancel is received, this will replace any  previously registered callback
00127     ## @param  cb The callback to invoke
00128     def register_cancel_callback(self,cancel_cb):
00129           self.cancel_callback = cancel_cb
00130 
00131     ## @brief  Start the action server
00132     def start(self):
00133         with self.lock:
00134             self.initialize();
00135             self.started = True;
00136             self.publish_status();
00137 
00138     ## @brief  Initialize all ROS connections and setup timers
00139     def initialize(self):
00140           self.status_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/status", GoalStatusArray, latch=True);
00141           self.result_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/result", self.ActionResult);
00142           self.feedback_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/feedback", self.ActionFeedback);
00143 
00144           self.goal_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/goal", self.ActionGoal,self.internal_goal_callback);
00145 
00146           self.cancel_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/cancel", GoalID,self.internal_cancel_callback);
00147 
00148           #read the frequency with which to publish status from the parameter server
00149           #if not specified locally explicitly, use search param to find actionlib_status_frequency
00150           resolved_status_frequency_name = rospy.remap_name(self.ns)+"/status_frequency"
00151           if rospy.has_param(resolved_status_frequency_name):
00152               self.status_frequency = rospy.get_param(resolved_status_frequency_name, 5.0);
00153               rospy.logwarn("You're using the deprecated status_frequency parameter, please switch to actionlib_status_frequency.")
00154           else:
00155               search_status_frequency_name = rospy.search_param("actionlib_status_frequency")
00156               if search_status_frequency_name is None:
00157                   self.status_frequency = 5.0
00158               else:
00159                   self.status_frequency = rospy.get_param(search_status_frequency_name, 5.0)
00160 
00161           status_list_timeout = rospy.get_param(rospy.remap_name(self.ns)+"/status_list_timeout", 5.0);
00162           self.status_list_timeout = rospy.Duration(status_list_timeout);
00163 
00164 
00165           self.status_timer = threading.Thread( None, ros_timer, None, (self.publish_status_async,self.status_frequency) );
00166           self.status_timer.start();
00167 
00168     ## @brief  Publishes a result for a given goal
00169     ## @param status The status of the goal with which the result is associated
00170     ## @param result The result to publish
00171     def publish_result(self, status, result):
00172         with self.lock:
00173             ar = self.ActionResult();
00174             ar.header.stamp = rospy.Time.now();
00175             ar.status = status;
00176             ar.result = result;
00177             self.result_pub.publish(ar);
00178             self.publish_status()
00179 
00180 
00181     ## @brief  Publishes feedback for a given goal
00182     ## @param status The status of the goal with which the feedback is associated
00183     ## @param feedback The feedback to publish
00184     def publish_feedback(self, status, feedback):
00185           with self.lock:
00186 
00187               af=self.ActionFeedback();
00188               af.header.stamp = rospy.Time.now();
00189               af.status = status;
00190               af.feedback = feedback;
00191               self.feedback_pub.publish(af);
00192 
00193 
00194     ## @brief  The ROS callback for cancel requests coming into the ActionServer
00195     def internal_cancel_callback(self, goal_id):
00196           with self.lock:
00197 
00198               #if we're not started... then we're not actually going to do anything
00199               if not self.started:
00200                   return;
00201 
00202               #we need to handle a cancel for the user
00203               rospy.logdebug("The action server has received a new cancel request");
00204 
00205               goal_id_found = False;
00206               for st in self.status_list[:]:
00207                   #check if the goal id is zero or if it is equal to the goal id of
00208                   #the iterator or if the time of the iterator warrants a cancel
00209 
00210                   cancel_everything = (goal_id.id == "" and goal_id.stamp == rospy.Time() )   #rospy::Time()) #id and stamp 0 --> cancel everything
00211                   cancel_this_one = ( goal_id.id == st.status.goal_id.id)   #ids match... cancel that goal
00212                   cancel_before_stamp = (goal_id.stamp != rospy.Time() and st.status.goal_id.stamp <= goal_id.stamp)  #//stamp != 0 --> cancel everything before stamp
00213 
00214                   if cancel_everything or cancel_this_one or cancel_before_stamp:
00215                       #we need to check if we need to store this cancel request for later
00216                       if goal_id.id == st.status.goal_id.id:
00217                           goal_id_found = True;
00218 
00219                       #attempt to get the handle_tracker for the list item if it exists
00220                       handle_tracker = st.handle_tracker;
00221 
00222                       if handle_tracker is None:
00223                           #if the handle tracker is expired, then we need to create a new one
00224                           handle_tracker = HandleTrackerDeleter(self, st);
00225                           st.handle_tracker = handle_tracker;
00226 
00227                           #we also need to reset the time that the status is supposed to be removed from the list
00228                           st.handle_destruction_time = rospy.Time.now()
00229 
00230 
00231                       #set the status of the goal to PREEMPTING or RECALLING as approriate
00232                       #and check if the request should be passed on to the user
00233                       gh = ServerGoalHandle(st, self, handle_tracker);
00234                       if gh.set_cancel_requested():
00235                           #call the user's cancel callback on the relevant goal
00236                           self.cancel_callback(gh);
00237 
00238 
00239               #if the requested goal_id was not found, and it is non-zero, then we need to store the cancel request
00240               if goal_id.id != "" and not goal_id_found:
00241                   tracker= StatusTracker(goal_id, actionlib_msgs.msg.GoalStatus.RECALLING);
00242                   self.status_list.append(tracker)
00243                   #start the timer for how long the status will live in the list without a goal handle to it
00244                   tracker.handle_destruction_time = rospy.Time.now()
00245 
00246               #make sure to set last_cancel_ based on the stamp associated with this cancel request
00247               if goal_id.stamp > self.last_cancel:
00248                   self.last_cancel = goal_id.stamp;
00249 
00250 
00251     ## @brief  The ROS callback for goals coming into the ActionServer
00252     def internal_goal_callback(self, goal):
00253           with self.lock:
00254               #if we're not started... then we're not actually going to do anything
00255               if not self.started:
00256                   return;
00257 
00258               rospy.logdebug("The action server has received a new goal request");
00259 
00260               #we need to check if this goal already lives in the status list
00261               for st in self.status_list[:]:
00262                   if goal.goal_id.id == st.status.goal_id.id:
00263                       rospy.logdebug("Goal %s was already in the status list with status %i" % (goal.goal_id.id, st.status.status))
00264                       # Goal could already be in recalling state if a cancel came in before the goal
00265                       if st.status.status == actionlib_msgs.msg.GoalStatus.RECALLING:
00266                           st.status.status = actionlib_msgs.msg.GoalStatus.RECALLED
00267                           self.publish_result(st.status, self.ActionResultType())
00268 
00269                       #if this is a request for a goal that has no active handles left,
00270                       #we'll bump how long it stays in the list
00271                       if st.handle_tracker is None:
00272                           st.handle_destruction_time = rospy.Time.now()
00273 
00274                       #make sure not to call any user callbacks or add duplicate status onto the list
00275                       return;
00276 
00277               #if the goal is not in our list, we need to create a StatusTracker associated with this goal and push it on
00278               st = StatusTracker(None,None,goal)
00279               self.status_list.append(st);
00280 
00281               #we need to create a handle tracker for the incoming goal and update the StatusTracker
00282               handle_tracker = HandleTrackerDeleter(self, st);
00283 
00284               st.handle_tracker = handle_tracker;
00285 
00286               #check if this goal has already been canceled based on its timestamp
00287               gh= ServerGoalHandle(st, self, handle_tracker);
00288               if goal.goal_id.stamp != rospy.Time() and goal.goal_id.stamp <= self.last_cancel:
00289                   #if it has... just create a GoalHandle for it and setCanceled
00290                   gh.set_canceled(None, "This goal handle was canceled by the action server because its timestamp is before the timestamp of the last cancel request");
00291               else:
00292                   #now, we need to create a goal handle and call the user's callback
00293                   self.goal_callback(gh);
00294 
00295 
00296     ## @brief  Publish status for all goals on a timer event
00297     def publish_status_async(self):
00298           rospy.logdebug("Status async");
00299           with self.lock:
00300               #we won't publish status unless we've been started
00301               if not self.started:
00302                   return
00303               self.publish_status();
00304 
00305 
00306 
00307     ## @brief  Explicitly publish status
00308     def publish_status(self):
00309           with self.lock:
00310               #build a status array
00311               status_array = actionlib_msgs.msg.GoalStatusArray()
00312 
00313               #status_array.set_status_list_size(len(self.status_list));
00314 
00315               i=0;
00316               while i<len(self.status_list):
00317                   st = self.status_list[i]
00318                   #check if the item is due for deletion from the status list
00319                   if st.handle_destruction_time != rospy.Time() and st.handle_destruction_time + self.status_list_timeout < rospy.Time.now():
00320                       rospy.logdebug("Item %s with destruction time of %.3f being removed from list.  Now = %.3f" % \
00321                                    (st.status.goal_id, st.handle_destruction_time.to_sec(), rospy.Time.now().to_sec()))
00322                       del self.status_list[i]
00323                   else:
00324                       status_array.status_list.append(st.status);
00325                       i+=1
00326 
00327 
00328               status_array.header.stamp = rospy.Time.now()
00329               self.status_pub.publish(status_array)
00330 
00331 


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep
autogenerated on Thu Jan 2 2014 11:03:49