$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* This file has the template implementation for CommStateMachine. It should be included with the 00036 * class definition. 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 //transitionToState( CommState::WAITING_FOR_GOAL_ACK ); 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 // Check if this feedback is for us 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 // Check if this feedback is for us 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 // A little bit of hackery to call all the right state transitions before processing result 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 // It's possible to receive old GoalStatus messages over the wire, even after receiving Result with a terminal state. 00161 // Thus, we want to ignore all status that we get after we're done, because it is irrelevant. (See trac #2721) 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 }