37 #ifndef ACTIONLIB__SERVER__SIMPLE_ACTION_SERVER_IMP_H_ 38 #define ACTIONLIB__SERVER__SIMPLE_ACTION_SERVER_IMP_H_ 46 template<
class ActionSpec>
50 : new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(
51 execute_callback), execute_thread_(NULL), need_to_terminate_(false)
53 if (execute_callback_ != NULL) {
54 execute_thread_ =
new boost::thread(boost::bind(&SimpleActionServer::executeLoop,
this));
59 boost::bind(&SimpleActionServer::goalCallback,
this, _1),
60 boost::bind(&SimpleActionServer::preemptCallback,
this, _1),
64 template<
class ActionSpec>
66 : new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(
67 NULL), execute_thread_(NULL), need_to_terminate_(false)
71 boost::bind(&SimpleActionServer::goalCallback,
this, _1),
72 boost::bind(&SimpleActionServer::preemptCallback,
this, _1),
75 if (execute_callback_ != NULL) {
76 execute_thread_ =
new boost::thread(boost::bind(&SimpleActionServer::executeLoop,
this));
80 template<
class ActionSpec>
83 : new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(
84 execute_callback), execute_thread_(NULL), need_to_terminate_(false)
88 boost::bind(&SimpleActionServer::goalCallback,
this, _1),
89 boost::bind(&SimpleActionServer::preemptCallback,
this, _1),
92 if (execute_callback_ != NULL) {
93 execute_thread_ =
new boost::thread(boost::bind(&SimpleActionServer::executeLoop,
this));
98 template<
class ActionSpec>
102 : n_(n), new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false),
103 execute_callback_(execute_callback), execute_thread_(NULL), need_to_terminate_(false)
107 boost::bind(&SimpleActionServer::goalCallback,
this, _1),
108 boost::bind(&SimpleActionServer::preemptCallback,
this, _1),
111 if (execute_callback_ != NULL) {
112 execute_thread_ =
new boost::thread(boost::bind(&SimpleActionServer::executeLoop,
this));
116 template<
class ActionSpec>
119 : n_(n), new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false),
120 execute_callback_(NULL), execute_thread_(NULL), need_to_terminate_(false)
124 boost::bind(&SimpleActionServer::goalCallback,
this, _1),
125 boost::bind(&SimpleActionServer::preemptCallback,
this, _1),
128 if (execute_callback_ != NULL) {
129 execute_thread_ =
new boost::thread(boost::bind(&SimpleActionServer::executeLoop,
this));
133 template<
class ActionSpec>
136 : n_(n), new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false),
137 execute_callback_(execute_callback), execute_thread_(NULL), need_to_terminate_(false)
141 boost::bind(&SimpleActionServer::goalCallback,
this, _1),
142 boost::bind(&SimpleActionServer::preemptCallback,
this, _1),
145 if (execute_callback_ != NULL) {
146 execute_thread_ =
new boost::thread(boost::bind(&SimpleActionServer::executeLoop,
this));
150 template<
class ActionSpec>
153 if (execute_thread_) {
158 template<
class ActionSpec>
161 if (execute_callback_) {
163 boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
164 need_to_terminate_ =
true;
167 assert(execute_thread_);
168 if (execute_thread_) {
169 execute_thread_->join();
170 delete execute_thread_;
171 execute_thread_ = NULL;
176 template<
class ActionSpec>
180 boost::recursive_mutex::scoped_lock lock(lock_);
182 if (!new_goal_ || !next_goal_.getGoal()) {
184 "Attempting to accept the next goal when a new goal is not available");
190 current_goal_.getGoal() &&
191 current_goal_ != next_goal_)
193 current_goal_.setCanceled(
195 "This goal was canceled because another goal was recieved by the simple action server");
201 current_goal_ = next_goal_;
205 preempt_request_ = new_goal_preempt_request_;
206 new_goal_preempt_request_ =
false;
209 current_goal_.setAccepted(
"This goal has been accepted by the simple action server");
211 return current_goal_.getGoal();
214 template<
class ActionSpec>
221 template<
class ActionSpec>
224 return preempt_request_;
227 template<
class ActionSpec>
230 if (!current_goal_.getGoal()) {
233 unsigned int status = current_goal_.getGoalStatus().status;
234 return status == actionlib_msgs::GoalStatus::ACTIVE ||
235 status == actionlib_msgs::GoalStatus::PREEMPTING;
238 template<
class ActionSpec>
241 boost::recursive_mutex::scoped_lock lock(lock_);
243 current_goal_.setSucceeded(result, text);
246 template<
class ActionSpec>
249 boost::recursive_mutex::scoped_lock lock(lock_);
251 current_goal_.setAborted(result, text);
254 template<
class ActionSpec>
257 boost::recursive_mutex::scoped_lock lock(lock_);
259 current_goal_.setCanceled(result, text);
262 template<
class ActionSpec>
266 if (execute_callback_) {
268 "Cannot call SimpleActionServer::registerGoalCallback() because an executeCallback exists. Not going to register it.");
274 template<
class ActionSpec>
277 preempt_callback_ = cb;
280 template<
class ActionSpec>
286 template<
class ActionSpec>
292 template<
class ActionSpec>
295 boost::recursive_mutex::scoped_lock lock(lock_);
296 ROS_DEBUG_NAMED(
"actionlib",
"A new goal has been recieved by the single goal action server");
299 if ((!current_goal_.getGoal() || goal.getGoalID().stamp >= current_goal_.getGoalID().stamp) &&
300 (!next_goal_.getGoal() || goal.getGoalID().stamp >= next_goal_.getGoalID().stamp))
303 if (next_goal_.getGoal() && (!current_goal_.getGoal() || next_goal_ != current_goal_)) {
304 next_goal_.setCanceled(
306 "This goal was canceled because another goal was recieved by the simple action server");
311 new_goal_preempt_request_ =
false;
315 preempt_request_ =
true;
317 if (preempt_callback_) {
323 if (goal_callback_) {
328 execute_condition_.notify_all();
333 "This goal was canceled because another goal was recieved by the simple action server");
337 template<
class ActionSpec>
340 boost::recursive_mutex::scoped_lock lock(lock_);
341 ROS_DEBUG_NAMED(
"actionlib",
"A preempt has been received by the SimpleActionServer");
344 if (preempt == current_goal_) {
346 "Setting preempt_request bit for the current goal to TRUE and invoking callback");
347 preempt_request_ =
true;
350 if (preempt_callback_) {
353 }
else if (preempt == next_goal_) {
355 ROS_DEBUG_NAMED(
"actionlib",
"Setting preempt request bit for the next goal to TRUE");
356 new_goal_preempt_request_ =
true;
360 template<
class ActionSpec>
367 boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
368 if (need_to_terminate_) {
373 boost::recursive_mutex::scoped_lock lock(lock_);
375 ROS_ERROR_NAMED(
"actionlib",
"Should never reach this code with an active goal");
376 }
else if (isNewGoalAvailable()) {
377 GoalConstPtr goal = acceptNewGoal();
380 "execute_callback_ must exist. This is a bug in SimpleActionServer");
384 boost::reverse_lock<boost::recursive_mutex::scoped_lock> unlocker(lock);
385 execute_callback_(goal);
389 ROS_WARN_NAMED(
"actionlib",
"Your executeCallback did not set the goal to a terminal status.\n" 390 "This is a bug in your ActionServer implementation. Fix your code!\n" 391 "For now, the ActionServer will set this goal to aborted");
394 "This goal was aborted by the simple action server. The user should have set a terminal status on this goal and did not");
397 execute_condition_.timed_wait(lock,
398 boost::posix_time::milliseconds(static_cast<int64_t>(loop_duration.
toSec() * 1000.0f)));
403 template<
class ActionSpec>
411 #endif // ACTIONLIB__SERVER__SIMPLE_ACTION_SERVER_IMP_H_ void publishFeedback(const FeedbackConstPtr &feedback)
Publishes feedback for a given goal.
#define ROS_WARN_NAMED(name,...)
SimpleActionServer(std::string name, ExecuteCallback execute_callback, bool auto_start)
Constructor for a SimpleActionServer.
#define ROS_FATAL_COND(cond,...)
boost::function< void(const GoalConstPtr &)> ExecuteCallback
void start()
Explicitly start the action server, used it auto_start is set to false.
#define ROS_DEBUG_NAMED(name,...)
Duration & fromSec(double t)
ROSCONSOLE_DECL void shutdown()
ActionServer< TestAction >::GoalHandle GoalHandle
#define ROS_ERROR_NAMED(name,...)
The ActionServer is a helpful tool for managing goal requests to a node. It allows the user to specif...