actionlib: server_goal_handle

00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2008, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of Willow Garage, Inc. nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Eitan Marder-Eppstein
00036 *********************************************************************/
00037 #ifndef ACTIONLIB_SERVER_GOAL_HANDLE_IMP_H_
00038 #define ACTIONLIB_SERVER_GOAL_HANDLE_IMP_H_
00039 namespace actionlib {
00040   template <class ActionSpec>
00041   ServerGoalHandle<ActionSpec>::ServerGoalHandle() : as_(NULL) {}
00042 
00043   template <class ActionSpec>
00044   ServerGoalHandle<ActionSpec>::ServerGoalHandle(const ServerGoalHandle& gh): 
00045     status_it_(gh.status_it_), goal_(gh.goal_), as_(gh.as_), handle_tracker_(gh.handle_tracker_), guard_(gh.guard_){}
00046 
00047   template <class ActionSpec>
00048   void ServerGoalHandle<ActionSpec>::setAccepted(const std::string& text){
00049     if(as_ == NULL){
00050       ROS_ERROR("You are attempting to call methods on an uninitialized goal handle");
00051       return;
00052     }
00053 
00054     //check to see if we can use the action server
00055     DestructionGuard::ScopedProtector protector(*guard_);
00056     if(!protector.isProtected()){
00057       ROS_ERROR("The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
00058       return;
00059     }
00060 
00061     ROS_DEBUG("Accepting goal, id: %s, stamp: %.2f", getGoalID().id.c_str(), getGoalID().stamp.toSec());
00062     if(goal_){
00063       boost::recursive_mutex::scoped_lock lock(as_->lock_);
00064       unsigned int status = (*status_it_).status_.status;
00065 
00066       //if we were pending before, then we'll go active
00067       if(status == actionlib_msgs::GoalStatus::PENDING){
00068         (*status_it_).status_.status = actionlib_msgs::GoalStatus::ACTIVE;
00069         (*status_it_).status_.text = text;
00070         as_->publishStatus();
00071       }
00072       //if we were recalling before, now we'll go to preempting
00073       else if(status == actionlib_msgs::GoalStatus::RECALLING){
00074         (*status_it_).status_.status = actionlib_msgs::GoalStatus::PREEMPTING;
00075         (*status_it_).status_.text = text;
00076         as_->publishStatus();
00077       }
00078       else
00079         ROS_ERROR("To transition to an active state, the goal must be in a pending or recalling state, it is currently in state: %d",
00080             (*status_it_).status_.status);
00081     }
00082     else
00083       ROS_ERROR("Attempt to set status on an uninitialized ServerGoalHandle");
00084   }
00085 
00086   template <class ActionSpec>
00087   void ServerGoalHandle<ActionSpec>::setCanceled(const Result& result, const std::string& text){
00088     if(as_ == NULL){
00089       ROS_ERROR("You are attempting to call methods on an uninitialized goal handle");
00090       return;
00091     }
00092 
00093     //check to see if we can use the action server
00094     DestructionGuard::ScopedProtector protector(*guard_);
00095     if(!protector.isProtected()){
00096       ROS_ERROR("The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
00097       return;
00098     }
00099 
00100     ROS_DEBUG("Setting status to canceled on goal, id: %s, stamp: %.2f", getGoalID().id.c_str(), getGoalID().stamp.toSec());
00101     if(goal_){
00102       boost::recursive_mutex::scoped_lock lock(as_->lock_);
00103       unsigned int status = (*status_it_).status_.status;
00104       if(status == actionlib_msgs::GoalStatus::PENDING || status == actionlib_msgs::GoalStatus::RECALLING){
00105         (*status_it_).status_.status = actionlib_msgs::GoalStatus::RECALLED;
00106         (*status_it_).status_.text = text;
00107         as_->publishResult((*status_it_).status_, result);
00108       }
00109       else if(status == actionlib_msgs::GoalStatus::ACTIVE || status == actionlib_msgs::GoalStatus::PREEMPTING){
00110         (*status_it_).status_.status = actionlib_msgs::GoalStatus::PREEMPTED;
00111         (*status_it_).status_.text = text;
00112         as_->publishResult((*status_it_).status_, result);
00113       }
00114       else
00115         ROS_ERROR("To transition to a cancelled state, the goal must be in a pending, recalling, active, or preempting state, it is currently in state: %d",
00116             (*status_it_).status_.status);
00117     }
00118     else
00119       ROS_ERROR("Attempt to set status on an uninitialized ServerGoalHandle");
00120   }
00121 
00122   template <class ActionSpec>
00123   void ServerGoalHandle<ActionSpec>::setRejected(const Result& result, const std::string& text){
00124     if(as_ == NULL){
00125       ROS_ERROR("You are attempting to call methods on an uninitialized goal handle");
00126       return;
00127     }
00128 
00129     //check to see if we can use the action server
00130     DestructionGuard::ScopedProtector protector(*guard_);
00131     if(!protector.isProtected()){
00132       ROS_ERROR("The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
00133       return;
00134     }
00135 
00136     ROS_DEBUG("Setting status to rejected on goal, id: %s, stamp: %.2f", getGoalID().id.c_str(), getGoalID().stamp.toSec());
00137     if(goal_){
00138       boost::recursive_mutex::scoped_lock lock(as_->lock_);
00139       unsigned int status = (*status_it_).status_.status;
00140       if(status == actionlib_msgs::GoalStatus::PENDING || status == actionlib_msgs::GoalStatus::RECALLING){
00141         (*status_it_).status_.status = actionlib_msgs::GoalStatus::REJECTED;
00142         (*status_it_).status_.text = text;
00143         as_->publishResult((*status_it_).status_, result);
00144       }
00145       else
00146         ROS_ERROR("To transition to a rejected state, the goal must be in a pending or recalling state, it is currently in state: %d",
00147             (*status_it_).status_.status);
00148     }
00149     else
00150       ROS_ERROR("Attempt to set status on an uninitialized ServerGoalHandle");
00151   }
00152 
00153   template <class ActionSpec>
00154   void ServerGoalHandle<ActionSpec>::setAborted(const Result& result, const std::string& text){
00155     if(as_ == NULL){
00156       ROS_ERROR("You are attempting to call methods on an uninitialized goal handle");
00157       return;
00158     }
00159 
00160     //check to see if we can use the action server
00161     DestructionGuard::ScopedProtector protector(*guard_);
00162     if(!protector.isProtected()){
00163       ROS_ERROR("The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
00164       return;
00165     }
00166 
00167     ROS_DEBUG("Setting status to aborted on goal, id: %s, stamp: %.2f", getGoalID().id.c_str(), getGoalID().stamp.toSec());
00168     if(goal_){
00169       boost::recursive_mutex::scoped_lock lock(as_->lock_);
00170       unsigned int status = (*status_it_).status_.status;
00171       if(status == actionlib_msgs::GoalStatus::PREEMPTING || status == actionlib_msgs::GoalStatus::ACTIVE){
00172         (*status_it_).status_.status = actionlib_msgs::GoalStatus::ABORTED;
00173         (*status_it_).status_.text = text;
00174         as_->publishResult((*status_it_).status_, result);
00175       }
00176       else
00177         ROS_ERROR("To transition to an aborted state, the goal must be in a preempting or active state, it is currently in state: %d",
00178             status);
00179     }
00180     else
00181       ROS_ERROR("Attempt to set status on an uninitialized ServerGoalHandle");
00182   }
00183 
00184   template <class ActionSpec>
00185   void ServerGoalHandle<ActionSpec>::setSucceeded(const Result& result, const std::string& text){
00186     if(as_ == NULL){
00187       ROS_ERROR("You are attempting to call methods on an uninitialized goal handle");
00188       return;
00189     }
00190 
00191     //check to see if we can use the action server
00192     DestructionGuard::ScopedProtector protector(*guard_);
00193     if(!protector.isProtected()){
00194       ROS_ERROR("The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
00195       return;
00196     }
00197 
00198     ROS_DEBUG("Setting status to succeeded on goal, id: %s, stamp: %.2f", getGoalID().id.c_str(), getGoalID().stamp.toSec());
00199     if(goal_){
00200       boost::recursive_mutex::scoped_lock lock(as_->lock_);
00201       unsigned int status = (*status_it_).status_.status;
00202       if(status == actionlib_msgs::GoalStatus::PREEMPTING || status == actionlib_msgs::GoalStatus::ACTIVE){
00203         (*status_it_).status_.status = actionlib_msgs::GoalStatus::SUCCEEDED;
00204         (*status_it_).status_.text = text;
00205         as_->publishResult((*status_it_).status_, result);
00206       }
00207       else
00208         ROS_ERROR("To transition to a succeeded state, the goal must be in a preempting or active state, it is currently in state: %d",
00209             status);
00210     }
00211     else
00212       ROS_ERROR("Attempt to set status on an uninitialized ServerGoalHandle");
00213   }
00214 
00215   template <class ActionSpec>
00216   void ServerGoalHandle<ActionSpec>::publishFeedback(const Feedback& feedback){
00217     if(as_ == NULL){
00218       ROS_ERROR("You are attempting to call methods on an uninitialized goal handle");
00219       return;
00220     }
00221 
00222     //check to see if we can use the action server
00223     DestructionGuard::ScopedProtector protector(*guard_);
00224     if(!protector.isProtected()){
00225       ROS_ERROR("The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
00226       return;
00227     }
00228 
00229     ROS_DEBUG("Publishing feedback for goal, id: %s, stamp: %.2f", getGoalID().id.c_str(), getGoalID().stamp.toSec());
00230     if(goal_) {
00231       boost::recursive_mutex::scoped_lock lock(as_->lock_);
00232       as_->publishFeedback((*status_it_).status_, feedback);
00233     }
00234     else
00235       ROS_ERROR("Attempt to publish feedback on an uninitialized ServerGoalHandle");
00236   }
00237 
00238   template <class ActionSpec>
00239   boost::shared_ptr<const typename ServerGoalHandle<ActionSpec>::Goal> ServerGoalHandle<ActionSpec>::getGoal() const{
00240     //if we have a goal that is non-null
00241     if(goal_){
00242       //create the deleter for our goal subtype
00243       EnclosureDeleter<const ActionGoal> d(goal_);
00244       return boost::shared_ptr<const Goal>(&(goal_->goal), d);
00245     }
00246     return boost::shared_ptr<const Goal>();
00247   }
00248 
00249   template <class ActionSpec>
00250   actionlib_msgs::GoalID ServerGoalHandle<ActionSpec>::getGoalID() const{
00251     if(goal_ && as_!= NULL){
00252       DestructionGuard::ScopedProtector protector(*guard_);
00253       if(protector.isProtected()){
00254         boost::recursive_mutex::scoped_lock lock(as_->lock_);
00255         return (*status_it_).status_.goal_id;
00256       }
00257       else
00258         return actionlib_msgs::GoalID();
00259     }
00260     else{
00261       ROS_ERROR("Attempt to get a goal id on an uninitialized ServerGoalHandle or one that has no ActionServer associated with it.");
00262       return actionlib_msgs::GoalID();
00263     }
00264   }
00265 
00266   template <class ActionSpec>
00267   actionlib_msgs::GoalStatus ServerGoalHandle<ActionSpec>::getGoalStatus() const{
00268     if(goal_ && as_!= NULL){
00269       DestructionGuard::ScopedProtector protector(*guard_);
00270       if(protector.isProtected()){
00271         boost::recursive_mutex::scoped_lock lock(as_->lock_);
00272         return (*status_it_).status_;
00273       }
00274       else
00275         return actionlib_msgs::GoalStatus();
00276     }
00277     else{
00278       ROS_ERROR("Attempt to get goal status on an uninitialized ServerGoalHandle or one that has no ActionServer associated with it.");
00279       return actionlib_msgs::GoalStatus();
00280     }
00281   }
00282 
00283   template <class ActionSpec>
00284   ServerGoalHandle<ActionSpec>& ServerGoalHandle<ActionSpec>::operator=(const ServerGoalHandle& gh){
00285     status_it_ = gh.status_it_;
00286     goal_ = gh.goal_;
00287     as_ = gh.as_;
00288     handle_tracker_ = gh.handle_tracker_;
00289     guard_ = gh.guard_;
00290     return *this;
00291   }
00292 
00293   template <class ActionSpec>
00294   bool ServerGoalHandle<ActionSpec>::operator==(const ServerGoalHandle& other){
00295     if(!goal_ && !other.goal_)
00296       return true;
00297 
00298     if(!goal_ || !other.goal_)
00299       return false;
00300 
00301     actionlib_msgs::GoalID my_id = getGoalID();
00302     actionlib_msgs::GoalID their_id = other.getGoalID();
00303     return my_id.id == their_id.id;
00304   }
00305 
00306   template <class ActionSpec>
00307   bool ServerGoalHandle<ActionSpec>::operator!=(const ServerGoalHandle& other){
00308     return !(*this == other);
00309   }
00310 
00311   template <class ActionSpec>
00312   ServerGoalHandle<ActionSpec>::ServerGoalHandle(typename std::list<StatusTracker<ActionSpec> >::iterator status_it,
00313       ActionServer<ActionSpec>* as, boost::shared_ptr<void> handle_tracker, boost::shared_ptr<DestructionGuard> guard)
00314     : status_it_(status_it), goal_((*status_it).goal_),
00315     as_(as), handle_tracker_(handle_tracker), guard_(guard){}
00316 
00317   template <class ActionSpec>
00318   bool ServerGoalHandle<ActionSpec>::setCancelRequested(){
00319     if(as_ == NULL){
00320       ROS_ERROR("You are attempting to call methods on an uninitialized goal handle");
00321       return false;
00322     }
00323 
00324     //check to see if we can use the action server
00325     DestructionGuard::ScopedProtector protector(*guard_);
00326     if(!protector.isProtected()){
00327       ROS_ERROR("The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
00328       return false;
00329     }
00330 
00331     ROS_DEBUG("Transisitoning to a cancel requested state on goal id: %s, stamp: %.2f", getGoalID().id.c_str(), getGoalID().stamp.toSec());
00332     if(goal_){
00333       boost::recursive_mutex::scoped_lock lock(as_->lock_);
00334       unsigned int status = (*status_it_).status_.status;
00335       if(status == actionlib_msgs::GoalStatus::PENDING){
00336         (*status_it_).status_.status = actionlib_msgs::GoalStatus::RECALLING;
00337         as_->publishStatus();
00338         return true;
00339       }
00340 
00341       if(status == actionlib_msgs::GoalStatus::ACTIVE){
00342         (*status_it_).status_.status = actionlib_msgs::GoalStatus::PREEMPTING;
00343         as_->publishStatus();
00344         return true;
00345       }
00346 
00347     }
00348     return false;
00349   }
00350 };
00351 #endif
server_goal_handle_imp.h
