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_(NULL), need_to_terminate_(false)
52 {
53  if (execute_callback_ != NULL) {
54  execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this));
55  }
56 
57  // create the action server
59  boost::bind(&SimpleActionServer::goalCallback, this, _1),
60  boost::bind(&SimpleActionServer::preemptCallback, this, _1),
61  auto_start));
62 }
63 
64 template<class ActionSpec>
65 SimpleActionServer<ActionSpec>::SimpleActionServer(std::string name, bool auto_start)
67  NULL), execute_thread_(NULL), need_to_terminate_(false)
68 {
69  // create the action server
71  boost::bind(&SimpleActionServer::goalCallback, this, _1),
72  boost::bind(&SimpleActionServer::preemptCallback, this, _1),
73  auto_start));
74 
75  if (execute_callback_ != NULL) {
76  execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this));
77  }
78 }
79 
80 template<class ActionSpec>
82  ExecuteCallback execute_callback)
84  execute_callback), execute_thread_(NULL), need_to_terminate_(false)
85 {
86  // create the action server
88  boost::bind(&SimpleActionServer::goalCallback, this, _1),
89  boost::bind(&SimpleActionServer::preemptCallback, this, _1),
90  true));
91 
92  if (execute_callback_ != NULL) {
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_(NULL), need_to_terminate_(false)
104 {
105  // create the action server
107  boost::bind(&SimpleActionServer::goalCallback, this, _1),
108  boost::bind(&SimpleActionServer::preemptCallback, this, _1),
109  auto_start));
110 
111  if (execute_callback_ != NULL) {
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),
121 {
122  // create the action server
124  boost::bind(&SimpleActionServer::goalCallback, this, _1),
125  boost::bind(&SimpleActionServer::preemptCallback, this, _1),
126  auto_start));
127 
128  if (execute_callback_ != NULL) {
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_(NULL), need_to_terminate_(false)
138 {
139  // create the action server
141  boost::bind(&SimpleActionServer::goalCallback, this, _1),
142  boost::bind(&SimpleActionServer::preemptCallback, this, _1),
143  true));
144 
145  if (execute_callback_ != NULL) {
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_ = NULL;
172  }
173  }
174 }
175 
176 template<class ActionSpec>
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() &&
192  {
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
202  new_goal_ = false;
203 
204  // set preempt to request to equal the preempt state of the new goal
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>
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>
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
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;
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_) {
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>
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_) {
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");
357  }
358 }
359 
360 template<class ActionSpec>
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 
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>
405 {
406  as_->start();
407 }
408 
409 } // namespace actionlib
410 
411 #endif // ACTIONLIB__SERVER__SIMPLE_ACTION_SERVER_IMP_H_
void publishFeedback(const Feedback &feedback)
Send feedback to any clients of the goal associated with this ServerGoalHandle.
boost::shared_ptr< const Goal > acceptNewGoal()
Accepts a new goal when one is available. The status of this goal is set to active upon acceptance...
void publishFeedback(const FeedbackConstPtr &feedback)
Publishes feedback for a given goal.
boost::function< void()> goal_callback_
void preemptCallback(GoalHandle preempt)
Callback for when the ActionServer receives a new preempt and passes it on.
#define ROS_WARN_NAMED(name,...)
actionlib_msgs::GoalID getGoalID() const
Accessor for the goal id associated with the ServerGoalHandle.
void goalCallback(GoalHandle goal)
Callback for when the ActionServer receives a new goal and passes it on.
void executeLoop()
Called from a separate thread to call blocking execute calls.
SimpleActionServer(std::string name, ExecuteCallback execute_callback, bool auto_start)
Constructor for a SimpleActionServer.
boost::shared_ptr< const Goal > getGoal() const
Accessor for the goal associated with the ServerGoalHandle.
#define ROS_FATAL_COND(cond,...)
bool isActive()
Allows polling implementations to query about the status of the current goal.
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
Sets the status of the active goal to succeeded.
void setCanceled(const Result &result=Result(), const std::string &text=std::string(""))
Set the status of the goal associated with the ServerGoalHandle to RECALLED or PREEMPTED depending on...
void shutdown()
Explicitly shutdown the action server.
boost::function< void(const GoalConstPtr &)> ExecuteCallback
void start()
Explicitly start the action server, used if auto_start is set to false.
void setAccepted(const std::string &text=std::string(""))
Accept the goal referenced by the goal handle. This will transition to the ACTIVE state or the PREEMP...
#define ROS_DEBUG_NAMED(name,...)
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
Sets the status of the active goal to preempted.
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
Set the status of the goal associated with the ServerGoalHandle to succeeded.
boost::function< void()> preempt_callback_
Duration & fromSec(double t)
void registerPreemptCallback(boost::function< void()> cb)
Allows users to register a callback to be invoked when a new preempt request is available.
bool isPreemptRequested()
Allows polling implementations to query about preempt requests.
Encapsulates a state machine for a given goal that the user can trigger transitions on...
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
Set the status of the goal associated with the ServerGoalHandle to aborted.
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
Sets the status of the active goal to aborted.
#define ROS_ERROR_NAMED(name,...)
The ActionServer is a helpful tool for managing goal requests to a node. It allows the user to specif...
Definition: action_server.h:72
boost::shared_ptr< ActionServer< ActionSpec > > as_
bool ok() const
void registerGoalCallback(boost::function< void()> cb)
Allows users to register a callback to be invoked when a new goal is available.
actionlib_msgs::GoalStatus getGoalStatus() const
Accessor for the status associated with the ServerGoalHandle.
bool isNewGoalAvailable()
Allows polling implementations to query about the availability of a new goal.


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep, Mikael Arguedas
autogenerated on Mon Aug 24 2020 03:40:47