Commit 960d75a5 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added an enable parameter to skip the operation of the action and service clients.

parent e98ea808
......@@ -155,6 +155,11 @@ class CModuleAction
* further details.
*/
CROSWatchdog feedback_watchdog;
/**
* \brief
*
*/
bool enabled;
// action attributes
/**
* \brief ROS topic messages names
......@@ -381,6 +386,21 @@ class CModuleAction
*
*/
std::string get_name(void);
/**
* \brief
*
*/
void enable(void);
/**
* \brief
*
*/
void disable(void);
/**
* \brief
*
*/
bool is_enabled(void);
/**
* \brief start the action
*
......@@ -399,39 +419,47 @@ class CModuleAction
*/
act_srv_status make_request(Goal &msg)
{
if(this->action_client->isServerConnected())
{
ROS_DEBUG_STREAM("CModuleAction::make_request: Server " << this->name << " is Available!");
this->current_num_retries=0;
this->action_client->sendGoal(msg,
boost::bind(&CModuleAction<action_ros>::action_done, this, _1, _2),
boost::bind(&CModuleAction<action_ros>::action_active, this),
boost::bind(&CModuleAction<action_ros>::action_feedback, this, _1));
this->feedback_watchdog.reset(ros::Duration(this->watchdog_time));
this->status=ACTION_RUNNING;
if(this->use_timeout)
{
if(this->timeout_value>0.0)
this->action_timeout.start(ros::Duration(this->timeout_value));
}
ROS_DEBUG_STREAM("CModuleAction::make_request: Goal Sent to server " << this->name << ". Wait for Result!");
return ACT_SRV_SUCCESS;
}
else
if(this->enabled)
{
this->current_num_retries++;
if(this->current_num_retries>this->max_num_retries)
if(this->action_client->isServerConnected())
{
ROS_DEBUG_STREAM("CModuleAction::make_request: Server " << this->name << " is Available!");
this->current_num_retries=0;
ROS_DEBUG_STREAM("CModuleAction::make_request: Action start on server " << this->name << " failed!");
return ACT_SRV_FAIL;
this->action_client->sendGoal(msg,
boost::bind(&CModuleAction<action_ros>::action_done, this, _1, _2),
boost::bind(&CModuleAction<action_ros>::action_active, this),
boost::bind(&CModuleAction<action_ros>::action_feedback, this, _1));
this->feedback_watchdog.reset(ros::Duration(this->watchdog_time));
this->status=ACTION_RUNNING;
if(this->use_timeout)
{
if(this->timeout_value>0.0)
this->action_timeout.start(ros::Duration(this->timeout_value));
}
ROS_DEBUG_STREAM("CModuleAction::make_request: Goal Sent to server " << this->name << ". Wait for Result!");
return ACT_SRV_SUCCESS;
}
else
{
ROS_DEBUG_STREAM("CModuleAction::make_request: Action start on server " << this->name << " pending!");
return ACT_SRV_PENDING;
this->current_num_retries++;
if(this->current_num_retries>this->max_num_retries)
{
this->current_num_retries=0;
ROS_DEBUG_STREAM("CModuleAction::make_request: Action start on server " << this->name << " failed!");
return ACT_SRV_FAIL;
}
else
{
ROS_DEBUG_STREAM("CModuleAction::make_request: Action start on server " << this->name << " pending!");
return ACT_SRV_PENDING;
}
}
}
else
{
this->status=ACTION_RUNNING;
return ACT_SRV_SUCCESS;
}
}
/**
......@@ -518,6 +546,8 @@ CModuleAction<action_ros>::CModuleAction(const std::string &name,const std::stri
// internal action
this->action_client=NULL;
this->action_client=new actionlib::SimpleActionClient<action_ros>(this->nh,name,true);
// enable parameter
this->enabled=true;
}
template<class action_ros>
......@@ -564,17 +594,22 @@ template<class action_ros>
bool CModuleAction<action_ros>::is_watchdog_active(void)
{
this->action_access.enter();
if(this->feedback_watchdog.is_active())
if(this->enabled)
{
this->status=ACTION_FB_WATCHDOG;
this->action_access.exit();
return true;
if(this->feedback_watchdog.is_active())
{
this->status=ACTION_FB_WATCHDOG;
this->action_access.exit();
return true;
}
else
{
this->action_access.exit();
return false;
}
}
else
{
this->action_access.exit();
return false;
}
}
template<class action_ros>
......@@ -653,6 +688,28 @@ bool CModuleAction<action_ros>::is_timeout_active(void)
}
}
template<class action_ros>
void CModuleAction<action_ros>::enable(void)
{
this->action_access.enter();
this->enabled=true;
this->action_access.exit();
}
template<class action_ros>
void CModuleAction<action_ros>::disable(void)
{
this->action_access.enter();
this->enabled=false;
this->action_access.exit();
}
template<class action_ros>
bool CModuleAction<action_ros>::is_enabled(void)
{
return this->enabled;
}
template<class action_ros>
std::string CModuleAction<action_ros>::get_name(void)
{
......@@ -663,10 +720,13 @@ template<class action_ros>
void CModuleAction<action_ros>::cancel(void)
{
actionlib::SimpleClientGoalState action_state(actionlib::SimpleClientGoalState::PENDING);
action_state=action_client->getState();
if(action_state==actionlib::SimpleClientGoalState::ACTIVE)
this->action_client->cancelGoal();
if(this->enabled)
{
action_state=action_client->getState();
if(action_state==actionlib::SimpleClientGoalState::ACTIVE)
this->action_client->cancelGoal();
}
}
template<class action_ros>
......@@ -674,23 +734,33 @@ 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;
else
if(this->enabled)
{
action_state=action_client->getState();
if(action_state==actionlib::SimpleClientGoalState::ACTIVE)
return false;
else
return true;
}
else
return true;
}
template<class action_ros>
action_status CModuleAction<action_ros>::get_state(void)
{
if(this->status==ACTION_RUNNING)
if(this->enabled)
{
if(this->use_timeout && this->is_timeout_active())
this->status=ACTION_TIMEOUT;
if(this->is_watchdog_active())
this->status=ACTION_FB_WATCHDOG;
if(this->status==ACTION_RUNNING)
{
if(this->use_timeout && this->is_timeout_active())
this->status=ACTION_TIMEOUT;
if(this->is_watchdog_active())
this->status=ACTION_FB_WATCHDOG;
}
}
else
this->status=ACTION_SUCCESS;
return this->status;
}
......@@ -698,10 +768,13 @@ action_status CModuleAction<action_ros>::get_state(void)
template<class action_ros>
CModuleAction<action_ros>::~CModuleAction()
{
if(this->status==ACTION_RUNNING)
if(this->enabled)
{
this->action_client->cancelGoal();
while(!this->is_finished());
if(this->status==ACTION_RUNNING)
{
this->action_client->cancelGoal();
while(!this->is_finished());
}
}
delete this->action_client;
}
......
......@@ -55,6 +55,11 @@ class CModuleService
*
*/
unsigned int max_num_retries;
/**
* \brief
*
*/
bool enabled;
/**
* \brief internal ROS node handle
*
......@@ -136,6 +141,21 @@ class CModuleService
*
*/
std::string get_name(void);
/**
* \brief
*
*/
void enable(void);
/**
* \brief
*
*/
void disable(void);
/**
* \brief
*
*/
bool is_enabled(void);
/**
* \brief Sets the callback function
*
......@@ -178,6 +198,8 @@ CModuleService<service_msg>::CModuleService(const std::string &name,const std::s
this->name=this->nh.getNamespace()+"/"+name;
// assign the default check function
this->call_successfull=boost::bind(&CModuleService::default_call_check, this, _1);
// enable parameter
this->enabled=true;
}
template<class service_msg>
......@@ -204,18 +226,54 @@ std::string CModuleService<service_msg>::get_name(void)
return this->name;
}
template<class service_msg>
void CModuleService<service_msg>::enable(void)
{
this->enabled=true;
}
template<class service_msg>
void CModuleService<service_msg>::disable(void)
{
this->enabled=false;
}
template<class service_msg>
bool CModuleService<service_msg>::is_enabled(void)
{
return this->enabled;
}
template<class service_msg>
act_srv_status CModuleService<service_msg>::call(service_msg &msg)
{
ROS_DEBUG_STREAM("CModuleService::call: Sending New Request to service " << this->service_client.getService());
if(this->service_client.call(msg))
if(this->enabled)
{
if(this->call_successfull(msg))
if(this->service_client.call(msg))
{
ROS_DEBUG_STREAM("CModuleService::call: Request successfull! (server: " << this->service_client.getService() << ")");
this->current_num_retries=0;
return ACT_SRV_SUCCESS;
if(this->call_successfull(msg))
{
ROS_DEBUG_STREAM("CModuleService::call: Request successfull! (server: " << this->service_client.getService() << ")");
this->current_num_retries=0;
return ACT_SRV_SUCCESS;
}
else
{
this->current_num_retries++;
if(this->current_num_retries>this->max_num_retries)
{
ROS_ERROR_STREAM("CModuleService::call: Request failed! (server: " << this->service_client.getService() << ")");
this->current_num_retries=0;
return ACT_SRV_FAIL;
}
else
{
ROS_WARN_STREAM("CModuleService::call: Request pending! (server: " << this->service_client.getService() << ")");
return ACT_SRV_PENDING;
}
}
}
else
{
......@@ -234,20 +292,7 @@ act_srv_status CModuleService<service_msg>::call(service_msg &msg)
}
}
else
{
this->current_num_retries++;
if(this->current_num_retries>this->max_num_retries)
{
ROS_ERROR_STREAM("CModuleService::call: Request failed! (server: " << this->service_client.getService() << ")");
this->current_num_retries=0;
return ACT_SRV_FAIL;
}
else
{
ROS_WARN_STREAM("CModuleService::call: Request pending! (server: " << this->service_client.getService() << ")");
return ACT_SRV_PENDING;
}
}
return ACT_SRV_SUCCESS;
}
template<class service_msg>
......
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