interruptable_action_server_imp.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2014, University of Texas at Austin 
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of UT Austin nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *
00034 *********************************************************************/
00035 
00036 #include <actionlib/client/simple_client_goal_state.h>
00037 
00038 namespace bwi_interruptable_action_server {
00039 
00040   template <class ActionSpec>
00041   InterruptableActionServer<ActionSpec>::InterruptableActionServer(ros::NodeHandle n, 
00042                                                                    std::string name,
00043                                                                    int max_attempts,
00044                                                                    NewGoalCallback new_goal_callback,
00045                                                                    ResultCallback result_callback) :
00046       n_(n),
00047       original_goal_available_(false),
00048       switch_to_original_goal_(false),
00049       next_goal_available_(false),
00050       pursue_current_goal_(false),
00051       pursuing_current_goal_(false),
00052       max_attempts_(max_attempts),
00053       result_callback_(result_callback),
00054       new_goal_callback_(new_goal_callback) {
00055 
00056     interruptable_server_name_ = name + "_interruptable";
00057 
00058     // create the action server.
00059     as_.reset(new actionlib::ActionServer<ActionSpec>(n_, 
00060                                                       interruptable_server_name_,
00061                                                       boost::bind(&InterruptableActionServer::goalCallback, this, _1),
00062                                                       boost::bind(&InterruptableActionServer::cancelCallback, this, _1),
00063                                                       false));
00064 
00065     // create the pause and resume services.
00066     pause_server_ = n_.advertiseService(interruptable_server_name_ + "/pause", 
00067                                         &InterruptableActionServer::pause, 
00068                                         this);
00069     resume_server_ = n_.advertiseService(interruptable_server_name_ + "/resume", 
00070                                          &InterruptableActionServer::resume, 
00071                                          this);
00072 
00073     // Create the lower level simple action client to the uninterruptable action server.
00074     ac_.reset(new actionlib::SimpleActionClient<ActionSpec>(name, true));
00075   }
00076 
00077   template <class ActionSpec>
00078   InterruptableActionServer<ActionSpec>::~InterruptableActionServer() {
00079     if (original_goal_available_) {
00080       original_goal_.setCanceled(Result(), "The goal was cancelled as the action server is shutting down.");
00081     }
00082 
00083     if (pursuing_current_goal_ || pursue_current_goal_) {
00084       current_goal_.setCanceled(Result(), "The goal was cancelled as the action server is shutting down.");
00085     }
00086 
00087     if (next_goal_available_) {
00088       next_goal_.setCanceled(Result(), "The goal was cancelled as the action server is shutting down.");
00089     }
00090   }
00091 
00092   template <class ActionSpec>
00093   bool InterruptableActionServer<ActionSpec>::pause(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
00094     boost::recursive_mutex::scoped_lock lock(lock_);
00095     bool ret_val = true;
00096     if (original_goal_available_) {
00097       ROS_ERROR_STREAM(interruptable_server_name_ + " : Already paused one goal, cannot pause another.");
00098       ret_val = false;
00099     } else if (!pursue_current_goal_) {
00100       ROS_ERROR_STREAM(interruptable_server_name_ + " : Not currently actively pursuing a goal, cannot pause.");
00101       ret_val = false;
00102     } else {
00103       pursue_current_goal_ = false;
00104       original_goal_ = current_goal_;
00105       original_goal_available_ = true;
00106     }
00107     return ret_val;
00108   }
00109 
00110   template <class ActionSpec>
00111   bool InterruptableActionServer<ActionSpec>::resume(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
00112     boost::recursive_mutex::scoped_lock lock(lock_);
00113     bool ret_val = true;
00114     if (!original_goal_available_) {
00115       ROS_ERROR_STREAM(interruptable_server_name_ + " : No paused goal available, cannot resume goal.");
00116       ret_val = false;
00117     } else {
00118       // Restart the current goal again.
00119       switch_to_original_goal_ = true;
00120     }
00121     return ret_val;
00122   }
00123 
00124   template <class ActionSpec>
00125   void InterruptableActionServer<ActionSpec>::goalCallback(GoalHandle goal) {
00126     boost::recursive_mutex::scoped_lock lock(lock_);
00127     // check that the timestamp is past or equal to that of the current goal and the next goal
00128     if ((!current_goal_.getGoal() || goal.getGoalID().stamp >= current_goal_.getGoalID().stamp)
00129         && (!next_goal_.getGoal() || goal.getGoalID().stamp >= next_goal_.getGoalID().stamp)) {
00130 
00131       //if next_goal has not been accepted already... its going to get bumped, but we need to let the client know we're preempting
00132       if (next_goal_available_) {
00133         next_goal_.setCanceled(Result(), "This goal was canceled because another goal was recieved by the simple action server");
00134         next_goal_available_ = false;
00135       }
00136 
00137       next_goal_ = goal;
00138       next_goal_available_ = true; 
00139       
00140       ROS_INFO_STREAM(interruptable_server_name_ + " : Received new goal! Passing it to low level action server.");
00141     } else {
00142       goal.setCanceled(Result(), "This goal was canceled because another goal was recieved by the simple action server");
00143     }
00144 
00145   }
00146   
00147   template <class ActionSpec>
00148   void InterruptableActionServer<ActionSpec>::cancelCallback(GoalHandle goal) {
00149     boost::recursive_mutex::scoped_lock lock(lock_);
00150     if (goal == current_goal_) {
00151       pursue_current_goal_ = false;
00152     } else if (goal == next_goal_) {
00153       next_goal_.setCanceled(Result());
00154       next_goal_available_ = false;
00155     } else if (goal == original_goal_ && original_goal_available_) {
00156       // Don't need to pursue the original goal again. Since we were in paused state, we might be pursuing a goal
00157       // currently. Cancel the current and next goals as well.
00158       original_goal_.setCanceled(Result());
00159       original_goal_available_ = false;
00160       if (next_goal_available_) {
00161         next_goal_.setCanceled(Result());
00162         next_goal_available_ = false;
00163       }
00164       pursue_current_goal_ = false;
00165     }
00166   }
00167 
00168   template <class ActionSpec>
00169   void InterruptableActionServer<ActionSpec>::spin() {
00170     as_->start();
00171     ros::Rate r(30);
00172     while (n_.ok()) {
00173       ros::spinOnce();
00174       boost::recursive_mutex::scoped_lock lock(lock_);
00175 
00176       // Switch to original goal if resume was called.
00177       if (switch_to_original_goal_) {
00178         if (pursuing_current_goal_) {
00179           current_goal_.setCanceled(Result(), "This goal was preempted as a paused goal was resumed.");
00180           pursuing_current_goal_ = false;
00181         }
00182         if (next_goal_available_) {
00183           next_goal_available_ = false;
00184           next_goal_.setCanceled(Result(), "This goal was preempted as a paused goal was resumed.");
00185         }
00186         current_attempts_ = 0;
00187         current_goal_ = original_goal_;
00188         original_goal_available_ = false;
00189         switch_to_original_goal_ = false;
00190         pursue_current_goal_ = true;
00191       } 
00192       
00193       // Switch to a new goal if a new goal is available.
00194       if (next_goal_available_) {
00195         if (pursuing_current_goal_) {
00196           current_goal_.setCanceled(Result(), "This goal was preempted by a new goal");
00197           pursuing_current_goal_ = false;
00198         }
00199         current_attempts_ = 0;
00200         current_goal_ = next_goal_;
00201         current_goal_.setAccepted();
00202         if (new_goal_callback_) {
00203           new_goal_callback_(current_goal_.getGoal());
00204         }
00205         pursue_current_goal_ = true;
00206         next_goal_available_ = false;
00207       } 
00208       
00209       // If we are not pursuing the current goal, and we should be, then start pursuing the current goal.
00210       if (!pursuing_current_goal_ && pursue_current_goal_) {
00211         // Send the current goal and hookup the feedback publisher.
00212         ac_->sendGoal(*(current_goal_.getGoal()),
00213                       typename actionlib::SimpleActionClient<ActionSpec>::SimpleDoneCallback(),
00214                       typename actionlib::SimpleActionClient<ActionSpec>::SimpleActiveCallback(),
00215                       boost::bind(&InterruptableActionServer::publishFeedback, this, _1));
00216         pursuing_current_goal_ = true;
00217       } else if (pursuing_current_goal_ && !pursue_current_goal_) {
00218         // We were pursuing a goal that we do not need to pursue any more.
00219         ac_->cancelGoal();
00220         pursuing_current_goal_ = false;
00221       } else if (pursuing_current_goal_ && pursue_current_goal_) {
00222         // See if goal is still being pursued.
00223         actionlib::SimpleClientGoalState state = ac_->getState();
00224         if (state != actionlib::SimpleClientGoalState::PENDING &&
00225             state != actionlib::SimpleClientGoalState::ACTIVE) {
00226           // This means that the the goal is no longer being pursued. Publish the result here from whatever information
00227           // we've received from the regular action client.
00228           ResultConstPtr result = ac_->getResult();
00229           current_attempts_ += 1;
00230           if (result_callback_) {
00231             result_callback_(result, state, current_attempts_);
00232           }
00233           if (state == actionlib::SimpleClientGoalState::SUCCEEDED) {
00234             current_goal_.setSucceeded(*result);
00235             // The goal should not be pursued again.
00236             pursuing_current_goal_ = false;
00237             pursue_current_goal_ = false;
00238           } else if (state == actionlib::SimpleClientGoalState::ABORTED || 
00239                      state == actionlib::SimpleClientGoalState::REJECTED) {
00240             if (current_attempts_ < max_attempts_) {
00241               ++current_attempts_;
00242               // This wall cause the same goal to be resent.
00243               pursuing_current_goal_ = false;
00244             } else {
00245               current_goal_.setAborted(*result);
00246               // The goal should not be pursued again.
00247               pursuing_current_goal_ = false;
00248               pursue_current_goal_ = false;
00249             }
00250           } else {
00251             current_goal_.setCanceled(*result);
00252             // Since the goal got completed, we no longer wish to pursue this goal.
00253             pursuing_current_goal_ = false;
00254             pursue_current_goal_ = false;
00255           }
00256 
00257         }
00258       }
00259       r.sleep();
00260     }
00261   }
00262 
00263   template <class ActionSpec>
00264   void InterruptableActionServer<ActionSpec>::publishFeedback(const FeedbackConstPtr& feedback) {
00265     current_goal_.publishFeedback(*feedback);
00266   }
00267 
00268 };
00269 


bwi_interruptable_action_server
Author(s):
autogenerated on Thu Jun 6 2019 17:57:17