37 #ifndef ACTIONLIB__SERVER__ACTION_SERVER_BASE_H_
38 #define ACTIONLIB__SERVER__ACTION_SERVER_BASE_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>
63 template<
class ActionSpec>
64 class ActionServerBase
82 bool auto_start =
false);
120 friend class ServerGoalHandle<ActionSpec>;
121 friend class HandleTrackerDeleter<ActionSpec>;
133 virtual void publishResult(
const actionlib_msgs::GoalStatus & status,
const Result & result) = 0;
140 virtual void publishFeedback(
const actionlib_msgs::GoalStatus & status,
141 const Feedback & feedback) = 0;
148 boost::recursive_mutex
lock_;
163 template<
class ActionSpec>
165 boost::function<
void(GoalHandle)> goal_cb,
166 boost::function<
void(GoalHandle)> cancel_cb,
168 : goal_callback_(goal_cb),
169 cancel_callback_(cancel_cb),
170 started_(auto_start),
175 template<
class ActionSpec>
176 ActionServerBase<ActionSpec>::~ActionServerBase()
182 template<
class ActionSpec>
188 template<
class ActionSpec>
191 cancel_callback_ = cb;
194 template<
class ActionSpec>
203 template<
class ActionSpec>
206 boost::recursive_mutex::scoped_lock lock(lock_);
213 ROS_DEBUG_NAMED(
"actionlib",
"The action server has received a new goal request");
217 it != status_list_.end(); ++it)
219 if (goal->goal_id.id == (*it).status_.goal_id.id) {
221 if ( (*it).status_.status == actionlib_msgs::GoalStatus::RECALLING) {
222 (*it).status_.status = actionlib_msgs::GoalStatus::RECALLED;
223 publishResult((*it).status_, Result());
228 if ((*it).handle_tracker_.expired()) {
229 (*it).handle_destruction_time_ = goal->goal_id.stamp;
238 typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.insert(
244 (*it).handle_tracker_ = handle_tracker;
247 if (goal->goal_id.stamp !=
ros::Time() && goal->goal_id.stamp <= last_cancel_) {
249 GoalHandle gh(it,
this, handle_tracker, guard_);
252 "This goal handle was canceled by the action server because its timestamp is before the timestamp of the last cancel request");
254 GoalHandle gh = GoalHandle(it,
this, handle_tracker, guard_);
257 boost::reverse_lock<boost::recursive_mutex::scoped_lock> unlocker(lock);
264 template<
class ActionSpec>
265 void ActionServerBase<ActionSpec>::cancelCallback(
268 boost::recursive_mutex::scoped_lock lock(lock_);
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)
284 (goal_id->id ==
"" && goal_id->stamp ==
ros::Time()) ||
285 goal_id->id == (*it).status_.goal_id.id ||
286 (goal_id->stamp !=
ros::Time() && (*it).status_.goal_id.stamp <= goal_id->stamp)
290 if (goal_id->id == (*it).status_.goal_id.id) {
291 goal_id_found =
true;
297 if ((*it).handle_tracker_.expired()) {
299 HandleTrackerDeleter<ActionSpec>
d(
this, it, guard_);
301 (*it).handle_tracker_ = handle_tracker;
304 (*it).handle_destruction_time_ =
ros::Time();
309 GoalHandle gh(it,
this, handle_tracker, guard_);
310 if (gh.setCancelRequested()) {
312 boost::reverse_lock<boost::recursive_mutex::scoped_lock> unlocker(lock);
315 cancel_callback_(gh);
321 if (goal_id->id !=
"" && !goal_id_found) {
322 typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.insert(
324 StatusTracker<ActionSpec>(*goal_id, actionlib_msgs::GoalStatus::RECALLING));
326 (*it).handle_destruction_time_ = goal_id->stamp;
330 if (goal_id->stamp > last_cancel_) {
331 last_cancel_ = goal_id->stamp;
336 #endif // ACTIONLIB__SERVER__ACTION_SERVER_BASE_H_