Commit d00886b0 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Merged the branch to update and upgrade the modules infrastructure.

parent 017c9c7a
......@@ -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()
{
......
......@@ -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>
......
......@@ -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