Skip to content
Snippets Groups Projects
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
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/iri_ros_tools/module_action.h
+ 121
− 48
View file @ 960d75a5
......@@ -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;
}
......
include/iri_ros_tools/module_service.h
+ 64
− 19
View file @ 960d75a5
......@@ -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>
......
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