actionlib: simple_action_server

00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2008, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of Willow Garage, Inc. nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Eitan Marder-Eppstein
00036 *********************************************************************/
00037 
00038 namespace actionlib {
00039 
00040   template <class ActionSpec>
00041   SimpleActionServer<ActionSpec>::SimpleActionServer(std::string name, ExecuteCallback execute_callback, bool auto_start)
00042     : new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(execute_callback), need_to_terminate_(false) {
00043 
00044     if (execute_callback_ != NULL)
00045     {
00046       execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this));
00047     }
00048 
00049     //create the action server
00050     as_ = boost::shared_ptr<ActionServer<ActionSpec> >(new ActionServer<ActionSpec>(n_, name,
00051           boost::bind(&SimpleActionServer::goalCallback, this, _1),
00052           boost::bind(&SimpleActionServer::preemptCallback, this, _1),
00053           auto_start));
00054 
00055   }
00056 
00057   template <class ActionSpec>
00058   SimpleActionServer<ActionSpec>::SimpleActionServer(std::string name, bool auto_start)
00059     : new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(NULL), need_to_terminate_(false) {
00060 
00061     //create the action server
00062     as_ = boost::shared_ptr<ActionServer<ActionSpec> >(new ActionServer<ActionSpec>(n_, name,
00063           boost::bind(&SimpleActionServer::goalCallback, this, _1),
00064           boost::bind(&SimpleActionServer::preemptCallback, this, _1),
00065           auto_start));
00066 
00067     if (execute_callback_ != NULL)
00068     {
00069       execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this));
00070     }
00071   }
00072 
00073   template <class ActionSpec>
00074   SimpleActionServer<ActionSpec>::SimpleActionServer(std::string name, ExecuteCallback execute_callback)
00075     : new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(execute_callback), need_to_terminate_(false) {
00076 
00077     //create the action server
00078     as_ = boost::shared_ptr<ActionServer<ActionSpec> >(new ActionServer<ActionSpec>(n_, name,
00079           boost::bind(&SimpleActionServer::goalCallback, this, _1),
00080           boost::bind(&SimpleActionServer::preemptCallback, this, _1),
00081           true));
00082 
00083     if (execute_callback_ != NULL)
00084     {
00085       execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this));
00086     }
00087   }
00088 
00089 
00090   template <class ActionSpec>
00091   SimpleActionServer<ActionSpec>::SimpleActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_callback, bool auto_start)
00092     : n_(n), new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(execute_callback), need_to_terminate_(false) {
00093 
00094     //create the action server
00095     as_ = boost::shared_ptr<ActionServer<ActionSpec> >(new ActionServer<ActionSpec>(n, name,
00096           boost::bind(&SimpleActionServer::goalCallback, this, _1),
00097           boost::bind(&SimpleActionServer::preemptCallback, this, _1),
00098           auto_start));
00099 
00100     if (execute_callback_ != NULL)
00101     {
00102       execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this));
00103     }
00104   }
00105 
00106   template <class ActionSpec>
00107   SimpleActionServer<ActionSpec>::SimpleActionServer(ros::NodeHandle n, std::string name, bool auto_start)
00108     : n_(n), new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(NULL), need_to_terminate_(false) {
00109 
00110     //create the action server
00111     as_ = boost::shared_ptr<ActionServer<ActionSpec> >(new ActionServer<ActionSpec>(n, name,
00112           boost::bind(&SimpleActionServer::goalCallback, this, _1),
00113           boost::bind(&SimpleActionServer::preemptCallback, this, _1),
00114           auto_start));
00115 
00116     if (execute_callback_ != NULL)
00117     {
00118       execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this));
00119     }
00120   }
00121 
00122   template <class ActionSpec>
00123   SimpleActionServer<ActionSpec>::SimpleActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_callback)
00124     : n_(n), new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(execute_callback), need_to_terminate_(false) {
00125 
00126     //create the action server
00127     as_ = boost::shared_ptr<ActionServer<ActionSpec> >(new ActionServer<ActionSpec>(n, name,
00128           boost::bind(&SimpleActionServer::goalCallback, this, _1),
00129           boost::bind(&SimpleActionServer::preemptCallback, this, _1),
00130           true));
00131 
00132     if (execute_callback_ != NULL)
00133     {
00134       execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this));
00135     }
00136   }
00137 
00138   template <class ActionSpec>
00139   SimpleActionServer<ActionSpec>::~SimpleActionServer()
00140   {
00141     if(execute_thread_)
00142       shutdown();
00143   }
00144 
00145   template <class ActionSpec>
00146   void SimpleActionServer<ActionSpec>::shutdown()
00147   {
00148     if (execute_callback_)
00149     {
00150       {
00151         boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
00152         need_to_terminate_ = true;
00153       }
00154 
00155       assert(execute_thread_);
00156       execute_thread_->join();
00157       delete execute_thread_;
00158       execute_thread_ = NULL;
00159     }
00160   }
00161 
00162   template <class ActionSpec>
00163   boost::shared_ptr<const typename SimpleActionServer<ActionSpec>::Goal> SimpleActionServer<ActionSpec>::acceptNewGoal(){
00164     boost::recursive_mutex::scoped_lock lock(lock_);
00165 
00166     if(!new_goal_ || !next_goal_.getGoal()){
00167       ROS_ERROR("Attempting to accept the next goal when a new goal is not available");
00168       return boost::shared_ptr<const Goal>();
00169     }
00170 
00171     //check if we need to send a preempted message for the goal that we're currently pursuing
00172     if(isActive()
00173         && current_goal_.getGoal()
00174         && current_goal_ != next_goal_){
00175       current_goal_.setCanceled(Result(), "This goal was canceled because another goal was recieved by the simple action server");
00176     }
00177 
00178     ROS_DEBUG("Accepting a new goal");
00179 
00180     //accept the next goal
00181     current_goal_ = next_goal_;
00182     new_goal_ = false;
00183 
00184     //set preempt to request to equal the preempt state of the new goal
00185     preempt_request_ = new_goal_preempt_request_;
00186     new_goal_preempt_request_ = false;
00187 
00188     //set the status of the current goal to be active
00189     current_goal_.setAccepted("This goal has been accepted by the simple action server");
00190 
00191     return current_goal_.getGoal();
00192   }
00193 
00194   template <class ActionSpec>
00195   bool SimpleActionServer<ActionSpec>::isNewGoalAvailable(){
00196     return new_goal_;
00197   }
00198 
00199 
00200   template <class ActionSpec>
00201   bool SimpleActionServer<ActionSpec>::isPreemptRequested(){
00202     return preempt_request_;
00203   }
00204 
00205   template <class ActionSpec>
00206   bool SimpleActionServer<ActionSpec>::isActive(){
00207     if(!current_goal_.getGoal())
00208       return false;
00209     unsigned int status = current_goal_.getGoalStatus().status;
00210     return status == actionlib_msgs::GoalStatus::ACTIVE || status == actionlib_msgs::GoalStatus::PREEMPTING;
00211   }
00212 
00213   template <class ActionSpec>
00214   void SimpleActionServer<ActionSpec>::setSucceeded(const Result& result, const std::string& text){
00215     boost::recursive_mutex::scoped_lock lock(lock_);
00216     ROS_DEBUG("Setting the current goal as succeeded");
00217     current_goal_.setSucceeded(result, text);
00218   }
00219 
00220   template <class ActionSpec>
00221   void SimpleActionServer<ActionSpec>::setAborted(const Result& result, const std::string& text){
00222     boost::recursive_mutex::scoped_lock lock(lock_);
00223     ROS_DEBUG("Setting the current goal as aborted");
00224     current_goal_.setAborted(result, text);
00225   }
00226 
00227   template <class ActionSpec>
00228   void SimpleActionServer<ActionSpec>::setPreempted(const Result& result, const std::string& text){
00229     boost::recursive_mutex::scoped_lock lock(lock_);
00230     ROS_DEBUG("Setting the current goal as canceled");
00231     current_goal_.setCanceled(result, text);
00232   }
00233 
00234   template <class ActionSpec>
00235   void SimpleActionServer<ActionSpec>::registerGoalCallback(boost::function<void ()> cb){
00236     // Cannot register a goal callback if an execute callback exists
00237     if (execute_callback_)
00238       ROS_WARN("Cannot call SimpleActionServer::registerGoalCallback() because an executeCallback exists. Not going to register it.");
00239     else
00240       goal_callback_ = cb;
00241   }
00242 
00243   template <class ActionSpec>
00244   void SimpleActionServer<ActionSpec>::registerPreemptCallback(boost::function<void ()> cb){
00245     preempt_callback_ = cb;
00246   }
00247 
00248   template <class ActionSpec>
00249   void SimpleActionServer<ActionSpec>::publishFeedback(const FeedbackConstPtr& feedback)
00250   {
00251     current_goal_.publishFeedback(*feedback);
00252   }
00253 
00254   template <class ActionSpec>
00255   void SimpleActionServer<ActionSpec>::publishFeedback(const Feedback& feedback)
00256   {
00257     current_goal_.publishFeedback(feedback);
00258   }
00259 
00260   template <class ActionSpec>
00261   void SimpleActionServer<ActionSpec>::goalCallback(GoalHandle goal){
00262     boost::recursive_mutex::scoped_lock lock(lock_);
00263     ROS_DEBUG("A new goal has been recieved by the single goal action server");
00264 
00265     //check that the timestamp is past or equal to that of the current goal and the next goal
00266     if((!current_goal_.getGoal() || goal.getGoalID().stamp >= current_goal_.getGoalID().stamp)
00267         && (!next_goal_.getGoal() || goal.getGoalID().stamp >= next_goal_.getGoalID().stamp)){
00268 
00269       //if next_goal has not been accepted already... its going to get bumped, but we need to let the client know we're preempting
00270       if(next_goal_.getGoal() && (!current_goal_.getGoal() || next_goal_ != current_goal_)){
00271         next_goal_.setCanceled(Result(), "This goal was canceled because another goal was recieved by the simple action server");
00272       }
00273 
00274       next_goal_ = goal;
00275       new_goal_ = true;
00276       new_goal_preempt_request_ = false;
00277 
00278       //if the server is active, we'll want to call the preempt callback for the current goal
00279       if(isActive()){
00280         preempt_request_ = true;
00281         //if the user has registered a preempt callback, we'll call it now
00282         if(preempt_callback_)
00283           preempt_callback_();
00284       }
00285 
00286       //if the user has defined a goal callback, we'll call it now
00287       if(goal_callback_)
00288         goal_callback_();
00289 
00290       // Trigger runLoop to call execute()
00291       execute_condition_.notify_all();
00292     }
00293     else{
00294       //the goal requested has already been preempted by a different goal, so we're not going to execute it
00295       goal.setCanceled(Result(), "This goal was canceled because another goal was recieved by the simple action server");
00296     }
00297   }
00298 
00299   template <class ActionSpec>
00300   void SimpleActionServer<ActionSpec>::preemptCallback(GoalHandle preempt){
00301     boost::recursive_mutex::scoped_lock lock(lock_);
00302     ROS_DEBUG("A preempt has been received by the SimpleActionServer");
00303 
00304     //if the preempt is for the current goal, then we'll set the preemptRequest flag and call the user's preempt callback
00305     if(preempt == current_goal_){
00306       ROS_DEBUG("Setting preempt_request bit for the current goal to TRUE and invoking callback");
00307       preempt_request_ = true;
00308 
00309       //if the user has registered a preempt callback, we'll call it now
00310       if(preempt_callback_)
00311         preempt_callback_();
00312     }
00313     //if the preempt applies to the next goal, we'll set the preempt bit for that
00314     else if(preempt == next_goal_){
00315       ROS_DEBUG("Setting preempt request bit for the next goal to TRUE");
00316       new_goal_preempt_request_ = true;
00317     }
00318   }
00319 
00320   template <class ActionSpec>
00321   void SimpleActionServer<ActionSpec>::executeLoop(){
00322 
00323     ros::Duration loop_duration = ros::Duration().fromSec(.1);
00324 
00325     while (n_.ok())
00326     {
00327       {
00328         boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
00329         if (need_to_terminate_)
00330           break;
00331       }
00332 
00333       boost::recursive_mutex::scoped_lock lock(lock_);
00334       if (isActive())
00335         ROS_ERROR("Should never reach this code with an active goal");
00336       else if (isNewGoalAvailable())
00337       {
00338         GoalConstPtr goal = acceptNewGoal();
00339 
00340         ROS_FATAL_COND(!execute_callback_, "execute_callback_ must exist. This is a bug in SimpleActionServer");
00341 
00342         // Make sure we're not locked when we call execute
00343         lock.unlock();
00344         execute_callback_(goal);
00345         lock.lock();
00346 
00347         if (isActive())
00348         {
00349           ROS_WARN("Your executeCallback did not set the goal to a terminal status.\n"
00350                    "This is a bug in your ActionServer implementation. Fix your code!\n"
00351                    "For now, the ActionServer will set this goal to aborted");
00352           setAborted(Result(), "This goal was aborted by the simple action server. The user should have set a terminal status on this goal and did not");
00353         }
00354       }
00355       else
00356         execute_condition_.timed_wait(lock, boost::posix_time::milliseconds(loop_duration.toSec() * 1000.0f));
00357     }
00358   }
00359 
00360   template <class ActionSpec>
00361   void SimpleActionServer<ActionSpec>::start(){
00362     as_->start();
00363   }
00364 
00365 };
00366
simple_action_server_imp.h
