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


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep
autogenerated on Fri Aug 28 2015 10:04:41