server_goal_handle.py
Go to the documentation of this file.
1 # Copyright (c) 2009, Willow Garage, Inc.
2 # All rights reserved.
3 #
4 # Redistribution and use in source and binary forms, with or without
5 # modification, are permitted provided that the following conditions are met:
6 #
7 # * Redistributions of source code must retain the above copyright
8 # notice, this list of conditions and the following disclaimer.
9 # * Redistributions in binary form must reproduce the above copyright
10 # notice, this list of conditions and the following disclaimer in the
11 # documentation and/or other materials provided with the distribution.
12 # * Neither the name of the Willow Garage, Inc. nor the names of its
13 # contributors may be used to endorse or promote products derived from
14 # this software without specific prior written permission.
15 #
16 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
20 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
23 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
25 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 # POSSIBILITY OF SUCH DAMAGE.
27 
28 # Author: Alexander Sorokin.
29 # Based on C++ goal_id_generator.h/cpp
30 import rospy
31 
32 import actionlib_msgs.msg
33 
34 
36  """
37  * @class ServerGoalHandle
38  * @brief Encapsulates a state machine for a given goal that the user can
39  * trigger transisions on. All ROS interfaces for the goal are managed by
40  * the ActionServer to lessen the burden on the user.
41 
42  """
43 
44  def __init__(self, status_tracker=None, action_server=None, handle_tracker=None):
45  """
46  A private constructor used by the ActionServer to initialize a ServerGoalHandle.
47  @node The default constructor was not ported.
48  """
49  self.status_tracker = status_tracker
50  self.action_server = action_server
51  self.handle_tracker = handle_tracker
52 
53  if status_tracker:
54  self.goal = status_tracker.goal
55  else:
56  self.goal = None
57 
58  def get_default_result(self):
59  return self.action_server.ActionResultType()
60 
61  def set_accepted(self, text=""):
62  """
63  Accept the goal referenced by the goal handle. This will
64  transition to the ACTIVE state or the PREEMPTING state depending
65  on whether a cancel request has been received for the goal
66  """
67 
68  rospy.logdebug("Accepting goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec())
69  if self.goal:
70  with self.action_server.lock:
71  status = self.status_tracker.status.status
72 
73  # if we were pending before, then we'll go active
74  if status == actionlib_msgs.msg.GoalStatus.PENDING:
75  self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.ACTIVE
76  self.status_tracker.status.text = text
77  self.action_server.publish_status()
78 
79  # if we were recalling before, now we'll go to preempting
80  elif status == actionlib_msgs.msg.GoalStatus.RECALLING:
81  self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.PREEMPTING
82  self.status_tracker.status.text = text
83  self.action_server.publish_status()
84 
85  else:
86  rospy.logerr("To transition to an active state, the goal must be in a pending or recalling state, it is currently in state: %d", self.status_tracker.status.status)
87 
88  else:
89  rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle")
90 
91  def set_canceled(self, result=None, text=""):
92  """
93  Set the status of the goal associated with the ServerGoalHandle to RECALLED or PREEMPTED
94  depending on what the current status of the goal is
95  @param result Optionally, the user can pass in a result to be sent to any clients of the goal
96  """
97  if not result:
98  result = self.get_default_result()
99 
100  rospy.logdebug("Setting status to canceled on goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec())
101 
102  if self.goal:
103  with self.action_server.lock:
104  status = self.status_tracker.status.status
105  if status == actionlib_msgs.msg.GoalStatus.PENDING or status == actionlib_msgs.msg.GoalStatus.RECALLING:
106  self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.RECALLED
107  self.status_tracker.status.text = text
108  # on transition to a terminal state, we'll also set the handle destruction time
109  self.status_tracker.handle_destruction_time = rospy.Time.now()
110  self.action_server.publish_result(self.status_tracker.status, result)
111  elif status == actionlib_msgs.msg.GoalStatus.ACTIVE or status == actionlib_msgs.msg.GoalStatus.PREEMPTING:
112  self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.PREEMPTED
113  self.status_tracker.status.text = text
114  # on transition to a terminal state, we'll also set the handle destruction time
115  self.status_tracker.handle_destruction_time = rospy.Time.now()
116  self.action_server.publish_result(self.status_tracker.status, result)
117 
118  else:
119  rospy.logerr("To transition to a cancelled state, the goal must be in a pending, recalling, active, or preempting state, it is currently in state: %d",
120  self.status_tracker.status.status)
121 
122  else:
123  rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle")
124 
125  def set_rejected(self, result=None, text=""):
126  """
127  * @brief Set the status of the goal associated with the ServerGoalHandle to rejected
128  * @param result Optionally, the user can pass in a result to be sent to any clients of the goal
129  """
130  if not result:
131  result = self.get_default_result()
132 
133  rospy.logdebug("Setting status to rejected on goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec())
134  if self.goal:
135  with self.action_server.lock:
136  status = self.status_tracker.status.status
137  if status == actionlib_msgs.msg.GoalStatus.PENDING or status == actionlib_msgs.msg.GoalStatus.RECALLING:
138  self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.REJECTED
139  self.status_tracker.status.text = text
140  # on transition to a terminal state, we'll also set the handle destruction time
141  self.status_tracker.handle_destruction_time = rospy.Time.now()
142  self.action_server.publish_result(self.status_tracker.status, result)
143 
144  else:
145  rospy.logerr("To transition to a rejected state, the goal must be in a pending or recalling state, it is currently in state: %d",
146  self.status_tracker.status.status)
147 
148  else:
149  rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle")
150 
151  def set_aborted(self, result=None, text=""):
152  """
153  Set the status of the goal associated with the ServerGoalHandle to aborted
154  @param result Optionally, the user can pass in a result to be sent to any clients of the goal
155  """
156  if not result:
157  result = self.get_default_result()
158 
159  rospy.logdebug("Setting status to aborted on goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec())
160  if self.goal:
161  with self.action_server.lock:
162  status = self.status_tracker.status.status
163  if status == actionlib_msgs.msg.GoalStatus.PREEMPTING or status == actionlib_msgs.msg.GoalStatus.ACTIVE:
164  self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.ABORTED
165  self.status_tracker.status.text = text
166  # on transition to a terminal state, we'll also set the handle destruction time
167  self.status_tracker.handle_destruction_time = rospy.Time.now()
168  self.action_server.publish_result(self.status_tracker.status, result)
169 
170  else:
171  rospy.logerr("To transition to an aborted state, the goal must be in a preempting or active state, it is currently in state: %d",
172  status)
173 
174  else:
175  rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle")
176 
177  def set_succeeded(self, result=None, text=""):
178  """
179  Set the status of the goal associated with the ServerGoalHandle to succeeded
180  @param result Optionally, the user can pass in a result to be sent to any clients of the goal
181  """
182  if not result:
183  result = self.get_default_result()
184 
185  rospy.logdebug("Setting status to succeeded on goal, id: %s, stamp: %.2f",
186  self.get_goal_id().id, self.get_goal_id().stamp.to_sec())
187  if self.goal:
188  with self.action_server.lock:
189  status = self.status_tracker.status.status
190  if status == actionlib_msgs.msg.GoalStatus.PREEMPTING or status == actionlib_msgs.msg.GoalStatus.ACTIVE:
191  self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.SUCCEEDED
192  self.status_tracker.status.text = text
193  # on transition to a terminal state, we'll also set the handle destruction time
194  self.status_tracker.handle_destruction_time = rospy.Time.now()
195  self.action_server.publish_result(self.status_tracker.status, result)
196 
197  else:
198  rospy.logerr("To transition to a succeeded state, the goal must be in a preempting or active state, it is currently in state: %d",
199  status)
200 
201  else:
202  rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle")
203 
204  def publish_feedback(self, feedback):
205  """
206  Send feedback to any clients of the goal associated with this ServerGoalHandle
207  @param feedback The feedback to send to the client
208  """
209  rospy.logdebug("Publishing feedback for goal, id: %s, stamp: %.2f",
210  self.get_goal_id().id, self.get_goal_id().stamp.to_sec())
211  if self.goal:
212  with self.action_server.lock:
213  self.action_server.publish_feedback(self.status_tracker.status, feedback)
214  else:
215  rospy.logerr("Attempt to publish feedback on an uninitialized ServerGoalHandle")
216 
217  def get_goal(self):
218  """
219  Accessor for the goal associated with the ServerGoalHandle
220  @return A shared_ptr to the goal object
221  """
222  # if we have a goal that is non-null
223  if self.goal:
224  # @todo Test that python reference counting automatically handles this.
225  # create the deleter for our goal subtype
226  # d = EnclosureDeleter(self.goal)
227  # weakref.ref(boost::shared_ptr<const Goal>(&(goal_->goal), d)
228  return self.goal.goal
229 
230  return None
231 
232  def get_goal_id(self):
233  """
234  Accessor for the goal id associated with the ServerGoalHandle
235  @return The goal id
236  """
237  if self.goal:
238  with self.action_server.lock:
239  return self.status_tracker.status.goal_id
240  else:
241  rospy.logerr("Attempt to get a goal id on an uninitialized ServerGoalHandle")
242  return actionlib_msgs.msg.GoalID()
243 
244  def get_goal_status(self):
245  """
246  Accessor for the status associated with the ServerGoalHandle
247  @return The goal status
248  """
249  if self.goal:
250  with self.action_server.lock:
251  return self.status_tracker.status
252  else:
253  rospy.logerr("Attempt to get goal status on an uninitialized ServerGoalHandle")
254  return actionlib_msgs.msg.GoalStatus()
255 
256  def __eq__(self, other):
257  """
258  Equals operator for ServerGoalHandles
259  @param other The ServerGoalHandle to compare to
260  @return True if the ServerGoalHandles refer to the same goal, false otherwise
261  """
262 
263  if not self.goal or not other.goal:
264  return False
265  my_id = self.get_goal_id()
266  their_id = other.get_goal_id()
267  return my_id.id == their_id.id
268 
269  def __ne__(self, other):
270  """
271  != operator for ServerGoalHandles
272  @param other The ServerGoalHandle to compare to
273  @return True if the ServerGoalHandles refer to different goals, false otherwise
274  """
275  if not self.goal or not other.goal:
276  return True
277  my_id = self.get_goal_id()
278  their_id = other.get_goal_id()
279 
280  return my_id.id != their_id.id
281 
282  def __hash__(self):
283  """
284  hash function for ServerGoalHandles
285  @return hash of the goal ID
286  """
287  return hash(self.get_goal_id().id)
288 
290  """
291  A private method to set status to PENDING or RECALLING
292  @return True if the cancel request should be passed on to the user, false otherwise
293  """
294  rospy.logdebug("Transisitoning to a cancel requested state on goal id: %s, stamp: %.2f",
295  self.get_goal_id().id, self.get_goal_id().stamp.to_sec())
296  if self.goal:
297  with self.action_server.lock:
298  status = self.status_tracker.status.status
299  if status == actionlib_msgs.msg.GoalStatus.PENDING:
300  self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.RECALLING
301  self.action_server.publish_status()
302  return True
303 
304  if status == actionlib_msgs.msg.GoalStatus.ACTIVE:
305  self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.PREEMPTING
306  self.action_server.publish_status()
307  return True
308 
309  return False
def __init__(self, status_tracker=None, action_server=None, handle_tracker=None)
def set_canceled(self, result=None, text="")
def set_aborted(self, result=None, text="")
def set_rejected(self, result=None, text="")
def set_succeeded(self, result=None, text="")


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