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