simple_action_server_imp.h
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2008, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Willow Garage, Inc. nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author: Eitan Marder-Eppstein
36 *********************************************************************/
37 #ifndef ACTIONLIB__SERVER__SIMPLE_ACTION_SERVER_IMP_H_
38 #define ACTIONLIB__SERVER__SIMPLE_ACTION_SERVER_IMP_H_
39 
40 #include <ros/ros.h>
41 #include <string>
42 
43 namespace actionlib
44 {
45 
46 template<class ActionSpec>
48  ExecuteCallback execute_callback,
49  bool auto_start)
50 : new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(
51  execute_callback), execute_thread_(nullptr), need_to_terminate_(false)
52 {
53  if (execute_callback_) {
54  execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this));
55  }
56 
57  // create the action server
58  as_ = boost::shared_ptr<ActionServer<ActionSpec> >(new ActionServer<ActionSpec>(n_, name,
59  boost::bind(&SimpleActionServer::goalCallback, this, boost::placeholders::_1),
60  boost::bind(&SimpleActionServer::preemptCallback, this, boost::placeholders::_1),
61  auto_start));
62 }
63 
64 template<class ActionSpec>
65 SimpleActionServer<ActionSpec>::SimpleActionServer(std::string name, bool auto_start)
66 : new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(
67  NULL), execute_thread_(nullptr), need_to_terminate_(false)
68 {
69  // create the action server
70  as_ = boost::shared_ptr<ActionServer<ActionSpec> >(new ActionServer<ActionSpec>(n_, name,
71  boost::bind(&SimpleActionServer::goalCallback, this, boost::placeholders::_1),
72  boost::bind(&SimpleActionServer::preemptCallback, this, boost::placeholders::_1),
73  auto_start));
74 
75  if (execute_callback_) {
76  execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this));
77  }
78 }
79 
80 template<class ActionSpec>
81 SimpleActionServer<ActionSpec>::SimpleActionServer(std::string name,
82  ExecuteCallback execute_callback)
83 : new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(
84  execute_callback), execute_thread_(nullptr), need_to_terminate_(false)
85 {
86  // create the action server
88  boost::bind(&SimpleActionServer::goalCallback, this, boost::placeholders::_1),
89  boost::bind(&SimpleActionServer::preemptCallback, this, boost::placeholders::_1),
90  true));
91 
92  if (execute_callback_) {
93  execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this));
94  }
95 }
96 
97 
98 template<class ActionSpec>
100  ExecuteCallback execute_callback,
101  bool auto_start)
102 : n_(n), new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false),
103  execute_callback_(execute_callback), execute_thread_(nullptr), need_to_terminate_(false)
104 {
105  // create the action server
107  boost::bind(&SimpleActionServer::goalCallback, this, boost::placeholders::_1),
108  boost::bind(&SimpleActionServer::preemptCallback, this, boost::placeholders::_1),
109  auto_start));
110 
111  if (execute_callback_) {
112  execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this));
113  }
114 }
115 
116 template<class ActionSpec>
118  bool auto_start)
119 : n_(n), new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false),
120  execute_callback_(NULL), execute_thread_(nullptr), need_to_terminate_(false)
121 {
122  // create the action server
124  boost::bind(&SimpleActionServer::goalCallback, this, boost::placeholders::_1),
125  boost::bind(&SimpleActionServer::preemptCallback, this, boost::placeholders::_1),
126  auto_start));
127 
128  if (execute_callback_) {
129  execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this));
130  }
131 }
132 
133 template<class ActionSpec>
135  ExecuteCallback execute_callback)
136 : n_(n), new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false),
137  execute_callback_(execute_callback), execute_thread_(nullptr), need_to_terminate_(false)
138 {
139  // create the action server
141  boost::bind(&SimpleActionServer::goalCallback, this, boost::placeholders::_1),
142  boost::bind(&SimpleActionServer::preemptCallback, this, boost::placeholders::_1),
143  true));
144 
145  if (execute_callback_) {
146  execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this));
147  }
148 }
149 
150 template<class ActionSpec>
152 {
153  if (execute_thread_) {
154  shutdown();
155  }
156 }
157 
158 template<class ActionSpec>
160 {
161  if (execute_callback_) {
162  {
163  boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
164  need_to_terminate_ = true;
165  }
166 
167  assert(execute_thread_);
168  if (execute_thread_) {
169  execute_thread_->join();
170  delete execute_thread_;
171  execute_thread_ = nullptr;
172  }
173  }
174 }
175 
176 template<class ActionSpec>
178 ::acceptNewGoal()
179 {
180  boost::recursive_mutex::scoped_lock lock(lock_);
181 
182  if (!new_goal_ || !next_goal_.getGoal()) {
183  ROS_ERROR_NAMED("actionlib",
184  "Attempting to accept the next goal when a new goal is not available");
186  }
187 
188  // check if we need to send a preempted message for the goal that we're currently pursuing
189  if (isActive() &&
190  current_goal_.getGoal() &&
191  current_goal_ != next_goal_)
192  {
193  current_goal_.setCanceled(
194  Result(),
195  "This goal was canceled because another goal was received by the simple action server");
196  }
197 
198  ROS_DEBUG_NAMED("actionlib", "Accepting a new goal");
199 
200  // accept the next goal
201  current_goal_ = next_goal_;
202  new_goal_ = false;
203 
204  // set preempt to request to equal the preempt state of the new goal
205  preempt_request_ = new_goal_preempt_request_;
206  new_goal_preempt_request_ = false;
207 
208  // set the status of the current goal to be active
209  current_goal_.setAccepted("This goal has been accepted by the simple action server");
210 
211  return current_goal_.getGoal();
212 }
213 
214 template<class ActionSpec>
216 {
217  return new_goal_;
218 }
219 
220 
221 template<class ActionSpec>
223 {
224  return preempt_request_;
225 }
226 
227 template<class ActionSpec>
228 bool SimpleActionServer<ActionSpec>::isActive()
229 {
230  if (!current_goal_.getGoal()) {
231  return false;
232  }
233  unsigned int status = current_goal_.getGoalStatus().status;
234  return status == actionlib_msgs::GoalStatus::ACTIVE ||
235  status == actionlib_msgs::GoalStatus::PREEMPTING;
236 }
237 
238 template<class ActionSpec>
239 void SimpleActionServer<ActionSpec>::setSucceeded(const Result & result, const std::string & text)
240 {
241  boost::recursive_mutex::scoped_lock lock(lock_);
242  ROS_DEBUG_NAMED("actionlib", "Setting the current goal as succeeded");
243  current_goal_.setSucceeded(result, text);
244 }
245 
246 template<class ActionSpec>
247 void SimpleActionServer<ActionSpec>::setAborted(const Result & result, const std::string & text)
248 {
249  boost::recursive_mutex::scoped_lock lock(lock_);
250  ROS_DEBUG_NAMED("actionlib", "Setting the current goal as aborted");
251  current_goal_.setAborted(result, text);
252 }
253 
254 template<class ActionSpec>
255 void SimpleActionServer<ActionSpec>::setPreempted(const Result & result, const std::string & text)
256 {
257  boost::recursive_mutex::scoped_lock lock(lock_);
258  ROS_DEBUG_NAMED("actionlib", "Setting the current goal as canceled");
259  current_goal_.setCanceled(result, text);
260 }
261 
262 template<class ActionSpec>
264 {
265  // Cannot register a goal callback if an execute callback exists
266  if (execute_callback_) {
267  ROS_WARN_NAMED("actionlib",
268  "Cannot call SimpleActionServer::registerGoalCallback() because an executeCallback exists. Not going to register it.");
269  } else {
270  goal_callback_ = cb;
271  }
272 }
273 
274 template<class ActionSpec>
275 void SimpleActionServer<ActionSpec>::registerPreemptCallback(boost::function<void()> cb)
276 {
277  preempt_callback_ = cb;
278 }
279 
280 template<class ActionSpec>
281 void SimpleActionServer<ActionSpec>::publishFeedback(const FeedbackConstPtr & feedback)
282 {
283  current_goal_.publishFeedback(*feedback);
284 }
285 
286 template<class ActionSpec>
287 void SimpleActionServer<ActionSpec>::publishFeedback(const Feedback & feedback)
288 {
289  current_goal_.publishFeedback(feedback);
290 }
291 
292 template<class ActionSpec>
294 {
295  boost::recursive_mutex::scoped_lock lock(lock_);
296  ROS_DEBUG_NAMED("actionlib", "A new goal has been received by the single goal action server");
297 
298  // check that the timestamp is past or equal to that of the current goal and the next goal
299  if ((!current_goal_.getGoal() || goal.getGoalID().stamp >= current_goal_.getGoalID().stamp) &&
300  (!next_goal_.getGoal() || goal.getGoalID().stamp >= next_goal_.getGoalID().stamp))
301  {
302  // if next_goal has not been accepted already... its going to get bumped, but we need to let the client know we're preempting
303  if (next_goal_.getGoal() && (!current_goal_.getGoal() || next_goal_ != current_goal_)) {
304  next_goal_.setCanceled(
305  Result(),
306  "This goal was canceled because another goal was received by the simple action server");
307  }
308 
309  next_goal_ = goal;
310  new_goal_ = true;
311  new_goal_preempt_request_ = false;
312 
313  // if the server is active, we'll want to call the preempt callback for the current goal
314  if (isActive()) {
315  preempt_request_ = true;
316  // if the user has registered a preempt callback, we'll call it now
317  if (preempt_callback_) {
318  preempt_callback_();
319  }
320  }
321 
322  // if the user has defined a goal callback, we'll call it now
323  if (goal_callback_) {
324  goal_callback_();
325  }
326 
327  // Trigger runLoop to call execute()
328  execute_condition_.notify_all();
329  } else {
330  // the goal requested has already been preempted by a different goal, so we're not going to execute it
331  goal.setCanceled(
332  Result(),
333  "This goal was canceled because another goal was received by the simple action server");
334  }
335 }
336 
337 template<class ActionSpec>
338 void SimpleActionServer<ActionSpec>::preemptCallback(GoalHandle preempt)
339 {
340  boost::recursive_mutex::scoped_lock lock(lock_);
341  ROS_DEBUG_NAMED("actionlib", "A preempt has been received by the SimpleActionServer");
342 
343  // if the preempt is for the current goal, then we'll set the preemptRequest flag and call the user's preempt callback
344  if (preempt == current_goal_) {
345  ROS_DEBUG_NAMED("actionlib",
346  "Setting preempt_request bit for the current goal to TRUE and invoking callback");
347  preempt_request_ = true;
348 
349  // if the user has registered a preempt callback, we'll call it now
350  if (preempt_callback_) {
351  preempt_callback_();
352  }
353  } else if (preempt == next_goal_) {
354  // if the preempt applies to the next goal, we'll set the preempt bit for that
355  ROS_DEBUG_NAMED("actionlib", "Setting preempt request bit for the next goal to TRUE");
356  new_goal_preempt_request_ = true;
357  }
358 }
359 
360 template<class ActionSpec>
361 void SimpleActionServer<ActionSpec>::executeLoop()
362 {
363  ros::Duration loop_duration = ros::Duration().fromSec(.1);
364 
365  while (n_.ok()) {
366  {
367  boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
368  if (need_to_terminate_) {
369  break;
370  }
371  }
372 
373  boost::recursive_mutex::scoped_lock lock(lock_);
374  if (isActive()) {
375  ROS_ERROR_NAMED("actionlib", "Should never reach this code with an active goal");
376  } else if (isNewGoalAvailable()) {
377  GoalConstPtr goal = acceptNewGoal();
378 
379  ROS_FATAL_COND(!execute_callback_,
380  "execute_callback_ must exist. This is a bug in SimpleActionServer");
381 
382  {
383  // Make sure we're not locked when we call execute
384  boost::reverse_lock<boost::recursive_mutex::scoped_lock> unlocker(lock);
385  execute_callback_(goal);
386  }
387 
388  if (isActive()) {
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");
392  setAborted(
393  Result(),
394  "This goal was aborted by the simple action server. The user should have set a terminal status on this goal and did not");
395  }
396  } else {
397  execute_condition_.timed_wait(lock,
398  boost::posix_time::milliseconds(static_cast<int64_t>(loop_duration.toSec() * 1000.0f)));
399  }
400  }
401 }
402 
403 template<class ActionSpec>
404 void SimpleActionServer<ActionSpec>::start()
405 {
406  as_->start();
407 }
408 
409 } // namespace actionlib
410 
411 #endif // ACTIONLIB__SERVER__SIMPLE_ACTION_SERVER_IMP_H_
actionlib::SimpleActionServer::publishFeedback
void publishFeedback(const FeedbackConstPtr &feedback)
Publishes feedback for a given goal.
Definition: simple_action_server_imp.h:316
DurationBase< Duration >::fromSec
Duration & fromSec(double t)
boost::shared_ptr
actionlib::ActionServer
The ActionServer is a helpful tool for managing goal requests to a node. It allows the user to specif...
Definition: action_server.h:107
ros.h
ROS_FATAL_COND
#define ROS_FATAL_COND(cond,...)
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
actionlib::SimpleActionServer< TestAction >::ExecuteCallback
boost::function< void(const GoalConstPtr &)> ExecuteCallback
Definition: simple_action_server.h:102
shutdown
ROSCONSOLE_DECL void shutdown()
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
actionlib::SimpleActionServer
Definition: simple_action_server.h:95
actionlib
Definition: action_definition.h:40
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
DurationBase< Duration >::toSec
double toSec() const
ros::Duration
actionlib::SimpleActionServer::SimpleActionServer
SimpleActionServer(std::string name, ExecuteCallback execute_callback, bool auto_start)
Constructor for a SimpleActionServer.
Definition: simple_action_server_imp.h:82
ros::NodeHandle


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep, Mikael Arguedas
autogenerated on Fri May 19 2023 02:36:55