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