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