38 #ifndef ACTIONLIB__CLIENT__COMM_STATE_MACHINE_IMP_H_ 39 #define ACTIONLIB__CLIENT__COMM_STATE_MACHINE_IMP_H_ 48 template<
class ActionSpec>
61 template<
class ActionSpec>
68 template<
class ActionSpec>
74 template<
class ActionSpec>
80 template<
class ActionSpec>
84 ResultConstPtr result;
92 template<
class ActionSpec>
98 template<
class ActionSpec>
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);
134 template<
class ActionSpec>
136 const ActionResultConstPtr & action_result)
139 if (
action_goal_->goal_id.id != action_result->status.goal_id.id) {
154 actionlib_msgs::GoalStatusArrayPtr status_array(
new actionlib_msgs::GoalStatusArray());
155 status_array->status_list.push_back(action_result->status);
162 ROS_ERROR_NAMED(
"actionlib",
"Got a result when we were already in the DONE 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);
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_RESUT to PENDING");
320 case actionlib_msgs::GoalStatus::PREEMPTING:
321 ROS_ERROR_NAMED(
"actionlib",
"Invalid Transition from WAITING_FOR_RESUT to PREEMPTING");
323 case actionlib_msgs::GoalStatus::RECALLING:
324 ROS_ERROR_NAMED(
"actionlib",
"Invalid Transition from WAITING_FOR_RESUT 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);
462 template<
class ActionSpec>
470 template<
class ActionSpec>
477 template<
class ActionSpec>
489 #endif // ACTIONLIB__CLIENT__COMM_STATE_MACHINE_IMP_H_
ActionGoalConstPtr getActionGoal() const
void transitionToState(GoalHandleT &gh, const CommState::StateEnum &next_state)
#define ROS_WARN_NAMED(name,...)
ActionResultConstPtr latest_result_
void updateFeedback(GoalHandleT &gh, const ActionFeedbackConstPtr &action_feedback)
actionlib_msgs::GoalStatus latest_goal_status_
ResultConstPtr getResult() const
void updateResult(GoalHandleT &gh, const ActionResultConstPtr &action_result)
const actionlib_msgs::GoalStatus * findGoalStatus(const std::vector< actionlib_msgs::GoalStatus > &status_vec) const
boost::function< void(const ClientGoalHandle< ActionSpec > &)> TransitionCallback
void setCommState(const CommState &state)
Change the state, as well as print out ROS_DEBUG info.
StateEnum
Defines the various states the Communication State Machine can be in.
#define ROS_DEBUG_NAMED(name,...)
std::string toString() const
ActionGoalConstPtr action_goal_
void updateStatus(GoalHandleT &gh, const actionlib_msgs::GoalStatusArrayConstPtr &status_array)
Thin wrapper around an enum in order to help interpret the state of the communication state machine...
CommState getCommState() const
FeedbackCallback feedback_cb_
boost::function< void(const ClientGoalHandle< ActionSpec > &, const FeedbackConstPtr &)> FeedbackCallback
#define ROS_ERROR_NAMED(name,...)
actionlib_msgs::GoalStatus getGoalStatus() const
Client side handle to monitor goal progress.
TransitionCallback transition_cb_
void processLost(GoalHandleT &gh)