diff --git a/include/iri_ros_tools/module.h b/include/iri_ros_tools/module.h
index 55513f4ce838ecdc1b2299e9f1bb02f5af98dcbe..db73bb10c1e692b152419f9b4c621f4756106bc5 100644
--- a/include/iri_ros_tools/module.h
+++ b/include/iri_ros_tools/module.h
@@ -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()
{
diff --git a/include/iri_ros_tools/module_action.h b/include/iri_ros_tools/module_action.h
index c282d2125dfac52d9ddb799a5f67edbdddab28d5..788e269c6362aabdc075cb46f8bb9343d58e44b6 100644
--- a/include/iri_ros_tools/module_action.h
+++ b/include/iri_ros_tools/module_action.h
@@ -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>
diff --git a/include/iri_ros_tools/module_service.h b/include/iri_ros_tools/module_service.h
index 776ccfcb9a69e4027c976f21a6dffc1013326976..518a927d6771379445aa7022f3fb48204644f250 100644
--- a/include/iri_ros_tools/module_service.h
+++ b/include/iri_ros_tools/module_service.h
@@ -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);
}