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 #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
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
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
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
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
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
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
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
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
00295 if (goal_) {
00296
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
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 }
00418 #endif // ACTIONLIB__SERVER__SERVER_GOAL_HANDLE_IMP_H_