action_server_base.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__ACTION_SERVER_BASE_H_
38 #define ACTIONLIB__SERVER__ACTION_SERVER_BASE_H_
39 
40 #include <ros/ros.h>
41 #include <boost/thread.hpp>
42 #include <boost/thread/reverse_lock.hpp>
43 #include <boost/shared_ptr.hpp>
44 #include <actionlib_msgs/GoalID.h>
45 #include <actionlib_msgs/GoalStatusArray.h>
46 #include <actionlib_msgs/GoalStatus.h>
54 
55 #include <list>
56 
57 namespace actionlib
58 {
63 template<class ActionSpec>
65 {
66 public:
67  // for convenience when referring to ServerGoalHandles
69 
70  // generates typedefs that we'll use to make our lives easier
71  ACTION_DEFINITION(ActionSpec)
72 
73 
80  boost::function<void(GoalHandle)> goal_cb,
81  boost::function<void(GoalHandle)> cancel_cb,
82  bool auto_start = false);
83 
84 
88  virtual ~ActionServerBase();
89 
94  void registerGoalCallback(boost::function<void(GoalHandle)> cb);
95 
100  void registerCancelCallback(boost::function<void(GoalHandle)> cb);
101 
105  void start();
106 
107 
111  void goalCallback(const boost::shared_ptr<const ActionGoal> & goal);
112 
116  void cancelCallback(const boost::shared_ptr<const actionlib_msgs::GoalID> & goal_id);
117 
118 protected:
119  // Allow access to protected fields for helper classes
120  friend class ServerGoalHandle<ActionSpec>;
121  friend class HandleTrackerDeleter<ActionSpec>;
122 
126  virtual void initialize() = 0;
127 
133  virtual void publishResult(const actionlib_msgs::GoalStatus & status, const Result & result) = 0;
134 
140  virtual void publishFeedback(const actionlib_msgs::GoalStatus & status,
141  const Feedback & feedback) = 0;
142 
146  virtual void publishStatus() = 0;
147 
148  boost::recursive_mutex lock_;
149 
150  std::list<StatusTracker<ActionSpec> > status_list_;
151 
152  boost::function<void(GoalHandle)> goal_callback_;
153  boost::function<void(GoalHandle)> cancel_callback_;
154 
157 
159  bool started_;
161 };
162 
163 template<class ActionSpec>
164 ActionServerBase<ActionSpec>::ActionServerBase(
165  boost::function<void(GoalHandle)> goal_cb,
166  boost::function<void(GoalHandle)> cancel_cb,
167  bool auto_start)
168 : goal_callback_(goal_cb),
169  cancel_callback_(cancel_cb),
170  started_(auto_start),
171  guard_(new DestructionGuard)
172 {
173 }
174 
175 template<class ActionSpec>
177 {
178  // Block until we can safely destruct
179  guard_->destruct();
180 }
181 
182 template<class ActionSpec>
183 void ActionServerBase<ActionSpec>::registerGoalCallback(boost::function<void(GoalHandle)> cb)
184 {
185  goal_callback_ = cb;
186 }
187 
188 template<class ActionSpec>
189 void ActionServerBase<ActionSpec>::registerCancelCallback(boost::function<void(GoalHandle)> cb)
190 {
191  cancel_callback_ = cb;
192 }
193 
194 template<class ActionSpec>
196 {
197  initialize();
198  started_ = true;
199  publishStatus();
200 }
201 
202 
203 template<class ActionSpec>
205 {
206  boost::recursive_mutex::scoped_lock lock(lock_);
207 
208  // if we're not started... then we're not actually going to do anything
209  if (!started_) {
210  return;
211  }
212 
213  ROS_DEBUG_NAMED("actionlib", "The action server has received a new goal request");
214 
215  // we need to check if this goal already lives in the status list
216  for (typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.begin();
217  it != status_list_.end(); ++it)
218  {
219  if (goal->goal_id.id == (*it).status_.goal_id.id) {
220  // The goal could already be in a recalling state if a cancel came in before the goal
221  if ( (*it).status_.status == actionlib_msgs::GoalStatus::RECALLING) {
222  (*it).status_.status = actionlib_msgs::GoalStatus::RECALLED;
223  publishResult((*it).status_, Result());
224  }
225 
226  // if this is a request for a goal that has no active handles left,
227  // we'll bump how long it stays in the list
228  if ((*it).handle_tracker_.expired()) {
229  (*it).handle_destruction_time_ = goal->goal_id.stamp;
230  }
231 
232  // make sure not to call any user callbacks or add duplicate status onto the list
233  return;
234  }
235  }
236 
237  // if the goal is not in our list, we need to create a StatusTracker associated with this goal and push it on
238  typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.insert(
240 
241  // we need to create a handle tracker for the incoming goal and update the StatusTracker
243  boost::shared_ptr<void> handle_tracker((void *)NULL, d);
244  (*it).handle_tracker_ = handle_tracker;
245 
246  // check if this goal has already been canceled based on its timestamp
247  if (goal->goal_id.stamp != ros::Time() && goal->goal_id.stamp <= last_cancel_) {
248  // if it has... just create a GoalHandle for it and setCanceled
249  GoalHandle gh(it, this, handle_tracker, guard_);
250  gh.setCanceled(
251  Result(),
252  "This goal handle was canceled by the action server because its timestamp is before the timestamp of the last cancel request");
253  } else {
254  GoalHandle gh = GoalHandle(it, this, handle_tracker, guard_);
255 
256  // make sure that we unlock before calling the users callback
257  boost::reverse_lock<boost::recursive_mutex::scoped_lock> unlocker(lock);
258 
259  // now, we need to create a goal handle and call the user's callback
260  goal_callback_(gh);
261  }
262 }
263 
264 template<class ActionSpec>
267 {
268  boost::recursive_mutex::scoped_lock lock(lock_);
269 
270  // if we're not started... then we're not actually going to do anything
271  if (!started_) {
272  return;
273  }
274 
275  // we need to handle a cancel for the user
276  ROS_DEBUG_NAMED("actionlib", "The action server has received a new cancel request");
277  bool goal_id_found = false;
278  for (typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.begin();
279  it != status_list_.end(); ++it)
280  {
281  // check if the goal id is zero or if it is equal to the goal id of
282  // the iterator or if the time of the iterator warrants a cancel
283  if (
284  (goal_id->id == "" && goal_id->stamp == ros::Time()) || // id and stamp 0 --> cancel everything
285  goal_id->id == (*it).status_.goal_id.id || // ids match... cancel that goal
286  (goal_id->stamp != ros::Time() && (*it).status_.goal_id.stamp <= goal_id->stamp) // stamp != 0 --> cancel everything before stamp
287  )
288  {
289  // we need to check if we need to store this cancel request for later
290  if (goal_id->id == (*it).status_.goal_id.id) {
291  goal_id_found = true;
292  }
293 
294  // attempt to get the handle_tracker for the list item if it exists
295  boost::shared_ptr<void> handle_tracker = (*it).handle_tracker_.lock();
296 
297  if ((*it).handle_tracker_.expired()) {
298  // if the handle tracker is expired, then we need to create a new one
300  handle_tracker = boost::shared_ptr<void>((void *)NULL, d);
301  (*it).handle_tracker_ = handle_tracker;
302 
303  // we also need to reset the time that the status is supposed to be removed from the list
304  (*it).handle_destruction_time_ = ros::Time();
305  }
306 
307  // set the status of the goal to PREEMPTING or RECALLING as approriate
308  // and check if the request should be passed on to the user
309  GoalHandle gh(it, this, handle_tracker, guard_);
310  if (gh.setCancelRequested()) {
311  // make sure that we're unlocked before we call the users callback
312  boost::reverse_lock<boost::recursive_mutex::scoped_lock> unlocker(lock);
313 
314  // call the user's cancel callback on the relevant goal
315  cancel_callback_(gh);
316  }
317  }
318  }
319 
320  // if the requested goal_id was not found, and it is non-zero, then we need to store the cancel request
321  if (goal_id->id != "" && !goal_id_found) {
322  typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.insert(
323  status_list_.end(),
324  StatusTracker<ActionSpec>(*goal_id, actionlib_msgs::GoalStatus::RECALLING));
325  // start the timer for how long the status will live in the list without a goal handle to it
326  (*it).handle_destruction_time_ = goal_id->stamp;
327  }
328 
329  // make sure to set last_cancel_ based on the stamp associated with this cancel request
330  if (goal_id->stamp > last_cancel_) {
331  last_cancel_ = goal_id->stamp;
332  }
333 }
334 
335 } // namespace actionlib
336 #endif // ACTIONLIB__SERVER__ACTION_SERVER_BASE_H_
d
Definition: setup.py:6
void goalCallback(const boost::shared_ptr< const ActionGoal > &goal)
The ROS callback for goals coming into the ActionServerBase.
virtual void publishFeedback(const actionlib_msgs::GoalStatus &status, const Feedback &feedback)=0
Publishes feedback for a given goal.
A class to help with tracking GoalHandles and removing goals from the status list when the last GoalH...
virtual void publishStatus()=0
Explicitly publish status.
boost::shared_ptr< DestructionGuard > guard_
virtual void initialize()=0
Initialize all ROS connections and setup timers.
ServerGoalHandle< ActionSpec > GoalHandle
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...
boost::recursive_mutex lock_
void registerGoalCallback(boost::function< void(GoalHandle)> cb)
Register a callback to be invoked when a new goal is received, this will replace any previously regis...
#define ROS_DEBUG_NAMED(name,...)
boost::function< void(GoalHandle)> goal_callback_
A class for storing the status of each goal the action server is currently working on...
std::list< StatusTracker< ActionSpec > > status_list_
The ActionServerBase implements the logic for an ActionServer.
Encapsulates a state machine for a given goal that the user can trigger transitions on...
This class protects an object from being destructed until all users of that object relinquish control...
#define ACTION_DEFINITION(ActionSpec)
void registerCancelCallback(boost::function< void(GoalHandle)> cb)
Register a callback to be invoked when a new cancel is received, this will replace any previously reg...
void start()
Explicitly start the action server, used it auto_start is set to false.
virtual void publishResult(const actionlib_msgs::GoalStatus &status, const Result &result)=0
Publishes a result for a given goal.
virtual ~ActionServerBase()
Destructor for the ActionServerBase.
void cancelCallback(const boost::shared_ptr< const actionlib_msgs::GoalID > &goal_id)
The ROS callback for cancel requests coming into the ActionServerBase.
boost::function< void(GoalHandle)> cancel_callback_
bool setCancelRequested()
A private method to set status to PENDING or RECALLING.


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