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__SERVER_GOAL_HANDLE_IMP_H_
00038 #define ACTIONLIB__SERVER__SERVER_GOAL_HANDLE_IMP_H_
00039 
00040 #include <list>
00041 #include <string>
00042 
00043 #include "ros/console.h"
00044 
00045 namespace actionlib
00046 {
00047 template<class ActionSpec>
00048 ServerGoalHandle<ActionSpec>::ServerGoalHandle()
00049 : as_(NULL) {}
00050 
00051 template<class ActionSpec>
00052 ServerGoalHandle<ActionSpec>::ServerGoalHandle(const ServerGoalHandle & gh)
00053 : status_it_(gh.status_it_), goal_(gh.goal_), as_(gh.as_), handle_tracker_(gh.handle_tracker_),
00054   guard_(gh.guard_) {}
00055 
00056 template<class ActionSpec>
00057 void ServerGoalHandle<ActionSpec>::setAccepted(const std::string & text)
00058 {
00059   if (as_ == NULL) {
00060     ROS_ERROR_NAMED("actionlib",
00061       "You are attempting to call methods on an uninitialized goal handle");
00062     return;
00063   }
00064 
00065   // check to see if we can use the action server
00066   DestructionGuard::ScopedProtector protector(*guard_);
00067   if (!protector.isProtected()) {
00068     ROS_ERROR_NAMED("actionlib",
00069       "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
00070     return;
00071   }
00072 
00073   ROS_DEBUG_NAMED("actionlib", "Accepting goal, id: %s, stamp: %.2f",
00074     getGoalID().id.c_str(), getGoalID().stamp.toSec());
00075   if (goal_) {
00076     boost::recursive_mutex::scoped_lock lock(as_->lock_);
00077     unsigned int status = (*status_it_).status_.status;
00078 
00079     // if we were pending before, then we'll go active
00080     if (status == actionlib_msgs::GoalStatus::PENDING) {
00081       (*status_it_).status_.status = actionlib_msgs::GoalStatus::ACTIVE;
00082       (*status_it_).status_.text = text;
00083       as_->publishStatus();
00084     } else if (status == actionlib_msgs::GoalStatus::RECALLING) {
00085       // if we were recalling before, now we'll go to preempting
00086       (*status_it_).status_.status = actionlib_msgs::GoalStatus::PREEMPTING;
00087       (*status_it_).status_.text = text;
00088       as_->publishStatus();
00089     } else {
00090       ROS_ERROR_NAMED("actionlib",
00091         "To transition to an active state, the goal must be in a pending or recalling state, it is currently in state: %d",
00092         (*status_it_).status_.status);
00093     }
00094   } else {
00095     ROS_ERROR_NAMED("actionlib", "Attempt to set status on an uninitialized ServerGoalHandle");
00096   }
00097 }
00098 
00099 template<class ActionSpec>
00100 void ServerGoalHandle<ActionSpec>::setCanceled(const Result & result, const std::string & text)
00101 {
00102   if (as_ == NULL) {
00103     ROS_ERROR_NAMED("actionlib",
00104       "You are attempting to call methods on an uninitialized goal handle");
00105     return;
00106   }
00107 
00108   // check to see if we can use the action server
00109   DestructionGuard::ScopedProtector protector(*guard_);
00110   if (!protector.isProtected()) {
00111     ROS_ERROR_NAMED("actionlib",
00112       "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
00113     return;
00114   }
00115 
00116   ROS_DEBUG_NAMED("actionlib", "Setting status to canceled on goal, id: %s, stamp: %.2f",
00117     getGoalID().id.c_str(), getGoalID().stamp.toSec());
00118   if (goal_) {
00119     boost::recursive_mutex::scoped_lock lock(as_->lock_);
00120     unsigned int status = (*status_it_).status_.status;
00121     if (status == actionlib_msgs::GoalStatus::PENDING ||
00122       status == actionlib_msgs::GoalStatus::RECALLING)
00123     {
00124       (*status_it_).status_.status = actionlib_msgs::GoalStatus::RECALLED;
00125       (*status_it_).status_.text = text;
00126       as_->publishResult((*status_it_).status_, result);
00127     } else if (status == actionlib_msgs::GoalStatus::ACTIVE ||
00128       status == actionlib_msgs::GoalStatus::PREEMPTING) {
00129       (*status_it_).status_.status = actionlib_msgs::GoalStatus::PREEMPTED;
00130       (*status_it_).status_.text = text;
00131       as_->publishResult((*status_it_).status_, result);
00132     } else {
00133       ROS_ERROR_NAMED("actionlib",
00134         "To transition to a cancelled state, the goal must be in a pending, recalling, active, or preempting state, it is currently in state: %d",
00135         (*status_it_).status_.status);
00136     }
00137   } else {
00138     ROS_ERROR_NAMED("actionlib", "Attempt to set status on an uninitialized ServerGoalHandle");
00139   }
00140 }
00141 
00142 template<class ActionSpec>
00143 void ServerGoalHandle<ActionSpec>::setRejected(const Result & result, const std::string & text)
00144 {
00145   if (as_ == NULL) {
00146     ROS_ERROR_NAMED("actionlib",
00147       "You are attempting to call methods on an uninitialized goal handle");
00148     return;
00149   }
00150 
00151   // check to see if we can use the action server
00152   DestructionGuard::ScopedProtector protector(*guard_);
00153   if (!protector.isProtected()) {
00154     ROS_ERROR_NAMED("actionlib",
00155       "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
00156     return;
00157   }
00158 
00159   ROS_DEBUG_NAMED("actionlib", "Setting status to rejected on goal, id: %s, stamp: %.2f",
00160     getGoalID().id.c_str(), getGoalID().stamp.toSec());
00161   if (goal_) {
00162     boost::recursive_mutex::scoped_lock lock(as_->lock_);
00163     unsigned int status = (*status_it_).status_.status;
00164     if (status == actionlib_msgs::GoalStatus::PENDING ||
00165       status == actionlib_msgs::GoalStatus::RECALLING)
00166     {
00167       (*status_it_).status_.status = actionlib_msgs::GoalStatus::REJECTED;
00168       (*status_it_).status_.text = text;
00169       as_->publishResult((*status_it_).status_, result);
00170     } else {
00171       ROS_ERROR_NAMED("actionlib",
00172         "To transition to a rejected state, the goal must be in a pending or recalling state, it is currently in state: %d",
00173         (*status_it_).status_.status);
00174     }
00175   } else {
00176     ROS_ERROR_NAMED("actionlib", "Attempt to set status on an uninitialized ServerGoalHandle");
00177   }
00178 }
00179 
00180 template<class ActionSpec>
00181 void ServerGoalHandle<ActionSpec>::setAborted(const Result & result, const std::string & text)
00182 {
00183   if (as_ == NULL) {
00184     ROS_ERROR_NAMED("actionlib",
00185       "You are attempting to call methods on an uninitialized goal handle");
00186     return;
00187   }
00188 
00189   // check to see if we can use the action server
00190   DestructionGuard::ScopedProtector protector(*guard_);
00191   if (!protector.isProtected()) {
00192     ROS_ERROR_NAMED("actionlib",
00193       "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
00194     return;
00195   }
00196 
00197   ROS_DEBUG_NAMED("actionlib", "Setting status to aborted on goal, id: %s, stamp: %.2f",
00198     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 ||
00203       status == actionlib_msgs::GoalStatus::ACTIVE)
00204     {
00205       (*status_it_).status_.status = actionlib_msgs::GoalStatus::ABORTED;
00206       (*status_it_).status_.text = text;
00207       as_->publishResult((*status_it_).status_, result);
00208     } else {
00209       ROS_ERROR_NAMED("actionlib",
00210         "To transition to an aborted state, the goal must be in a preempting or active state, it is currently in state: %d",
00211         status);
00212     }
00213   } else {
00214     ROS_ERROR_NAMED("actionlib", "Attempt to set status on an uninitialized ServerGoalHandle");
00215   }
00216 }
00217 
00218 template<class ActionSpec>
00219 void ServerGoalHandle<ActionSpec>::setSucceeded(const Result & result, const std::string & text)
00220 {
00221   if (as_ == NULL) {
00222     ROS_ERROR_NAMED("actionlib",
00223       "You are attempting to call methods on an uninitialized goal handle");
00224     return;
00225   }
00226 
00227   // check to see if we can use the action server
00228   DestructionGuard::ScopedProtector protector(*guard_);
00229   if (!protector.isProtected()) {
00230     ROS_ERROR_NAMED("actionlib",
00231       "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
00232     return;
00233   }
00234 
00235   ROS_DEBUG_NAMED("actionlib", "Setting status to succeeded on goal, id: %s, stamp: %.2f",
00236     getGoalID().id.c_str(), getGoalID().stamp.toSec());
00237   if (goal_) {
00238     boost::recursive_mutex::scoped_lock lock(as_->lock_);
00239     unsigned int status = (*status_it_).status_.status;
00240     if (status == actionlib_msgs::GoalStatus::PREEMPTING ||
00241       status == actionlib_msgs::GoalStatus::ACTIVE)
00242     {
00243       (*status_it_).status_.status = actionlib_msgs::GoalStatus::SUCCEEDED;
00244       (*status_it_).status_.text = text;
00245       as_->publishResult((*status_it_).status_, result);
00246     } else {
00247       ROS_ERROR_NAMED("actionlib",
00248         "To transition to a succeeded state, the goal must be in a preempting or active state, it is currently in state: %d",
00249         status);
00250     }
00251   } else {
00252     ROS_ERROR_NAMED("actionlib", "Attempt to set status on an uninitialized ServerGoalHandle");
00253   }
00254 }
00255 
00256 template<class ActionSpec>
00257 void ServerGoalHandle<ActionSpec>::publishFeedback(const Feedback & feedback)
00258 {
00259   if (as_ == NULL) {
00260     ROS_ERROR_NAMED("actionlib",
00261       "You are attempting to call methods on an uninitialized goal handle");
00262     return;
00263   }
00264 
00265   // check to see if we can use the action server
00266   DestructionGuard::ScopedProtector protector(*guard_);
00267   if (!protector.isProtected()) {
00268     ROS_ERROR_NAMED("actionlib",
00269       "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
00270     return;
00271   }
00272 
00273   ROS_DEBUG_NAMED("actionlib", "Publishing feedback for goal, id: %s, stamp: %.2f",
00274     getGoalID().id.c_str(), getGoalID().stamp.toSec());
00275   if (goal_) {
00276     boost::recursive_mutex::scoped_lock lock(as_->lock_);
00277     as_->publishFeedback((*status_it_).status_, feedback);
00278   } else {
00279     ROS_ERROR_NAMED("actionlib",
00280       "Attempt to publish feedback on an uninitialized ServerGoalHandle");
00281   }
00282 }
00283 
00284 template<class ActionSpec>
00285 bool ServerGoalHandle<ActionSpec>::isValid() const
00286 {
00287   return goal_ && as_ != NULL;
00288 }
00289 
00290 template<class ActionSpec>
00291 boost::shared_ptr<const typename ServerGoalHandle<ActionSpec>::Goal> ServerGoalHandle<ActionSpec>::
00292 getGoal() const
00293 {
00294   // if we have a goal that is non-null
00295   if (goal_) {
00296     // create the deleter for our goal subtype
00297     EnclosureDeleter<const ActionGoal> d(goal_);
00298     return boost::shared_ptr<const Goal>(&(goal_->goal), d);
00299   }
00300   return boost::shared_ptr<const Goal>();
00301 }
00302 
00303 template<class ActionSpec>
00304 actionlib_msgs::GoalID ServerGoalHandle<ActionSpec>::getGoalID() const
00305 {
00306   if (goal_ && as_ != NULL) {
00307     DestructionGuard::ScopedProtector protector(*guard_);
00308     if (protector.isProtected()) {
00309       boost::recursive_mutex::scoped_lock lock(as_->lock_);
00310       return (*status_it_).status_.goal_id;
00311     } else {
00312       return actionlib_msgs::GoalID();
00313     }
00314   } else {
00315     ROS_ERROR_NAMED("actionlib",
00316       "Attempt to get a goal id on an uninitialized ServerGoalHandle or one that has no ActionServer associated with it.");
00317     return actionlib_msgs::GoalID();
00318   }
00319 }
00320 
00321 template<class ActionSpec>
00322 actionlib_msgs::GoalStatus ServerGoalHandle<ActionSpec>::getGoalStatus() const
00323 {
00324   if (goal_ && as_ != NULL) {
00325     DestructionGuard::ScopedProtector protector(*guard_);
00326     if (protector.isProtected()) {
00327       boost::recursive_mutex::scoped_lock lock(as_->lock_);
00328       return (*status_it_).status_;
00329     } else {
00330       return actionlib_msgs::GoalStatus();
00331     }
00332   } else {
00333     ROS_ERROR_NAMED("actionlib",
00334       "Attempt to get goal status on an uninitialized ServerGoalHandle or one that has no ActionServer associated with it.");
00335     return actionlib_msgs::GoalStatus();
00336   }
00337 }
00338 
00339 template<class ActionSpec>
00340 ServerGoalHandle<ActionSpec> & ServerGoalHandle<ActionSpec>::operator=(const ServerGoalHandle & gh)
00341 {
00342   status_it_ = gh.status_it_;
00343   goal_ = gh.goal_;
00344   as_ = gh.as_;
00345   handle_tracker_ = gh.handle_tracker_;
00346   guard_ = gh.guard_;
00347   return *this;
00348 }
00349 
00350 template<class ActionSpec>
00351 bool ServerGoalHandle<ActionSpec>::operator==(const ServerGoalHandle & other) const
00352 {
00353   if (!goal_ && !other.goal_) {
00354     return true;
00355   }
00356 
00357   if (!goal_ || !other.goal_) {
00358     return false;
00359   }
00360 
00361   actionlib_msgs::GoalID my_id = getGoalID();
00362   actionlib_msgs::GoalID their_id = other.getGoalID();
00363   return my_id.id == their_id.id;
00364 }
00365 
00366 template<class ActionSpec>
00367 bool ServerGoalHandle<ActionSpec>::operator!=(const ServerGoalHandle & other) const
00368 {
00369   return !(*this == other);
00370 }
00371 
00372 template<class ActionSpec>
00373 ServerGoalHandle<ActionSpec>::ServerGoalHandle(
00374   typename std::list<StatusTracker<ActionSpec> >::iterator status_it,
00375   ActionServerBase<ActionSpec> * as, boost::shared_ptr<void> handle_tracker,
00376   boost::shared_ptr<DestructionGuard> guard)
00377 : status_it_(status_it), goal_((*status_it).goal_),
00378   as_(as), handle_tracker_(handle_tracker), guard_(guard) {}
00379 
00380 template<class ActionSpec>
00381 bool ServerGoalHandle<ActionSpec>::setCancelRequested()
00382 {
00383   if (as_ == NULL) {
00384     ROS_ERROR_NAMED("actionlib",
00385       "You are attempting to call methods on an uninitialized goal handle");
00386     return false;
00387   }
00388 
00389   // check to see if we can use the action server
00390   DestructionGuard::ScopedProtector protector(*guard_);
00391   if (!protector.isProtected()) {
00392     ROS_ERROR_NAMED("actionlib",
00393       "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
00394     return false;
00395   }
00396 
00397   ROS_DEBUG_NAMED("actionlib",
00398     "Transitioning to a cancel requested state on goal id: %s, stamp: %.2f",
00399     getGoalID().id.c_str(), getGoalID().stamp.toSec());
00400   if (goal_) {
00401     boost::recursive_mutex::scoped_lock lock(as_->lock_);
00402     unsigned int status = (*status_it_).status_.status;
00403     if (status == actionlib_msgs::GoalStatus::PENDING) {
00404       (*status_it_).status_.status = actionlib_msgs::GoalStatus::RECALLING;
00405       as_->publishStatus();
00406       return true;
00407     }
00408 
00409     if (status == actionlib_msgs::GoalStatus::ACTIVE) {
00410       (*status_it_).status_.status = actionlib_msgs::GoalStatus::PREEMPTING;
00411       as_->publishStatus();
00412       return true;
00413     }
00414   }
00415   return false;
00416 }
00417 }  // namespace actionlib
00418 #endif  // ACTIONLIB__SERVER__SERVER_GOAL_HANDLE_IMP_H_


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep, Mikael Arguedas
autogenerated on Sat Feb 16 2019 03:21:28