32 from move_base.msg import *
33 rospy.init_node('foo')
36 from move_base.msg import *
37 from geometry_msgs.msg import *
38 g1 = MoveBaseGoal(PoseStamped(Header(frame_id = 'base_link'),
40 Quaternion(0, 0, 0, 1))))
41 g2 = MoveBaseGoal(PoseStamped(Header(frame_id = 'base_link'),
43 Quaternion(0, 0, 0, 1))))
45 client = ActionClient('move_base', MoveBaseAction)
47 h1 = client.send_goal(g1)
48 h2 = client.send_goal(g2)
49 client.cancel_all_goals()
56 from rospy
import Header
57 from actionlib_msgs.msg
import GoalID, GoalStatus, GoalStatusArray
64 for k, v
in C.__dict__.items():
65 if isinstance(v, int)
and v == n:
67 return "NO_SUCH_STATE_%d" % n
71 WAITING_FOR_GOAL_ACK = 0
74 WAITING_FOR_RESULT = 3
75 WAITING_FOR_CANCEL_ACK = 4
83 RECALLED = GoalStatus.RECALLED
84 REJECTED = GoalStatus.REJECTED
85 PREEMPTED = GoalStatus.PREEMPTED
86 ABORTED = GoalStatus.ABORTED
87 SUCCEEDED = GoalStatus.SUCCEEDED
88 LOST = GoalStatus.LOST
91 GoalStatus.to_string = classmethod(get_name_of_constant)
92 CommState.to_string = classmethod(get_name_of_constant)
93 TerminalState.to_string = classmethod(get_name_of_constant)
97 for s
in status_array.status_list:
98 if s.goal_id.id == id:
140 cancel_msg = GoalID(stamp=rospy.Time(0),
153 rospy.logerr(
"Trying to get_comm_state on an inactive ClientGoalHandle.")
154 return CommState.LOST
165 rospy.logerr(
"Trying to get_goal_status on an inactive ClientGoalHandle.")
166 return GoalStatus.PENDING
176 rospy.logerr(
"Trying to get_goal_status_text on an inactive ClientGoalHandle.")
177 return "ERROR: Trying to get_goal_status_text on an inactive ClientGoalHandle."
185 rospy.logerr(
"Trying to get_result on an inactive ClientGoalHandle.")
200 rospy.logerr(
"Trying to get_terminal_state on an inactive ClientGoalHandle.")
201 return GoalStatus.LOST
205 rospy.logwarn(
"Asking for the terminal state when we're in [%s]",
209 if goal_status
in [GoalStatus.PREEMPTED, GoalStatus.SUCCEEDED,
210 GoalStatus.ABORTED, GoalStatus.REJECTED,
211 GoalStatus.RECALLED, GoalStatus.LOST]:
214 rospy.logerr(
"Asking for a terminal state, but the goal status is %d", goal_status)
215 return GoalStatus.LOST
219 INVALID_TRANSITION = -2
221 CommState.WAITING_FOR_GOAL_ACK: {
222 GoalStatus.PENDING: CommState.PENDING,
223 GoalStatus.ACTIVE: CommState.ACTIVE,
224 GoalStatus.REJECTED: (CommState.PENDING, CommState.WAITING_FOR_RESULT),
225 GoalStatus.RECALLING: (CommState.PENDING, CommState.RECALLING),
226 GoalStatus.RECALLED: (CommState.PENDING, CommState.WAITING_FOR_RESULT),
227 GoalStatus.PREEMPTED: (CommState.ACTIVE, CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
228 GoalStatus.SUCCEEDED: (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
229 GoalStatus.ABORTED: (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
230 GoalStatus.PREEMPTING: (CommState.ACTIVE, CommState.PREEMPTING)},
232 GoalStatus.PENDING: NO_TRANSITION,
233 GoalStatus.ACTIVE: CommState.ACTIVE,
234 GoalStatus.REJECTED: CommState.WAITING_FOR_RESULT,
235 GoalStatus.RECALLING: CommState.RECALLING,
236 GoalStatus.RECALLED: (CommState.RECALLING, CommState.WAITING_FOR_RESULT),
237 GoalStatus.PREEMPTED: (CommState.ACTIVE, CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
238 GoalStatus.SUCCEEDED: (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
239 GoalStatus.ABORTED: (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
240 GoalStatus.PREEMPTING: (CommState.ACTIVE, CommState.PREEMPTING)},
242 GoalStatus.PENDING: INVALID_TRANSITION,
243 GoalStatus.ACTIVE: NO_TRANSITION,
244 GoalStatus.REJECTED: INVALID_TRANSITION,
245 GoalStatus.RECALLING: INVALID_TRANSITION,
246 GoalStatus.RECALLED: INVALID_TRANSITION,
247 GoalStatus.PREEMPTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
248 GoalStatus.SUCCEEDED: CommState.WAITING_FOR_RESULT,
249 GoalStatus.ABORTED: CommState.WAITING_FOR_RESULT,
250 GoalStatus.PREEMPTING: CommState.PREEMPTING},
251 CommState.WAITING_FOR_RESULT: {
252 GoalStatus.PENDING: INVALID_TRANSITION,
253 GoalStatus.ACTIVE: NO_TRANSITION,
254 GoalStatus.REJECTED: NO_TRANSITION,
255 GoalStatus.RECALLING: INVALID_TRANSITION,
256 GoalStatus.RECALLED: NO_TRANSITION,
257 GoalStatus.PREEMPTED: NO_TRANSITION,
258 GoalStatus.SUCCEEDED: NO_TRANSITION,
259 GoalStatus.ABORTED: NO_TRANSITION,
260 GoalStatus.PREEMPTING: INVALID_TRANSITION},
261 CommState.WAITING_FOR_CANCEL_ACK: {
262 GoalStatus.PENDING: NO_TRANSITION,
263 GoalStatus.ACTIVE: NO_TRANSITION,
264 GoalStatus.REJECTED: CommState.WAITING_FOR_RESULT,
265 GoalStatus.RECALLING: CommState.RECALLING,
266 GoalStatus.RECALLED: (CommState.RECALLING, CommState.WAITING_FOR_RESULT),
267 GoalStatus.PREEMPTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
268 GoalStatus.SUCCEEDED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
269 GoalStatus.ABORTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
270 GoalStatus.PREEMPTING: CommState.PREEMPTING},
271 CommState.RECALLING: {
272 GoalStatus.PENDING: INVALID_TRANSITION,
273 GoalStatus.ACTIVE: INVALID_TRANSITION,
274 GoalStatus.REJECTED: CommState.WAITING_FOR_RESULT,
275 GoalStatus.RECALLING: NO_TRANSITION,
276 GoalStatus.RECALLED: CommState.WAITING_FOR_RESULT,
277 GoalStatus.PREEMPTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
278 GoalStatus.SUCCEEDED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
279 GoalStatus.ABORTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
280 GoalStatus.PREEMPTING: CommState.PREEMPTING},
281 CommState.PREEMPTING: {
282 GoalStatus.PENDING: INVALID_TRANSITION,
283 GoalStatus.ACTIVE: INVALID_TRANSITION,
284 GoalStatus.REJECTED: INVALID_TRANSITION,
285 GoalStatus.RECALLING: INVALID_TRANSITION,
286 GoalStatus.RECALLED: INVALID_TRANSITION,
287 GoalStatus.PREEMPTED: CommState.WAITING_FOR_RESULT,
288 GoalStatus.SUCCEEDED: CommState.WAITING_FOR_RESULT,
289 GoalStatus.ABORTED: CommState.WAITING_FOR_RESULT,
290 GoalStatus.PREEMPTING: NO_TRANSITION},
292 GoalStatus.PENDING: INVALID_TRANSITION,
293 GoalStatus.ACTIVE: INVALID_TRANSITION,
294 GoalStatus.REJECTED: NO_TRANSITION,
295 GoalStatus.RECALLING: INVALID_TRANSITION,
296 GoalStatus.RECALLED: NO_TRANSITION,
297 GoalStatus.PREEMPTED: NO_TRANSITION,
298 GoalStatus.SUCCEEDED: NO_TRANSITION,
299 GoalStatus.ABORTED: NO_TRANSITION,
300 GoalStatus.PREEMPTING: INVALID_TRANSITION}}
304 def __init__(self, action_goal, transition_cb, feedback_cb, send_goal_fn, send_cancel_fn):
311 self.
state = CommState.WAITING_FOR_GOAL_ACK
317 return self.
action_goal.goal_id.id == o.action_goal.goal_id.id
324 rospy.logdebug(
"Transitioning CommState from %s to %s",
325 CommState.to_string(self.
state), CommState.to_string(state))
333 if self.
state == CommState.DONE:
340 if self.
state not in [CommState.WAITING_FOR_GOAL_ACK,
341 CommState.WAITING_FOR_RESULT,
349 if self.
state not in _transitions:
350 rospy.logerr(
"CommStateMachine is in a funny state: %i" % self.
state)
352 if status.status
not in _transitions[self.
state]:
353 rospy.logerr(
"Got an unknown status from the ActionServer: %i" % status.status)
355 next_state = _transitions[self.
state][status.status]
358 if next_state == NO_TRANSITION:
360 elif next_state == INVALID_TRANSITION:
361 rospy.logerr(
"Invalid goal status transition from %s to %s" %
362 (CommState.to_string(self.
state), GoalStatus.to_string(status.status)))
364 if hasattr(next_state,
'__getitem__'):
371 rospy.logdebug(
"Transitioning to %s (from %s, goal: %s)",
372 CommState.to_string(state), CommState.to_string(self.
state),
384 if self.
action_goal.goal_id.id != action_result.status.goal_id.id:
391 if self.
state in [CommState.WAITING_FOR_GOAL_ACK,
392 CommState.WAITING_FOR_CANCEL_ACK,
395 CommState.WAITING_FOR_RESULT,
397 CommState.PREEMPTING]:
399 status_array = GoalStatusArray()
400 status_array.status_list.append(action_result.status)
404 elif self.
state == CommState.DONE:
405 rospy.logerr(
"Got a result when we were already in the DONE state")
407 rospy.logerr(
"In a funny state: %i" % self.
state)
411 if self.
action_goal.goal_id.id != action_feedback.status.goal_id.id:
435 except AttributeError:
436 raise ActionException(
"Type is not an action spec: %s" % str(ActionSpec))
440 id, g_goal_id = g_goal_id, g_goal_id + 1
441 now = rospy.Time.now()
442 return GoalID(id=
"%s-%i-%.3f" % (rospy.get_caller_id(), id, now.to_sec()), stamp=now)
453 def init_goal(self, goal, transition_cb=None, feedback_cb=None):
454 action_goal = self.
ActionGoal(header=Header(),
457 action_goal.header.stamp = rospy.get_rostime()
463 self.
statuses.append(weakref.ref(csm))
473 live_statuses = [r()
for r
in self.
statuses]
474 live_statuses = [x
for x
in live_statuses
if x]
487 status.update_status(status_array)
491 status.update_result(action_result)
495 status.update_feedback(action_feedback)
517 except AttributeError:
518 raise ActionException(
"Type is not an action spec: %s" % str(ActionSpec))
559 def send_goal(self, goal, transition_cb=None, feedback_cb=None):
560 return self.
manager.init_goal(goal, transition_cb, feedback_cb)
567 cancel_msg = GoalID(stamp=rospy.Time.from_sec(0.0), id=
"")
577 cancel_msg = GoalID(stamp=time, id=
"")
591 timeout_time = rospy.get_rostime() + timeout
592 while not rospy.is_shutdown():
596 if self.
pub_goal.impl.has_connection(server_id)
and \
597 self.
pub_cancel.impl.has_connection(server_id):
611 result_sub_connections = self.
result_sub.impl.connections
612 result_sub_found =
False
613 for c
in result_sub_connections:
614 if c.callerid_pub == server_id:
615 result_sub_found =
True
616 feedback_sub_connections = self.
feedback_sub.impl.connections
617 feedback_sub_found =
False
618 for c
in feedback_sub_connections:
619 if c.callerid_pub == server_id:
620 feedback_sub_found =
True
622 if result_sub_found
and feedback_sub_found:
626 if timeout != rospy.Duration(0.0)
and rospy.get_rostime() >= timeout_time:
635 self.
manager.update_statuses(msg)
638 self.
manager.update_results(msg)
641 self.
manager.update_feedbacks(msg)