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