00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
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
00105
00106
00107
00108
00109
00110 class ClientGoalHandle:
00111
00112
00113
00114
00115 def __init__(self, comm_state_machine):
00116 self.comm_state_machine = comm_state_machine
00117
00118
00119
00120
00121 def __eq__(self, o):
00122 if not o:
00123 return False
00124 return self.comm_state_machine == o.comm_state_machine
00125
00126
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
00133 def __hash__(self):
00134 return hash(self.comm_state_machine)
00135
00136
00137
00138
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
00147
00148
00149
00150
00151
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
00159
00160
00161
00162
00163
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
00171
00172
00173
00174
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
00182
00183
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
00190 return None
00191 return self.comm_state_machine.latest_result.result
00192
00193
00194
00195
00196
00197
00198
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
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
00331
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
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
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
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
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
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
00412 if self.action_goal.goal_id.id != action_feedback.status.goal_id.id:
00413 return
00414
00415
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
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
00452
00453
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
00471
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
00479
00480
00481 def update_statuses(self, status_array):
00482
00483 with self.list_mutex:
00484
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
00501
00502
00503
00504
00505
00506
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
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551
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
00556
00557
00558
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
00564
00565
00566
00567
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
00574 def wait_for_action_server_to_start(self, timeout=rospy.Duration(0.0)):
00575 return self.wait_for_server(timeout)
00576
00577
00578
00579
00580
00581
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
00592
00593
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)