action_server.py
Go to the documentation of this file.
1 #! /usr/bin/env python
2 # Copyright (c) 2009, Willow Garage, Inc.
3 # All rights reserved.
4 #
5 # Redistribution and use in source and binary forms, with or without
6 # modification, are permitted provided that the following conditions are met:
7 #
8 # * Redistributions of source code must retain the above copyright
9 # notice, this list of conditions and the following disclaimer.
10 # * Redistributions in binary form must reproduce the above copyright
11 # notice, this list of conditions and the following disclaimer in the
12 # documentation and/or other materials provided with the distribution.
13 # * Neither the name of the Willow Garage, Inc. nor the names of its
14 # contributors may be used to endorse or promote products derived from
15 # this software without specific prior written permission.
16 #
17 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 # POSSIBILITY OF SUCH DAMAGE.
28 #
29 # Author: Alexander Sorokin.
30 # Based on C++ action_server.h by Eitan Marder-Eppstein
31 import rospy
32 import threading
33 
34 from actionlib_msgs.msg import GoalID, GoalStatus, GoalStatusArray
35 
36 from actionlib.goal_id_generator import GoalIDGenerator
37 from actionlib.status_tracker import StatusTracker
38 
39 from actionlib.handle_tracker_deleter import HandleTrackerDeleter
40 from actionlib.server_goal_handle import ServerGoalHandle
41 
42 from actionlib.exceptions import ActionException
43 
44 
45 def nop_cb(goal_handle):
46  pass
47 
48 
49 def ros_timer(callable, frequency):
50  rate = rospy.Rate(frequency)
51 
52  rospy.logdebug("Starting timer")
53  while not rospy.is_shutdown():
54  try:
55  rate.sleep()
56  callable()
57  except rospy.exceptions.ROSInterruptException:
58  rospy.logdebug("Sleep interrupted")
59 
60 
61 ## @class ActionServer
62 ## @brief The ActionServer is a helpful tool for managing goal requests to a
63 ## node. It allows the user to specify callbacks that are invoked when goal
64 ## or cancel requests come over the wire, and passes back GoalHandles that
65 ## can be used to track the state of a given goal request. The ActionServer
66 ## makes no assumptions about the policy used to service these goals, and
67 ## sends status for each goal over the wire until the last GoalHandle
68 ## associated with a goal request is destroyed.
70  ## @brief Constructor for an ActionServer
71  ## @param ns/name A namespace for the action server
72  ## @param actionspec An explicit specification of the action
73  ## @param goal_cb A goal callback to be called when the ActionServer receives a new goal over the wire
74  ## @param cancel_cb A cancel callback to be called when the ActionServer receives a new cancel request over the wire
75  ## @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.
76  def __init__(self, ns, ActionSpec, goal_cb, cancel_cb=nop_cb, auto_start=True):
77  self.ns = ns
78 
79  try:
80  a = ActionSpec()
81 
82  self.ActionSpec = ActionSpec
83  self.ActionGoal = type(a.action_goal)
84  self.ActionResult = type(a.action_result)
85  self.ActionResultType = type(a.action_result.result)
86  self.ActionFeedback = type(a.action_feedback)
87  except AttributeError:
88  raise ActionException("Type is not an action spec: %s" % str(ActionSpec))
89 
90  self.goal_sub = None
91  self.cancel_sub = None
92  self.status_pub = None
93  self.result_pub = None
94  self.feedback_pub = None
95 
96  self.lock = threading.RLock()
97 
98  self.status_timer = None
99 
100  self.status_list = []
101 
102  self.last_cancel = rospy.Time()
103  self.status_list_timeout = rospy.Duration()
104 
106 
107  self.goal_callback = goal_cb
108  assert(self.goal_callback)
109 
110  self.cancel_callback = cancel_cb
111  self.auto_start = auto_start
112 
113  self.started = False
114 
115  if self.auto_start:
116  rospy.logwarn("You've passed in true for auto_start to the python action server, you should always pass "
117  "in false to avoid race conditions.")
118  self.start()
119 
120  ## @brief Register a callback to be invoked when a new goal is received, this will replace any previously registered callback
121  ## @param cb The callback to invoke
122  def register_goal_callback(self, cb):
123  self.goal_callback = cb
124 
125  ## @brief Register a callback to be invoked when a new cancel is received, this will replace any previously registered callback
126  ## @param cb The callback to invoke
127  def register_cancel_callback(self, cancel_cb):
128  self.cancel_callback = cancel_cb
129 
130  ## @brief Start the action server
131  def start(self):
132  with self.lock:
133  self.initialize()
134  self.started = True
135  self.publish_status()
136 
137  ## @brief Initialize all ROS connections and setup timers
138  def initialize(self):
139  self.pub_queue_size = rospy.get_param('actionlib_server_pub_queue_size', 50)
140  if self.pub_queue_size < 0:
141  self.pub_queue_size = 50
142  self.status_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/status", GoalStatusArray, latch=True, queue_size=self.pub_queue_size)
143  self.result_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/result", self.ActionResult, queue_size=self.pub_queue_size)
144  self.feedback_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/feedback", self.ActionFeedback, queue_size=self.pub_queue_size)
145 
146  self.sub_queue_size = rospy.get_param('actionlib_server_sub_queue_size', -1)
147  if self.sub_queue_size < 0:
148  self.sub_queue_size = None
149  self.goal_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/goal", self.ActionGoal, callback=self.internal_goal_callback, queue_size=self.sub_queue_size)
150 
151  self.cancel_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/cancel", GoalID, callback=self.internal_cancel_callback, queue_size=self.sub_queue_size)
152 
153  # read the frequency with which to publish status from the parameter server
154  # if not specified locally explicitly, use search param to find actionlib_status_frequency
155  resolved_status_frequency_name = rospy.remap_name(self.ns)+"/status_frequency"
156  if rospy.has_param(resolved_status_frequency_name):
157  self.status_frequency = rospy.get_param(resolved_status_frequency_name, 5.0)
158  rospy.logwarn("You're using the deprecated status_frequency parameter, please switch to actionlib_status_frequency.")
159  else:
160  search_status_frequency_name = rospy.search_param("actionlib_status_frequency")
161  if search_status_frequency_name is None:
162  self.status_frequency = 5.0
163  else:
164  self.status_frequency = rospy.get_param(search_status_frequency_name, 5.0)
165 
166  status_list_timeout = rospy.get_param(rospy.remap_name(self.ns)+"/status_list_timeout", 5.0)
167  self.status_list_timeout = rospy.Duration(status_list_timeout)
168 
169  self.status_timer = threading.Thread(None, ros_timer, None, (self.publish_status_async, self.status_frequency))
170  self.status_timer.start()
171 
172  ## @brief Publishes a result for a given goal
173  ## @param status The status of the goal with which the result is associated
174  ## @param result The result to publish
175  def publish_result(self, status, result):
176  with self.lock:
177  ar = self.ActionResult()
178  ar.header.stamp = rospy.Time.now()
179  ar.status = status
180  ar.result = result
181  if not rospy.is_shutdown():
182  self.result_pub.publish(ar)
183  self.publish_status()
184 
185  ## @brief Publishes feedback for a given goal
186  ## @param status The status of the goal with which the feedback is associated
187  ## @param feedback The feedback to publish
188  def publish_feedback(self, status, feedback):
189  with self.lock:
190  af = self.ActionFeedback()
191  af.header.stamp = rospy.Time.now()
192  af.status = status
193  af.feedback = feedback
194  if not rospy.is_shutdown():
195  self.feedback_pub.publish(af)
196 
197  ## @brief The ROS callback for cancel requests coming into the ActionServer
198  def internal_cancel_callback(self, goal_id):
199  with self.lock:
200 
201  # if we're not started... then we're not actually going to do anything
202  if not self.started:
203  return
204 
205  # we need to handle a cancel for the user
206  rospy.logdebug("The action server has received a new cancel request")
207 
208  goal_id_found = False
209  for st in self.status_list[:]:
210  # check if the goal id is zero or if it is equal to the goal id of
211  # the iterator or if the time of the iterator warrants a cancel
212 
213  cancel_everything = (goal_id.id == "" and goal_id.stamp == rospy.Time()) # rospy::Time()) #id and stamp 0 --> cancel everything
214  cancel_this_one = (goal_id.id == st.status.goal_id.id) # ids match... cancel that goal
215  cancel_before_stamp = (goal_id.stamp != rospy.Time() and st.status.goal_id.stamp <= goal_id.stamp) # //stamp != 0 --> cancel everything before stamp
216 
217  if cancel_everything or cancel_this_one or cancel_before_stamp:
218  # we need to check if we need to store this cancel request for later
219  if goal_id.id == st.status.goal_id.id:
220  goal_id_found = True
221 
222  # attempt to get the handle_tracker for the list item if it exists
223  handle_tracker = st.handle_tracker
224 
225  if handle_tracker is None:
226  # if the handle tracker is expired, then we need to create a new one
227  handle_tracker = HandleTrackerDeleter(self, st)
228  st.handle_tracker = handle_tracker
229 
230  # we also need to reset the time that the status is supposed to be removed from the list
231  st.handle_destruction_time = rospy.Time.now()
232 
233  # set the status of the goal to PREEMPTING or RECALLING as approriate
234  # and check if the request should be passed on to the user
235  gh = ServerGoalHandle(st, self, handle_tracker)
236  if gh.set_cancel_requested():
237  # call the user's cancel callback on the relevant goal
238  self.cancel_callback(gh)
239 
240  # if the requested goal_id was not found, and it is non-zero, then we need to store the cancel request
241  if goal_id.id != "" and not goal_id_found:
242  tracker = StatusTracker(goal_id, GoalStatus.RECALLING)
243  self.status_list.append(tracker)
244  # start the timer for how long the status will live in the list without a goal handle to it
245  tracker.handle_destruction_time = rospy.Time.now()
246 
247  # make sure to set last_cancel_ based on the stamp associated with this cancel request
248  if goal_id.stamp > self.last_cancel:
249  self.last_cancel = goal_id.stamp
250 
251  ## @brief The ROS callback for goals coming into the ActionServer
252  def internal_goal_callback(self, goal):
253  with self.lock:
254  # if we're not started... then we're not actually going to do anything
255  if not self.started:
256  return
257 
258  rospy.logdebug("The action server has received a new goal request")
259 
260  # we need to check if this goal already lives in the status list
261  for st in self.status_list[:]:
262  if goal.goal_id.id == st.status.goal_id.id:
263  rospy.logdebug("Goal %s was already in the status list with status %i" % (goal.goal_id.id, st.status.status))
264  # Goal could already be in recalling state if a cancel came in before the goal
265  if st.status.status == GoalStatus.RECALLING:
266  st.status.status = GoalStatus.RECALLED
267  self.publish_result(st.status, self.ActionResultType())
268 
269  # if this is a request for a goal that has no active handles left,
270  # we'll bump how long it stays in the list
271  if st.handle_tracker is None:
272  st.handle_destruction_time = rospy.Time.now()
273 
274  # make sure not to call any user callbacks or add duplicate status onto the list
275  return
276 
277  # if the goal is not in our list, we need to create a StatusTracker associated with this goal and push it on
278  st = StatusTracker(None, None, goal)
279  self.status_list.append(st)
280 
281  # we need to create a handle tracker for the incoming goal and update the StatusTracker
282  handle_tracker = HandleTrackerDeleter(self, st)
283 
284  st.handle_tracker = handle_tracker
285 
286  # check if this goal has already been canceled based on its timestamp
287  gh = ServerGoalHandle(st, self, handle_tracker)
288  if goal.goal_id.stamp != rospy.Time() and goal.goal_id.stamp <= self.last_cancel:
289  # if it has... just create a GoalHandle for it and setCanceled
290  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")
291  else:
292  # now, we need to create a goal handle and call the user's callback
293  self.goal_callback(gh)
294 
295  ## @brief Publish status for all goals on a timer event
297  with self.lock:
298  # we won't publish status unless we've been started
299  if not self.started:
300  return
301  self.publish_status()
302 
303  ## @brief Explicitly publish status
304  def publish_status(self):
305  with self.lock:
306  # build a status array
307  status_array = GoalStatusArray()
308 
309  # status_array.set_status_list_size(len(self.status_list));
310 
311  i = 0
312  while i < len(self.status_list):
313  st = self.status_list[i]
314  # check if the item is due for deletion from the status list
315  if st.handle_destruction_time != rospy.Time() and st.handle_destruction_time + self.status_list_timeout < rospy.Time.now():
316  rospy.logdebug("Item %s with destruction time of %.3f being removed from list. Now = %.3f" %
317  (st.status.goal_id, st.handle_destruction_time.to_sec(), rospy.Time.now().to_sec()))
318  del self.status_list[i]
319  else:
320  status_array.status_list.append(st.status)
321  i += 1
322 
323  status_array.header.stamp = rospy.Time.now()
324  if not rospy.is_shutdown():
325  self.status_pub.publish(status_array)
def start(self)
Start the action server.
def publish_feedback(self, status, feedback)
Publishes feedback for a given goal.
def publish_status_async(self)
Publish status for all goals on a timer event.
def register_cancel_callback(self, cancel_cb)
Register a callback to be invoked when a new cancel is received, this will replace any previously reg...
def initialize(self)
Initialize all ROS connections and setup timers.
def __init__(self, ns, ActionSpec, goal_cb, cancel_cb=nop_cb, auto_start=True)
Constructor for an ActionServer.
def nop_cb(goal_handle)
def internal_cancel_callback(self, goal_id)
The ROS callback for cancel requests coming into the ActionServer.
def publish_result(self, status, result)
Publishes a result for a given goal.
def register_goal_callback(self, cb)
Register a callback to be invoked when a new goal is received, this will replace any previously regis...
def publish_status(self)
Explicitly publish status.
def ros_timer(callable, frequency)
def internal_goal_callback(self, goal)
The ROS callback for goals coming into the ActionServer.
The ActionServer is a helpful tool for managing goal requests to a node.


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep, Mikael Arguedas
autogenerated on Tue Sep 3 2019 03:41:41