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