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


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep
autogenerated on Thu Sep 28 2017 02:51:16