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 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
00067 typedef ServerGoalHandle<ActionSpec> GoalHandle;
00068
00069
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
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
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
00208 if (!started_) {
00209 return;
00210 }
00211
00212 ROS_DEBUG_NAMED("actionlib", "The action server has received a new goal request");
00213
00214
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
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
00226
00227 if ((*it).handle_tracker_.expired()) {
00228 (*it).handle_destruction_time_ = goal->goal_id.stamp;
00229 }
00230
00231
00232 return;
00233 }
00234 }
00235
00236
00237 typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.insert(
00238 status_list_.end(), StatusTracker<ActionSpec>(goal));
00239
00240
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
00246 if (goal->goal_id.stamp != ros::Time() && goal->goal_id.stamp <= last_cancel_) {
00247
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
00256 boost::reverse_lock<boost::recursive_mutex::scoped_lock> unlocker(lock);
00257
00258
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
00270 if (!started_) {
00271 return;
00272 }
00273
00274
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
00281
00282 if (
00283 (goal_id->id == "" && goal_id->stamp == ros::Time()) ||
00284 goal_id->id == (*it).status_.goal_id.id ||
00285 (goal_id->stamp != ros::Time() && (*it).status_.goal_id.stamp <= goal_id->stamp)
00286 )
00287 {
00288
00289 if (goal_id->id == (*it).status_.goal_id.id) {
00290 goal_id_found = true;
00291 }
00292
00293
00294 boost::shared_ptr<void> handle_tracker = (*it).handle_tracker_.lock();
00295
00296 if ((*it).handle_tracker_.expired()) {
00297
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
00303 (*it).handle_destruction_time_ = ros::Time();
00304 }
00305
00306
00307
00308 GoalHandle gh(it, this, handle_tracker, guard_);
00309 if (gh.setCancelRequested()) {
00310
00311 boost::reverse_lock<boost::recursive_mutex::scoped_lock> unlocker(lock);
00312
00313
00314 cancel_callback_(gh);
00315 }
00316 }
00317 }
00318
00319
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
00325 (*it).handle_destruction_time_ = goal_id->stamp;
00326 }
00327
00328
00329 if (goal_id->stamp > last_cancel_) {
00330 last_cancel_ = goal_id->stamp;
00331 }
00332 }
00333
00334 }
00335 #endif // ACTIONLIB__SERVER__ACTION_SERVER_BASE_H_