00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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
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
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
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
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
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
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
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
00246 if(goal_){
00247
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
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