Go to the documentation of this file.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
00038 #ifndef ACTIONLIB__CLIENT__CLIENT_GOAL_HANDLE_IMP_H_
00039 #define ACTIONLIB__CLIENT__CLIENT_GOAL_HANDLE_IMP_H_
00040
00041 #include <ros/ros.h>
00042
00043 namespace actionlib
00044 {
00045
00046 template<class ActionSpec>
00047 ClientGoalHandle<ActionSpec>::ClientGoalHandle()
00048 {
00049 gm_ = NULL;
00050 active_ = false;
00051 }
00052
00053 template<class ActionSpec>
00054 ClientGoalHandle<ActionSpec>::~ClientGoalHandle()
00055 {
00056 reset();
00057 }
00058
00059 template<class ActionSpec>
00060 ClientGoalHandle<ActionSpec>::ClientGoalHandle(GoalManagerT * gm,
00061 typename ManagedListT::Handle handle,
00062 const boost::shared_ptr<DestructionGuard> & guard)
00063 {
00064 gm_ = gm;
00065 active_ = true;
00066 list_handle_ = handle;
00067 guard_ = guard;
00068 }
00069
00070 template<class ActionSpec>
00071 void ClientGoalHandle<ActionSpec>::reset()
00072 {
00073 if (active_) {
00074 DestructionGuard::ScopedProtector protector(*guard_);
00075 if (!protector.isProtected()) {
00076 ROS_ERROR_NAMED("actionlib",
00077 "This action client associated with the goal handle has already been destructed. Ignoring this reset() call");
00078 return;
00079 }
00080
00081 boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_);
00082 list_handle_.reset();
00083 active_ = false;
00084 gm_ = NULL;
00085 }
00086 }
00087
00088 template<class ActionSpec>
00089 bool ClientGoalHandle<ActionSpec>::isExpired() const
00090 {
00091 return !active_;
00092 }
00093
00094
00095 template<class ActionSpec>
00096 CommState ClientGoalHandle<ActionSpec>::getCommState() const
00097 {
00098 assert(gm_);
00099 if (!gm_) {
00100 ROS_ERROR_NAMED("actionlib", "Client should have valid GoalManager");
00101 return CommState(CommState::DONE);
00102 }
00103
00104 boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_);
00105 if (!active_) {
00106 ROS_ERROR_NAMED("actionlib",
00107 "Trying to getCommState on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle");
00108 return CommState(CommState::DONE);
00109 }
00110
00111 DestructionGuard::ScopedProtector protector(*guard_);
00112 if (!protector.isProtected()) {
00113 ROS_ERROR_NAMED("actionlib",
00114 "This action client associated with the goal handle has already been destructed. Ignoring this getCommState() call");
00115 return CommState(CommState::DONE);
00116 }
00117
00118 return list_handle_.getElem()->getCommState();
00119 }
00120
00121 template<class ActionSpec>
00122 TerminalState ClientGoalHandle<ActionSpec>::getTerminalState() const
00123 {
00124 if (!active_) {
00125 ROS_ERROR_NAMED("actionlib",
00126 "Trying to getTerminalState on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle");
00127 return TerminalState(TerminalState::LOST);
00128 }
00129
00130 DestructionGuard::ScopedProtector protector(*guard_);
00131 if (!protector.isProtected()) {
00132 ROS_ERROR_NAMED("actionlib",
00133 "This action client associated with the goal handle has already been destructed. Ignoring this getTerminalState() call");
00134 return TerminalState(TerminalState::LOST);
00135 }
00136
00137 assert(gm_);
00138 if (!gm_)
00139 {
00140 ROS_ERROR_NAMED("actionlib", "Client should have valid GoalManager");
00141 return TerminalState(TerminalState::LOST);
00142 }
00143
00144 boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_);
00145 CommState comm_state_ = list_handle_.getElem()->getCommState();
00146 if (comm_state_ != CommState::DONE) {
00147 ROS_WARN_NAMED("actionlib", "Asking for the terminal state when we're in [%s]",
00148 comm_state_.toString().c_str());
00149 }
00150
00151 actionlib_msgs::GoalStatus goal_status = list_handle_.getElem()->getGoalStatus();
00152
00153 switch (goal_status.status) {
00154 case actionlib_msgs::GoalStatus::PENDING:
00155 case actionlib_msgs::GoalStatus::ACTIVE:
00156 case actionlib_msgs::GoalStatus::PREEMPTING:
00157 case actionlib_msgs::GoalStatus::RECALLING:
00158 ROS_ERROR_NAMED("actionlib", "Asking for terminal state, but latest goal status is %u",
00159 goal_status.status); return TerminalState(TerminalState::LOST,
00160 goal_status.text);
00161 case actionlib_msgs::GoalStatus::PREEMPTED: return TerminalState(TerminalState::PREEMPTED,
00162 goal_status.text);
00163 case actionlib_msgs::GoalStatus::SUCCEEDED: return TerminalState(TerminalState::SUCCEEDED,
00164 goal_status.text);
00165 case actionlib_msgs::GoalStatus::ABORTED: return TerminalState(TerminalState::ABORTED,
00166 goal_status.text);
00167 case actionlib_msgs::GoalStatus::REJECTED: return TerminalState(TerminalState::REJECTED,
00168 goal_status.text);
00169 case actionlib_msgs::GoalStatus::RECALLED: return TerminalState(TerminalState::RECALLED,
00170 goal_status.text);
00171 case actionlib_msgs::GoalStatus::LOST: return TerminalState(TerminalState::LOST,
00172 goal_status.text);
00173 default:
00174 ROS_ERROR_NAMED("actionlib", "Unknown goal status: %u", goal_status.status); break;
00175 }
00176
00177 ROS_ERROR_NAMED("actionlib", "Bug in determining terminal state");
00178 return TerminalState(TerminalState::LOST, goal_status.text);
00179 }
00180
00181 template<class ActionSpec>
00182 typename ClientGoalHandle<ActionSpec>::ResultConstPtr ClientGoalHandle<ActionSpec>::getResult()
00183 const
00184 {
00185 if (!active_) {
00186 ROS_ERROR_NAMED("actionlib",
00187 "Trying to getResult on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle");
00188 }
00189 assert(gm_);
00190 if (!gm_) {
00191 ROS_ERROR_NAMED("actionlib", "Client should have valid GoalManager");
00192 return typename ClientGoalHandle<ActionSpec>::ResultConstPtr() ;
00193 }
00194
00195 DestructionGuard::ScopedProtector protector(*guard_);
00196 if (!protector.isProtected()) {
00197 ROS_ERROR_NAMED("actionlib",
00198 "This action client associated with the goal handle has already been destructed. Ignoring this getResult() call");
00199 return typename ClientGoalHandle<ActionSpec>::ResultConstPtr();
00200 }
00201
00202 boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_);
00203 return list_handle_.getElem()->getResult();
00204 }
00205
00206 template<class ActionSpec>
00207 void ClientGoalHandle<ActionSpec>::resend()
00208 {
00209 if (!active_) {
00210 ROS_ERROR_NAMED("actionlib",
00211 "Trying to resend() on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle");
00212 }
00213
00214 DestructionGuard::ScopedProtector protector(*guard_);
00215 if (!protector.isProtected()) {
00216 ROS_ERROR_NAMED("actionlib",
00217 "This action client associated with the goal handle has already been destructed. Ignoring this resend() call");
00218 return;
00219 }
00220
00221 assert(gm_);
00222 if (!gm_)
00223 {
00224 ROS_ERROR_NAMED("actionlib", "Client should have valid GoalManager");
00225 return;
00226 }
00227
00228 boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_);
00229
00230 ActionGoalConstPtr action_goal = list_handle_.getElem()->getActionGoal();
00231
00232 if (!action_goal) {
00233 ROS_ERROR_NAMED("actionlib", "BUG: Got a NULL action_goal");
00234 }
00235
00236 if (gm_->send_goal_func_) {
00237 gm_->send_goal_func_(action_goal);
00238 }
00239 }
00240
00241 template<class ActionSpec>
00242 void ClientGoalHandle<ActionSpec>::cancel()
00243 {
00244 if (!active_) {
00245 ROS_ERROR_NAMED("actionlib",
00246 "Trying to cancel() on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle");
00247 return;
00248 }
00249
00250 assert(gm_);
00251 if (!gm_)
00252 {
00253 ROS_ERROR_NAMED("actionlib", "Client should have valid GoalManager");
00254 return;
00255 }
00256
00257 DestructionGuard::ScopedProtector protector(*guard_);
00258 if (!protector.isProtected()) {
00259 ROS_ERROR_NAMED("actionlib",
00260 "This action client associated with the goal handle has already been destructed. Ignoring this call");
00261 return;
00262 }
00263
00264 boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_);
00265
00266 switch (list_handle_.getElem()->getCommState().state_) {
00267 case CommState::WAITING_FOR_GOAL_ACK:
00268 case CommState::PENDING:
00269 case CommState::ACTIVE:
00270 case CommState::WAITING_FOR_CANCEL_ACK:
00271 break;
00272 case CommState::WAITING_FOR_RESULT:
00273 case CommState::RECALLING:
00274 case CommState::PREEMPTING:
00275 case CommState::DONE:
00276 ROS_DEBUG_NAMED("actionlib", "Got a cancel() request while in state [%s], so ignoring it",
00277 list_handle_.getElem()->getCommState().toString().c_str());
00278 return;
00279 default:
00280 ROS_ERROR_NAMED("actionlib", "BUG: Unhandled CommState: %u",
00281 list_handle_.getElem()->getCommState().state_);
00282 return;
00283 }
00284
00285 ActionGoalConstPtr action_goal = list_handle_.getElem()->getActionGoal();
00286
00287 actionlib_msgs::GoalID cancel_msg;
00288 cancel_msg.stamp = ros::Time(0, 0);
00289 cancel_msg.id = list_handle_.getElem()->getActionGoal()->goal_id.id;
00290
00291 if (gm_->cancel_func_) {
00292 gm_->cancel_func_(cancel_msg);
00293 }
00294
00295 list_handle_.getElem()->transitionToState(*this, CommState::WAITING_FOR_CANCEL_ACK);
00296 }
00297
00298 template<class ActionSpec>
00299 bool ClientGoalHandle<ActionSpec>::operator==(const ClientGoalHandle<ActionSpec> & rhs) const
00300 {
00301
00302 if (!active_ && !rhs.active_) {
00303 return true;
00304 }
00305
00306
00307 if (!active_ || !rhs.active_) {
00308 return false;
00309 }
00310
00311 DestructionGuard::ScopedProtector protector(*guard_);
00312 if (!protector.isProtected()) {
00313 ROS_ERROR_NAMED("actionlib",
00314 "This action client associated with the goal handle has already been destructed. Ignoring this operator==() call");
00315 return false;
00316 }
00317
00318 return list_handle_ == rhs.list_handle_;
00319 }
00320
00321 template<class ActionSpec>
00322 bool ClientGoalHandle<ActionSpec>::operator!=(const ClientGoalHandle<ActionSpec> & rhs) const
00323 {
00324 return !(*this == rhs);
00325 }
00326
00327 }
00328
00329 #endif // ACTIONLIB__CLIENT__CLIENT_GOAL_HANDLE_IMP_H_