server_goal_handle_imp.h
Go to the documentation of this file.
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_NAMED("actionlib", "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_NAMED("actionlib", "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
00058       return;
00059     }
00060 
00061     ROS_DEBUG_NAMED("actionlib", "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_NAMED("actionlib", "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_NAMED("actionlib", "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_NAMED("actionlib", "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_NAMED("actionlib", "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
00097       return;
00098     }
00099 
00100     ROS_DEBUG_NAMED("actionlib", "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_NAMED("actionlib", "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_NAMED("actionlib", "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_NAMED("actionlib", "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_NAMED("actionlib", "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
00133       return;
00134     }
00135 
00136     ROS_DEBUG_NAMED("actionlib", "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_NAMED("actionlib", "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_NAMED("actionlib", "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_NAMED("actionlib", "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_NAMED("actionlib", "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
00164       return;
00165     }
00166 
00167     ROS_DEBUG_NAMED("actionlib", "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_NAMED("actionlib", "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_NAMED("actionlib", "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_NAMED("actionlib", "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_NAMED("actionlib", "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
00195       return;
00196     }
00197 
00198     ROS_DEBUG_NAMED("actionlib", "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_NAMED("actionlib", "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_NAMED("actionlib", "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_NAMED("actionlib", "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_NAMED("actionlib", "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
00226       return;
00227     }
00228 
00229     ROS_DEBUG_NAMED("actionlib", "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_NAMED("actionlib", "Attempt to publish feedback on an uninitialized ServerGoalHandle");
00236   }
00237 
00238   template <class ActionSpec>
00239   bool ServerGoalHandle<ActionSpec>::isValid() const{
00240     return goal_ && as_!= NULL;
00241   }
00242 
00243   template <class ActionSpec>
00244   boost::shared_ptr<const typename ServerGoalHandle<ActionSpec>::Goal> ServerGoalHandle<ActionSpec>::getGoal() const{
00245     //if we have a goal that is non-null
00246     if(goal_){
00247       //create the deleter for our goal subtype
00248       EnclosureDeleter<const ActionGoal> d(goal_);
00249       return boost::shared_ptr<const Goal>(&(goal_->goal), d);
00250     }
00251     return boost::shared_ptr<const Goal>();
00252   }
00253 
00254   template <class ActionSpec>
00255   actionlib_msgs::GoalID ServerGoalHandle<ActionSpec>::getGoalID() const{
00256     if(goal_ && as_!= NULL){
00257       DestructionGuard::ScopedProtector protector(*guard_);
00258       if(protector.isProtected()){
00259         boost::recursive_mutex::scoped_lock lock(as_->lock_);
00260         return (*status_it_).status_.goal_id;
00261       }
00262       else
00263         return actionlib_msgs::GoalID();
00264     }
00265     else{
00266       ROS_ERROR_NAMED("actionlib", "Attempt to get a goal id on an uninitialized ServerGoalHandle or one that has no ActionServer associated with it.");
00267       return actionlib_msgs::GoalID();
00268     }
00269   }
00270 
00271   template <class ActionSpec>
00272   actionlib_msgs::GoalStatus ServerGoalHandle<ActionSpec>::getGoalStatus() const{
00273     if(goal_ && as_!= NULL){
00274       DestructionGuard::ScopedProtector protector(*guard_);
00275       if(protector.isProtected()){
00276         boost::recursive_mutex::scoped_lock lock(as_->lock_);
00277         return (*status_it_).status_;
00278       }
00279       else
00280         return actionlib_msgs::GoalStatus();
00281     }
00282     else{
00283       ROS_ERROR_NAMED("actionlib", "Attempt to get goal status on an uninitialized ServerGoalHandle or one that has no ActionServer associated with it.");
00284       return actionlib_msgs::GoalStatus();
00285     }
00286   }
00287 
00288   template <class ActionSpec>
00289   ServerGoalHandle<ActionSpec>& ServerGoalHandle<ActionSpec>::operator=(const ServerGoalHandle& gh){
00290     status_it_ = gh.status_it_;
00291     goal_ = gh.goal_;
00292     as_ = gh.as_;
00293     handle_tracker_ = gh.handle_tracker_;
00294     guard_ = gh.guard_;
00295     return *this;
00296   }
00297 
00298   template <class ActionSpec>
00299   bool ServerGoalHandle<ActionSpec>::operator==(const ServerGoalHandle& other) const{
00300     if(!goal_ && !other.goal_)
00301       return true;
00302 
00303     if(!goal_ || !other.goal_)
00304       return false;
00305 
00306     actionlib_msgs::GoalID my_id = getGoalID();
00307     actionlib_msgs::GoalID their_id = other.getGoalID();
00308     return my_id.id == their_id.id;
00309   }
00310 
00311   template <class ActionSpec>
00312   bool ServerGoalHandle<ActionSpec>::operator!=(const ServerGoalHandle& other) const{
00313     return !(*this == other);
00314   }
00315 
00316   template <class ActionSpec>
00317   ServerGoalHandle<ActionSpec>::ServerGoalHandle(typename std::list<StatusTracker<ActionSpec> >::iterator status_it,
00318       ActionServerBase<ActionSpec>* as, boost::shared_ptr<void> handle_tracker, boost::shared_ptr<DestructionGuard> guard)
00319     : status_it_(status_it), goal_((*status_it).goal_),
00320     as_(as), handle_tracker_(handle_tracker), guard_(guard){}
00321 
00322   template <class ActionSpec>
00323   bool ServerGoalHandle<ActionSpec>::setCancelRequested(){
00324     if(as_ == NULL){
00325       ROS_ERROR_NAMED("actionlib", "You are attempting to call methods on an uninitialized goal handle");
00326       return false;
00327     }
00328 
00329     //check to see if we can use the action server
00330     DestructionGuard::ScopedProtector protector(*guard_);
00331     if(!protector.isProtected()){
00332       ROS_ERROR_NAMED("actionlib", "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
00333       return false;
00334     }
00335 
00336     ROS_DEBUG_NAMED("actionlib", "Transisitoning to a cancel requested state on goal id: %s, stamp: %.2f", getGoalID().id.c_str(), getGoalID().stamp.toSec());
00337     if(goal_){
00338       boost::recursive_mutex::scoped_lock lock(as_->lock_);
00339       unsigned int status = (*status_it_).status_.status;
00340       if(status == actionlib_msgs::GoalStatus::PENDING){
00341         (*status_it_).status_.status = actionlib_msgs::GoalStatus::RECALLING;
00342         as_->publishStatus();
00343         return true;
00344       }
00345 
00346       if(status == actionlib_msgs::GoalStatus::ACTIVE){
00347         (*status_it_).status_.status = actionlib_msgs::GoalStatus::PREEMPTING;
00348         as_->publishStatus();
00349         return true;
00350       }
00351 
00352     }
00353     return false;
00354   }
00355 };
00356 #endif


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep
autogenerated on Fri Aug 28 2015 10:04:41