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__SIMPLE_ACTION_SERVER_IMP_H_
00038 #define ACTIONLIB__SERVER__SIMPLE_ACTION_SERVER_IMP_H_
00039
00040 #include <string>
00041
00042 namespace actionlib
00043 {
00044
00045 template<class ActionSpec>
00046 SimpleActionServer<ActionSpec>::SimpleActionServer(std::string name,
00047 ExecuteCallback execute_callback,
00048 bool auto_start)
00049 : new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(
00050 execute_callback), execute_thread_(NULL), need_to_terminate_(false)
00051 {
00052 if (execute_callback_ != NULL) {
00053 execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this));
00054 }
00055
00056
00057 as_ = boost::shared_ptr<ActionServer<ActionSpec> >(new ActionServer<ActionSpec>(n_, name,
00058 boost::bind(&SimpleActionServer::goalCallback, this, _1),
00059 boost::bind(&SimpleActionServer::preemptCallback, this, _1),
00060 auto_start));
00061 }
00062
00063 template<class ActionSpec>
00064 SimpleActionServer<ActionSpec>::SimpleActionServer(std::string name, bool auto_start)
00065 : new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(
00066 NULL), execute_thread_(NULL), need_to_terminate_(false)
00067 {
00068
00069 as_ = boost::shared_ptr<ActionServer<ActionSpec> >(new ActionServer<ActionSpec>(n_, name,
00070 boost::bind(&SimpleActionServer::goalCallback, this, _1),
00071 boost::bind(&SimpleActionServer::preemptCallback, this, _1),
00072 auto_start));
00073
00074 if (execute_callback_ != NULL) {
00075 execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this));
00076 }
00077 }
00078
00079 template<class ActionSpec>
00080 SimpleActionServer<ActionSpec>::SimpleActionServer(std::string name,
00081 ExecuteCallback execute_callback)
00082 : new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(
00083 execute_callback), execute_thread_(NULL), need_to_terminate_(false)
00084 {
00085
00086 as_ = boost::shared_ptr<ActionServer<ActionSpec> >(new ActionServer<ActionSpec>(n_, name,
00087 boost::bind(&SimpleActionServer::goalCallback, this, _1),
00088 boost::bind(&SimpleActionServer::preemptCallback, this, _1),
00089 true));
00090
00091 if (execute_callback_ != NULL) {
00092 execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this));
00093 }
00094 }
00095
00096
00097 template<class ActionSpec>
00098 SimpleActionServer<ActionSpec>::SimpleActionServer(ros::NodeHandle n, std::string name,
00099 ExecuteCallback execute_callback,
00100 bool auto_start)
00101 : n_(n), new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false),
00102 execute_callback_(execute_callback), execute_thread_(NULL), need_to_terminate_(false)
00103 {
00104
00105 as_ = boost::shared_ptr<ActionServer<ActionSpec> >(new ActionServer<ActionSpec>(n, name,
00106 boost::bind(&SimpleActionServer::goalCallback, this, _1),
00107 boost::bind(&SimpleActionServer::preemptCallback, this, _1),
00108 auto_start));
00109
00110 if (execute_callback_ != NULL) {
00111 execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this));
00112 }
00113 }
00114
00115 template<class ActionSpec>
00116 SimpleActionServer<ActionSpec>::SimpleActionServer(ros::NodeHandle n, std::string name,
00117 bool auto_start)
00118 : n_(n), new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false),
00119 execute_callback_(NULL), execute_thread_(NULL), need_to_terminate_(false)
00120 {
00121
00122 as_ = boost::shared_ptr<ActionServer<ActionSpec> >(new ActionServer<ActionSpec>(n, name,
00123 boost::bind(&SimpleActionServer::goalCallback, this, _1),
00124 boost::bind(&SimpleActionServer::preemptCallback, this, _1),
00125 auto_start));
00126
00127 if (execute_callback_ != NULL) {
00128 execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this));
00129 }
00130 }
00131
00132 template<class ActionSpec>
00133 SimpleActionServer<ActionSpec>::SimpleActionServer(ros::NodeHandle n, std::string name,
00134 ExecuteCallback execute_callback)
00135 : n_(n), new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false),
00136 execute_callback_(execute_callback), execute_thread_(NULL), need_to_terminate_(false)
00137 {
00138
00139 as_ = boost::shared_ptr<ActionServer<ActionSpec> >(new ActionServer<ActionSpec>(n, name,
00140 boost::bind(&SimpleActionServer::goalCallback, this, _1),
00141 boost::bind(&SimpleActionServer::preemptCallback, this, _1),
00142 true));
00143
00144 if (execute_callback_ != NULL) {
00145 execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this));
00146 }
00147 }
00148
00149 template<class ActionSpec>
00150 SimpleActionServer<ActionSpec>::~SimpleActionServer()
00151 {
00152 if (execute_thread_) {
00153 shutdown();
00154 }
00155 }
00156
00157 template<class ActionSpec>
00158 void SimpleActionServer<ActionSpec>::shutdown()
00159 {
00160 if (execute_callback_) {
00161 {
00162 {
00163 boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
00164 need_to_terminate_ = true;
00165 }
00166
00167 assert(execute_thread_);
00168 if (execute_thread_) {
00169 execute_thread_->join();
00170 delete execute_thread_;
00171 execute_thread_ = NULL;
00172 }
00173 }
00174
00175 assert(execute_thread_);
00176 execute_thread_->join();
00177 delete execute_thread_;
00178 execute_thread_ = NULL;
00179 }
00180 }
00181
00182 template<class ActionSpec>
00183 boost::shared_ptr<const typename SimpleActionServer<ActionSpec>::Goal> SimpleActionServer<ActionSpec>
00184 ::acceptNewGoal()
00185 {
00186 boost::recursive_mutex::scoped_lock lock(lock_);
00187
00188 if (!new_goal_ || !next_goal_.getGoal()) {
00189 ROS_ERROR_NAMED("actionlib",
00190 "Attempting to accept the next goal when a new goal is not available");
00191 return boost::shared_ptr<const Goal>();
00192 }
00193
00194
00195 if (isActive() &&
00196 current_goal_.getGoal() &&
00197 current_goal_ != next_goal_)
00198 {
00199 current_goal_.setCanceled(
00200 Result(),
00201 "This goal was canceled because another goal was recieved by the simple action server");
00202 }
00203
00204 ROS_DEBUG_NAMED("actionlib", "Accepting a new goal");
00205
00206
00207 current_goal_ = next_goal_;
00208 new_goal_ = false;
00209
00210
00211 preempt_request_ = new_goal_preempt_request_;
00212 new_goal_preempt_request_ = false;
00213
00214
00215 current_goal_.setAccepted("This goal has been accepted by the simple action server");
00216
00217 return current_goal_.getGoal();
00218 }
00219
00220 template<class ActionSpec>
00221 bool SimpleActionServer<ActionSpec>::isNewGoalAvailable()
00222 {
00223 return new_goal_;
00224 }
00225
00226
00227 template<class ActionSpec>
00228 bool SimpleActionServer<ActionSpec>::isPreemptRequested()
00229 {
00230 return preempt_request_;
00231 }
00232
00233 template<class ActionSpec>
00234 bool SimpleActionServer<ActionSpec>::isActive()
00235 {
00236 if (!current_goal_.getGoal()) {
00237 return false;
00238 }
00239 unsigned int status = current_goal_.getGoalStatus().status;
00240 return status == actionlib_msgs::GoalStatus::ACTIVE ||
00241 status == actionlib_msgs::GoalStatus::PREEMPTING;
00242 }
00243
00244 template<class ActionSpec>
00245 void SimpleActionServer<ActionSpec>::setSucceeded(const Result & result, const std::string & text)
00246 {
00247 boost::recursive_mutex::scoped_lock lock(lock_);
00248 ROS_DEBUG_NAMED("actionlib", "Setting the current goal as succeeded");
00249 current_goal_.setSucceeded(result, text);
00250 }
00251
00252 template<class ActionSpec>
00253 void SimpleActionServer<ActionSpec>::setAborted(const Result & result, const std::string & text)
00254 {
00255 boost::recursive_mutex::scoped_lock lock(lock_);
00256 ROS_DEBUG_NAMED("actionlib", "Setting the current goal as aborted");
00257 current_goal_.setAborted(result, text);
00258 }
00259
00260 template<class ActionSpec>
00261 void SimpleActionServer<ActionSpec>::setPreempted(const Result & result, const std::string & text)
00262 {
00263 boost::recursive_mutex::scoped_lock lock(lock_);
00264 ROS_DEBUG_NAMED("actionlib", "Setting the current goal as canceled");
00265 current_goal_.setCanceled(result, text);
00266 }
00267
00268 template<class ActionSpec>
00269 void SimpleActionServer<ActionSpec>::registerGoalCallback(boost::function<void()> cb)
00270 {
00271
00272 if (execute_callback_) {
00273 ROS_WARN_NAMED("actionlib",
00274 "Cannot call SimpleActionServer::registerGoalCallback() because an executeCallback exists. Not going to register it.");
00275 } else {
00276 goal_callback_ = cb;
00277 }
00278 }
00279
00280 template<class ActionSpec>
00281 void SimpleActionServer<ActionSpec>::registerPreemptCallback(boost::function<void()> cb)
00282 {
00283 preempt_callback_ = cb;
00284 }
00285
00286 template<class ActionSpec>
00287 void SimpleActionServer<ActionSpec>::publishFeedback(const FeedbackConstPtr & feedback)
00288 {
00289 current_goal_.publishFeedback(*feedback);
00290 }
00291
00292 template<class ActionSpec>
00293 void SimpleActionServer<ActionSpec>::publishFeedback(const Feedback & feedback)
00294 {
00295 current_goal_.publishFeedback(feedback);
00296 }
00297
00298 template<class ActionSpec>
00299 void SimpleActionServer<ActionSpec>::goalCallback(GoalHandle goal)
00300 {
00301 boost::recursive_mutex::scoped_lock lock(lock_);
00302 ROS_DEBUG_NAMED("actionlib", "A new goal has been recieved by the single goal action server");
00303
00304
00305 if ((!current_goal_.getGoal() || goal.getGoalID().stamp >= current_goal_.getGoalID().stamp) &&
00306 (!next_goal_.getGoal() || goal.getGoalID().stamp >= next_goal_.getGoalID().stamp))
00307 {
00308
00309 if (next_goal_.getGoal() && (!current_goal_.getGoal() || next_goal_ != current_goal_)) {
00310 next_goal_.setCanceled(
00311 Result(),
00312 "This goal was canceled because another goal was recieved by the simple action server");
00313 }
00314
00315 next_goal_ = goal;
00316 new_goal_ = true;
00317 new_goal_preempt_request_ = false;
00318
00319
00320 if (isActive()) {
00321 preempt_request_ = true;
00322
00323 if (preempt_callback_) {
00324 preempt_callback_();
00325 }
00326 }
00327
00328
00329 if (goal_callback_) {
00330 goal_callback_();
00331 }
00332
00333
00334 execute_condition_.notify_all();
00335 } else {
00336
00337 goal.setCanceled(
00338 Result(),
00339 "This goal was canceled because another goal was recieved by the simple action server");
00340 }
00341 }
00342
00343 template<class ActionSpec>
00344 void SimpleActionServer<ActionSpec>::preemptCallback(GoalHandle preempt)
00345 {
00346 boost::recursive_mutex::scoped_lock lock(lock_);
00347 ROS_DEBUG_NAMED("actionlib", "A preempt has been received by the SimpleActionServer");
00348
00349
00350 if (preempt == current_goal_) {
00351 ROS_DEBUG_NAMED("actionlib",
00352 "Setting preempt_request bit for the current goal to TRUE and invoking callback");
00353 preempt_request_ = true;
00354
00355
00356 if (preempt_callback_) {
00357 preempt_callback_();
00358 }
00359 } else if (preempt == next_goal_) {
00360
00361 ROS_DEBUG_NAMED("actionlib", "Setting preempt request bit for the next goal to TRUE");
00362 new_goal_preempt_request_ = true;
00363 }
00364 }
00365
00366 template<class ActionSpec>
00367 void SimpleActionServer<ActionSpec>::executeLoop()
00368 {
00369 ros::Duration loop_duration = ros::Duration().fromSec(.1);
00370
00371 while (n_.ok()) {
00372 {
00373 boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
00374 if (need_to_terminate_) {
00375 break;
00376 }
00377 }
00378
00379 boost::recursive_mutex::scoped_lock lock(lock_);
00380 if (isActive()) {
00381 ROS_ERROR_NAMED("actionlib", "Should never reach this code with an active goal");
00382 } else if (isNewGoalAvailable()) {
00383 GoalConstPtr goal = acceptNewGoal();
00384
00385 ROS_FATAL_COND(!execute_callback_,
00386 "execute_callback_ must exist. This is a bug in SimpleActionServer");
00387
00388 {
00389
00390 boost::reverse_lock<boost::recursive_mutex::scoped_lock> unlocker(lock);
00391 execute_callback_(goal);
00392 }
00393
00394 if (isActive()) {
00395 ROS_WARN_NAMED("actionlib", "Your executeCallback did not set the goal to a terminal status.\n"
00396 "This is a bug in your ActionServer implementation. Fix your code!\n"
00397 "For now, the ActionServer will set this goal to aborted");
00398 setAborted(
00399 Result(),
00400 "This goal was aborted by the simple action server. The user should have set a terminal status on this goal and did not");
00401 }
00402 } else {
00403 execute_condition_.timed_wait(lock,
00404 boost::posix_time::milliseconds(loop_duration.toSec() * 1000.0f));
00405 }
00406 }
00407 }
00408
00409 template<class ActionSpec>
00410 void SimpleActionServer<ActionSpec>::start()
00411 {
00412 as_->start();
00413 }
00414
00415 }
00416
00417 #endif // ACTIONLIB__SERVER__SIMPLE_ACTION_SERVER_IMP_H_