actionlib: actionlib: client_goal_handle

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* This file has the template implementation for ClientGoalHandle. It should be included with the
00036  * class definition.
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; // Continue standard processing
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)
00255 {
00256   // Check if both are inactive
00257   if (!active_ && !rhs.active_)
00258     return true;
00259 
00260   // Check if one or the other is inactive
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)
00276 {
00277   return !(*this==rhs);
00278 }
00279 
00280 }
client_goal_handle_imp.h
