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 *
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
00101
00102
00103
00104
00105
00106 class ClientGoalHandle:
00107
00108
00109
00110
00111 def __init__(self, comm_state_machine):
00112 self.comm_state_machine = comm_state_machine
00113
00114
00115
00116
00117 def __eq__(self, o):
00118 if not o:
00119 return False
00120 return self.comm_state_machine == o.comm_state_machine
00121
00122
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
00130
00131
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
00140
00141
00142
00143
00144
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
00152
00153
00154
00155
00156
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
00164
00165
00166
00167
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
00175
00176
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
00183 return None
00184 return self.comm_state_machine.latest_result.result
00185
00186
00187
00188
00189
00190
00191
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
00323
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
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
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
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
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
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
00404 if self.action_goal.goal_id.id != action_feedback.status.goal_id.id:
00405 return
00406
00407
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
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
00444
00445
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
00464
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
00473
00474
00475 def update_statuses(self, status_array):
00476 live_statuses = []
00477
00478 with self.list_mutex:
00479
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
00496
00497
00498
00499
00500
00501
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
00528
00529
00530
00531
00532
00533
00534
00535
00536
00537
00538
00539
00540
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
00545
00546
00547
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
00554
00555
00556
00557
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
00565 def wait_for_action_server_to_start(self, timeout = rospy.Duration(0.0)):
00566 return self.wait_for_server(timeout)
00567
00568
00569
00570
00571
00572
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
00583
00584
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