Skip to content
Snippets Groups Projects
Commit 3322344f authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Modified the iri action server to use the isActive() function in the execute callback function.

parent 3cc8a8f9
No related branches found
No related tags found
No related merge requests found
...@@ -73,7 +73,7 @@ class IriActionServer ...@@ -73,7 +73,7 @@ class IriActionServer
typedef boost::shared_ptr<Feedback> FeedbackPtr; typedef boost::shared_ptr<Feedback> FeedbackPtr;
private: private:
/** /**
* \brief Start Callback object * \brief Start Callback object
* *
...@@ -307,6 +307,7 @@ IriActionServer<ActionSpec>::IriActionServer(ros::NodeHandle & nh, const std::st ...@@ -307,6 +307,7 @@ IriActionServer<ActionSpec>::IriActionServer(ros::NodeHandle & nh, const std::st
template <class ActionSpec> template <class ActionSpec>
IriActionServer<ActionSpec>::~IriActionServer(void) IriActionServer<ActionSpec>::~IriActionServer(void)
{ {
ROS_DEBUG("IriActionServer::Destructor"); ROS_DEBUG("IriActionServer::Destructor");
} }
...@@ -359,10 +360,10 @@ void IriActionServer<ActionSpec>::executeCallback(const GoalConstPtr& goal) ...@@ -359,10 +360,10 @@ void IriActionServer<ActionSpec>::executeCallback(const GoalConstPtr& goal)
start_action_callback_(goal); start_action_callback_(goal);
// boolean to control action status // boolean to control action status
bool is_active = true; //bool is_active = true;
// while action is active // while action is active
while( is_active ) while(this->as_.isActive() )
{ {
ROS_DEBUG("IriActionServer::executeCallback::Active!"); ROS_DEBUG("IriActionServer::executeCallback::Active!");
...@@ -371,17 +372,17 @@ void IriActionServer<ActionSpec>::executeCallback(const GoalConstPtr& goal) ...@@ -371,17 +372,17 @@ void IriActionServer<ActionSpec>::executeCallback(const GoalConstPtr& goal)
if( as_.isPreemptRequested() || !ros::ok() ) if( as_.isPreemptRequested() || !ros::ok() )
{ {
ROS_DEBUG("IriActionServer::executeCallback::PREEMPTED!"); ROS_DEBUG("IriActionServer::executeCallback::PREEMPTED!");
is_active = false; //is_active = false;
// stop action // stop action
stop_action_callback_(); stop_action_callback_();
as_.setPreempted(); as_.setAborted();
} }
// check if action has finished // check if action has finished
else if( is_finished_callback_() ) else if( is_finished_callback_() )
{ {
ROS_DEBUG("IriActionServer::executeCallback::FINISH!"); ROS_DEBUG("IriActionServer::executeCallback::FINISH!");
is_active = false; //is_active = false;
// get action result // get action result
ResultPtr result(new Result); ResultPtr result(new Result);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment