Skip to content
GitLab
Commit d00886b0 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Merged the branch to update and upgrade the modules infrastructure.

parent 017c9c7a
Hide whitespace changes
Inline Side-by-side
include/iri_ros_tools/module.h
View file @ d00886b0
......@@ -123,6 +123,10 @@ class CModule
* start_operation() functions is called. This function monitors the
* internal event to know when it has to stop, and calls the state_machine()
* function at the desired rate.
*
* \param name string with the desired name for the action.
*
* \param namespace string with the base namespace for the action.
*/
static void *module_thread(void *param);
/**
......@@ -194,7 +198,7 @@ class CModule
*
* \param namespace string with the base namespace for the module.
*/
CModule(const std::string &name);
CModule(const std::string &name,const std::string &name_space=std::string(""));
/**
* \brief Function to set the rate
*
......@@ -224,26 +228,6 @@ class CModule
* \return the name of the module.
*/
std::string get_name(void);
/**
* \brief
*
*/
bool set_parameter(ros::ServiceClient &service,const std::string &name,bool value);
/**
* \brief
*
*/
bool set_parameter(ros::ServiceClient &service,const std::string &name,int value);
/**
* \brief
*
*/
bool set_parameter(ros::ServiceClient &service,const std::string &name,std::string &value);
/**
* \brief
*
*/
bool set_parameter(ros::ServiceClient &service,const std::string &name,double value);
/**
* \brief Destructor
*
......@@ -254,10 +238,10 @@ class CModule
};
template<class ModuleCfg>
CModule<ModuleCfg>::CModule(const std::string &name) :
CModule<ModuleCfg>::CModule(const std::string &name,const std::string &name_space) :
module_rate(MODULE_DEFAULT_RATE),
dyn_reconf(module_nh),
module_nh(ros::this_node::getName(),name)
module_nh(ros::this_node::getName()+"/"+name_space+"/"+name)
{
try
{
......@@ -268,7 +252,7 @@ CModule<ModuleCfg>::CModule(const std::string &name) :
this->module_thread_id=name+"_module_thread";
this->thread_server->create_thread(this->module_thread_id);
this->thread_server->attach_thread(this->module_thread_id,this->module_thread,this);
this->name=name;
this->name=this->module_nh.getNamespace()+"/"+name;
}
catch (CException &ex)
{
......@@ -341,202 +325,6 @@ void CModule<ModuleCfg>::unlock(void)
this->module_access.exit();
}
template<class ModuleCfg>
bool CModule<ModuleCfg>::set_parameter(ros::ServiceClient &service,const std::string &name,bool value)
{
unsigned int i=0;
bool changed=true,call_status;
dynamic_reconfigure::Reconfigure set_params_srv;
ROS_DEBUG_STREAM("CModule::set_parameter: Sending new request on service " << service.getService());
ROS_DEBUG_STREAM("CModule::set boolean parameter " << name << " to " << value);
this->module_access.enter();
set_params_srv.request.config.bools.resize(1);
set_params_srv.request.config.bools[0].name=name;
set_params_srv.request.config.bools[0].value=value;
call_status=service.call(set_params_srv);
if(call_status)
{
for(i=0;i<set_params_srv.response.config.bools.size();i++)
{
if(set_params_srv.response.config.bools[i].name==name)
{
if(set_params_srv.response.config.bools[i].value!=value)
{
changed=false;
break;
}
}
}
if(changed)
{
ROS_DEBUG_STREAM("CModule::set_parameters: Request successfull on server: " << service.getService());
this->module_access.exit();
return true;
}
else
{
ROS_ERROR_STREAM("CModule::set_parameters: Request failed on server: " << service.getService());
this->module_access.exit();
return false;
}
}
else
{
ROS_ERROR_STREAM("CModule::set_parameters: Request failed on server: " << service.getService());
this->module_access.exit();
return false;
}
}
template<class ModuleCfg>
bool CModule<ModuleCfg>::set_parameter(ros::ServiceClient &service,const std::string &name,int value)
{
unsigned int i=0;
bool changed=true,call_status;
dynamic_reconfigure::Reconfigure set_params_srv;
ROS_DEBUG_STREAM("CModule::set_parameter: Sending new request on service " << service.getService());
ROS_DEBUG_STREAM("CModule::set integer parameter " << name << " to " << value);
this->module_access.enter();
set_params_srv.request.config.ints.resize(1);
set_params_srv.request.config.ints[0].name=name;
set_params_srv.request.config.ints[0].value=value;
call_status=service.call(set_params_srv);
if(call_status)
{
for(i=0;i<set_params_srv.response.config.ints.size();i++)
{
if(set_params_srv.response.config.ints[i].name==name)
{
if(set_params_srv.response.config.ints[i].value!=value)
{
changed=false;
break;
}
}
}
if(changed)
{
ROS_DEBUG_STREAM("CModule::set_parameters: Request successfull on server: " << service.getService());
this->module_access.exit();
return true;
}
else
{
ROS_ERROR_STREAM("CModule::set_parameters: Request failed on server: " << service.getService());
this->module_access.exit();
return false;
}
}
else
{
ROS_ERROR_STREAM("CModule::set_parameters: Request failed on server: " << service.getService());
this->module_access.exit();
return false;
}
}
template<class ModuleCfg>
bool CModule<ModuleCfg>::set_parameter(ros::ServiceClient &service,const std::string &name,std::string &value)
{
unsigned int i=0;
bool changed=true,call_status;
dynamic_reconfigure::Reconfigure set_params_srv;
ROS_DEBUG_STREAM("CModule::set_parameter: Sending new request on service " << service.getService());
ROS_DEBUG_STREAM("CModule::set string parameter " << name << " to " << value);
this->module_access.enter();
set_params_srv.request.config.strs.resize(1);
set_params_srv.request.config.strs[0].name=name;
set_params_srv.request.config.strs[0].value=value;
call_status=service.call(set_params_srv);
if(call_status)
{
for(i=0;i<set_params_srv.response.config.strs.size();i++)
{
if(set_params_srv.response.config.strs[i].name==name)
{
if(set_params_srv.response.config.strs[i].value!=value)
{
changed=false;
break;
}
}
}
if(changed)
{
ROS_DEBUG_STREAM("CModule::set_parameters: Request successfull on server: " << service.getService());
this->module_access.exit();
return true;
}
else
{
ROS_ERROR_STREAM("CModule::set_parameters: Request failed on server: " << service.getService());
this->module_access.exit();
return false;
}
}
else
{
ROS_ERROR_STREAM("CModule::set_parameters: Request failed on server: " << service.getService());
this->module_access.exit();
return false;
}
}
template<class ModuleCfg>
bool CModule<ModuleCfg>::set_parameter(ros::ServiceClient &service,const std::string &name,double value)
{
unsigned int i=0;
bool changed=true,call_status;
dynamic_reconfigure::Reconfigure set_params_srv;
ROS_DEBUG_STREAM("CModule::set_parameter: Sending new request on service " << service.getService());
ROS_DEBUG_STREAM("CModule::set double parameter " << name << " to " << value);
this->module_access.enter();
set_params_srv.request.config.doubles.resize(1);
set_params_srv.request.config.doubles[0].name=name;
set_params_srv.request.config.doubles[0].value=value;
call_status=service.call(set_params_srv);
if(call_status)
{
for(i=0;i<set_params_srv.response.config.doubles.size();i++)
{
if(set_params_srv.response.config.doubles[i].name==name)
{
if(set_params_srv.response.config.doubles[i].value!=value)
{
changed=false;
break;
}
}
}
if(changed)
{
ROS_DEBUG_STREAM("CModule::set_parameters: Request successfull on server: " << service.getService());
this->module_access.exit();
return true;
}
else
{
ROS_ERROR_STREAM("CModule::set_parameters: Request failed on server: " << service.getService());
this->module_access.exit();
return false;
}
}
else
{
ROS_ERROR_STREAM("CModule::set_parameters: Request failed on server: " << service.getService());
this->module_access.exit();
return false;
}
}
template<class ModuleCfg>
CModule<ModuleCfg>::~CModule()
{
......
include/iri_ros_tools/module_action.h
View file @ d00886b0
......@@ -314,8 +314,11 @@ class CModuleAction
* This function enables the internal action timeout to limit the maximum
* time the action can be active without finishing properly. By default
* this feature is disabled.
*
* \param time_s the desired maximum ammount of time the action can be
* active before reporting and error.
*/
void enable_timeout(void);
void enable_timeout(double time_s);
/**
* \brief Disables the action timeout
*
......@@ -324,12 +327,6 @@ class CModuleAction
* this feature is disabled.
*/
void disable_timeout(void);
/**
* \brief
*
* TODO: do it automatically in the make_request function
*/
void start_timeout(double time_s);
/**
* \brief Updates the current timeout value
*
......@@ -406,6 +403,11 @@ class CModuleAction
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;
}
......@@ -497,7 +499,7 @@ CModuleAction<action_ros>::CModuleAction(const std::string &name,const std::stri
{
std::size_t position;
this->name=name;
this->name=this->nh.getNamespace()+"/"+name;
this->status=ACTION_IDLE;
// retry control
this->current_num_retries=0;
......@@ -505,6 +507,7 @@ CModuleAction<action_ros>::CModuleAction(const std::string &name,const std::stri
// assign the full service name
// timeouts
this->use_timeout=false;
this->timeout_value=0.0;
this->timeout_value=DEFAULT_ACTION_TIMEOUT;
// waychdog
this->watchdog_time=DEFAULT_WATCHDOG_TIME;
......@@ -571,10 +574,19 @@ bool CModuleAction<action_ros>::is_watchdog_active(void)
}
template<class action_ros>
void CModuleAction<action_ros>::enable_timeout(void)
void CModuleAction<action_ros>::enable_timeout(double time_s)
{
this->action_access.enter();
this->use_timeout=true;
if(time_s>0.0)
{
this->timeout_value=time_s;
this->use_timeout=true;
}
else
{
this->action_access.exit();
throw CModuleException(_HERE_,"Invalid timeout value",this->name);
}
this->action_access.exit();
}
......@@ -586,26 +598,6 @@ void CModuleAction<action_ros>::disable_timeout(void)
this->action_access.exit();
}
template<class action_ros>
void CModuleAction<action_ros>::start_timeout(double time_s)
{
this->action_access.enter();
if(this->use_timeout)
{
if(time_s>0.0)
{
this->timeout_value=time_s;
this->action_timeout.start(ros::Duration(time_s));
}
else
{
this->action_access.exit();
throw CModuleException(_HERE_,"Invalid timeout value",this->name);
}
}
this->action_access.exit();
}
template<class action_ros>
void CModuleAction<action_ros>::update_timeout(double time_s)
{
......@@ -666,7 +658,11 @@ std::string CModuleAction<action_ros>::get_name(void)
template<class action_ros>
void CModuleAction<action_ros>::cancel(void)
{
this->action_client->cancelGoal();
actionlib::SimpleClientGoalState action_state(actionlib::SimpleClientGoalState::PENDING);
action_state=action_client->getState();
if(action_state==actionlib::SimpleClientGoalState::ACTIVE)
this->action_client->cancelGoal();
}
template<class action_ros>
......
include/iri_ros_tools/module_service.h
View file @ d00886b0
......@@ -177,7 +177,7 @@ CModuleService<service_msg>::CModuleService(const std::string &name,const std::s
this->max_num_retries=DEFAULT_SERVICE_MAX_RETRIES;
this->service_client=this->nh.template serviceClient<service_msg>(name);
// assign the full service name
this->name=name;
this->name=this->nh.getNamespace()+"/"+name;
// assign the default check function
this->call_successfull=boost::bind(&CModuleService::default_call_check, this, _1);
}
......
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