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 #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
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
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
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
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
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
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
00157
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
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
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
00210 if (!pursuing_current_goal_ && pursue_current_goal_) {
00211
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
00219 ac_->cancelGoal();
00220 pursuing_current_goal_ = false;
00221 } else if (pursuing_current_goal_ && pursue_current_goal_) {
00222
00223 actionlib::SimpleClientGoalState state = ac_->getState();
00224 if (state != actionlib::SimpleClientGoalState::PENDING &&
00225 state != actionlib::SimpleClientGoalState::ACTIVE) {
00226
00227
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
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
00243 pursuing_current_goal_ = false;
00244 } else {
00245 current_goal_.setAborted(*result);
00246
00247 pursuing_current_goal_ = false;
00248 pursue_current_goal_ = false;
00249 }
00250 } else {
00251 current_goal_.setCanceled(*result);
00252
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