38 #ifndef ACTIONLIB__CLIENT__COMM_STATE_MACHINE_IMP_H_
39 #define ACTIONLIB__CLIENT__COMM_STATE_MACHINE_IMP_H_
48 template<
class ActionSpec>
50 TransitionCallback transition_cb,
51 FeedbackCallback feedback_cb)
52 : state_(CommState::WAITING_FOR_GOAL_ACK)
55 action_goal_ = action_goal;
56 transition_cb_ = transition_cb;
57 feedback_cb_ = feedback_cb;
61 template<
class ActionSpec>
68 template<
class ActionSpec>
74 template<
class ActionSpec>
77 return latest_goal_status_;
80 template<
class ActionSpec>
84 ResultConstPtr result;
87 result = ResultConstPtr(&(latest_result_->result),
d);
92 template<
class ActionSpec>
98 template<
class ActionSpec>
102 state_.toString().c_str(), state.
toString().c_str());
106 template<
class ActionSpec>
108 const std::vector<actionlib_msgs::GoalStatus> & status_vec)
const
110 for (
unsigned int i = 0; i < status_vec.size(); i++) {
111 if (status_vec[i].goal_id.id == action_goal_->goal_id.id) {
112 return &status_vec[i];
118 template<
class ActionSpec>
120 const ActionFeedbackConstPtr & action_feedback)
123 if (action_goal_->goal_id.id != action_feedback->status.goal_id.id) {
129 FeedbackConstPtr feedback(&(action_feedback->feedback),
d);
130 feedback_cb_(gh, feedback);
134 template<
class ActionSpec>
136 const ActionResultConstPtr & action_result)
139 if (action_goal_->goal_id.id != action_result->status.goal_id.id) {
142 latest_goal_status_ = action_result->status;
143 latest_result_ = action_result;
144 switch (state_.state_) {
154 actionlib_msgs::GoalStatusArrayPtr status_array(
new actionlib_msgs::GoalStatusArray());
155 status_array->status_list.push_back(action_result->status);
156 updateStatus(gh, status_array);
162 ROS_ERROR_NAMED(
"actionlib",
"Got a result when we were already in the DONE state");
break;
164 ROS_ERROR_NAMED(
"actionlib",
"In a funny comm state: %u", state_.state_);
break;
168 template<
class ActionSpec>
170 const actionlib_msgs::GoalStatusArrayConstPtr & status_array)
172 const actionlib_msgs::GoalStatus * goal_status = findGoalStatus(status_array->status_list);
181 latest_goal_status_ = *goal_status;
192 switch (state_.state_) {
196 switch (goal_status->status) {
197 case actionlib_msgs::GoalStatus::PENDING:
200 case actionlib_msgs::GoalStatus::ACTIVE:
203 case actionlib_msgs::GoalStatus::PREEMPTED:
208 case actionlib_msgs::GoalStatus::SUCCEEDED:
212 case actionlib_msgs::GoalStatus::ABORTED:
216 case actionlib_msgs::GoalStatus::REJECTED:
220 case actionlib_msgs::GoalStatus::RECALLED:
224 case actionlib_msgs::GoalStatus::PREEMPTING:
228 case actionlib_msgs::GoalStatus::RECALLING:
234 "BUG: Got an unknown status from the ActionServer. status = %u",
235 goal_status->status);
243 switch (goal_status->status) {
244 case actionlib_msgs::GoalStatus::PENDING:
246 case actionlib_msgs::GoalStatus::ACTIVE:
249 case actionlib_msgs::GoalStatus::PREEMPTED:
254 case actionlib_msgs::GoalStatus::SUCCEEDED:
258 case actionlib_msgs::GoalStatus::ABORTED:
262 case actionlib_msgs::GoalStatus::REJECTED:
265 case actionlib_msgs::GoalStatus::RECALLED:
269 case actionlib_msgs::GoalStatus::PREEMPTING:
273 case actionlib_msgs::GoalStatus::RECALLING:
278 "BUG: Got an unknown goal status from the ActionServer. status = %u",
279 goal_status->status);
286 switch (goal_status->status) {
287 case actionlib_msgs::GoalStatus::PENDING:
288 ROS_ERROR_NAMED(
"actionlib",
"Invalid transition from ACTIVE to PENDING");
break;
289 case actionlib_msgs::GoalStatus::ACTIVE:
291 case actionlib_msgs::GoalStatus::REJECTED:
292 ROS_ERROR_NAMED(
"actionlib",
"Invalid transition from ACTIVE to REJECTED");
break;
293 case actionlib_msgs::GoalStatus::RECALLING:
294 ROS_ERROR_NAMED(
"actionlib",
"Invalid transition from ACTIVE to RECALLING");
break;
295 case actionlib_msgs::GoalStatus::RECALLED:
296 ROS_ERROR_NAMED(
"actionlib",
"Invalid transition from ACTIVE to RECALLED");
break;
297 case actionlib_msgs::GoalStatus::PREEMPTED:
301 case actionlib_msgs::GoalStatus::SUCCEEDED:
302 case actionlib_msgs::GoalStatus::ABORTED:
304 case actionlib_msgs::GoalStatus::PREEMPTING:
308 "BUG: Got an unknown goal status from the ActionServer. status = %u",
309 goal_status->status);
316 switch (goal_status->status) {
317 case actionlib_msgs::GoalStatus::PENDING:
318 ROS_ERROR_NAMED(
"actionlib",
"Invalid Transition from WAITING_FOR_RESULT to PENDING");
320 case actionlib_msgs::GoalStatus::PREEMPTING:
321 ROS_ERROR_NAMED(
"actionlib",
"Invalid Transition from WAITING_FOR_RESULT to PREEMPTING");
323 case actionlib_msgs::GoalStatus::RECALLING:
324 ROS_ERROR_NAMED(
"actionlib",
"Invalid Transition from WAITING_FOR_RESULT to RECALLING");
326 case actionlib_msgs::GoalStatus::ACTIVE:
327 case actionlib_msgs::GoalStatus::PREEMPTED:
328 case actionlib_msgs::GoalStatus::SUCCEEDED:
329 case actionlib_msgs::GoalStatus::ABORTED:
330 case actionlib_msgs::GoalStatus::REJECTED:
331 case actionlib_msgs::GoalStatus::RECALLED:
335 "BUG: Got an unknown state from the ActionServer. status = %u",
336 goal_status->status);
343 switch (goal_status->status) {
344 case actionlib_msgs::GoalStatus::PENDING:
346 case actionlib_msgs::GoalStatus::ACTIVE:
348 case actionlib_msgs::GoalStatus::SUCCEEDED:
349 case actionlib_msgs::GoalStatus::ABORTED:
350 case actionlib_msgs::GoalStatus::PREEMPTED:
354 case actionlib_msgs::GoalStatus::RECALLED:
358 case actionlib_msgs::GoalStatus::REJECTED:
360 case actionlib_msgs::GoalStatus::PREEMPTING:
362 case actionlib_msgs::GoalStatus::RECALLING:
366 "BUG: Got an unknown state from the ActionServer. status = %u",
367 goal_status->status);
374 switch (goal_status->status) {
375 case actionlib_msgs::GoalStatus::PENDING:
376 ROS_ERROR_NAMED(
"actionlib",
"Invalid Transition from RECALLING to PENDING");
break;
377 case actionlib_msgs::GoalStatus::ACTIVE:
378 ROS_ERROR_NAMED(
"actionlib",
"Invalid Transition from RECALLING to ACTIVE");
break;
379 case actionlib_msgs::GoalStatus::SUCCEEDED:
380 case actionlib_msgs::GoalStatus::ABORTED:
381 case actionlib_msgs::GoalStatus::PREEMPTED:
385 case actionlib_msgs::GoalStatus::RECALLED:
388 case actionlib_msgs::GoalStatus::REJECTED:
390 case actionlib_msgs::GoalStatus::PREEMPTING:
392 case actionlib_msgs::GoalStatus::RECALLING:
396 "BUG: Got an unknown state from the ActionServer. status = %u",
397 goal_status->status);
404 switch (goal_status->status) {
405 case actionlib_msgs::GoalStatus::PENDING:
406 ROS_ERROR_NAMED(
"actionlib",
"Invalid Transition from PREEMPTING to PENDING");
break;
407 case actionlib_msgs::GoalStatus::ACTIVE:
408 ROS_ERROR_NAMED(
"actionlib",
"Invalid Transition from PREEMPTING to ACTIVE");
break;
409 case actionlib_msgs::GoalStatus::REJECTED:
410 ROS_ERROR_NAMED(
"actionlib",
"Invalid Transition from PREEMPTING to REJECTED");
break;
411 case actionlib_msgs::GoalStatus::RECALLING:
412 ROS_ERROR_NAMED(
"actionlib",
"Invalid Transition from PREEMPTING to RECALLING");
break;
413 case actionlib_msgs::GoalStatus::RECALLED:
414 ROS_ERROR_NAMED(
"actionlib",
"Invalid Transition from PREEMPTING to RECALLED");
break;
416 case actionlib_msgs::GoalStatus::PREEMPTED:
417 case actionlib_msgs::GoalStatus::SUCCEEDED:
418 case actionlib_msgs::GoalStatus::ABORTED:
420 case actionlib_msgs::GoalStatus::PREEMPTING:
424 "BUG: Got an unknown state from the ActionServer. status = %u",
425 goal_status->status);
432 switch (goal_status->status) {
433 case actionlib_msgs::GoalStatus::PENDING:
434 ROS_ERROR_NAMED(
"actionlib",
"Invalid Transition from DONE to PENDING");
break;
435 case actionlib_msgs::GoalStatus::ACTIVE:
436 ROS_ERROR_NAMED(
"actionlib",
"Invalid Transition from DONE to ACTIVE");
break;
437 case actionlib_msgs::GoalStatus::RECALLING:
438 ROS_ERROR_NAMED(
"actionlib",
"Invalid Transition from DONE to RECALLING");
break;
439 case actionlib_msgs::GoalStatus::PREEMPTING:
440 ROS_ERROR_NAMED(
"actionlib",
"Invalid Transition from DONE to PREEMPTING");
break;
441 case actionlib_msgs::GoalStatus::PREEMPTED:
442 case actionlib_msgs::GoalStatus::SUCCEEDED:
443 case actionlib_msgs::GoalStatus::ABORTED:
444 case actionlib_msgs::GoalStatus::RECALLED:
445 case actionlib_msgs::GoalStatus::REJECTED:
449 "BUG: Got an unknown state from the ActionServer. status = %u",
450 goal_status->status);
456 ROS_ERROR_NAMED(
"actionlib",
"In a funny comm state: %u", state_.state_);
462 template<
class ActionSpec>
466 latest_goal_status_.status = actionlib_msgs::GoalStatus::LOST;
470 template<
class ActionSpec>
474 transitionToState(gh, CommState(next_state));
477 template<
class ActionSpec>
480 ROS_DEBUG_NAMED(
"actionlib",
"Trying to transition to %s", next_state.toString().c_str());
481 setCommState(next_state);
482 if (transition_cb_) {
489 #endif // ACTIONLIB__CLIENT__COMM_STATE_MACHINE_IMP_H_