simple_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++ simple_action_server.h by Eitan Marder-Eppstein
31 import rospy
32 import threading
33 import traceback
34 
35 from actionlib_msgs.msg import GoalStatus
36 
37 from actionlib import ActionServer
38 from actionlib.server_goal_handle import ServerGoalHandle
39 
40 
41 def nop_cb(goal_handle):
42  pass
43 
44 
45 ## @class SimpleActionServer
46 ## @brief The SimpleActionServer
47 ## implements a singe goal policy on top of the ActionServer class. The
48 ## specification of the policy is as follows: only one goal can have an
49 ## active status at a time, new goals preempt previous goals based on the
50 ## stamp in their GoalID field (later goals preempt earlier ones), an
51 ## explicit preempt goal preempts all goals with timestamps that are less
52 ## than or equal to the stamp associated with the preempt, accepting a new
53 ## goal implies successful preemption of any old goal and the status of the
54 ## old goal will be change automatically to reflect this.
56  ## @brief Constructor for a SimpleActionServer
57  ## @param name A name for the action server
58  ## @param execute_cb Optional callback that gets called in a separate thread whenever
59  ## a new goal is received, allowing users to have blocking callbacks.
60  ## Adding an execute callback also deactivates the goalCallback.
61  ## @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.
62  def __init__(self, name, ActionSpec, execute_cb=None, auto_start=True):
63 
64  self.new_goal = False
65  self.preempt_request = False
67 
68  self.execute_callback = execute_cb
69  self.goal_callback = None
70  self.preempt_callback = None
71 
72  self.need_to_terminate = False
73  self.terminate_mutex = threading.RLock()
74 
75  # since the internal_goal/preempt_callbacks are invoked from the
76  # ActionServer while holding the self.action_server.lock
77  # self.lock must always be locked after the action server lock
78  # to avoid an inconsistent lock acquisition order
79  self.lock = threading.RLock()
80 
81  self.execute_condition = threading.Condition(self.lock)
82 
85 
86  if self.execute_callback:
87  self.execute_thread = threading.Thread(None, self.executeLoop)
88  self.execute_thread.start()
89  else:
90  self.execute_thread = None
91 
92  # create the action server
93  self.action_server = ActionServer(name, ActionSpec, self.internal_goal_callback, self.internal_preempt_callback, auto_start)
94 
95  def __del__(self):
96  if hasattr(self, 'execute_callback') and self.execute_callback:
97  with self.terminate_mutex:
98  self.need_to_terminate = True
99 
100  assert(self.execute_thread)
101  self.execute_thread.join()
102 
103  ## @brief Accepts a new goal when one is available The status of this
104  ## goal is set to active upon acceptance, and the status of any
105  ## previously active goal is set to preempted. Preempts received for the
106  ## new goal between checking if isNewGoalAvailable or invokation of a
107  ## goal callback and the acceptNewGoal call will not trigger a preempt
108  ## callback. This means, isPreemptReqauested should be called after
109  ## accepting the goal even for callback-based implementations to make
110  ## sure the new goal does not have a pending preempt request.
111  ## @return A shared_ptr to the new goal.
112  def accept_new_goal(self):
113  with self.action_server.lock, self.lock:
114  if not self.new_goal or not self.next_goal.get_goal():
115  rospy.logerr("Attempting to accept the next goal when a new goal is not available")
116  return None
117 
118  # check if we need to send a preempted message for the goal that we're currently pursuing
119  if self.is_active() and self.current_goal.get_goal() and self.current_goal != self.next_goal:
120  self.current_goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server")
121 
122  rospy.logdebug("Accepting a new goal")
123 
124  # accept the next goal
125  self.current_goal = self.next_goal
126  self.new_goal = False
127 
128  # set preempt to request to equal the preempt state of the new goal
130  self.new_goal_preempt_request = False
131 
132  # set the status of the current goal to be active
133  self.current_goal.set_accepted("This goal has been accepted by the simple action server")
134 
135  return self.current_goal.get_goal()
136 
137  ## @brief Allows polling implementations to query about the availability of a new goal
138  ## @return True if a new goal is available, false otherwise
140  return self.new_goal
141 
142  ## @brief Allows polling implementations to query about preempt requests
143  ## @return True if a preempt is requested, false otherwise
145  return self.preempt_request
146 
147  ## @brief Allows polling implementations to query about the status of the current goal
148  ## @return True if a goal is active, false otherwise
149  def is_active(self):
150  if not self.current_goal.get_goal():
151  return False
152 
153  status = self.current_goal.get_goal_status().status
154  return status == GoalStatus.ACTIVE or status == GoalStatus.PREEMPTING
155 
156  ## @brief Sets the status of the active goal to succeeded
157  ## @param result An optional result to send back to any clients of the goal
158  def set_succeeded(self, result=None, text=""):
159  with self.action_server.lock, self.lock:
160  if not result:
161  result = self.get_default_result()
162  self.current_goal.set_succeeded(result, text)
163 
164  ## @brief Sets the status of the active goal to aborted
165  ## @param result An optional result to send back to any clients of the goal
166  def set_aborted(self, result=None, text=""):
167  with self.action_server.lock, self.lock:
168  if not result:
169  result = self.get_default_result()
170  self.current_goal.set_aborted(result, text)
171 
172  ## @brief Publishes feedback for a given goal
173  ## @param feedback Shared pointer to the feedback to publish
174  def publish_feedback(self, feedback):
175  self.current_goal.publish_feedback(feedback)
176 
178  return self.action_server.ActionResultType()
179 
180  ## @brief Sets the status of the active goal to preempted
181  ## @param result An optional result to send back to any clients of the goal
182  def set_preempted(self, result=None, text=""):
183  if not result:
184  result = self.get_default_result()
185  with self.action_server.lock, self.lock:
186  rospy.logdebug("Setting the current goal as canceled")
187  self.current_goal.set_canceled(result, text)
188 
189  ## @brief Allows users to register a callback to be invoked when a new goal is available
190  ## @param cb The callback to be invoked
191  def register_goal_callback(self, cb):
192  if self.execute_callback:
193  rospy.logwarn("Cannot call SimpleActionServer.register_goal_callback() because an executeCallback exists. Not going to register it.")
194  else:
195  self.goal_callback = cb
196 
197  ## @brief Allows users to register a callback to be invoked when a new preempt request is available
198  ## @param cb The callback to be invoked
200  self.preempt_callback = cb
201 
202  ## @brief Explicitly start the action server, used it auto_start is set to false
203  def start(self):
204  self.action_server.start()
205 
206  ## @brief Callback for when the ActionServer receives a new goal and passes it on
207  def internal_goal_callback(self, goal):
208  self.execute_condition.acquire()
209 
210  try:
211  rospy.logdebug("A new goal %shas been received by the single goal action server", goal.get_goal_id().id)
212 
213  # check that the timestamp is past that of the current goal and the next goal
214  if((not self.current_goal.get_goal() or goal.get_goal_id().stamp >= self.current_goal.get_goal_id().stamp)
215  and (not self.next_goal.get_goal() or goal.get_goal_id().stamp >= self.next_goal.get_goal_id().stamp)):
216  # if next_goal has not been accepted already... its going to get bumped, but we need to let the client know we're preempting
217  if(self.next_goal.get_goal() and (not self.current_goal.get_goal() or self.next_goal != self.current_goal)):
218  self.next_goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server")
219 
220  self.next_goal = goal
221  self.new_goal = True
222  self.new_goal_preempt_request = False
223 
224  # if the server is active, we'll want to call the preempt callback for the current goal
225  if(self.is_active()):
226  self.preempt_request = True
227  # if the user has registered a preempt callback, we'll call it now
228  if(self.preempt_callback):
229  self.preempt_callback()
230 
231  # if the user has defined a goal callback, we'll call it now
232  if self.goal_callback:
233  self.goal_callback()
234 
235  # Trigger runLoop to call execute()
236  self.execute_condition.notify()
237  self.execute_condition.release()
238  else:
239  # the goal requested has already been preempted by a different goal, so we're not going to execute it
240  goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server")
241  self.execute_condition.release()
242  except Exception as e:
243  rospy.logerr("SimpleActionServer.internal_goal_callback - exception %s", str(e))
244  self.execute_condition.release()
245 
246  ## @brief Callback for when the ActionServer receives a new preempt and passes it on
247  def internal_preempt_callback(self, preempt):
248  with self.lock:
249  rospy.logdebug("A preempt has been received by the SimpleActionServer")
250 
251  # if the preempt is for the current goal, then we'll set the preemptRequest flag and call the user's preempt callback
252  if(preempt == self.current_goal):
253  rospy.logdebug("Setting preempt_request bit for the current goal to TRUE and invoking callback")
254  self.preempt_request = True
255 
256  # if the user has registered a preempt callback, we'll call it now
257  if(self.preempt_callback):
258  self.preempt_callback()
259  # if the preempt applies to the next goal, we'll set the preempt bit for that
260  elif(preempt == self.next_goal):
261  rospy.logdebug("Setting preempt request bit for the next goal to TRUE")
262  self.new_goal_preempt_request = True
263 
264  ## @brief Called from a separate thread to call blocking execute calls
265  def executeLoop(self):
266  loop_duration = rospy.Duration.from_sec(.1)
267 
268  while (not rospy.is_shutdown()):
269  with self.terminate_mutex:
270  if (self.need_to_terminate):
271  break
272 
273  # the following checks (is_active, is_new_goal_available)
274  # are performed without locking
275  # the worst thing that might happen in case of a race
276  # condition is a warning/error message on the console
277  if (self.is_active()):
278  rospy.logerr("Should never reach this code with an active goal")
279  return
280 
281  if (self.is_new_goal_available()):
282  # accept_new_goal() is performing its own locking
283  goal = self.accept_new_goal()
284  if not self.execute_callback:
285  rospy.logerr("execute_callback_ must exist. This is a bug in SimpleActionServer")
286  return
287 
288  try:
289  self.execute_callback(goal)
290 
291  if self.is_active():
292  rospy.logwarn("Your executeCallback did not set the goal to a terminal status. " +
293  "This is a bug in your ActionServer implementation. Fix your code! " +
294  "For now, the ActionServer will set this goal to aborted")
295  self.set_aborted(None, "No terminal state was set.")
296  except Exception as ex:
297  rospy.logerr("Exception in your execute callback: %s\n%s", str(ex),
298  traceback.format_exc())
299  self.set_aborted(None, "Exception in execute callback: %s" % str(ex))
300 
301  with self.execute_condition:
302  self.execute_condition.wait(loop_duration.to_sec())
def is_preempt_requested(self)
Allows polling implementations to query about preempt requests.
def accept_new_goal(self)
Accepts a new goal when one is available The status of this goal is set to active upon acceptance...
def internal_preempt_callback(self, preempt)
Callback for when the ActionServer receives a new preempt and passes it on.
def internal_goal_callback(self, goal)
Callback for when the ActionServer receives a new goal and passes it on.
def register_goal_callback(self, cb)
Allows users to register a callback to be invoked when a new goal is available.
def is_active(self)
Allows polling implementations to query about the status of the current goal.
def set_aborted(self, result=None, text="")
Sets the status of the active goal to aborted.
def publish_feedback(self, feedback)
Publishes feedback for a given goal.
def set_succeeded(self, result=None, text="")
Sets the status of the active goal to succeeded.
def set_preempted(self, result=None, text="")
Sets the status of the active goal to preempted.
def register_preempt_callback(self, cb)
Allows users to register a callback to be invoked when a new preempt request is available.
def is_new_goal_available(self)
Allows polling implementations to query about the availability of a new goal.
def start(self)
Explicitly start the action server, used it auto_start is set to false.
The ActionServer is a helpful tool for managing goal requests to a node. It allows the user to specif...
Definition: action_server.h:72
def __init__(self, name, ActionSpec, execute_cb=None, auto_start=True)
Constructor for a SimpleActionServer.
The SimpleActionServer implements a singe goal policy on top of the ActionServer class.
def executeLoop(self)
Called from a separate thread to call blocking execute calls.


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep, Mikael Arguedas
autogenerated on Mon Aug 24 2020 03:40:47