action_server_base.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__SERVER__ACTION_SERVER_BASE_H_
00038 #define ACTIONLIB__SERVER__ACTION_SERVER_BASE_H_
00039 
00040 #include <ros/ros.h>
00041 #include <boost/thread.hpp>
00042 #include <boost/thread/reverse_lock.hpp>
00043 #include <boost/shared_ptr.hpp>
00044 #include <actionlib_msgs/GoalID.h>
00045 #include <actionlib_msgs/GoalStatusArray.h>
00046 #include <actionlib_msgs/GoalStatus.h>
00047 #include <actionlib/enclosure_deleter.h>
00048 #include <actionlib/goal_id_generator.h>
00049 #include <actionlib/action_definition.h>
00050 #include <actionlib/server/status_tracker.h>
00051 #include <actionlib/server/handle_tracker_deleter.h>
00052 #include <actionlib/server/server_goal_handle.h>
00053 #include <actionlib/destruction_guard.h>
00054 
00055 #include <list>
00056 
00057 namespace actionlib
00058 {
00063 template<class ActionSpec>
00064 class ActionServerBase
00065 {
00066 public:
00067   // for convenience when referring to ServerGoalHandles
00068   typedef ServerGoalHandle<ActionSpec> GoalHandle;
00069 
00070   // generates typedefs that we'll use to make our lives easier
00071   ACTION_DEFINITION(ActionSpec);
00072 
00079   ActionServerBase(
00080     boost::function<void(GoalHandle)> goal_cb,
00081     boost::function<void(GoalHandle)> cancel_cb,
00082     bool auto_start = false);
00083 
00084 
00088   virtual ~ActionServerBase();
00089 
00094   void registerGoalCallback(boost::function<void(GoalHandle)> cb);
00095 
00100   void registerCancelCallback(boost::function<void(GoalHandle)> cb);
00101 
00105   void start();
00106 
00107 
00111   void goalCallback(const boost::shared_ptr<const ActionGoal> & goal);
00112 
00116   void cancelCallback(const boost::shared_ptr<const actionlib_msgs::GoalID> & goal_id);
00117 
00118 protected:
00119   // Allow access to protected fields for helper classes
00120   friend class ServerGoalHandle<ActionSpec>;
00121   friend class HandleTrackerDeleter<ActionSpec>;
00122 
00126   virtual void initialize() = 0;
00127 
00133   virtual void publishResult(const actionlib_msgs::GoalStatus & status, const Result & result) = 0;
00134 
00140   virtual void publishFeedback(const actionlib_msgs::GoalStatus & status,
00141     const Feedback & feedback) = 0;
00142 
00146   virtual void publishStatus() = 0;
00147 
00148   boost::recursive_mutex lock_;
00149 
00150   std::list<StatusTracker<ActionSpec> > status_list_;
00151 
00152   boost::function<void(GoalHandle)> goal_callback_;
00153   boost::function<void(GoalHandle)> cancel_callback_;
00154 
00155   ros::Time last_cancel_;
00156   ros::Duration status_list_timeout_;
00157 
00158   GoalIDGenerator id_generator_;
00159   bool started_;
00160   boost::shared_ptr<DestructionGuard> guard_;
00161 };
00162 
00163 template<class ActionSpec>
00164 ActionServerBase<ActionSpec>::ActionServerBase(
00165   boost::function<void(GoalHandle)> goal_cb,
00166   boost::function<void(GoalHandle)> cancel_cb,
00167   bool auto_start)
00168 : goal_callback_(goal_cb),
00169   cancel_callback_(cancel_cb),
00170   started_(auto_start),
00171   guard_(new DestructionGuard)
00172 {
00173 }
00174 
00175 template<class ActionSpec>
00176 ActionServerBase<ActionSpec>::~ActionServerBase()
00177 {
00178   // Block until we can safely destruct
00179   guard_->destruct();
00180 }
00181 
00182 template<class ActionSpec>
00183 void ActionServerBase<ActionSpec>::registerGoalCallback(boost::function<void(GoalHandle)> cb)
00184 {
00185   goal_callback_ = cb;
00186 }
00187 
00188 template<class ActionSpec>
00189 void ActionServerBase<ActionSpec>::registerCancelCallback(boost::function<void(GoalHandle)> cb)
00190 {
00191   cancel_callback_ = cb;
00192 }
00193 
00194 template<class ActionSpec>
00195 void ActionServerBase<ActionSpec>::start()
00196 {
00197   initialize();
00198   started_ = true;
00199   publishStatus();
00200 }
00201 
00202 
00203 template<class ActionSpec>
00204 void ActionServerBase<ActionSpec>::goalCallback(const boost::shared_ptr<const ActionGoal> & goal)
00205 {
00206   boost::recursive_mutex::scoped_lock lock(lock_);
00207 
00208   // if we're not started... then we're not actually going to do anything
00209   if (!started_) {
00210     return;
00211   }
00212 
00213   ROS_DEBUG_NAMED("actionlib", "The action server has received a new goal request");
00214 
00215   // we need to check if this goal already lives in the status list
00216   for (typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.begin();
00217     it != status_list_.end(); ++it)
00218   {
00219     if (goal->goal_id.id == (*it).status_.goal_id.id) {
00220       // The goal could already be in a recalling state if a cancel came in before the goal
00221       if ( (*it).status_.status == actionlib_msgs::GoalStatus::RECALLING) {
00222         (*it).status_.status = actionlib_msgs::GoalStatus::RECALLED;
00223         publishResult((*it).status_, Result());
00224       }
00225 
00226       // if this is a request for a goal that has no active handles left,
00227       // we'll bump how long it stays in the list
00228       if ((*it).handle_tracker_.expired()) {
00229         (*it).handle_destruction_time_ = goal->goal_id.stamp;
00230       }
00231 
00232       // make sure not to call any user callbacks or add duplicate status onto the list
00233       return;
00234     }
00235   }
00236 
00237   // if the goal is not in our list, we need to create a StatusTracker associated with this goal and push it on
00238   typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.insert(
00239     status_list_.end(), StatusTracker<ActionSpec>(goal));
00240 
00241   // we need to create a handle tracker for the incoming goal and update the StatusTracker
00242   HandleTrackerDeleter<ActionSpec> d(this, it, guard_);
00243   boost::shared_ptr<void> handle_tracker((void *)NULL, d);
00244   (*it).handle_tracker_ = handle_tracker;
00245 
00246   // check if this goal has already been canceled based on its timestamp
00247   if (goal->goal_id.stamp != ros::Time() && goal->goal_id.stamp <= last_cancel_) {
00248     // if it has... just create a GoalHandle for it and setCanceled
00249     GoalHandle gh(it, this, handle_tracker, guard_);
00250     gh.setCanceled(
00251       Result(),
00252       "This goal handle was canceled by the action server because its timestamp is before the timestamp of the last cancel request");
00253   } else {
00254     GoalHandle gh = GoalHandle(it, this, handle_tracker, guard_);
00255 
00256     // make sure that we unlock before calling the users callback
00257     boost::reverse_lock<boost::recursive_mutex::scoped_lock> unlocker(lock);
00258 
00259     // now, we need to create a goal handle and call the user's callback
00260     goal_callback_(gh);
00261   }
00262 }
00263 
00264 template<class ActionSpec>
00265 void ActionServerBase<ActionSpec>::cancelCallback(
00266   const boost::shared_ptr<const actionlib_msgs::GoalID> & goal_id)
00267 {
00268   boost::recursive_mutex::scoped_lock lock(lock_);
00269 
00270   // if we're not started... then we're not actually going to do anything
00271   if (!started_) {
00272     return;
00273   }
00274 
00275   // we need to handle a cancel for the user
00276   ROS_DEBUG_NAMED("actionlib", "The action server has received a new cancel request");
00277   bool goal_id_found = false;
00278   for (typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.begin();
00279     it != status_list_.end(); ++it)
00280   {
00281     // check if the goal id is zero or if it is equal to the goal id of
00282     // the iterator or if the time of the iterator warrants a cancel
00283     if (
00284       (goal_id->id == "" && goal_id->stamp == ros::Time()) ||  // id and stamp 0 --> cancel everything
00285       goal_id->id == (*it).status_.goal_id.id ||    // ids match... cancel that goal
00286       (goal_id->stamp != ros::Time() && (*it).status_.goal_id.stamp <= goal_id->stamp)       // stamp != 0 --> cancel everything before stamp
00287     )
00288     {
00289       // we need to check if we need to store this cancel request for later
00290       if (goal_id->id == (*it).status_.goal_id.id) {
00291         goal_id_found = true;
00292       }
00293 
00294       // attempt to get the handle_tracker for the list item if it exists
00295       boost::shared_ptr<void> handle_tracker = (*it).handle_tracker_.lock();
00296 
00297       if ((*it).handle_tracker_.expired()) {
00298         // if the handle tracker is expired, then we need to create a new one
00299         HandleTrackerDeleter<ActionSpec> d(this, it, guard_);
00300         handle_tracker = boost::shared_ptr<void>((void *)NULL, d);
00301         (*it).handle_tracker_ = handle_tracker;
00302 
00303         // we also need to reset the time that the status is supposed to be removed from the list
00304         (*it).handle_destruction_time_ = ros::Time();
00305       }
00306 
00307       // set the status of the goal to PREEMPTING or RECALLING as approriate
00308       // and check if the request should be passed on to the user
00309       GoalHandle gh(it, this, handle_tracker, guard_);
00310       if (gh.setCancelRequested()) {
00311         // make sure that we're unlocked before we call the users callback
00312         boost::reverse_lock<boost::recursive_mutex::scoped_lock> unlocker(lock);
00313 
00314         // call the user's cancel callback on the relevant goal
00315         cancel_callback_(gh);
00316       }
00317     }
00318   }
00319 
00320   // if the requested goal_id was not found, and it is non-zero, then we need to store the cancel request
00321   if (goal_id->id != "" && !goal_id_found) {
00322     typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.insert(
00323       status_list_.end(),
00324       StatusTracker<ActionSpec>(*goal_id, actionlib_msgs::GoalStatus::RECALLING));
00325     // start the timer for how long the status will live in the list without a goal handle to it
00326     (*it).handle_destruction_time_ = goal_id->stamp;
00327   }
00328 
00329   // make sure to set last_cancel_ based on the stamp associated with this cancel request
00330   if (goal_id->stamp > last_cancel_) {
00331     last_cancel_ = goal_id->stamp;
00332   }
00333 }
00334 
00335 }  // namespace actionlib
00336 #endif  // ACTIONLIB__SERVER__ACTION_SERVER_BASE_H_


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep, Mikael Arguedas
autogenerated on Sat Feb 16 2019 03:21:28