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