Commit 0b655d6c authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added the handling of the rejected state in the module_action.

parent d00886b0
......@@ -33,7 +33,8 @@ typedef enum {ACTION_IDLE,
ACTION_TIMEOUT,
ACTION_FB_WATCHDOG,
ACTION_ABORTED,
ACTION_PREEMPTED} action_status;
ACTION_PREEMPTED,
ACTION_REJECTED} action_status;
/**
* \brief Action client wrapper
......@@ -205,6 +206,11 @@ class CModuleAction
ROS_ERROR_STREAM("CModuleAction::action_done: goal on server " << this->name << " aborted");
this->status=ACTION_ABORTED;
}
if(state==actionlib::SimpleClientGoalState::REJECTED)
{
ROS_ERROR_STREAM("CModuleAction::action_done: goal on server " << this->name << " rejected");
this->status=ACTION_REJECTED;
}
else if(state==actionlib::SimpleClientGoalState::PREEMPTED)
{
ROS_WARN_STREAM("CModuleAction::action_done: goal on server " << this->name << " preempted");
......@@ -669,7 +675,7 @@ template<class action_ros>
bool CModuleAction<action_ros>::is_finished(void)
{
actionlib::SimpleClientGoalState action_state(actionlib::SimpleClientGoalState::PENDING);
action_state=action_client->getState();
if(action_state==actionlib::SimpleClientGoalState::ACTIVE)
return false;
......@@ -680,6 +686,9 @@ bool CModuleAction<action_ros>::is_finished(void)
template<class action_ros>
action_status CModuleAction<action_ros>::get_state(void)
{
actionlib::SimpleClientGoalState action_state(actionlib::SimpleClientGoalState::PENDING);
action_state=action_client->getState();
if(this->status==ACTION_RUNNING)
{
if(this->use_timeout && this->is_timeout_active())
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment