diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8e1731e326b8cb78c6e68a1d7f06f2c64fb25ceb..4ad90c9b4d09f17e5b48b28a386d0d8714402d83 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -95,7 +95,7 @@ include_directories(include)
# )
add_library(${PROJECT_NAME} src/timeout.cpp src/watchdog.cpp)
-add_library(modules include/${PROJECT_NAME}/module_common.h include/${PROJECT_NAME}/module.h include/${PROJECT_NAME}/module_service.h include/${PROJECT_NAME}/module_action.h)
+add_library(modules include/${PROJECT_NAME}/module_common.h include/${PROJECT_NAME}/module.h include/${PROJECT_NAME}/module_service.h include/${PROJECT_NAME}/module_action.h include/${PROJECT_NAME}/module_exceptions.h include/${PROJECT_NAME}/module_dyn_reconf.h)
set_target_properties(modules PROPERTIES LINKER_LANGUAGE CXX)
target_link_libraries(modules ${PROJECT_NAME})
diff --git a/include/iri_ros_tools/module.h b/include/iri_ros_tools/module.h
index 2d81420c4f0cdeb7fd58aae2d4be6589ead4ab2a..eef27e581387ae94ae3bd7a98f3c5c4df3c4ef58 100644
--- a/include/iri_ros_tools/module.h
+++ b/include/iri_ros_tools/module.h
@@ -4,6 +4,7 @@
#include <iri_ros_tools/module_common.h>
#include <iri_ros_tools/module_service.h>
#include <iri_ros_tools/module_action.h>
+#include <iri_ros_tools/module_exceptions.h>
#include <dynamic_reconfigure/server.h>
#include <dynamic_reconfigure/Reconfigure.h>
@@ -11,7 +12,6 @@
#include "threadserver.h"
#include "eventserver.h"
#include "mutex.h"
-#include "exceptions.h"
#define MODULE_DEFAULT_RATE 10
@@ -34,14 +34,16 @@ class CModule
std::string finish_thread_event_id;
protected:
static void *module_thread(void *param);
- virtual void state_machine(void);
- virtual void reconfigure_callback(ModuleCfg &config, uint32_t level);
+ virtual void state_machine(void)=0;
+ virtual void reconfigure_callback(ModuleCfg &config, uint32_t level)=0;
+ void start_operation(void);
void lock(void);
void unlock(void);
public:
CModule(const std::string &name);
void set_rate(double rate_hz);
double get_rate(void);
+ std::string get_name(void);
bool set_parameter(ros::ServiceClient &service,const std::string &name,bool value);
bool set_parameter(ros::ServiceClient &service,const std::string &name,int value);
bool set_parameter(ros::ServiceClient &service,const std::string &name,std::string &value);
@@ -64,21 +66,22 @@ 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->thread_server->start_thread(this->module_thread_id);
this->name=name;
- /* initialize the dynamic reconfigure */
- this->dyn_reconf.setCallback(boost::bind(&CModule<ModuleCfg>::reconfigure_callback,this,_1,_2));
}
catch (CException &ex)
{
ROS_ERROR_STREAM("CModule::constructor: exception caught: " << ex.what());
+ throw CModuleException(_HERE_,"Impossible to build module",this->name);
}
}
template<class ModuleCfg>
void CModule<ModuleCfg>::set_rate(double rate_hz)
{
- this->module_rate=rate_hz;
+ if(rate_hz<=0.0)
+ throw CModuleException(_HERE_,"Module rate must be positive",this->name);
+ else
+ this->module_rate=rate_hz;
}
template<class ModuleCfg>
@@ -87,11 +90,20 @@ double CModule<ModuleCfg>::get_rate(void)
return this->module_rate;
}
+template<class ModuleCfg>
+std::string CModule<ModuleCfg>::get_name(void)
+{
+ return this->name;
+}
+
template<class ModuleCfg>
void *CModule<ModuleCfg>::module_thread(void *param)
{
CModule *module=(CModule *)param;
bool end =false;
+
+ /* initialize the dynamic reconfigure */
+ module->dyn_reconf.setCallback(boost::bind(&CModule<ModuleCfg>::reconfigure_callback,module,_1,_2));
while(!end)
{
@@ -110,15 +122,9 @@ void *CModule<ModuleCfg>::module_thread(void *param)
}
template<class ModuleCfg>
-void CModule<ModuleCfg>::state_machine(void)
+void CModule<ModuleCfg>::start_operation(void)
{
-
-}
-
-template<class ModuleCfg>
-void CModule<ModuleCfg>::reconfigure_callback(ModuleCfg &config, uint32_t level)
-{
-
+ this->thread_server->start_thread(this->module_thread_id);
}
template<class ModuleCfg>
diff --git a/include/iri_ros_tools/module_action.h b/include/iri_ros_tools/module_action.h
index f9d918f1a96531de8f3e2ee9c1af67597d7c80a2..a827d66d86a2d5c34b943205c3edce15018dc8bd 100644
--- a/include/iri_ros_tools/module_action.h
+++ b/include/iri_ros_tools/module_action.h
@@ -2,6 +2,7 @@
#define _MODULE_ACTION_H
#include <iri_ros_tools/module_common.h>
+#include <iri_ros_tools/module_exceptions.h>
#include <ros/ros.h>
#include <iri_ros_tools/timeout.h>
@@ -180,6 +181,11 @@ void CModuleAction<action_ros>::set_feedback_watchdog_time(double time_s)
this->action_access.enter();
if(time_s>0.0)
this->watchdog_time=time_s;
+ else
+ {
+ this->action_access.exit();
+ throw CModuleException(_HERE_,"Invalid watchdog time value",this->name);
+ }
this->action_access.exit();
}
@@ -226,10 +232,18 @@ template<class action_ros>
void CModuleAction<action_ros>::start_timeout(double time_s)
{
this->action_access.enter();
- if(this->use_timeout && time_s>0.0)
+ if(this->use_timeout)
{
- this->timeout_value=time_s;
- this->action_timeout.start(ros::Duration(time_s));
+ 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();
}
@@ -238,10 +252,18 @@ template<class action_ros>
void CModuleAction<action_ros>::update_timeout(double time_s)
{
this->action_access.enter();
- if(this->use_timeout && time_s>0.0)
+ if(this->use_timeout)
{
- this->action_timeout.start(ros::Duration(time_s));
- this->timeout_value=time_s;
+ if(time_s>0.0)
+ {
+ this->action_timeout.start(ros::Duration(time_s));
+ this->timeout_value=time_s;
+ }
+ else
+ {
+ this->action_access.exit();
+ throw CModuleException(_HERE_,"Invalid timeout value",this->name);
+ }
}
this->action_access.exit();
}
diff --git a/include/iri_ros_tools/module_dyn_reconf.h b/include/iri_ros_tools/module_dyn_reconf.h
new file mode 100644
index 0000000000000000000000000000000000000000..d1a77425a168bdc3562e26e18b53af25bd555769
--- /dev/null
+++ b/include/iri_ros_tools/module_dyn_reconf.h
@@ -0,0 +1,180 @@
+#ifndef _MODULE_DYN_RECONF_H
+#define _MODULE_DYN_RECONF_H
+
+#include <dynamic_reconfigure/Reconfigure.h>
+#include <iri_ros_tools/module_service.h>
+
+class CModuleDynReconf : protected CModuleService<dynamic_reconfigure::Reconfigure>
+{
+ private:
+ std::string name;
+ bool bool_value;
+ int int_value;
+ std::string string_value;
+ double double_value;
+ protected:
+ bool check_dyn_reconf(dynamic_reconfigure::Reconfigure &msg);
+ public:
+ CModuleDynReconf(const std::string &name,const std::string &name_space=std::string(""));
+ bool set_parameter(const std::string &name,bool value);
+ bool set_parameter(const std::string &name,int value);
+ bool set_parameter(const std::string &name,std::string &value);
+ bool set_parameter(const std::string &name,double value);
+ ~CModuleDynReconf();
+};
+
+CModuleDynReconf::CModuleDynReconf(const std::string &name,const std::string &name_space) : CModuleService(name,name_space)
+{
+ this->set_max_num_retries(1);
+ this->set_call_check_function(boost::bind(&CModuleDynReconf::check_dyn_reconf,this,_1));
+}
+bool CModuleDynReconf::check_dyn_reconf(dynamic_reconfigure::Reconfigure &msg)
+{
+ unsigned int i=0;
+
+ for(i=0;i<msg.response.config.bools.size();i++)
+ {
+ if(msg.response.config.bools[i].name==this->name)
+ {
+ if(msg.response.config.bools[i].value!=this->bool_value)
+ {
+ return false;
+ }
+ }
+ }
+ for(i=0;i<msg.response.config.ints.size();i++)
+ {
+ if(msg.response.config.ints[i].name==this->name)
+ {
+ if(msg.response.config.ints[i].value!=this->int_value)
+ {
+ return false;
+ }
+ }
+ }
+ for(i=0;i<msg.response.config.strs.size();i++)
+ {
+ if(msg.response.config.strs[i].name==this->name)
+ {
+ if(msg.response.config.strs[i].value!=this->string_value)
+ {
+ return false;
+ }
+ }
+ }
+ for(i=0;i<msg.response.config.doubles.size();i++)
+ {
+ if(msg.response.config.doubles[i].name==this->name)
+ {
+ if(msg.response.config.doubles[i].value!=this->double_value)
+ {
+ return false;
+ }
+ }
+ }
+
+ return true;
+}
+
+bool CModuleDynReconf::set_parameter(const std::string &name,bool value)
+{
+ dynamic_reconfigure::Reconfigure set_params_srv;
+
+ ROS_DEBUG_STREAM("CModuleDynReconf::set_parameter: Sending new request on service " << this->get_name());
+ ROS_DEBUG_STREAM("CModuleDynReconf::set boolean parameter " << name << " to " << value);
+
+ 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;
+ this->name=name;
+ this->bool_value=value;
+ switch(this->call(set_params_srv))
+ {
+ case ACT_SRV_SUCCESS: ROS_DEBUG_STREAM("CModule::set_parameters: Request successfull on server: " << this->get_name());
+ return true;
+ break;
+ case ACT_SRV_PENDING:
+ case ACT_SRV_FAIL: ROS_ERROR_STREAM("CModuleDynReconf::set_parameters: Request failed on server: " << this->get_name());
+ return false;
+ break;
+ }
+}
+
+bool CModuleDynReconf::set_parameter(const std::string &name,int value)
+{
+ dynamic_reconfigure::Reconfigure set_params_srv;
+
+ ROS_DEBUG_STREAM("CModuleDynReconf::set_parameter: Sending new request on service " << this->get_name());
+ ROS_DEBUG_STREAM("CModule::set integer parameter " << name << " to " << value);
+
+ 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;
+ this->name=name;
+ this->int_value=value;
+ switch(this->call(set_params_srv))
+ {
+ case ACT_SRV_SUCCESS: ROS_DEBUG_STREAM("CModule::set_parameters: Request successfull on server: " << this->get_name());
+ return true;
+ break;
+ case ACT_SRV_PENDING:
+ case ACT_SRV_FAIL: ROS_ERROR_STREAM("CModuleDynReconf::set_parameters: Request failed on server: " << this->get_name());
+ return false;
+ break;
+ }
+}
+
+bool CModuleDynReconf::set_parameter(const std::string &name,std::string &value)
+{
+ dynamic_reconfigure::Reconfigure set_params_srv;
+
+ ROS_DEBUG_STREAM("CModuleDynReconf::set_parameter: Sending new request on service " << this->get_name());
+ ROS_DEBUG_STREAM("CModuleDynReconf::set string parameter " << name << " to " << value);
+
+ 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;
+ this->name=name;
+ this->string_value=value;
+ switch(this->call(set_params_srv))
+ {
+ case ACT_SRV_SUCCESS: ROS_DEBUG_STREAM("CModule::set_parameters: Request successfull on server: " << this->get_name());
+ return true;
+ break;
+ case ACT_SRV_PENDING:
+ case ACT_SRV_FAIL: ROS_ERROR_STREAM("CModuleDynReconf::set_parameters: Request failed on server: " << this->get_name());
+ return false;
+ break;
+ }
+}
+
+bool CModuleDynReconf::set_parameter(const std::string &name,double value)
+{
+ dynamic_reconfigure::Reconfigure set_params_srv;
+
+ ROS_DEBUG_STREAM("CModule::set_parameter: Sending new request on service " << this->get_name());
+ ROS_DEBUG_STREAM("CModule::set double parameter " << name << " to " << value);
+
+ 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;
+ this->name=name;
+ this->double_value=value;
+ switch(this->call(set_params_srv))
+ {
+ case ACT_SRV_SUCCESS: ROS_DEBUG_STREAM("CModule::set_parameters: Request successfull on server: " << this->get_name());
+ return true;
+ break;
+ case ACT_SRV_PENDING:
+ case ACT_SRV_FAIL: ROS_ERROR_STREAM("CModuleDynReconf::set_parameters: Request failed on server: " << this->get_name());
+ return false;
+ break;
+ }
+}
+
+CModuleDynReconf::~CModuleDynReconf()
+{
+
+}
+
+#endif
diff --git a/include/iri_ros_tools/module_exceptions.h b/include/iri_ros_tools/module_exceptions.h
new file mode 100644
index 0000000000000000000000000000000000000000..9ebdc4998b6355c9f46f80d6c97f3aaaad054abc
--- /dev/null
+++ b/include/iri_ros_tools/module_exceptions.h
@@ -0,0 +1,38 @@
+// Copyright (C) 2009-2010 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Author Sergi Hernandez (shernand@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of iriutils
+// iriutils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef MODULE_EXCEPTIONS_H
+#define MODULE_EXCEPTIONS_H
+
+#include "exceptions.h"
+#include <string>
+
+const std::string module_exception_msg="[CModule class] - ";
+
+class CModuleException : public CException
+{
+ public:
+ CModuleException(const std::string& where,const std::string& error_msg,const std::string& module_name) : CException(where,module_exception_msg)
+ {
+ this->error_msg+=error_msg;
+ this->error_msg+=" - ";
+ this->error_msg+=module_name;
+ }
+};
+
+#endif