action_client.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: Stuart Glaser
00030 '''
00031 Example:
00032 
00033 from move_base.msg import *
00034 rospy.init_node('foo')
00035 
00036 
00037 from move_base.msg import *
00038 from geometry_msgs.msg import *
00039 g1 = MoveBaseGoal(PoseStamped(Header(frame_id = 'base_link'),
00040                               Pose(Point(2, 0, 0),
00041                                    Quaternion(0, 0, 0, 1))))
00042 g2 = MoveBaseGoal(PoseStamped(Header(frame_id = 'base_link'),
00043                               Pose(Point(5, 0, 0),
00044                                    Quaternion(0, 0, 0, 1))))
00045 
00046 client = ActionClient('move_base', MoveBaseAction)
00047 
00048 h1 = client.send_goal(g1)
00049 h2 = client.send_goal(g2)
00050 client.cancel_all_goals()
00051 '''
00052 
00053 import threading
00054 import weakref
00055 import time
00056 import rospy
00057 from rospy import Header
00058 from actionlib_msgs.msg import GoalID, GoalStatus, GoalStatusArray
00059 from actionlib.exceptions import ActionException
00060 
00061 g_goal_id = 1
00062 
00063 
00064 def get_name_of_constant(C, n):
00065     for k, v in C.__dict__.items():
00066         if isinstance(v, int) and v == n:
00067             return k
00068     return "NO_SUCH_STATE_%d" % n
00069 
00070 
00071 class CommState(object):
00072     WAITING_FOR_GOAL_ACK = 0
00073     PENDING = 1
00074     ACTIVE = 2
00075     WAITING_FOR_RESULT = 3
00076     WAITING_FOR_CANCEL_ACK = 4
00077     RECALLING = 5
00078     PREEMPTING = 6
00079     DONE = 7
00080     LOST = 8
00081 
00082 
00083 class TerminalState(object):
00084     RECALLED = GoalStatus.RECALLED
00085     REJECTED = GoalStatus.REJECTED
00086     PREEMPTED = GoalStatus.PREEMPTED
00087     ABORTED = GoalStatus.ABORTED
00088     SUCCEEDED = GoalStatus.SUCCEEDED
00089     LOST = GoalStatus.LOST
00090 
00091 
00092 GoalStatus.to_string = classmethod(get_name_of_constant)
00093 CommState.to_string = classmethod(get_name_of_constant)
00094 TerminalState.to_string = classmethod(get_name_of_constant)
00095 
00096 
00097 def _find_status_by_goal_id(status_array, id):
00098     for s in status_array.status_list:
00099         if s.goal_id.id == id:
00100             return s
00101     return None
00102 
00103 
00104 ## @brief Client side handle to monitor goal progress.
00105 ##
00106 ## A ClientGoalHandle is a reference counted object that is used to
00107 ## manipulate and monitor the progress of an already dispatched
00108 ## goal. Once all the goal handles go out of scope (or are reset), an
00109 ## ActionClient stops maintaining state for that goal.
00110 class ClientGoalHandle:
00111     ## @brief Internal use only
00112     ##
00113     ## ClientGoalHandle objects should be created by the action
00114     ## client.  You should never need to construct one yourself.
00115     def __init__(self, comm_state_machine):
00116         self.comm_state_machine = comm_state_machine
00117 
00118         # print "GH created.  id = %.3f" % self.comm_state_machine.action_goal.goal_id.stamp.to_sec()
00119 
00120     ## @brief True iff the two ClientGoalHandle's are tracking the same goal
00121     def __eq__(self, o):
00122         if not o:
00123             return False
00124         return self.comm_state_machine == o.comm_state_machine
00125 
00126     ## @brief True iff the two ClientGoalHandle's are tracking different goals
00127     def __ne__(self, o):
00128         if not o:
00129             return True
00130         return not (self.comm_state_machine == o.comm_state_machine)
00131 
00132     ## @brieft Hash function for ClientGoalHandle
00133     def __hash__(self):
00134         return hash(self.comm_state_machine)
00135 
00136     ## @brief Sends a cancel message for this specific goal to the ActionServer.
00137     ##
00138     ## Also transitions the client state to WAITING_FOR_CANCEL_ACK
00139     def cancel(self):
00140         with self.comm_state_machine.mutex:
00141             cancel_msg = GoalID(stamp=rospy.Time(0),
00142                                 id=self.comm_state_machine.action_goal.goal_id.id)
00143             self.comm_state_machine.send_cancel_fn(cancel_msg)
00144             self.comm_state_machine.transition_to(CommState.WAITING_FOR_CANCEL_ACK)
00145 
00146     ## @brief Get the state of this goal's communication state machine from interaction with the server
00147     ##
00148     ## Possible States are: WAITING_FOR_GOAL_ACK, PENDING, ACTIVE, WAITING_FOR_RESULT,
00149     ##                      WAITING_FOR_CANCEL_ACK, RECALLING, PREEMPTING, DONE
00150     ##
00151     ## @return The current goal's communication state with the server
00152     def get_comm_state(self):
00153         if not self.comm_state_machine:
00154             rospy.logerr("Trying to get_comm_state on an inactive ClientGoalHandle.")
00155             return CommState.LOST
00156         return self.comm_state_machine.state
00157 
00158     ## @brief Returns the current status of the goal.
00159     ##
00160     ## Possible states are listed in the enumeration in the
00161     ## actionlib_msgs/GoalStatus message.
00162     ##
00163     ## @return The current status of the goal.
00164     def get_goal_status(self):
00165         if not self.comm_state_machine:
00166             rospy.logerr("Trying to get_goal_status on an inactive ClientGoalHandle.")
00167             return GoalStatus.PENDING
00168         return self.comm_state_machine.latest_goal_status.status
00169 
00170     ## @brief Returns the current status text of the goal.
00171     ##
00172     ## The text is sent by the action server.
00173     ##
00174     ## @return The current status text of the goal.
00175     def get_goal_status_text(self):
00176         if not self.comm_state_machine:
00177             rospy.logerr("Trying to get_goal_status_text on an inactive ClientGoalHandle.")
00178             return "ERROR: Trying to get_goal_status_text on an inactive ClientGoalHandle."
00179         return self.comm_state_machine.latest_goal_status.text
00180 
00181     ## @brief Gets the result produced by the action server for this goal.
00182     ##
00183     ## @return None if no result was receieved.  Otherwise the goal's result as a *Result message.
00184     def get_result(self):
00185         if not self.comm_state_machine:
00186             rospy.logerr("Trying to get_result on an inactive ClientGoalHandle.")
00187             return None
00188         if not self.comm_state_machine.latest_result:
00189             # rospy.logerr("Trying to get_result on a ClientGoalHandle when no result has been received.")
00190             return None
00191         return self.comm_state_machine.latest_result.result
00192 
00193     ## @brief Gets the terminal state information for this goal.
00194     ##
00195     ## Possible States Are: RECALLED, REJECTED, PREEMPTED, ABORTED, SUCCEEDED, LOST
00196     ## This call only makes sense if CommState==DONE. This will send ROS_WARNs if we're not in DONE
00197     ##
00198     ## @return The terminal state as an integer from the GoalStatus message.
00199     def get_terminal_state(self):
00200         if not self.comm_state_machine:
00201             rospy.logerr("Trying to get_terminal_state on an inactive ClientGoalHandle.")
00202             return GoalStatus.LOST
00203 
00204         with self.comm_state_machine.mutex:
00205             if self.comm_state_machine.state != CommState.DONE:
00206                 rospy.logwarn("Asking for the terminal state when we're in [%s]",
00207                               CommState.to_string(self.comm_state_machine.state))
00208 
00209             goal_status = self.comm_state_machine.latest_goal_status.status
00210             if goal_status in [GoalStatus.PREEMPTED, GoalStatus.SUCCEEDED,
00211                                GoalStatus.ABORTED, GoalStatus.REJECTED,
00212                                GoalStatus.RECALLED, GoalStatus.LOST]:
00213                 return goal_status
00214 
00215             rospy.logerr("Asking for a terminal state, but the goal status is %d", goal_status)
00216             return GoalStatus.LOST
00217 
00218 
00219 NO_TRANSITION = -1
00220 INVALID_TRANSITION = -2
00221 _transitions = {
00222     CommState.WAITING_FOR_GOAL_ACK: {
00223         GoalStatus.PENDING:    CommState.PENDING,
00224         GoalStatus.ACTIVE:     CommState.ACTIVE,
00225         GoalStatus.REJECTED:   (CommState.PENDING, CommState.WAITING_FOR_RESULT),
00226         GoalStatus.RECALLING:  (CommState.PENDING, CommState.RECALLING),
00227         GoalStatus.RECALLED:   (CommState.PENDING, CommState.WAITING_FOR_RESULT),
00228         GoalStatus.PREEMPTED:  (CommState.ACTIVE, CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
00229         GoalStatus.SUCCEEDED:  (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
00230         GoalStatus.ABORTED:    (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
00231         GoalStatus.PREEMPTING: (CommState.ACTIVE, CommState.PREEMPTING)},
00232     CommState.PENDING: {
00233         GoalStatus.PENDING:    NO_TRANSITION,
00234         GoalStatus.ACTIVE:     CommState.ACTIVE,
00235         GoalStatus.REJECTED:   CommState.WAITING_FOR_RESULT,
00236         GoalStatus.RECALLING:  CommState.RECALLING,
00237         GoalStatus.RECALLED:   (CommState.RECALLING, CommState.WAITING_FOR_RESULT),
00238         GoalStatus.PREEMPTED:  (CommState.ACTIVE, CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
00239         GoalStatus.SUCCEEDED:  (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
00240         GoalStatus.ABORTED:    (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
00241         GoalStatus.PREEMPTING: (CommState.ACTIVE, CommState.PREEMPTING)},
00242     CommState.ACTIVE: {
00243         GoalStatus.PENDING:    INVALID_TRANSITION,
00244         GoalStatus.ACTIVE:     NO_TRANSITION,
00245         GoalStatus.REJECTED:   INVALID_TRANSITION,
00246         GoalStatus.RECALLING:  INVALID_TRANSITION,
00247         GoalStatus.RECALLED:   INVALID_TRANSITION,
00248         GoalStatus.PREEMPTED:  (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
00249         GoalStatus.SUCCEEDED:  CommState.WAITING_FOR_RESULT,
00250         GoalStatus.ABORTED:    CommState.WAITING_FOR_RESULT,
00251         GoalStatus.PREEMPTING: CommState.PREEMPTING},
00252     CommState.WAITING_FOR_RESULT: {
00253         GoalStatus.PENDING:    INVALID_TRANSITION,
00254         GoalStatus.ACTIVE:     NO_TRANSITION,
00255         GoalStatus.REJECTED:   NO_TRANSITION,
00256         GoalStatus.RECALLING:  INVALID_TRANSITION,
00257         GoalStatus.RECALLED:   NO_TRANSITION,
00258         GoalStatus.PREEMPTED:  NO_TRANSITION,
00259         GoalStatus.SUCCEEDED:  NO_TRANSITION,
00260         GoalStatus.ABORTED:    NO_TRANSITION,
00261         GoalStatus.PREEMPTING: INVALID_TRANSITION},
00262     CommState.WAITING_FOR_CANCEL_ACK: {
00263         GoalStatus.PENDING:    NO_TRANSITION,
00264         GoalStatus.ACTIVE:     NO_TRANSITION,
00265         GoalStatus.REJECTED:   CommState.WAITING_FOR_RESULT,
00266         GoalStatus.RECALLING:  CommState.RECALLING,
00267         GoalStatus.RECALLED:   (CommState.RECALLING, CommState.WAITING_FOR_RESULT),
00268         GoalStatus.PREEMPTED:  (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
00269         GoalStatus.SUCCEEDED:  (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
00270         GoalStatus.ABORTED:    (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
00271         GoalStatus.PREEMPTING: CommState.PREEMPTING},
00272     CommState.RECALLING: {
00273         GoalStatus.PENDING:    INVALID_TRANSITION,
00274         GoalStatus.ACTIVE:     INVALID_TRANSITION,
00275         GoalStatus.REJECTED:   CommState.WAITING_FOR_RESULT,
00276         GoalStatus.RECALLING:  NO_TRANSITION,
00277         GoalStatus.RECALLED:   CommState.WAITING_FOR_RESULT,
00278         GoalStatus.PREEMPTED:  (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
00279         GoalStatus.SUCCEEDED:  (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
00280         GoalStatus.ABORTED:    (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
00281         GoalStatus.PREEMPTING: CommState.PREEMPTING},
00282     CommState.PREEMPTING: {
00283         GoalStatus.PENDING:    INVALID_TRANSITION,
00284         GoalStatus.ACTIVE:     INVALID_TRANSITION,
00285         GoalStatus.REJECTED:   INVALID_TRANSITION,
00286         GoalStatus.RECALLING:  INVALID_TRANSITION,
00287         GoalStatus.RECALLED:   INVALID_TRANSITION,
00288         GoalStatus.PREEMPTED:  CommState.WAITING_FOR_RESULT,
00289         GoalStatus.SUCCEEDED:  CommState.WAITING_FOR_RESULT,
00290         GoalStatus.ABORTED:    CommState.WAITING_FOR_RESULT,
00291         GoalStatus.PREEMPTING: NO_TRANSITION},
00292     CommState.DONE: {
00293         GoalStatus.PENDING:    INVALID_TRANSITION,
00294         GoalStatus.ACTIVE:     INVALID_TRANSITION,
00295         GoalStatus.REJECTED:   NO_TRANSITION,
00296         GoalStatus.RECALLING:  INVALID_TRANSITION,
00297         GoalStatus.RECALLED:   NO_TRANSITION,
00298         GoalStatus.PREEMPTED:  NO_TRANSITION,
00299         GoalStatus.SUCCEEDED:  NO_TRANSITION,
00300         GoalStatus.ABORTED:    NO_TRANSITION,
00301         GoalStatus.PREEMPTING: INVALID_TRANSITION}}
00302 
00303 
00304 class CommStateMachine:
00305     def __init__(self, action_goal, transition_cb, feedback_cb, send_goal_fn, send_cancel_fn):
00306         self.action_goal = action_goal
00307         self.transition_cb = transition_cb
00308         self.feedback_cb = feedback_cb
00309         self.send_goal_fn = send_goal_fn
00310         self.send_cancel_fn = send_cancel_fn
00311 
00312         self.state = CommState.WAITING_FOR_GOAL_ACK
00313         self.mutex = threading.RLock()
00314         self.latest_goal_status = GoalStatus(status=GoalStatus.PENDING)
00315         self.latest_result = None
00316 
00317     def __eq__(self, o):
00318         return self.action_goal.goal_id.id == o.action_goal.goal_id.id
00319 
00320     ## @brieft Hash function for CommStateMachine
00321     def __hash__(self):
00322         return hash(self.action_goal.goal_id.id)
00323 
00324     def set_state(self, state):
00325         rospy.logdebug("Transitioning CommState from %s to %s",
00326                        CommState.to_string(self.state), CommState.to_string(state))
00327         self.state = state
00328 
00329     ##
00330     ## @param gh ClientGoalHandle
00331     ## @param status_array actionlib_msgs/GoalStatusArray
00332     def update_status(self, status_array):
00333         with self.mutex:
00334             if self.state == CommState.DONE:
00335                 return
00336 
00337             status = _find_status_by_goal_id(status_array, self.action_goal.goal_id.id)
00338 
00339             # You mean you haven't heard of me?
00340             if not status:
00341                 if self.state not in [CommState.WAITING_FOR_GOAL_ACK,
00342                                       CommState.WAITING_FOR_RESULT,
00343                                       CommState.DONE]:
00344                     self._mark_as_lost()
00345                 return
00346 
00347             self.latest_goal_status = status
00348 
00349             # Determines the next state from the lookup table
00350             if self.state not in _transitions:
00351                 rospy.logerr("CommStateMachine is in a funny state: %i" % self.state)
00352                 return
00353             if status.status not in _transitions[self.state]:
00354                 rospy.logerr("Got an unknown status from the ActionServer: %i" % status.status)
00355                 return
00356             next_state = _transitions[self.state][status.status]
00357 
00358             # Knowing the next state, what should we do?
00359             if next_state == NO_TRANSITION:
00360                 pass
00361             elif next_state == INVALID_TRANSITION:
00362                 rospy.logerr("Invalid goal status transition from %s to %s" %
00363                              (CommState.to_string(self.state), GoalStatus.to_string(status.status)))
00364             else:
00365                 if hasattr(next_state, '__getitem__'):
00366                     for s in next_state:
00367                         self.transition_to(s)
00368                 else:
00369                     self.transition_to(next_state)
00370 
00371     def transition_to(self, state):
00372         rospy.logdebug("Transitioning to %s (from %s, goal: %s)",
00373                        CommState.to_string(state), CommState.to_string(self.state),
00374                        self.action_goal.goal_id.id)
00375         self.state = state
00376         if self.transition_cb:
00377             self.transition_cb(ClientGoalHandle(self))
00378 
00379     def _mark_as_lost(self):
00380         self.latest_goal_status.status = GoalStatus.LOST
00381         self.transition_to(CommState.DONE)
00382 
00383     def update_result(self, action_result):
00384         # Might not be for us
00385         if self.action_goal.goal_id.id != action_result.status.goal_id.id:
00386             return
00387 
00388         with self.mutex:
00389             self.latest_goal_status = action_result.status
00390             self.latest_result = action_result
00391 
00392             if self.state in [CommState.WAITING_FOR_GOAL_ACK,
00393                               CommState.WAITING_FOR_CANCEL_ACK,
00394                               CommState.PENDING,
00395                               CommState.ACTIVE,
00396                               CommState.WAITING_FOR_RESULT,
00397                               CommState.RECALLING,
00398                               CommState.PREEMPTING]:
00399                 # Stuffs the goal status in the result into a GoalStatusArray
00400                 status_array = GoalStatusArray()
00401                 status_array.status_list.append(action_result.status)
00402                 self.update_status(status_array)
00403 
00404                 self.transition_to(CommState.DONE)
00405             elif self.state == CommState.DONE:
00406                 rospy.logerr("Got a result when we were already in the DONE state")
00407             else:
00408                 rospy.logerr("In a funny state: %i" % self.state)
00409 
00410     def update_feedback(self, action_feedback):
00411         # Might not be for us
00412         if self.action_goal.goal_id.id != action_feedback.status.goal_id.id:
00413             return
00414 
00415         # with self.mutex:
00416         if self.feedback_cb and self.state != CommState.DONE:
00417             self.feedback_cb(ClientGoalHandle(self), action_feedback.feedback)
00418 
00419 
00420 class GoalManager:
00421 
00422     # statuses - a list of weak references to CommStateMachine objects
00423 
00424     def __init__(self, ActionSpec):
00425         self.list_mutex = threading.RLock()
00426         self.statuses = []
00427         self.send_goal_fn = None
00428 
00429         try:
00430             a = ActionSpec()
00431 
00432             self.ActionSpec = ActionSpec
00433             self.ActionGoal = type(a.action_goal)
00434             self.ActionResult = type(a.action_result)
00435             self.ActionFeedback = type(a.action_feedback)
00436         except AttributeError:
00437             raise ActionException("Type is not an action spec: %s" % str(ActionSpec))
00438 
00439     def _generate_id(self):
00440         global g_goal_id
00441         id, g_goal_id = g_goal_id, g_goal_id + 1
00442         now = rospy.Time.now()
00443         return GoalID(id="%s-%i-%.3f" % (rospy.get_caller_id(), id, now.to_sec()), stamp=now)
00444 
00445     def register_send_goal_fn(self, fn):
00446         self.send_goal_fn = fn
00447 
00448     def register_cancel_fn(self, fn):
00449         self.cancel_fn = fn
00450 
00451     ## Sends off a goal and starts tracking its status.
00452     ##
00453     ## @return ClientGoalHandle for the sent goal.
00454     def init_goal(self, goal, transition_cb=None, feedback_cb=None):
00455         action_goal = self.ActionGoal(header=Header(),
00456                                       goal_id=self._generate_id(),
00457                                       goal=goal)
00458         action_goal.header.stamp = rospy.get_rostime()
00459 
00460         csm = CommStateMachine(action_goal, transition_cb, feedback_cb,
00461                                self.send_goal_fn, self.cancel_fn)
00462 
00463         with self.list_mutex:
00464             self.statuses.append(weakref.ref(csm))
00465 
00466         self.send_goal_fn(action_goal)
00467 
00468         return ClientGoalHandle(csm)
00469 
00470     # Pulls out the statuses that are still live (creating strong
00471     # references to them)
00472     def _get_live_statuses(self):
00473         with self.list_mutex:
00474             live_statuses = [r() for r in self.statuses]
00475             live_statuses = filter(lambda x: x, live_statuses)
00476             return live_statuses
00477 
00478     ## Updates the statuses of all goals from the information in status_array.
00479     ##
00480     ## @param status_array (\c actionlib_msgs/GoalStatusArray)
00481     def update_statuses(self, status_array):
00482 
00483         with self.list_mutex:
00484             # Garbage collects dead status objects
00485             self.statuses = [r for r in self.statuses if r()]
00486 
00487         for status in self._get_live_statuses():
00488             status.update_status(status_array)
00489 
00490     def update_results(self, action_result):
00491         for status in self._get_live_statuses():
00492             status.update_result(action_result)
00493 
00494     def update_feedbacks(self, action_feedback):
00495         for status in self._get_live_statuses():
00496             status.update_feedback(action_feedback)
00497 
00498 
00499 class ActionClient:
00500     ## @brief Constructs an ActionClient and opens connections to an ActionServer.
00501     ##
00502     ## @param ns The namespace in which to access the action.  For
00503     ## example, the "goal" topic should occur under ns/goal
00504     ##
00505     ## @param ActionSpec The *Action message type.  The ActionClient
00506     ## will grab the other message types from this type.
00507     def __init__(self, ns, ActionSpec):
00508         self.ns = ns
00509         self.last_status_msg = None
00510 
00511         try:
00512             a = ActionSpec()
00513 
00514             self.ActionSpec = ActionSpec
00515             self.ActionGoal = type(a.action_goal)
00516             self.ActionResult = type(a.action_result)
00517             self.ActionFeedback = type(a.action_feedback)
00518         except AttributeError:
00519             raise ActionException("Type is not an action spec: %s" % str(ActionSpec))
00520 
00521         self.pub_queue_size = rospy.get_param('actionlib_client_pub_queue_size', 10)
00522         if self.pub_queue_size < 0:
00523             self.pub_queue_size = 10
00524         self.pub_goal = rospy.Publisher(rospy.remap_name(ns) + '/goal', self.ActionGoal, queue_size=self.pub_queue_size)
00525         self.pub_cancel = rospy.Publisher(rospy.remap_name(ns) + '/cancel', GoalID, queue_size=self.pub_queue_size)
00526 
00527         self.manager = GoalManager(ActionSpec)
00528         self.manager.register_send_goal_fn(self.pub_goal.publish)
00529         self.manager.register_cancel_fn(self.pub_cancel.publish)
00530 
00531         self.sub_queue_size = rospy.get_param('actionlib_client_sub_queue_size', -1)
00532         if self.sub_queue_size < 0:
00533             self.sub_queue_size = None
00534         self.status_sub = rospy.Subscriber(rospy.remap_name(ns) + '/status', GoalStatusArray, callback=self._status_cb, queue_size=self.sub_queue_size)
00535         self.result_sub = rospy.Subscriber(rospy.remap_name(ns) + '/result', self.ActionResult, callback=self._result_cb, queue_size=self.sub_queue_size)
00536         self.feedback_sub = rospy.Subscriber(rospy.remap_name(ns) + '/feedback', self.ActionFeedback, callback=self._feedback_cb, queue_size=self.sub_queue_size)
00537 
00538     ## @brief Sends a goal to the action server
00539     ##
00540     ## @param goal An instance of the *Goal message.
00541     ##
00542     ## @param transition_cb Callback that gets called on every client
00543     ## state transition for the sent goal.  It should take in a
00544     ## ClientGoalHandle as an argument.
00545     ##
00546     ## @param feedback_cb Callback that gets called every time
00547     ## feedback is received for the sent goal.  It takes two
00548     ## parameters: a ClientGoalHandle and an instance of the *Feedback
00549     ## message.
00550     ##
00551     ## @return ClientGoalHandle for the sent goal.
00552     def send_goal(self, goal, transition_cb=None, feedback_cb=None):
00553         return self.manager.init_goal(goal, transition_cb, feedback_cb)
00554 
00555     ## @brief Cancels all goals currently running on the action server.
00556     ##
00557     ## Preempts all goals running on the action server at the point
00558     ## that the cancel message is serviced by the action server.
00559     def cancel_all_goals(self):
00560         cancel_msg = GoalID(stamp=rospy.Time.from_sec(0.0), id="")
00561         self.pub_cancel.publish(cancel_msg)
00562 
00563     ## @brief Cancels all goals prior to a given timestamp
00564     ##
00565     ## This preempts all goals running on the action server for which the
00566     ## time stamp is earlier than the specified time stamp
00567     ## this message is serviced by the ActionServer.
00568 
00569     def cancel_goals_at_and_before_time(self, time):
00570         cancel_msg = GoalID(stamp=time, id="")
00571         self.pub_cancel.publish(cancel_msg)
00572 
00573     ## @brief [Deprecated] Use wait_for_server
00574     def wait_for_action_server_to_start(self, timeout=rospy.Duration(0.0)):
00575         return self.wait_for_server(timeout)
00576 
00577     ## @brief Waits for the ActionServer to connect to this client
00578     ##
00579     ## Often, it can take a second for the action server & client to negotiate
00580     ## a connection, thus, risking the first few goals to be dropped. This call lets
00581     ## the user wait until the network connection to the server is negotiated
00582     def wait_for_server(self, timeout=rospy.Duration(0.0)):
00583         started = False
00584         timeout_time = rospy.get_rostime() + timeout
00585         while not rospy.is_shutdown():
00586             if self.last_status_msg:
00587                 server_id = self.last_status_msg._connection_header['callerid']
00588 
00589                 if self.pub_goal.impl.has_connection(server_id) and \
00590                         self.pub_cancel.impl.has_connection(server_id):
00591                     # We'll also check that all of the subscribers have at least
00592                     # one publisher, this isn't a perfect check, but without
00593                     # publisher callbacks... it'll have to do
00594                     status_num_pubs = 0
00595                     for stat in self.status_sub.impl.get_stats()[1]:
00596                         if stat[4]:
00597                             status_num_pubs += 1
00598 
00599                     result_num_pubs = 0
00600                     for stat in self.result_sub.impl.get_stats()[1]:
00601                         if stat[4]:
00602                             result_num_pubs += 1
00603 
00604                     feedback_num_pubs = 0
00605                     for stat in self.feedback_sub.impl.get_stats()[1]:
00606                         if stat[4]:
00607                             feedback_num_pubs += 1
00608 
00609                     if status_num_pubs > 0 and result_num_pubs > 0 and feedback_num_pubs > 0:
00610                         started = True
00611                         break
00612 
00613             if timeout != rospy.Duration(0.0) and rospy.get_rostime() >= timeout_time:
00614                 break
00615 
00616             time.sleep(0.01)
00617 
00618         return started
00619 
00620     def _status_cb(self, msg):
00621         self.last_status_msg = msg
00622         self.manager.update_statuses(msg)
00623 
00624     def _result_cb(self, msg):
00625         self.manager.update_results(msg)
00626 
00627     def _feedback_cb(self, msg):
00628         self.manager.update_feedbacks(msg)


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