action_server_imp.h
Go to the documentation of this file.
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 #ifndef ACTIONLIB_ACTION_SERVER_IMP_H_
00038 #define ACTIONLIB_ACTION_SERVER_IMP_H_
00039 namespace actionlib {
00040   template <class ActionSpec>
00041   ActionServer<ActionSpec>::ActionServer(ros::NodeHandle n, std::string name,
00042       bool auto_start)
00043     : node_(n, name), goal_callback_(boost::function<void (GoalHandle)>()),
00044       cancel_callback_(boost::function<void (GoalHandle)>()), started_(auto_start), guard_(new DestructionGuard){
00045     //if we're to autostart... then we'll initialize things
00046     if(started_){
00047       ROS_WARN_NAMED("actionlib", "You've passed in true for auto_start for the C++ action server at [%s]. You should always pass in false to avoid race conditions.", node_.getNamespace().c_str());
00048       initialize();
00049       publishStatus();
00050     }
00051   }
00052 
00053   template <class ActionSpec>
00054   ActionServer<ActionSpec>::ActionServer(ros::NodeHandle n, std::string name)
00055     : node_(n, name), goal_callback_(boost::function<void (GoalHandle)>()),
00056       cancel_callback_(boost::function<void (GoalHandle)>()), started_(true), guard_(new DestructionGuard){
00057     //if we're to autostart... then we'll initialize things
00058     if(started_){
00059       ROS_WARN_NAMED("actionlib", "You've passed in true for auto_start for the C++ action server at [%s]. You should always pass in false to avoid race conditions.", node_.getNamespace().c_str());
00060       initialize();
00061       publishStatus();
00062     }
00063   }
00064 
00065   template <class ActionSpec>
00066   ActionServer<ActionSpec>::ActionServer(ros::NodeHandle n, std::string name,
00067       boost::function<void (GoalHandle)> goal_cb,
00068       boost::function<void (GoalHandle)> cancel_cb,
00069       bool auto_start)
00070     : node_(n, name), goal_callback_(goal_cb), cancel_callback_(cancel_cb), started_(auto_start), guard_(new DestructionGuard) {
00071     //if we're to autostart... then we'll initialize things
00072     if(started_){
00073       ROS_WARN_NAMED("actionlib", "You've passed in true for auto_start for the C++ action server at [%s]. You should always pass in false to avoid race conditions.", node_.getNamespace().c_str());
00074       initialize();
00075       publishStatus();
00076     }
00077   }
00078 
00079   template <class ActionSpec>
00080   ActionServer<ActionSpec>::ActionServer(ros::NodeHandle n, std::string name,
00081       boost::function<void (GoalHandle)> goal_cb,
00082       boost::function<void (GoalHandle)> cancel_cb)
00083     : node_(n, name), goal_callback_(goal_cb), cancel_callback_(cancel_cb), started_(true), guard_(new DestructionGuard) {
00084     //if we're to autostart... then we'll initialize things
00085     if(started_){
00086       ROS_WARN_NAMED("actionlib", "You've passed in true for auto_start for the C++ action server at [%s]. You should always pass in false to avoid race conditions.", node_.getNamespace().c_str());
00087       initialize();
00088       publishStatus();
00089     }
00090   }
00091 
00092   template <class ActionSpec>
00093   ActionServer<ActionSpec>::ActionServer(ros::NodeHandle n, std::string name,
00094       boost::function<void (GoalHandle)> goal_cb,
00095       bool auto_start)
00096     : node_(n, name), goal_callback_(goal_cb), cancel_callback_(boost::function<void (GoalHandle)>()), started_(auto_start), guard_(new DestructionGuard) {
00097     //if we're to autostart... then we'll initialize things
00098     if(started_){
00099       ROS_WARN_NAMED("actionlib", "You've passed in true for auto_start for the C++ action server at [%s]. You should always pass in false to avoid race conditions.", node_.getNamespace().c_str());
00100       initialize();
00101       publishStatus();
00102     }
00103   }
00104 
00105   template <class ActionSpec>
00106   ActionServer<ActionSpec>::~ActionServer(){
00107     //block until we can safely destruct
00108     guard_->destruct();
00109   }
00110 
00111   template <class ActionSpec>
00112   void ActionServer<ActionSpec>::initialize(){
00113     result_pub_ = node_.advertise<ActionResult>("result", 50);
00114     feedback_pub_ = node_.advertise<ActionFeedback>("feedback", 50);
00115     status_pub_ = node_.advertise<actionlib_msgs::GoalStatusArray>("status", 50, true);
00116 
00117     //read the frequency with which to publish status from the parameter server
00118     //if not specified locally explicitly, use search param to find actionlib_status_frequency
00119     double status_frequency, status_list_timeout;
00120     if(!node_.getParam("status_frequency", status_frequency))
00121     {
00122       std::string status_frequency_param_name;
00123       if(!node_.searchParam("actionlib_status_frequency", status_frequency_param_name))
00124         status_frequency = 5.0;
00125       else
00126         node_.param(status_frequency_param_name, status_frequency, 5.0);
00127     }
00128     else
00129       ROS_WARN_NAMED("actionlib", "You're using the deprecated status_frequency parameter, please switch to actionlib_status_frequency.");
00130 
00131     node_.param("status_list_timeout", status_list_timeout, 5.0);
00132 
00133     status_list_timeout_ = ros::Duration(status_list_timeout);
00134 
00135     if(status_frequency > 0){
00136       status_timer_ = node_.createTimer(ros::Duration(1.0 / status_frequency),
00137           boost::bind(&ActionServer::publishStatus, this, _1));
00138     }
00139 
00140     goal_sub_ = node_.subscribe<ActionGoal>("goal", 50,
00141         boost::bind(&ActionServer::goalCallback, this, _1));
00142 
00143     cancel_sub_ = node_.subscribe<actionlib_msgs::GoalID>("cancel", 50,
00144         boost::bind(&ActionServer::cancelCallback, this, _1));
00145 
00146   }
00147 
00148   template <class ActionSpec>
00149   void ActionServer<ActionSpec>::registerGoalCallback(boost::function<void (GoalHandle)> cb){
00150     goal_callback_ = cb;
00151   }
00152 
00153   template <class ActionSpec>
00154   void ActionServer<ActionSpec>::registerCancelCallback(boost::function<void (GoalHandle)> cb){
00155     cancel_callback_ = cb;
00156   }
00157 
00158   template <class ActionSpec>
00159   void ActionServer<ActionSpec>::publishResult(const actionlib_msgs::GoalStatus& status, const Result& result){
00160     boost::recursive_mutex::scoped_lock lock(lock_);
00161     //we'll create a shared_ptr to pass to ROS to limit copying
00162     boost::shared_ptr<ActionResult> ar(new ActionResult);
00163     ar->header.stamp = ros::Time::now();
00164     ar->status = status;
00165     ar->result = result;
00166     ROS_DEBUG_NAMED("actionlib", "Publishing result for goal with id: %s and stamp: %.2f", status.goal_id.id.c_str(), status.goal_id.stamp.toSec());
00167     result_pub_.publish(ar);
00168     publishStatus();
00169   }
00170 
00171   template <class ActionSpec>
00172   void ActionServer<ActionSpec>::publishFeedback(const actionlib_msgs::GoalStatus& status, const Feedback& feedback){
00173     boost::recursive_mutex::scoped_lock lock(lock_);
00174     //we'll create a shared_ptr to pass to ROS to limit copying
00175     boost::shared_ptr<ActionFeedback> af(new ActionFeedback);
00176     af->header.stamp = ros::Time::now();
00177     af->status = status;
00178     af->feedback = feedback;
00179     ROS_DEBUG_NAMED("actionlib", "Publishing feedback for goal with id: %s and stamp: %.2f", status.goal_id.id.c_str(), status.goal_id.stamp.toSec());
00180     feedback_pub_.publish(af);
00181   }
00182 
00183   template <class ActionSpec>
00184   void ActionServer<ActionSpec>::cancelCallback(const boost::shared_ptr<const actionlib_msgs::GoalID>& goal_id){
00185     boost::recursive_mutex::scoped_lock lock(lock_);
00186 
00187     //if we're not started... then we're not actually going to do anything
00188     if(!started_)
00189       return;
00190 
00191     //we need to handle a cancel for the user
00192     ROS_DEBUG_NAMED("actionlib", "The action server has received a new cancel request");
00193     bool goal_id_found = false;
00194     for(typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.begin(); it != status_list_.end(); ++it){
00195       //check if the goal id is zero or if it is equal to the goal id of
00196       //the iterator or if the time of the iterator warrants a cancel
00197       if(
00198           (goal_id->id == "" && goal_id->stamp == ros::Time()) //id and stamp 0 --> cancel everything
00199           || goal_id->id == (*it).status_.goal_id.id //ids match... cancel that goal
00200           || (goal_id->stamp != ros::Time() && (*it).status_.goal_id.stamp <= goal_id->stamp) //stamp != 0 --> cancel everything before stamp
00201         ){
00202         //we need to check if we need to store this cancel request for later
00203         if(goal_id->id == (*it).status_.goal_id.id)
00204           goal_id_found = true;
00205 
00206         //attempt to get the handle_tracker for the list item if it exists
00207         boost::shared_ptr<void> handle_tracker = (*it).handle_tracker_.lock();
00208 
00209         if((*it).handle_tracker_.expired()){
00210           //if the handle tracker is expired, then we need to create a new one
00211           HandleTrackerDeleter<ActionSpec> d(this, it, guard_);
00212           handle_tracker = boost::shared_ptr<void>((void *)NULL, d);
00213           (*it).handle_tracker_ = handle_tracker;
00214 
00215           //we also need to reset the time that the status is supposed to be removed from the list
00216           (*it).handle_destruction_time_ = ros::Time();
00217         }
00218 
00219         //set the status of the goal to PREEMPTING or RECALLING as approriate
00220         //and check if the request should be passed on to the user
00221         GoalHandle gh(it, this, handle_tracker, guard_);
00222         if(gh.setCancelRequested()){
00223           //make sure that we're unlocked before we call the users callback
00224           lock_.unlock();
00225 
00226           //call the user's cancel callback on the relevant goal
00227           cancel_callback_(gh);
00228 
00229           //lock for further modification of the status list
00230           lock_.lock();
00231         }
00232       }
00233     }
00234 
00235     //if the requested goal_id was not found, and it is non-zero, then we need to store the cancel request
00236     if(goal_id->id != "" && !goal_id_found){
00237       typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.insert(status_list_.end(),
00238           StatusTracker<ActionSpec> (*goal_id, actionlib_msgs::GoalStatus::RECALLING));
00239       //start the timer for how long the status will live in the list without a goal handle to it
00240       (*it).handle_destruction_time_ = ros::Time::now();
00241     }
00242 
00243     //make sure to set last_cancel_ based on the stamp associated with this cancel request
00244     if(goal_id->stamp > last_cancel_)
00245       last_cancel_ = goal_id->stamp;
00246   }
00247 
00248   template <class ActionSpec>
00249   void ActionServer<ActionSpec>::goalCallback(const boost::shared_ptr<const ActionGoal>& goal){
00250     boost::recursive_mutex::scoped_lock lock(lock_);
00251 
00252     //if we're not started... then we're not actually going to do anything
00253     if(!started_)
00254       return;
00255 
00256     ROS_DEBUG_NAMED("actionlib", "The action server has received a new goal request");
00257 
00258     //we need to check if this goal already lives in the status list
00259     for(typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.begin(); it != status_list_.end(); ++it){
00260       if(goal->goal_id.id == (*it).status_.goal_id.id){
00261 
00262         // The goal could already be in a recalling state if a cancel came in before the goal
00263         if ( (*it).status_.status == actionlib_msgs::GoalStatus::RECALLING ) {
00264           (*it).status_.status = actionlib_msgs::GoalStatus::RECALLED;
00265           publishResult((*it).status_, Result());
00266         }
00267 
00268         //if this is a request for a goal that has no active handles left,
00269         //we'll bump how long it stays in the list
00270         if((*it).handle_tracker_.expired()){
00271           (*it).handle_destruction_time_ = ros::Time::now();
00272         }
00273 
00274         //make sure not to call any user callbacks or add duplicate status onto the list
00275         return;
00276       }
00277     }
00278 
00279     //if the goal is not in our list, we need to create a StatusTracker associated with this goal and push it on
00280     typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.insert(status_list_.end(), StatusTracker<ActionSpec> (goal));
00281 
00282     //we need to create a handle tracker for the incoming goal and update the StatusTracker
00283     HandleTrackerDeleter<ActionSpec> d(this, it, guard_);
00284     boost::shared_ptr<void> handle_tracker((void *)NULL, d);
00285     (*it).handle_tracker_ = handle_tracker;
00286 
00287     //check if this goal has already been canceled based on its timestamp
00288     if(goal->goal_id.stamp != ros::Time() && goal->goal_id.stamp <= last_cancel_){
00289       //if it has... just create a GoalHandle for it and setCanceled
00290       GoalHandle gh(it, this, handle_tracker, guard_);
00291       gh.setCanceled(Result(), "This goal handle was canceled by the action server because its timestamp is before the timestamp of the last cancel request");
00292     }
00293     else{
00294       GoalHandle gh = GoalHandle(it, this, handle_tracker, guard_);
00295 
00296       //make sure that we unlock before calling the users callback
00297       lock_.unlock();
00298 
00299       //now, we need to create a goal handle and call the user's callback
00300       goal_callback_(gh);
00301 
00302       lock_.lock();
00303     }
00304   }
00305 
00306   template <class ActionSpec>
00307   void ActionServer<ActionSpec>::start(){
00308     initialize();
00309     started_ = true;
00310     publishStatus();
00311   }
00312 
00313   template <class ActionSpec>
00314   void ActionServer<ActionSpec>::publishStatus(const ros::TimerEvent& e){
00315     boost::recursive_mutex::scoped_lock lock(lock_);
00316     //we won't publish status unless we've been started
00317     if(!started_)
00318       return;
00319 
00320     publishStatus();
00321   }
00322 
00323   template <class ActionSpec>
00324   void ActionServer<ActionSpec>::publishStatus(){
00325     boost::recursive_mutex::scoped_lock lock(lock_);
00326     //build a status array
00327     actionlib_msgs::GoalStatusArray status_array;
00328 
00329     status_array.header.stamp = ros::Time::now();
00330 
00331     status_array.status_list.resize(status_list_.size());
00332 
00333     unsigned int i = 0;
00334     for(typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.begin(); it != status_list_.end();){
00335       status_array.status_list[i] = (*it).status_;
00336 
00337       //check if the item is due for deletion from the status list
00338       if((*it).handle_destruction_time_ != ros::Time()
00339           && (*it).handle_destruction_time_ + status_list_timeout_ < ros::Time::now()){
00340         it = status_list_.erase(it);
00341       }
00342       else
00343         ++it;
00344 
00345       ++i;
00346     }
00347 
00348     status_pub_.publish(status_array);
00349   }
00350 };
00351 #endif


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep
autogenerated on Thu Jan 2 2014 11:03:49