actionlib: comm_state_machine

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("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("Got a result when we were already in the DONE state"); break;
00150     default:
00151       ROS_ERROR("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("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("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("Invalid transition from ACTIVE to PENDING"); break;
00276         case actionlib_msgs::GoalStatus::ACTIVE:
00277           break;
00278         case actionlib_msgs::GoalStatus::REJECTED:
00279           ROS_ERROR("Invalid transition from ACTIVE to REJECTED"); break;
00280         case actionlib_msgs::GoalStatus::RECALLING:
00281           ROS_ERROR("Invalid transition from ACTIVE to RECALLING"); break;
00282         case actionlib_msgs::GoalStatus::RECALLED:
00283           ROS_ERROR("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("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("Invalid Transition from WAITING_FOR_RESUT to PENDING"); break;
00305         case actionlib_msgs::GoalStatus::PREEMPTING:
00306           ROS_ERROR("Invalid Transition from WAITING_FOR_RESUT to PREEMPTING"); break;
00307         case actionlib_msgs::GoalStatus::RECALLING:
00308           ROS_ERROR("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("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("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("Invalid Transition from RECALLING to PENDING"); break;
00358         case actionlib_msgs::GoalStatus::ACTIVE:
00359           ROS_ERROR("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("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("Invalid Transition from PREEMPTING to PENDING"); break;
00387         case actionlib_msgs::GoalStatus::ACTIVE:
00388           ROS_ERROR("Invalid Transition from PREEMPTING to ACTIVE"); break;
00389         case actionlib_msgs::GoalStatus::REJECTED:
00390           ROS_ERROR("Invalid Transition from PREEMPTING to REJECTED"); break;
00391         case actionlib_msgs::GoalStatus::RECALLING:
00392           ROS_ERROR("Invalid Transition from PREEMPTING to RECALLING"); break;
00393         case actionlib_msgs::GoalStatus::RECALLED:
00394           ROS_ERROR("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("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("Invalid Transition from DONE to PENDING"); break;
00414         case actionlib_msgs::GoalStatus::ACTIVE:
00415           ROS_ERROR("Invalid Transition from DONE to ACTIVE"); break;
00416         case actionlib_msgs::GoalStatus::RECALLING:
00417           ROS_ERROR("Invalid Transition from DONE to RECALLING"); break;
00418         case actionlib_msgs::GoalStatus::PREEMPTING:
00419           ROS_ERROR("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("BUG: Got an unknown state from the ActionServer. status = %u", goal_status->status);
00428           break;
00429       }
00430       break;
00431     }
00432     default:
00433       ROS_ERROR("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("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("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 }
comm_state_machine_imp.h
