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 import rospy
00032 import threading
00033 
00034 from actionlib_msgs.msg import GoalID, GoalStatus, GoalStatusArray
00035 
00036 from actionlib.goal_id_generator import GoalIDGenerator
00037 from actionlib.status_tracker import StatusTracker
00038 
00039 from actionlib.handle_tracker_deleter import HandleTrackerDeleter
00040 from actionlib.server_goal_handle import ServerGoalHandle
00041 
00042 from actionlib.exceptions import ActionException
00043 
00044 
00045 def nop_cb(goal_handle):
00046     pass
00047 
00048 
00049 def ros_timer(callable, frequency):
00050     rate = rospy.Rate(frequency)
00051 
00052     rospy.logdebug("Starting timer")
00053     while not rospy.is_shutdown():
00054         try:
00055             rate.sleep()
00056             callable()
00057         except rospy.exceptions.ROSInterruptException:
00058             rospy.logdebug("Sleep interrupted")
00059 
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         self.goal_sub = None
00091         self.cancel_sub = None
00092         self.status_pub = None
00093         self.result_pub = None
00094         self.feedback_pub = None
00095 
00096         self.lock = threading.RLock()
00097 
00098         self.status_timer = None
00099 
00100         self.status_list = []
00101 
00102         self.last_cancel = rospy.Time()
00103         self.status_list_timeout = rospy.Duration()
00104 
00105         self.id_generator = GoalIDGenerator()
00106 
00107         self.goal_callback = goal_cb
00108         assert(self.goal_callback)
00109 
00110         self.cancel_callback = cancel_cb
00111         self.auto_start = auto_start
00112 
00113         self.started = False
00114 
00115         if self.auto_start:
00116             rospy.logwarn("You've passed in true for auto_start to the python action server, you should always pass "
00117                           "in false to avoid race conditions.")
00118             self.start()
00119 
00120     ## @brief  Register a callback to be invoked when a new goal is received, this will replace any  previously registered callback
00121     ## @param  cb The callback to invoke
00122     def register_goal_callback(self, cb):
00123         self.goal_callback = cb
00124 
00125     ## @brief  Register a callback to be invoked when a new cancel is received, this will replace any  previously registered callback
00126     ## @param  cb The callback to invoke
00127     def register_cancel_callback(self, cancel_cb):
00128         self.cancel_callback = cancel_cb
00129 
00130     ## @brief  Start the action server
00131     def start(self):
00132         with self.lock:
00133             self.initialize()
00134             self.started = True
00135             self.publish_status()
00136 
00137     ## @brief  Initialize all ROS connections and setup timers
00138     def initialize(self):
00139         self.pub_queue_size = rospy.get_param('actionlib_server_pub_queue_size', 50)
00140         if self.pub_queue_size < 0:
00141             self.pub_queue_size = 50
00142         self.status_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/status", GoalStatusArray, latch=True, queue_size=self.pub_queue_size)
00143         self.result_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/result", self.ActionResult, queue_size=self.pub_queue_size)
00144         self.feedback_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/feedback", self.ActionFeedback, queue_size=self.pub_queue_size)
00145 
00146         self.sub_queue_size = rospy.get_param('actionlib_server_sub_queue_size', -1)
00147         if self.sub_queue_size < 0:
00148             self.sub_queue_size = None
00149         self.goal_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/goal", self.ActionGoal, callback=self.internal_goal_callback, queue_size=self.sub_queue_size)
00150 
00151         self.cancel_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/cancel", GoalID, callback=self.internal_cancel_callback, queue_size=self.sub_queue_size)
00152 
00153         # read the frequency with which to publish status from the parameter server
00154         # if not specified locally explicitly, use search param to find actionlib_status_frequency
00155         resolved_status_frequency_name = rospy.remap_name(self.ns)+"/status_frequency"
00156         if rospy.has_param(resolved_status_frequency_name):
00157             self.status_frequency = rospy.get_param(resolved_status_frequency_name, 5.0)
00158             rospy.logwarn("You're using the deprecated status_frequency parameter, please switch to actionlib_status_frequency.")
00159         else:
00160             search_status_frequency_name = rospy.search_param("actionlib_status_frequency")
00161             if search_status_frequency_name is None:
00162                 self.status_frequency = 5.0
00163             else:
00164                 self.status_frequency = rospy.get_param(search_status_frequency_name, 5.0)
00165 
00166         status_list_timeout = rospy.get_param(rospy.remap_name(self.ns)+"/status_list_timeout", 5.0)
00167         self.status_list_timeout = rospy.Duration(status_list_timeout)
00168 
00169         self.status_timer = threading.Thread(None, ros_timer, None, (self.publish_status_async, self.status_frequency))
00170         self.status_timer.start()
00171 
00172     ## @brief  Publishes a result for a given goal
00173     ## @param status The status of the goal with which the result is associated
00174     ## @param result The result to publish
00175     def publish_result(self, status, result):
00176         with self.lock:
00177             ar = self.ActionResult()
00178             ar.header.stamp = rospy.Time.now()
00179             ar.status = status
00180             ar.result = result
00181             if not rospy.is_shutdown():
00182                 self.result_pub.publish(ar)
00183             self.publish_status()
00184 
00185     ## @brief  Publishes feedback for a given goal
00186     ## @param status The status of the goal with which the feedback is associated
00187     ## @param feedback The feedback to publish
00188     def publish_feedback(self, status, feedback):
00189         with self.lock:
00190             af = self.ActionFeedback()
00191             af.header.stamp = rospy.Time.now()
00192             af.status = status
00193             af.feedback = feedback
00194             if not rospy.is_shutdown():
00195                 self.feedback_pub.publish(af)
00196 
00197     ## @brief  The ROS callback for cancel requests coming into the ActionServer
00198     def internal_cancel_callback(self, goal_id):
00199         with self.lock:
00200 
00201             # if we're not started... then we're not actually going to do anything
00202             if not self.started:
00203                 return
00204 
00205             # we need to handle a cancel for the user
00206             rospy.logdebug("The action server has received a new cancel request")
00207 
00208             goal_id_found = False
00209             for st in self.status_list[:]:
00210                 # check if the goal id is zero or if it is equal to the goal id of
00211                 # the iterator or if the time of the iterator warrants a cancel
00212 
00213                 cancel_everything = (goal_id.id == "" and goal_id.stamp == rospy.Time())   # rospy::Time()) #id and stamp 0 --> cancel everything
00214                 cancel_this_one = (goal_id.id == st.status.goal_id.id)   # ids match... cancel that goal
00215                 cancel_before_stamp = (goal_id.stamp != rospy.Time() and st.status.goal_id.stamp <= goal_id.stamp)  # //stamp != 0 --> cancel everything before stamp
00216 
00217                 if cancel_everything or cancel_this_one or cancel_before_stamp:
00218                     # we need to check if we need to store this cancel request for later
00219                     if goal_id.id == st.status.goal_id.id:
00220                         goal_id_found = True
00221 
00222                     # attempt to get the handle_tracker for the list item if it exists
00223                     handle_tracker = st.handle_tracker
00224 
00225                     if handle_tracker is None:
00226                         # if the handle tracker is expired, then we need to create a new one
00227                         handle_tracker = HandleTrackerDeleter(self, st)
00228                         st.handle_tracker = handle_tracker
00229 
00230                         # we also need to reset the time that the status is supposed to be removed from the list
00231                         st.handle_destruction_time = rospy.Time.now()
00232 
00233                     # set the status of the goal to PREEMPTING or RECALLING as approriate
00234                     # and check if the request should be passed on to the user
00235                     gh = ServerGoalHandle(st, self, handle_tracker)
00236                     if gh.set_cancel_requested():
00237                         # call the user's cancel callback on the relevant goal
00238                         self.cancel_callback(gh)
00239 
00240             # if the requested goal_id was not found, and it is non-zero, then we need to store the cancel request
00241             if goal_id.id != "" and not goal_id_found:
00242                 tracker = StatusTracker(goal_id, GoalStatus.RECALLING)
00243                 self.status_list.append(tracker)
00244                 # start the timer for how long the status will live in the list without a goal handle to it
00245                 tracker.handle_destruction_time = rospy.Time.now()
00246 
00247             # make sure to set last_cancel_ based on the stamp associated with this cancel request
00248             if goal_id.stamp > self.last_cancel:
00249                 self.last_cancel = goal_id.stamp
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 == GoalStatus.RECALLING:
00266                         st.status.status = 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     ## @brief  Publish status for all goals on a timer event
00296     def publish_status_async(self):
00297         with self.lock:
00298             # we won't publish status unless we've been started
00299             if not self.started:
00300                 return
00301             self.publish_status()
00302 
00303     ## @brief  Explicitly publish status
00304     def publish_status(self):
00305         with self.lock:
00306             # build a status array
00307             status_array = GoalStatusArray()
00308 
00309             # status_array.set_status_list_size(len(self.status_list));
00310 
00311             i = 0
00312             while i < len(self.status_list):
00313                 st = self.status_list[i]
00314                 # check if the item is due for deletion from the status list
00315                 if st.handle_destruction_time != rospy.Time() and st.handle_destruction_time + self.status_list_timeout < rospy.Time.now():
00316                     rospy.logdebug("Item %s with destruction time of %.3f being removed from list.  Now = %.3f" %
00317                                    (st.status.goal_id, st.handle_destruction_time.to_sec(), rospy.Time.now().to_sec()))
00318                     del self.status_list[i]
00319                 else:
00320                     status_array.status_list.append(st.status)
00321                     i += 1
00322 
00323             status_array.header.stamp = rospy.Time.now()
00324             if not rospy.is_shutdown():
00325                 self.status_pub.publish(status_array)


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep, Mikael Arguedas
autogenerated on Sat Feb 16 2019 03:21:28