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