action_client.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: Stuart Glaser
29 '''
30 Example:
31 
32 from move_base.msg import *
33 rospy.init_node('foo')
34 
35 
36 from move_base.msg import *
37 from geometry_msgs.msg import *
38 g1 = MoveBaseGoal(PoseStamped(Header(frame_id = 'base_link'),
39  Pose(Point(2, 0, 0),
40  Quaternion(0, 0, 0, 1))))
41 g2 = MoveBaseGoal(PoseStamped(Header(frame_id = 'base_link'),
42  Pose(Point(5, 0, 0),
43  Quaternion(0, 0, 0, 1))))
44 
45 client = ActionClient('move_base', MoveBaseAction)
46 
47 h1 = client.send_goal(g1)
48 h2 = client.send_goal(g2)
49 client.cancel_all_goals()
50 '''
51 
52 import threading
53 import weakref
54 import time
55 import rospy
56 from rospy import Header
57 from actionlib_msgs.msg import GoalID, GoalStatus, GoalStatusArray
58 from actionlib.exceptions import ActionException
59 
60 g_goal_id = 1
61 
62 
64  for k, v in C.__dict__.items():
65  if isinstance(v, int) and v == n:
66  return k
67  return "NO_SUCH_STATE_%d" % n
68 
69 
70 class CommState(object):
71  WAITING_FOR_GOAL_ACK = 0
72  PENDING = 1
73  ACTIVE = 2
74  WAITING_FOR_RESULT = 3
75  WAITING_FOR_CANCEL_ACK = 4
76  RECALLING = 5
77  PREEMPTING = 6
78  DONE = 7
79  LOST = 8
80 
81 
82 class TerminalState(object):
83  RECALLED = GoalStatus.RECALLED
84  REJECTED = GoalStatus.REJECTED
85  PREEMPTED = GoalStatus.PREEMPTED
86  ABORTED = GoalStatus.ABORTED
87  SUCCEEDED = GoalStatus.SUCCEEDED
88  LOST = GoalStatus.LOST
89 
90 
91 GoalStatus.to_string = classmethod(get_name_of_constant)
92 CommState.to_string = classmethod(get_name_of_constant)
93 TerminalState.to_string = classmethod(get_name_of_constant)
94 
95 
96 def _find_status_by_goal_id(status_array, id):
97  for s in status_array.status_list:
98  if s.goal_id.id == id:
99  return s
100  return None
101 
102 
103 
110 
114  def __init__(self, comm_state_machine):
115  self.comm_state_machine = comm_state_machine
116 
117  # print "GH created. id = %.3f" % self.comm_state_machine.action_goal.goal_id.stamp.to_sec()
118 
119 
120  def __eq__(self, o):
121  if not o:
122  return False
123  return self.comm_state_machine == o.comm_state_machine
124 
125 
126  def __ne__(self, o):
127  if not o:
128  return True
129  return not (self.comm_state_machine == o.comm_state_machine)
130 
131 
132  def __hash__(self):
133  return hash(self.comm_state_machine)
134 
135 
138  def cancel(self):
139  with self.comm_state_machine.mutex:
140  cancel_msg = GoalID(stamp=rospy.Time(0),
141  id=self.comm_state_machine.action_goal.goal_id.id)
142  self.comm_state_machine.send_cancel_fn(cancel_msg)
143  self.comm_state_machine.transition_to(CommState.WAITING_FOR_CANCEL_ACK)
144 
145 
151  def get_comm_state(self):
152  if not self.comm_state_machine:
153  rospy.logerr("Trying to get_comm_state on an inactive ClientGoalHandle.")
154  return CommState.LOST
155  return self.comm_state_machine.state
156 
157 
163  def get_goal_status(self):
164  if not self.comm_state_machine:
165  rospy.logerr("Trying to get_goal_status on an inactive ClientGoalHandle.")
166  return GoalStatus.PENDING
167  return self.comm_state_machine.latest_goal_status.status
168 
169 
175  if not self.comm_state_machine:
176  rospy.logerr("Trying to get_goal_status_text on an inactive ClientGoalHandle.")
177  return "ERROR: Trying to get_goal_status_text on an inactive ClientGoalHandle."
178  return self.comm_state_machine.latest_goal_status.text
179 
180 
183  def get_result(self):
184  if not self.comm_state_machine:
185  rospy.logerr("Trying to get_result on an inactive ClientGoalHandle.")
186  return None
187  if not self.comm_state_machine.latest_result:
188  # rospy.logerr("Trying to get_result on a ClientGoalHandle when no result has been received.")
189  return None
190  return self.comm_state_machine.latest_result.result
191 
192 
199  if not self.comm_state_machine:
200  rospy.logerr("Trying to get_terminal_state on an inactive ClientGoalHandle.")
201  return GoalStatus.LOST
202 
203  with self.comm_state_machine.mutex:
204  if self.comm_state_machine.state != CommState.DONE:
205  rospy.logwarn("Asking for the terminal state when we're in [%s]",
206  CommState.to_string(self.comm_state_machine.state))
207 
208  goal_status = self.comm_state_machine.latest_goal_status.status
209  if goal_status in [GoalStatus.PREEMPTED, GoalStatus.SUCCEEDED,
210  GoalStatus.ABORTED, GoalStatus.REJECTED,
211  GoalStatus.RECALLED, GoalStatus.LOST]:
212  return goal_status
213 
214  rospy.logerr("Asking for a terminal state, but the goal status is %d", goal_status)
215  return GoalStatus.LOST
216 
217 
218 NO_TRANSITION = -1
219 INVALID_TRANSITION = -2
220 _transitions = {
221  CommState.WAITING_FOR_GOAL_ACK: {
222  GoalStatus.PENDING: CommState.PENDING,
223  GoalStatus.ACTIVE: CommState.ACTIVE,
224  GoalStatus.REJECTED: (CommState.PENDING, CommState.WAITING_FOR_RESULT),
225  GoalStatus.RECALLING: (CommState.PENDING, CommState.RECALLING),
226  GoalStatus.RECALLED: (CommState.PENDING, CommState.WAITING_FOR_RESULT),
227  GoalStatus.PREEMPTED: (CommState.ACTIVE, CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
228  GoalStatus.SUCCEEDED: (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
229  GoalStatus.ABORTED: (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
230  GoalStatus.PREEMPTING: (CommState.ACTIVE, CommState.PREEMPTING)},
231  CommState.PENDING: {
232  GoalStatus.PENDING: NO_TRANSITION,
233  GoalStatus.ACTIVE: CommState.ACTIVE,
234  GoalStatus.REJECTED: CommState.WAITING_FOR_RESULT,
235  GoalStatus.RECALLING: CommState.RECALLING,
236  GoalStatus.RECALLED: (CommState.RECALLING, CommState.WAITING_FOR_RESULT),
237  GoalStatus.PREEMPTED: (CommState.ACTIVE, CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
238  GoalStatus.SUCCEEDED: (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
239  GoalStatus.ABORTED: (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
240  GoalStatus.PREEMPTING: (CommState.ACTIVE, CommState.PREEMPTING)},
241  CommState.ACTIVE: {
242  GoalStatus.PENDING: INVALID_TRANSITION,
243  GoalStatus.ACTIVE: NO_TRANSITION,
244  GoalStatus.REJECTED: INVALID_TRANSITION,
245  GoalStatus.RECALLING: INVALID_TRANSITION,
246  GoalStatus.RECALLED: INVALID_TRANSITION,
247  GoalStatus.PREEMPTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
248  GoalStatus.SUCCEEDED: CommState.WAITING_FOR_RESULT,
249  GoalStatus.ABORTED: CommState.WAITING_FOR_RESULT,
250  GoalStatus.PREEMPTING: CommState.PREEMPTING},
251  CommState.WAITING_FOR_RESULT: {
252  GoalStatus.PENDING: INVALID_TRANSITION,
253  GoalStatus.ACTIVE: NO_TRANSITION,
254  GoalStatus.REJECTED: NO_TRANSITION,
255  GoalStatus.RECALLING: INVALID_TRANSITION,
256  GoalStatus.RECALLED: NO_TRANSITION,
257  GoalStatus.PREEMPTED: NO_TRANSITION,
258  GoalStatus.SUCCEEDED: NO_TRANSITION,
259  GoalStatus.ABORTED: NO_TRANSITION,
260  GoalStatus.PREEMPTING: INVALID_TRANSITION},
261  CommState.WAITING_FOR_CANCEL_ACK: {
262  GoalStatus.PENDING: NO_TRANSITION,
263  GoalStatus.ACTIVE: NO_TRANSITION,
264  GoalStatus.REJECTED: CommState.WAITING_FOR_RESULT,
265  GoalStatus.RECALLING: CommState.RECALLING,
266  GoalStatus.RECALLED: (CommState.RECALLING, CommState.WAITING_FOR_RESULT),
267  GoalStatus.PREEMPTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
268  GoalStatus.SUCCEEDED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
269  GoalStatus.ABORTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
270  GoalStatus.PREEMPTING: CommState.PREEMPTING},
271  CommState.RECALLING: {
272  GoalStatus.PENDING: INVALID_TRANSITION,
273  GoalStatus.ACTIVE: INVALID_TRANSITION,
274  GoalStatus.REJECTED: CommState.WAITING_FOR_RESULT,
275  GoalStatus.RECALLING: NO_TRANSITION,
276  GoalStatus.RECALLED: CommState.WAITING_FOR_RESULT,
277  GoalStatus.PREEMPTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
278  GoalStatus.SUCCEEDED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
279  GoalStatus.ABORTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
280  GoalStatus.PREEMPTING: CommState.PREEMPTING},
281  CommState.PREEMPTING: {
282  GoalStatus.PENDING: INVALID_TRANSITION,
283  GoalStatus.ACTIVE: INVALID_TRANSITION,
284  GoalStatus.REJECTED: INVALID_TRANSITION,
285  GoalStatus.RECALLING: INVALID_TRANSITION,
286  GoalStatus.RECALLED: INVALID_TRANSITION,
287  GoalStatus.PREEMPTED: CommState.WAITING_FOR_RESULT,
288  GoalStatus.SUCCEEDED: CommState.WAITING_FOR_RESULT,
289  GoalStatus.ABORTED: CommState.WAITING_FOR_RESULT,
290  GoalStatus.PREEMPTING: NO_TRANSITION},
291  CommState.DONE: {
292  GoalStatus.PENDING: INVALID_TRANSITION,
293  GoalStatus.ACTIVE: INVALID_TRANSITION,
294  GoalStatus.REJECTED: NO_TRANSITION,
295  GoalStatus.RECALLING: INVALID_TRANSITION,
296  GoalStatus.RECALLED: NO_TRANSITION,
297  GoalStatus.PREEMPTED: NO_TRANSITION,
298  GoalStatus.SUCCEEDED: NO_TRANSITION,
299  GoalStatus.ABORTED: NO_TRANSITION,
300  GoalStatus.PREEMPTING: INVALID_TRANSITION}}
301 
302 
304  def __init__(self, action_goal, transition_cb, feedback_cb, send_goal_fn, send_cancel_fn):
305  self.action_goal = action_goal
306  self.transition_cb = transition_cb
307  self.feedback_cb = feedback_cb
308  self.send_goal_fn = send_goal_fn
309  self.send_cancel_fn = send_cancel_fn
310 
311  self.state = CommState.WAITING_FOR_GOAL_ACK
312  self.mutex = threading.RLock()
313  self.latest_goal_status = GoalStatus(status=GoalStatus.PENDING)
314  self.latest_result = None
315 
316  def __eq__(self, o):
317  return self.action_goal.goal_id.id == o.action_goal.goal_id.id
318 
319 
320  def __hash__(self):
321  return hash(self.action_goal.goal_id.id)
322 
323  def set_state(self, state):
324  rospy.logdebug("Transitioning CommState from %s to %s",
325  CommState.to_string(self.state), CommState.to_string(state))
326  self.state = state
327 
328 
331  def update_status(self, status_array):
332  with self.mutex:
333  if self.state == CommState.DONE:
334  return
335 
336  status = _find_status_by_goal_id(status_array, self.action_goal.goal_id.id)
337 
338  # You mean you haven't heard of me?
339  if not status:
340  if self.state not in [CommState.WAITING_FOR_GOAL_ACK,
341  CommState.WAITING_FOR_RESULT,
342  CommState.DONE]:
343  self._mark_as_lost()
344  return
345 
346  self.latest_goal_status = status
347 
348  # Determines the next state from the lookup table
349  if self.state not in _transitions:
350  rospy.logerr("CommStateMachine is in a funny state: %i" % self.state)
351  return
352  if status.status not in _transitions[self.state]:
353  rospy.logerr("Got an unknown status from the ActionServer: %i" % status.status)
354  return
355  next_state = _transitions[self.state][status.status]
356 
357  # Knowing the next state, what should we do?
358  if next_state == NO_TRANSITION:
359  pass
360  elif next_state == INVALID_TRANSITION:
361  rospy.logerr("Invalid goal status transition from %s to %s" %
362  (CommState.to_string(self.state), GoalStatus.to_string(status.status)))
363  else:
364  if hasattr(next_state, '__getitem__'):
365  for s in next_state:
366  self.transition_to(s)
367  else:
368  self.transition_to(next_state)
369 
370  def transition_to(self, state):
371  rospy.logdebug("Transitioning to %s (from %s, goal: %s)",
372  CommState.to_string(state), CommState.to_string(self.state),
373  self.action_goal.goal_id.id)
374  self.state = state
375  if self.transition_cb:
376  self.transition_cb(ClientGoalHandle(self))
377 
378  def _mark_as_lost(self):
379  self.latest_goal_status.status = GoalStatus.LOST
380  self.transition_to(CommState.DONE)
381 
382  def update_result(self, action_result):
383  # Might not be for us
384  if self.action_goal.goal_id.id != action_result.status.goal_id.id:
385  return
386 
387  with self.mutex:
388  self.latest_goal_status = action_result.status
389  self.latest_result = action_result
390 
391  if self.state in [CommState.WAITING_FOR_GOAL_ACK,
392  CommState.WAITING_FOR_CANCEL_ACK,
393  CommState.PENDING,
394  CommState.ACTIVE,
395  CommState.WAITING_FOR_RESULT,
396  CommState.RECALLING,
397  CommState.PREEMPTING]:
398  # Stuffs the goal status in the result into a GoalStatusArray
399  status_array = GoalStatusArray()
400  status_array.status_list.append(action_result.status)
401  self.update_status(status_array)
402 
403  self.transition_to(CommState.DONE)
404  elif self.state == CommState.DONE:
405  rospy.logerr("Got a result when we were already in the DONE state")
406  else:
407  rospy.logerr("In a funny state: %i" % self.state)
408 
409  def update_feedback(self, action_feedback):
410  # Might not be for us
411  if self.action_goal.goal_id.id != action_feedback.status.goal_id.id:
412  return
413 
414  # with self.mutex:
415  if self.feedback_cb and self.state != CommState.DONE:
416  self.feedback_cb(ClientGoalHandle(self), action_feedback.feedback)
417 
418 
420 
421  # statuses - a list of weak references to CommStateMachine objects
422 
423  def __init__(self, ActionSpec):
424  self.list_mutex = threading.RLock()
425  self.statuses = []
426  self.send_goal_fn = None
427 
428  try:
429  a = ActionSpec()
430 
431  self.ActionSpec = ActionSpec
432  self.ActionGoal = type(a.action_goal)
433  self.ActionResult = type(a.action_result)
434  self.ActionFeedback = type(a.action_feedback)
435  except AttributeError:
436  raise ActionException("Type is not an action spec: %s" % str(ActionSpec))
437 
438  def _generate_id(self):
439  global g_goal_id
440  id, g_goal_id = g_goal_id, g_goal_id + 1
441  now = rospy.Time.now()
442  return GoalID(id="%s-%i-%.3f" % (rospy.get_caller_id(), id, now.to_sec()), stamp=now)
443 
444  def register_send_goal_fn(self, fn):
445  self.send_goal_fn = fn
446 
447  def register_cancel_fn(self, fn):
448  self.cancel_fn = fn
449 
450 
453  def init_goal(self, goal, transition_cb=None, feedback_cb=None):
454  action_goal = self.ActionGoal(header=Header(),
455  goal_id=self._generate_id(),
456  goal=goal)
457  action_goal.header.stamp = rospy.get_rostime()
458 
459  csm = CommStateMachine(action_goal, transition_cb, feedback_cb,
460  self.send_goal_fn, self.cancel_fn)
461 
462  with self.list_mutex:
463  self.statuses.append(weakref.ref(csm))
464 
465  self.send_goal_fn(action_goal)
466 
467  return ClientGoalHandle(csm)
468 
469  # Pulls out the statuses that are still live (creating strong
470  # references to them)
472  with self.list_mutex:
473  live_statuses = [r() for r in self.statuses]
474  live_statuses = [x for x in live_statuses if x]
475  return live_statuses
476 
477 
480  def update_statuses(self, status_array):
481 
482  with self.list_mutex:
483  # Garbage collects dead status objects
484  self.statuses = [r for r in self.statuses if r()]
485 
486  for status in self._get_live_statuses():
487  status.update_status(status_array)
488 
489  def update_results(self, action_result):
490  for status in self._get_live_statuses():
491  status.update_result(action_result)
492 
493  def update_feedbacks(self, action_feedback):
494  for status in self._get_live_statuses():
495  status.update_feedback(action_feedback)
496 
497 
499 
506  def __init__(self, ns, ActionSpec):
507  self.ns = ns
508  self.last_status_msg = None
509 
510  try:
511  a = ActionSpec()
512 
513  self.ActionSpec = ActionSpec
514  self.ActionGoal = type(a.action_goal)
515  self.ActionResult = type(a.action_result)
516  self.ActionFeedback = type(a.action_feedback)
517  except AttributeError:
518  raise ActionException("Type is not an action spec: %s" % str(ActionSpec))
519 
520  self.pub_queue_size = rospy.get_param('actionlib_client_pub_queue_size', 10)
521  if self.pub_queue_size < 0:
522  self.pub_queue_size = 10
523  self.pub_goal = rospy.Publisher(rospy.remap_name(ns) + '/goal', self.ActionGoal, queue_size=self.pub_queue_size)
524  self.pub_cancel = rospy.Publisher(rospy.remap_name(ns) + '/cancel', GoalID, queue_size=self.pub_queue_size)
525 
526  self.manager = GoalManager(ActionSpec)
527  self.manager.register_send_goal_fn(self.pub_goal.publish)
528  self.manager.register_cancel_fn(self.pub_cancel.publish)
529 
530  self.sub_queue_size = rospy.get_param('actionlib_client_sub_queue_size', -1)
531  if self.sub_queue_size < 0:
532  self.sub_queue_size = None
533  self.status_sub = rospy.Subscriber(rospy.remap_name(ns) + '/status', GoalStatusArray, callback=self._status_cb, queue_size=self.sub_queue_size)
534  self.result_sub = rospy.Subscriber(rospy.remap_name(ns) + '/result', self.ActionResult, callback=self._result_cb, queue_size=self.sub_queue_size)
535  self.feedback_sub = rospy.Subscriber(rospy.remap_name(ns) + '/feedback', self.ActionFeedback, callback=self._feedback_cb, queue_size=self.sub_queue_size)
536 
537 
538  def stop(self):
539  self.pub_goal.unregister()
540  self.pub_cancel.unregister()
541  self.status_sub.unregister()
542  self.result_sub.unregister()
543  self.feedback_sub.unregister()
544 
545 
559  def send_goal(self, goal, transition_cb=None, feedback_cb=None):
560  return self.manager.init_goal(goal, transition_cb, feedback_cb)
561 
562 
566  def cancel_all_goals(self):
567  cancel_msg = GoalID(stamp=rospy.Time.from_sec(0.0), id="")
568  self.pub_cancel.publish(cancel_msg)
569 
570 
575 
577  cancel_msg = GoalID(stamp=time, id="")
578  self.pub_cancel.publish(cancel_msg)
579 
580 
581  def wait_for_action_server_to_start(self, timeout=rospy.Duration(0.0)):
582  return self.wait_for_server(timeout)
583 
584 
589  def wait_for_server(self, timeout=rospy.Duration(0.0)):
590  started = False
591  timeout_time = rospy.get_rostime() + timeout
592  while not rospy.is_shutdown():
593  if self.last_status_msg:
594  server_id = self.last_status_msg._connection_header['callerid']
595 
596  if self.pub_goal.impl.has_connection(server_id) and \
597  self.pub_cancel.impl.has_connection(server_id):
598  # Check that the connections to the result and feedback
599  # topics have completed and are ready to receive data.
600  # Check the connections by checking the callerid from the
601  # publisher's header against the server_id.
602  # Note: there is no need to check the status topic, as
603  # we already received a status message in last_status_msg
604  # and are using it as the source of truth for the server_id.
605  # Note: there is no need to grab the c_lock, the
606  # implementation guarantees to never mutate the connection
607  # list, only replace the reference. So all we need to do is
608  # to copy a reference ourselves. This behavior is
609  # documented in the comment above the c_lock creation in
610  # rospy._TopicImpl.__init__.
611  result_sub_connections = self.result_sub.impl.connections
612  result_sub_found = False
613  for c in result_sub_connections:
614  if c.callerid_pub == server_id:
615  result_sub_found = True
616  feedback_sub_connections = self.feedback_sub.impl.connections
617  feedback_sub_found = False
618  for c in feedback_sub_connections:
619  if c.callerid_pub == server_id:
620  feedback_sub_found = True
621 
622  if result_sub_found and feedback_sub_found:
623  started = True
624  break
625 
626  if timeout != rospy.Duration(0.0) and rospy.get_rostime() >= timeout_time:
627  break
628 
629  time.sleep(0.01)
630 
631  return started
632 
633  def _status_cb(self, msg):
634  self.last_status_msg = msg
635  self.manager.update_statuses(msg)
636 
637  def _result_cb(self, msg):
638  self.manager.update_results(msg)
639 
640  def _feedback_cb(self, msg):
641  self.manager.update_feedbacks(msg)
actionlib.action_client.ClientGoalHandle
Client side handle to monitor goal progress.
Definition: action_client.py:109
actionlib.action_client.ActionClient.pub_cancel
pub_cancel
Definition: action_client.py:524
actionlib.action_client._find_status_by_goal_id
def _find_status_by_goal_id(status_array, id)
Definition: action_client.py:96
actionlib.action_client.GoalManager.list_mutex
list_mutex
Definition: action_client.py:424
actionlib.action_client.get_name_of_constant
def get_name_of_constant(C, n)
Definition: action_client.py:63
actionlib.action_client.ClientGoalHandle.__hash__
def __hash__(self)
@brieft Hash function for ClientGoalHandle
Definition: action_client.py:132
actionlib.action_client.ActionClient._result_cb
def _result_cb(self, msg)
Definition: action_client.py:637
actionlib.action_client.ActionClient.feedback_sub
feedback_sub
Definition: action_client.py:535
actionlib.action_client.GoalManager.update_results
def update_results(self, action_result)
Definition: action_client.py:489
actionlib.action_client.ActionClient.stop
def stop(self)
Stop the action client.
Definition: action_client.py:538
actionlib.action_client.GoalManager.init_goal
def init_goal(self, goal, transition_cb=None, feedback_cb=None)
Sends off a goal and starts tracking its status.
Definition: action_client.py:453
actionlib.action_client.ClientGoalHandle.get_goal_status_text
def get_goal_status_text(self)
Returns the current status text of the goal.
Definition: action_client.py:174
actionlib.action_client.CommStateMachine.__init__
def __init__(self, action_goal, transition_cb, feedback_cb, send_goal_fn, send_cancel_fn)
Definition: action_client.py:304
actionlib.action_client.CommStateMachine.update_feedback
def update_feedback(self, action_feedback)
Definition: action_client.py:409
actionlib.action_client.ActionClient.pub_queue_size
pub_queue_size
Definition: action_client.py:520
actionlib.action_client.GoalManager.ActionGoal
ActionGoal
Definition: action_client.py:432
actionlib.action_client.ActionClient.wait_for_server
def wait_for_server(self, timeout=rospy.Duration(0.0))
Waits for the ActionServer to connect to this client.
Definition: action_client.py:589
actionlib.exceptions
Definition: exceptions.py:1
actionlib.action_client.GoalManager.__init__
def __init__(self, ActionSpec)
Definition: action_client.py:423
actionlib.action_client.GoalManager.register_send_goal_fn
def register_send_goal_fn(self, fn)
Definition: action_client.py:444
actionlib.action_client.GoalManager
Definition: action_client.py:419
actionlib.action_client.CommStateMachine.send_goal_fn
send_goal_fn
Definition: action_client.py:308
actionlib.action_client.GoalManager.ActionSpec
ActionSpec
Definition: action_client.py:431
actionlib.action_client.ClientGoalHandle.get_terminal_state
def get_terminal_state(self)
Gets the terminal state information for this goal.
Definition: action_client.py:198
actionlib.action_client.CommStateMachine.state
state
Definition: action_client.py:311
actionlib.action_client.GoalManager.update_feedbacks
def update_feedbacks(self, action_feedback)
Definition: action_client.py:493
actionlib.action_client.ClientGoalHandle.__eq__
def __eq__(self, o)
True iff the two ClientGoalHandle's are tracking the same goal.
Definition: action_client.py:120
actionlib.action_client.GoalManager.update_statuses
def update_statuses(self, status_array)
Updates the statuses of all goals from the information in status_array.
Definition: action_client.py:480
actionlib.action_client.ClientGoalHandle.get_goal_status
def get_goal_status(self)
Returns the current status of the goal.
Definition: action_client.py:163
actionlib.action_client.ActionClient.pub_goal
pub_goal
Definition: action_client.py:523
actionlib.action_client.ActionClient.wait_for_action_server_to_start
def wait_for_action_server_to_start(self, timeout=rospy.Duration(0.0))
[Deprecated] Use wait_for_server
Definition: action_client.py:581
actionlib.action_client.ClientGoalHandle.__init__
def __init__(self, comm_state_machine)
Internal use only.
Definition: action_client.py:114
actionlib.action_client.GoalManager.register_cancel_fn
def register_cancel_fn(self, fn)
Definition: action_client.py:447
actionlib.action_client.ClientGoalHandle.comm_state_machine
comm_state_machine
Definition: action_client.py:115
actionlib.action_client.ActionClient._feedback_cb
def _feedback_cb(self, msg)
Definition: action_client.py:640
actionlib.action_client.ActionClient.cancel_goals_at_and_before_time
def cancel_goals_at_and_before_time(self, time)
Cancels all goals prior to a given timestamp.
Definition: action_client.py:576
actionlib.action_client.ClientGoalHandle.__ne__
def __ne__(self, o)
True iff the two ClientGoalHandle's are tracking different goals.
Definition: action_client.py:126
actionlib.action_client.GoalManager.ActionFeedback
ActionFeedback
Definition: action_client.py:434
actionlib.action_client.ActionClient.ActionFeedback
ActionFeedback
Definition: action_client.py:516
actionlib.action_client.GoalManager._get_live_statuses
def _get_live_statuses(self)
Definition: action_client.py:471
actionlib.action_client.ActionClient.__init__
def __init__(self, ns, ActionSpec)
Constructs an ActionClient and opens connections to an ActionServer.
Definition: action_client.py:506
actionlib.action_client.CommStateMachine.feedback_cb
feedback_cb
Definition: action_client.py:307
actionlib.action_client.ClientGoalHandle.get_comm_state
def get_comm_state(self)
Get the state of this goal's communication state machine from interaction with the server.
Definition: action_client.py:151
actionlib.action_client.ActionClient.result_sub
result_sub
Definition: action_client.py:534
actionlib.action_client.ActionClient.sub_queue_size
sub_queue_size
Definition: action_client.py:530
actionlib.action_client.CommStateMachine.action_goal
action_goal
Definition: action_client.py:305
actionlib.exceptions.ActionException
Definition: exceptions.py:31
actionlib.action_client.ActionClient.last_status_msg
last_status_msg
Definition: action_client.py:508
actionlib.action_client.GoalManager._generate_id
def _generate_id(self)
Definition: action_client.py:438
actionlib.action_client.CommStateMachine.latest_goal_status
latest_goal_status
Definition: action_client.py:313
actionlib.action_client.ActionClient.status_sub
status_sub
Definition: action_client.py:533
actionlib.action_client.GoalManager.statuses
statuses
Definition: action_client.py:425
actionlib.action_client.CommState
Definition: action_client.py:70
actionlib.action_client.CommStateMachine.__hash__
def __hash__(self)
@brieft Hash function for CommStateMachine
Definition: action_client.py:320
actionlib.action_client.GoalManager.cancel_fn
cancel_fn
Definition: action_client.py:448
actionlib.action_client.ActionClient.manager
manager
Definition: action_client.py:526
actionlib.action_client.CommStateMachine.__eq__
def __eq__(self, o)
Definition: action_client.py:316
actionlib.action_client.ActionClient.send_goal
def send_goal(self, goal, transition_cb=None, feedback_cb=None)
Sends a goal to the action server.
Definition: action_client.py:559
actionlib.action_client.CommStateMachine.mutex
mutex
Definition: action_client.py:312
actionlib.action_client.ActionClient.ActionSpec
ActionSpec
Definition: action_client.py:513
actionlib.action_client.ActionClient._status_cb
def _status_cb(self, msg)
Definition: action_client.py:633
actionlib.action_client.CommStateMachine.transition_cb
transition_cb
Definition: action_client.py:306
actionlib.action_client.ClientGoalHandle.get_result
def get_result(self)
Gets the result produced by the action server for this goal.
Definition: action_client.py:183
actionlib.action_client.TerminalState
Definition: action_client.py:82
actionlib.action_client.CommStateMachine.latest_result
latest_result
Definition: action_client.py:314
actionlib.action_client.ActionClient
Definition: action_client.py:498
actionlib.action_client.CommStateMachine
Definition: action_client.py:303
actionlib.action_client.ActionClient.ns
ns
Definition: action_client.py:507
actionlib.action_client.ActionClient.cancel_all_goals
def cancel_all_goals(self)
Cancels all goals currently running on the action server.
Definition: action_client.py:566
actionlib.action_client.CommStateMachine.set_state
def set_state(self, state)
Definition: action_client.py:323
actionlib.action_client.ActionClient.ActionGoal
ActionGoal
Definition: action_client.py:514
actionlib.action_client.CommStateMachine._mark_as_lost
def _mark_as_lost(self)
Definition: action_client.py:378
actionlib.action_client.CommStateMachine.update_status
def update_status(self, status_array)
Definition: action_client.py:331
actionlib.action_client.GoalManager.ActionResult
ActionResult
Definition: action_client.py:433
actionlib.action_client.CommStateMachine.transition_to
def transition_to(self, state)
Definition: action_client.py:370
actionlib.action_client.CommStateMachine.update_result
def update_result(self, action_result)
Definition: action_client.py:382
actionlib.action_client.GoalManager.send_goal_fn
send_goal_fn
Definition: action_client.py:426
actionlib.action_client.CommStateMachine.send_cancel_fn
send_cancel_fn
Definition: action_client.py:309
actionlib.action_client.ActionClient.ActionResult
ActionResult
Definition: action_client.py:515
actionlib.action_client.ClientGoalHandle.cancel
def cancel(self)
Sends a cancel message for this specific goal to the ActionServer.
Definition: action_client.py:138


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep, Mikael Arguedas
autogenerated on Fri May 19 2023 02:36:55