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
00032
00033
00034
00035
00036
00037
00038 #ifndef ACTIONLIB__CLIENT__COMM_STATE_MACHINE_IMP_H_
00039 #define ACTIONLIB__CLIENT__COMM_STATE_MACHINE_IMP_H_
00040
00041 #include <vector>
00042
00043 namespace actionlib
00044 {
00045
00046 template<class ActionSpec>
00047 CommStateMachine<ActionSpec>::CommStateMachine(const ActionGoalConstPtr & action_goal,
00048 TransitionCallback transition_cb,
00049 FeedbackCallback feedback_cb)
00050 : state_(CommState::WAITING_FOR_GOAL_ACK)
00051 {
00052 assert(action_goal);
00053 action_goal_ = action_goal;
00054 transition_cb_ = transition_cb;
00055 feedback_cb_ = feedback_cb;
00056
00057 }
00058
00059 template<class ActionSpec>
00060 typename CommStateMachine<ActionSpec>::ActionGoalConstPtr CommStateMachine<ActionSpec>::
00061 getActionGoal() const
00062 {
00063 return action_goal_;
00064 }
00065
00066 template<class ActionSpec>
00067 CommState CommStateMachine<ActionSpec>::getCommState() const
00068 {
00069 return state_;
00070 }
00071
00072 template<class ActionSpec>
00073 actionlib_msgs::GoalStatus CommStateMachine<ActionSpec>::getGoalStatus() const
00074 {
00075 return latest_goal_status_;
00076 }
00077
00078 template<class ActionSpec>
00079 typename CommStateMachine<ActionSpec>::ResultConstPtr CommStateMachine<ActionSpec>::getResult()
00080 const
00081 {
00082 ResultConstPtr result;
00083 if (latest_result_) {
00084 EnclosureDeleter<const ActionResult> d(latest_result_);
00085 result = ResultConstPtr(&(latest_result_->result), d);
00086 }
00087 return result;
00088 }
00089
00090 template<class ActionSpec>
00091 void CommStateMachine<ActionSpec>::setCommState(const CommState::StateEnum & state)
00092 {
00093 setCommState(CommState(state));
00094 }
00095
00096 template<class ActionSpec>
00097 void CommStateMachine<ActionSpec>::setCommState(const CommState & state)
00098 {
00099 ROS_DEBUG_NAMED("actionlib", "Transitioning CommState from %s to %s",
00100 state_.toString().c_str(), state.toString().c_str());
00101 state_ = state;
00102 }
00103
00104 template<class ActionSpec>
00105 const actionlib_msgs::GoalStatus * CommStateMachine<ActionSpec>::findGoalStatus(
00106 const std::vector<actionlib_msgs::GoalStatus> & status_vec) const
00107 {
00108 for (unsigned int i = 0; i < status_vec.size(); i++) {
00109 if (status_vec[i].goal_id.id == action_goal_->goal_id.id) {
00110 return &status_vec[i];
00111 }
00112 }
00113 return NULL;
00114 }
00115
00116 template<class ActionSpec>
00117 void CommStateMachine<ActionSpec>::updateFeedback(GoalHandleT & gh,
00118 const ActionFeedbackConstPtr & action_feedback)
00119 {
00120
00121 if (action_goal_->goal_id.id != action_feedback->status.goal_id.id) {
00122 return;
00123 }
00124
00125 if (feedback_cb_) {
00126 EnclosureDeleter<const ActionFeedback> d(action_feedback);
00127 FeedbackConstPtr feedback(&(action_feedback->feedback), d);
00128 feedback_cb_(gh, feedback);
00129 }
00130 }
00131
00132 template<class ActionSpec>
00133 void CommStateMachine<ActionSpec>::updateResult(GoalHandleT & gh,
00134 const ActionResultConstPtr & action_result)
00135 {
00136
00137 if (action_goal_->goal_id.id != action_result->status.goal_id.id) {
00138 return;
00139 }
00140 latest_goal_status_ = action_result->status;
00141 latest_result_ = action_result;
00142 switch (state_.state_) {
00143 case CommState::WAITING_FOR_GOAL_ACK:
00144 case CommState::PENDING:
00145 case CommState::ACTIVE:
00146 case CommState::WAITING_FOR_RESULT:
00147 case CommState::WAITING_FOR_CANCEL_ACK:
00148 case CommState::RECALLING:
00149 case CommState::PREEMPTING:
00150 {
00151
00152 actionlib_msgs::GoalStatusArrayPtr status_array(new actionlib_msgs::GoalStatusArray());
00153 status_array->status_list.push_back(action_result->status);
00154 updateStatus(gh, status_array);
00155
00156 transitionToState(gh, CommState::DONE);
00157 break;
00158 }
00159 case CommState::DONE:
00160 ROS_ERROR_NAMED("actionlib", "Got a result when we were already in the DONE state"); break;
00161 default:
00162 ROS_ERROR_NAMED("actionlib", "In a funny comm state: %u", state_.state_); break;
00163 }
00164 }
00165
00166 template<class ActionSpec>
00167 void CommStateMachine<ActionSpec>::updateStatus(GoalHandleT & gh,
00168 const actionlib_msgs::GoalStatusArrayConstPtr & status_array)
00169 {
00170 const actionlib_msgs::GoalStatus * goal_status = findGoalStatus(status_array->status_list);
00171
00172
00173
00174 if (state_ == CommState::DONE) {
00175 return;
00176 }
00177
00178 if (goal_status) {
00179 latest_goal_status_ = *goal_status;
00180 } else {
00181 if (state_ != CommState::WAITING_FOR_GOAL_ACK &&
00182 state_ != CommState::WAITING_FOR_RESULT &&
00183 state_ != CommState::DONE)
00184 {
00185 processLost(gh);
00186 }
00187 return;
00188 }
00189
00190 switch (state_.state_) {
00191 case CommState::WAITING_FOR_GOAL_ACK:
00192 {
00193 if (goal_status) {
00194 switch (goal_status->status) {
00195 case actionlib_msgs::GoalStatus::PENDING:
00196 transitionToState(gh, CommState::PENDING);
00197 break;
00198 case actionlib_msgs::GoalStatus::ACTIVE:
00199 transitionToState(gh, CommState::ACTIVE);
00200 break;
00201 case actionlib_msgs::GoalStatus::PREEMPTED:
00202 transitionToState(gh, CommState::ACTIVE);
00203 transitionToState(gh, CommState::PREEMPTING);
00204 transitionToState(gh, CommState::WAITING_FOR_RESULT);
00205 break;
00206 case actionlib_msgs::GoalStatus::SUCCEEDED:
00207 transitionToState(gh, CommState::ACTIVE);
00208 transitionToState(gh, CommState::WAITING_FOR_RESULT);
00209 break;
00210 case actionlib_msgs::GoalStatus::ABORTED:
00211 transitionToState(gh, CommState::ACTIVE);
00212 transitionToState(gh, CommState::WAITING_FOR_RESULT);
00213 break;
00214 case actionlib_msgs::GoalStatus::REJECTED:
00215 transitionToState(gh, CommState::PENDING);
00216 transitionToState(gh, CommState::WAITING_FOR_RESULT);
00217 break;
00218 case actionlib_msgs::GoalStatus::RECALLED:
00219 transitionToState(gh, CommState::PENDING);
00220 transitionToState(gh, CommState::WAITING_FOR_RESULT);
00221 break;
00222 case actionlib_msgs::GoalStatus::PREEMPTING:
00223 transitionToState(gh, CommState::ACTIVE);
00224 transitionToState(gh, CommState::PREEMPTING);
00225 break;
00226 case actionlib_msgs::GoalStatus::RECALLING:
00227 transitionToState(gh, CommState::PENDING);
00228 transitionToState(gh, CommState::RECALLING);
00229 break;
00230 default:
00231 ROS_ERROR_NAMED("actionlib",
00232 "BUG: Got an unknown status from the ActionServer. status = %u",
00233 goal_status->status);
00234 break;
00235 }
00236 }
00237 break;
00238 }
00239 case CommState::PENDING:
00240 {
00241 switch (goal_status->status) {
00242 case actionlib_msgs::GoalStatus::PENDING:
00243 break;
00244 case actionlib_msgs::GoalStatus::ACTIVE:
00245 transitionToState(gh, CommState::ACTIVE);
00246 break;
00247 case actionlib_msgs::GoalStatus::PREEMPTED:
00248 transitionToState(gh, CommState::ACTIVE);
00249 transitionToState(gh, CommState::PREEMPTING);
00250 transitionToState(gh, CommState::WAITING_FOR_RESULT);
00251 break;
00252 case actionlib_msgs::GoalStatus::SUCCEEDED:
00253 transitionToState(gh, CommState::ACTIVE);
00254 transitionToState(gh, CommState::WAITING_FOR_RESULT);
00255 break;
00256 case actionlib_msgs::GoalStatus::ABORTED:
00257 transitionToState(gh, CommState::ACTIVE);
00258 transitionToState(gh, CommState::WAITING_FOR_RESULT);
00259 break;
00260 case actionlib_msgs::GoalStatus::REJECTED:
00261 transitionToState(gh, CommState::WAITING_FOR_RESULT);
00262 break;
00263 case actionlib_msgs::GoalStatus::RECALLED:
00264 transitionToState(gh, CommState::RECALLING);
00265 transitionToState(gh, CommState::WAITING_FOR_RESULT);
00266 break;
00267 case actionlib_msgs::GoalStatus::PREEMPTING:
00268 transitionToState(gh, CommState::ACTIVE);
00269 transitionToState(gh, CommState::PREEMPTING);
00270 break;
00271 case actionlib_msgs::GoalStatus::RECALLING:
00272 transitionToState(gh, CommState::RECALLING);
00273 break;
00274 default:
00275 ROS_ERROR_NAMED("actionlib",
00276 "BUG: Got an unknown goal status from the ActionServer. status = %u",
00277 goal_status->status);
00278 break;
00279 }
00280 break;
00281 }
00282 case CommState::ACTIVE:
00283 {
00284 switch (goal_status->status) {
00285 case actionlib_msgs::GoalStatus::PENDING:
00286 ROS_ERROR_NAMED("actionlib", "Invalid transition from ACTIVE to PENDING"); break;
00287 case actionlib_msgs::GoalStatus::ACTIVE:
00288 break;
00289 case actionlib_msgs::GoalStatus::REJECTED:
00290 ROS_ERROR_NAMED("actionlib", "Invalid transition from ACTIVE to REJECTED"); break;
00291 case actionlib_msgs::GoalStatus::RECALLING:
00292 ROS_ERROR_NAMED("actionlib", "Invalid transition from ACTIVE to RECALLING"); break;
00293 case actionlib_msgs::GoalStatus::RECALLED:
00294 ROS_ERROR_NAMED("actionlib", "Invalid transition from ACTIVE to RECALLED"); break;
00295 case actionlib_msgs::GoalStatus::PREEMPTED:
00296 transitionToState(gh, CommState::PREEMPTING);
00297 transitionToState(gh, CommState::WAITING_FOR_RESULT);
00298 break;
00299 case actionlib_msgs::GoalStatus::SUCCEEDED:
00300 case actionlib_msgs::GoalStatus::ABORTED:
00301 transitionToState(gh, CommState::WAITING_FOR_RESULT); break;
00302 case actionlib_msgs::GoalStatus::PREEMPTING:
00303 transitionToState(gh, CommState::PREEMPTING); break;
00304 default:
00305 ROS_ERROR_NAMED("actionlib",
00306 "BUG: Got an unknown goal status from the ActionServer. status = %u",
00307 goal_status->status);
00308 break;
00309 }
00310 break;
00311 }
00312 case CommState::WAITING_FOR_RESULT:
00313 {
00314 switch (goal_status->status) {
00315 case actionlib_msgs::GoalStatus::PENDING:
00316 ROS_ERROR_NAMED("actionlib", "Invalid Transition from WAITING_FOR_RESUT to PENDING");
00317 break;
00318 case actionlib_msgs::GoalStatus::PREEMPTING:
00319 ROS_ERROR_NAMED("actionlib", "Invalid Transition from WAITING_FOR_RESUT to PREEMPTING");
00320 break;
00321 case actionlib_msgs::GoalStatus::RECALLING:
00322 ROS_ERROR_NAMED("actionlib", "Invalid Transition from WAITING_FOR_RESUT to RECALLING");
00323 break;
00324 case actionlib_msgs::GoalStatus::ACTIVE:
00325 case actionlib_msgs::GoalStatus::PREEMPTED:
00326 case actionlib_msgs::GoalStatus::SUCCEEDED:
00327 case actionlib_msgs::GoalStatus::ABORTED:
00328 case actionlib_msgs::GoalStatus::REJECTED:
00329 case actionlib_msgs::GoalStatus::RECALLED:
00330 break;
00331 default:
00332 ROS_ERROR_NAMED("actionlib",
00333 "BUG: Got an unknown state from the ActionServer. status = %u",
00334 goal_status->status);
00335 break;
00336 }
00337 break;
00338 }
00339 case CommState::WAITING_FOR_CANCEL_ACK:
00340 {
00341 switch (goal_status->status) {
00342 case actionlib_msgs::GoalStatus::PENDING:
00343 break;
00344 case actionlib_msgs::GoalStatus::ACTIVE:
00345 break;
00346 case actionlib_msgs::GoalStatus::SUCCEEDED:
00347 case actionlib_msgs::GoalStatus::ABORTED:
00348 case actionlib_msgs::GoalStatus::PREEMPTED:
00349 transitionToState(gh, CommState::PREEMPTING);
00350 transitionToState(gh, CommState::WAITING_FOR_RESULT);
00351 break;
00352 case actionlib_msgs::GoalStatus::RECALLED:
00353 transitionToState(gh, CommState::RECALLING);
00354 transitionToState(gh, CommState::WAITING_FOR_RESULT);
00355 break;
00356 case actionlib_msgs::GoalStatus::REJECTED:
00357 transitionToState(gh, CommState::WAITING_FOR_RESULT); break;
00358 case actionlib_msgs::GoalStatus::PREEMPTING:
00359 transitionToState(gh, CommState::PREEMPTING); break;
00360 case actionlib_msgs::GoalStatus::RECALLING:
00361 transitionToState(gh, CommState::RECALLING); break;
00362 default:
00363 ROS_ERROR_NAMED("actionlib",
00364 "BUG: Got an unknown state from the ActionServer. status = %u",
00365 goal_status->status);
00366 break;
00367 }
00368 break;
00369 }
00370 case CommState::RECALLING:
00371 {
00372 switch (goal_status->status) {
00373 case actionlib_msgs::GoalStatus::PENDING:
00374 ROS_ERROR_NAMED("actionlib", "Invalid Transition from RECALLING to PENDING"); break;
00375 case actionlib_msgs::GoalStatus::ACTIVE:
00376 ROS_ERROR_NAMED("actionlib", "Invalid Transition from RECALLING to ACTIVE"); break;
00377 case actionlib_msgs::GoalStatus::SUCCEEDED:
00378 case actionlib_msgs::GoalStatus::ABORTED:
00379 case actionlib_msgs::GoalStatus::PREEMPTED:
00380 transitionToState(gh, CommState::PREEMPTING);
00381 transitionToState(gh, CommState::WAITING_FOR_RESULT);
00382 break;
00383 case actionlib_msgs::GoalStatus::RECALLED:
00384 transitionToState(gh, CommState::WAITING_FOR_RESULT);
00385 break;
00386 case actionlib_msgs::GoalStatus::REJECTED:
00387 transitionToState(gh, CommState::WAITING_FOR_RESULT); break;
00388 case actionlib_msgs::GoalStatus::PREEMPTING:
00389 transitionToState(gh, CommState::PREEMPTING); break;
00390 case actionlib_msgs::GoalStatus::RECALLING:
00391 break;
00392 default:
00393 ROS_ERROR_NAMED("actionlib",
00394 "BUG: Got an unknown state from the ActionServer. status = %u",
00395 goal_status->status);
00396 break;
00397 }
00398 break;
00399 }
00400 case CommState::PREEMPTING:
00401 {
00402 switch (goal_status->status) {
00403 case actionlib_msgs::GoalStatus::PENDING:
00404 ROS_ERROR_NAMED("actionlib", "Invalid Transition from PREEMPTING to PENDING"); break;
00405 case actionlib_msgs::GoalStatus::ACTIVE:
00406 ROS_ERROR_NAMED("actionlib", "Invalid Transition from PREEMPTING to ACTIVE"); break;
00407 case actionlib_msgs::GoalStatus::REJECTED:
00408 ROS_ERROR_NAMED("actionlib", "Invalid Transition from PREEMPTING to REJECTED"); break;
00409 case actionlib_msgs::GoalStatus::RECALLING:
00410 ROS_ERROR_NAMED("actionlib", "Invalid Transition from PREEMPTING to RECALLING"); break;
00411 case actionlib_msgs::GoalStatus::RECALLED:
00412 ROS_ERROR_NAMED("actionlib", "Invalid Transition from PREEMPTING to RECALLED"); break;
00413 break;
00414 case actionlib_msgs::GoalStatus::PREEMPTED:
00415 case actionlib_msgs::GoalStatus::SUCCEEDED:
00416 case actionlib_msgs::GoalStatus::ABORTED:
00417 transitionToState(gh, CommState::WAITING_FOR_RESULT); break;
00418 case actionlib_msgs::GoalStatus::PREEMPTING:
00419 break;
00420 default:
00421 ROS_ERROR_NAMED("actionlib",
00422 "BUG: Got an unknown state from the ActionServer. status = %u",
00423 goal_status->status);
00424 break;
00425 }
00426 break;
00427 }
00428 case CommState::DONE:
00429 {
00430 switch (goal_status->status) {
00431 case actionlib_msgs::GoalStatus::PENDING:
00432 ROS_ERROR_NAMED("actionlib", "Invalid Transition from DONE to PENDING"); break;
00433 case actionlib_msgs::GoalStatus::ACTIVE:
00434 ROS_ERROR_NAMED("actionlib", "Invalid Transition from DONE to ACTIVE"); break;
00435 case actionlib_msgs::GoalStatus::RECALLING:
00436 ROS_ERROR_NAMED("actionlib", "Invalid Transition from DONE to RECALLING"); break;
00437 case actionlib_msgs::GoalStatus::PREEMPTING:
00438 ROS_ERROR_NAMED("actionlib", "Invalid Transition from DONE to PREEMPTING"); break;
00439 case actionlib_msgs::GoalStatus::PREEMPTED:
00440 case actionlib_msgs::GoalStatus::SUCCEEDED:
00441 case actionlib_msgs::GoalStatus::ABORTED:
00442 case actionlib_msgs::GoalStatus::RECALLED:
00443 case actionlib_msgs::GoalStatus::REJECTED:
00444 break;
00445 default:
00446 ROS_ERROR_NAMED("actionlib",
00447 "BUG: Got an unknown state from the ActionServer. status = %u",
00448 goal_status->status);
00449 break;
00450 }
00451 break;
00452 }
00453 default:
00454 ROS_ERROR_NAMED("actionlib", "In a funny comm state: %u", state_.state_);
00455 break;
00456 }
00457 }
00458
00459
00460 template<class ActionSpec>
00461 void CommStateMachine<ActionSpec>::processLost(GoalHandleT & gh)
00462 {
00463 ROS_WARN_NAMED("actionlib", "Transitioning goal to LOST");
00464 latest_goal_status_.status = actionlib_msgs::GoalStatus::LOST;
00465 transitionToState(gh, CommState::DONE);
00466 }
00467
00468 template<class ActionSpec>
00469 void CommStateMachine<ActionSpec>::transitionToState(GoalHandleT & gh,
00470 const CommState::StateEnum & next_state)
00471 {
00472 transitionToState(gh, CommState(next_state));
00473 }
00474
00475 template<class ActionSpec>
00476 void CommStateMachine<ActionSpec>::transitionToState(GoalHandleT & gh, const CommState & next_state)
00477 {
00478 ROS_DEBUG_NAMED("actionlib", "Trying to transition to %s", next_state.toString().c_str());
00479 setCommState(next_state);
00480 if (transition_cb_) {
00481 transition_cb_(gh);
00482 }
00483 }
00484
00485 }
00486
00487 #endif // ACTIONLIB__CLIENT__COMM_STATE_MACHINE_IMP_H_