Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef ACTION_LIB_ACTION_SERVER_BASE
00038 #define ACTION_LIB_ACTION_SERVER_BASE
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 {
00061 template <class ActionSpec>
00062 class ActionServerBase {
00063 public:
00064
00065 typedef ServerGoalHandle<ActionSpec> GoalHandle;
00066
00067
00068 ACTION_DEFINITION(ActionSpec);
00069
00076 ActionServerBase(
00077 boost::function<void (GoalHandle)> goal_cb,
00078 boost::function<void (GoalHandle)> cancel_cb,
00079 bool auto_start = false);
00080
00081
00085 virtual ~ActionServerBase();
00086
00091 void registerGoalCallback(boost::function<void (GoalHandle)> cb);
00092
00097 void registerCancelCallback(boost::function<void (GoalHandle)> cb);
00098
00102 void start();
00103
00104
00108 void goalCallback(const boost::shared_ptr<const ActionGoal>& goal);
00109
00113 void cancelCallback(const boost::shared_ptr<const actionlib_msgs::GoalID>& goal_id);
00114
00115 protected:
00116
00117 friend class ServerGoalHandle<ActionSpec>;
00118 friend class HandleTrackerDeleter<ActionSpec>;
00119
00123 virtual void initialize() = 0;
00124
00130 virtual void publishResult(const actionlib_msgs::GoalStatus& status, const Result& result) = 0;
00131
00137 virtual void publishFeedback(const actionlib_msgs::GoalStatus& status, const Feedback& feedback) = 0;
00138
00142 virtual void publishStatus() = 0;
00143
00144 boost::recursive_mutex lock_;
00145
00146 std::list<StatusTracker<ActionSpec> > status_list_;
00147
00148 boost::function<void (GoalHandle)> goal_callback_;
00149 boost::function<void (GoalHandle)> cancel_callback_;
00150
00151 ros::Time last_cancel_;
00152 ros::Duration status_list_timeout_;
00153
00154 GoalIDGenerator id_generator_;
00155 bool started_;
00156 boost::shared_ptr<DestructionGuard> guard_;
00157 };
00158
00159 template <class ActionSpec>
00160 ActionServerBase<ActionSpec>::ActionServerBase(
00161 boost::function<void (GoalHandle)> goal_cb,
00162 boost::function<void (GoalHandle)> cancel_cb,
00163 bool auto_start) :
00164 goal_callback_(goal_cb),
00165 cancel_callback_(cancel_cb),
00166 started_(auto_start),
00167 guard_(new DestructionGuard)
00168 {
00169 }
00170
00171 template <class ActionSpec>
00172 ActionServerBase<ActionSpec>::~ActionServerBase()
00173 {
00174
00175 guard_->destruct();
00176 }
00177
00178 template <class ActionSpec>
00179 void ActionServerBase<ActionSpec>::registerGoalCallback(boost::function<void (GoalHandle)> cb)
00180 {
00181 goal_callback_ = cb;
00182 }
00183
00184 template <class ActionSpec>
00185 void ActionServerBase<ActionSpec>::registerCancelCallback(boost::function<void (GoalHandle)> cb)
00186 {
00187 cancel_callback_ = cb;
00188 }
00189
00190 template <class ActionSpec>
00191 void ActionServerBase<ActionSpec>::start()
00192 {
00193 initialize();
00194 started_ = true;
00195 publishStatus();
00196 }
00197
00198
00199 template <class ActionSpec>
00200 void ActionServerBase<ActionSpec>::goalCallback(const boost::shared_ptr<const ActionGoal>& goal)
00201 {
00202 boost::recursive_mutex::scoped_lock lock(lock_);
00203
00204
00205 if(!started_)
00206 return;
00207
00208 ROS_DEBUG_NAMED("actionlib", "The action server has received a new goal request");
00209
00210
00211 for(typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.begin(); it != status_list_.end(); ++it){
00212 if(goal->goal_id.id == (*it).status_.goal_id.id){
00213
00214
00215 if ( (*it).status_.status == actionlib_msgs::GoalStatus::RECALLING ) {
00216 (*it).status_.status = actionlib_msgs::GoalStatus::RECALLED;
00217 publishResult((*it).status_, Result());
00218 }
00219
00220
00221
00222 if((*it).handle_tracker_.expired()){
00223 (*it).handle_destruction_time_ = goal->goal_id.stamp;
00224 }
00225
00226
00227 return;
00228 }
00229 }
00230
00231
00232 typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.insert(status_list_.end(), StatusTracker<ActionSpec> (goal));
00233
00234
00235 HandleTrackerDeleter<ActionSpec> d(this, it, guard_);
00236 boost::shared_ptr<void> handle_tracker((void *)NULL, d);
00237 (*it).handle_tracker_ = handle_tracker;
00238
00239
00240 if(goal->goal_id.stamp != ros::Time() && goal->goal_id.stamp <= last_cancel_){
00241
00242 GoalHandle gh(it, this, handle_tracker, guard_);
00243 gh.setCanceled(Result(), "This goal handle was canceled by the action server because its timestamp is before the timestamp of the last cancel request");
00244 }
00245 else{
00246 GoalHandle gh = GoalHandle(it, this, handle_tracker, guard_);
00247
00248
00249 lock_.unlock();
00250
00251
00252 goal_callback_(gh);
00253
00254 lock_.lock();
00255 }
00256 }
00257
00258 template <class ActionSpec>
00259 void ActionServerBase<ActionSpec>::cancelCallback(const boost::shared_ptr<const actionlib_msgs::GoalID>& goal_id)
00260 {
00261 boost::recursive_mutex::scoped_lock lock(lock_);
00262
00263
00264 if(!started_)
00265 return;
00266
00267
00268 ROS_DEBUG_NAMED("actionlib", "The action server has received a new cancel request");
00269 bool goal_id_found = false;
00270 for(typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.begin(); it != status_list_.end(); ++it){
00271
00272
00273 if(
00274 (goal_id->id == "" && goal_id->stamp == ros::Time())
00275 || goal_id->id == (*it).status_.goal_id.id
00276 || (goal_id->stamp != ros::Time() && (*it).status_.goal_id.stamp <= goal_id->stamp)
00277 ){
00278
00279 if(goal_id->id == (*it).status_.goal_id.id)
00280 goal_id_found = true;
00281
00282
00283 boost::shared_ptr<void> handle_tracker = (*it).handle_tracker_.lock();
00284
00285 if((*it).handle_tracker_.expired()){
00286
00287 HandleTrackerDeleter<ActionSpec> d(this, it, guard_);
00288 handle_tracker = boost::shared_ptr<void>((void *)NULL, d);
00289 (*it).handle_tracker_ = handle_tracker;
00290
00291
00292 (*it).handle_destruction_time_ = ros::Time();
00293 }
00294
00295
00296
00297 GoalHandle gh(it, this, handle_tracker, guard_);
00298 if(gh.setCancelRequested()){
00299
00300 lock_.unlock();
00301
00302
00303 cancel_callback_(gh);
00304
00305
00306 lock_.lock();
00307 }
00308 }
00309 }
00310
00311
00312 if(goal_id->id != "" && !goal_id_found){
00313 typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.insert(status_list_.end(),
00314 StatusTracker<ActionSpec> (*goal_id, actionlib_msgs::GoalStatus::RECALLING));
00315
00316 (*it).handle_destruction_time_ = goal_id->stamp;
00317 }
00318
00319
00320 if(goal_id->stamp > last_cancel_)
00321 last_cancel_ = goal_id->stamp;
00322 }
00323 }
00324
00325
00326 #endif