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