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>
80 boost::function<
void(GoalHandle)> goal_cb,
81 boost::function<
void(GoalHandle)> cancel_cb,
82 bool auto_start = false);
141 const Feedback & feedback) = 0;
163 template<class ActionSpec>
164 ActionServerBase<ActionSpec>::ActionServerBase(
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>
182 template<
class ActionSpec>
188 template<
class ActionSpec>
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");
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;
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;
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");
257 boost::reverse_lock<boost::recursive_mutex::scoped_lock> unlocker(lock);
264 template<
class ActionSpec>
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;
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()) {
301 (*it).handle_tracker_ = handle_tracker;
304 (*it).handle_destruction_time_ =
ros::Time();
309 GoalHandle gh(it,
this, handle_tracker,
guard_);
312 boost::reverse_lock<boost::recursive_mutex::scoped_lock> unlocker(lock);
321 if (goal_id->id !=
"" && !goal_id_found) {
322 typename std::list<StatusTracker<ActionSpec> >::iterator it =
status_list_.insert(
326 (*it).handle_destruction_time_ = goal_id->stamp;
336 #endif // ACTIONLIB__SERVER__ACTION_SERVER_BASE_H_
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.
GoalIDGenerator id_generator_
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_
ros::Duration status_list_timeout_
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.