comm_state_machine_imp.h
Go to the documentation of this file.
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 #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   // transitionToState( CommState::WAITING_FOR_GOAL_ACK );
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   // Check if this feedback is for us
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   // Check if this feedback is for us
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         // A little bit of hackery to call all the right state transitions before processing result
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   // It's possible to receive old GoalStatus messages over the wire, even after receiving Result with a terminal state.
00173   //   Thus, we want to ignore all status that we get after we're done, because it is irrelevant. (See trac #2721)
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 }  // namespace actionlib
00486 
00487 #endif  // ACTIONLIB__CLIENT__COMM_STATE_MACHINE_IMP_H_


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep
autogenerated on Thu Sep 28 2017 02:51:16