From 9c9070b11c38fed0f51d8be294dfe7640927bb21 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 21 Oct 2020 14:45:42 +0200
Subject: [PATCH] Modified the module service to handle generic dynamic
reconfigure parameters.
---
CMakeLists.txt | 2 +-
include/iri_ros_tools/module_dyn_reconf.h | 28 ++++---
include/iri_ros_tools/module_service.h | 89 +++++++++++++++++------
3 files changed, 86 insertions(+), 33 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1f80392..e2ea37a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -7,7 +7,7 @@ project(iri_ros_tools)
find_package(catkin REQUIRED COMPONENTS roscpp)
## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
+find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
diff --git a/include/iri_ros_tools/module_dyn_reconf.h b/include/iri_ros_tools/module_dyn_reconf.h
index e589f66..f58982d 100644
--- a/include/iri_ros_tools/module_dyn_reconf.h
+++ b/include/iri_ros_tools/module_dyn_reconf.h
@@ -15,7 +15,8 @@ typedef enum {DYN_RECONF_NO_SUCH_PARAM,
* the specifics of the dynamic reconfigure service. It ptovides functions
* to set integer, boolean, float and string parameters of remote ROS nodes.
*/
-class CModuleDynReconf : protected CModuleService<dynamic_reconfigure::Reconfigure>
+template<class dyn_reconf_config>
+class CModuleDynReconf : protected CModuleService<dynamic_reconfigure::Reconfigure,dyn_reconf_config>
{
private:
/**
@@ -176,13 +177,16 @@ class CModuleDynReconf : protected CModuleService<dynamic_reconfigure::Reconfigu
~CModuleDynReconf();
};
-CModuleDynReconf::CModuleDynReconf(const std::string &name,const std::string &name_space) : CModuleService(name,name_space)
+template<class dyn_reconf_config>
+CModuleDynReconf<dyn_reconf_config>::CModuleDynReconf(const std::string &name,const std::string &name_space) : CModuleService<dynamic_reconfigure::Reconfigure,dyn_reconf_config>(name,name_space)
{
this->dyn_reconf_status=DYN_RECONF_SUCCESSFULL;
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)
+
+template<class dyn_reconf_config>
+bool CModuleDynReconf<dyn_reconf_config>::check_dyn_reconf(dynamic_reconfigure::Reconfigure &msg)
{
unsigned int i=0;
@@ -255,7 +259,8 @@ bool CModuleDynReconf::check_dyn_reconf(dynamic_reconfigure::Reconfigure &msg)
return false;
}
-bool CModuleDynReconf::set_parameter(const std::string &name,bool value)
+template<class dyn_reconf_config>
+bool CModuleDynReconf<dyn_reconf_config>::set_parameter(const std::string &name,bool value)
{
dynamic_reconfigure::Reconfigure set_params_srv;
@@ -280,7 +285,8 @@ bool CModuleDynReconf::set_parameter(const std::string &name,bool value)
}
}
-bool CModuleDynReconf::set_parameter(const std::string &name,int value)
+template<class dyn_reconf_config>
+bool CModuleDynReconf<dyn_reconf_config>::set_parameter(const std::string &name,int value)
{
dynamic_reconfigure::Reconfigure set_params_srv;
@@ -305,7 +311,8 @@ bool CModuleDynReconf::set_parameter(const std::string &name,int value)
}
}
-bool CModuleDynReconf::set_parameter(const std::string &name,std::string &value)
+template<class dyn_reconf_config>
+bool CModuleDynReconf<dyn_reconf_config>::set_parameter(const std::string &name,std::string &value)
{
dynamic_reconfigure::Reconfigure set_params_srv;
@@ -330,7 +337,8 @@ bool CModuleDynReconf::set_parameter(const std::string &name,std::string &value)
}
}
-bool CModuleDynReconf::set_parameter(const std::string &name,double value)
+template<class dyn_reconf_config>
+bool CModuleDynReconf<dyn_reconf_config>::set_parameter(const std::string &name,double value)
{
dynamic_reconfigure::Reconfigure set_params_srv;
@@ -355,12 +363,14 @@ bool CModuleDynReconf::set_parameter(const std::string &name,double value)
}
}
-dyn_reconf_status_t CModuleDynReconf::get_status(void)
+template<class dyn_reconf_config>
+dyn_reconf_status_t CModuleDynReconf<dyn_reconf_config>::get_status(void)
{
return this->dyn_reconf_status;
}
-CModuleDynReconf::~CModuleDynReconf()
+template<class dyn_reconf_config>
+CModuleDynReconf<dyn_reconf_config>::~CModuleDynReconf()
{
}
diff --git a/include/iri_ros_tools/module_service.h b/include/iri_ros_tools/module_service.h
index ab94cd0..61fe04b 100644
--- a/include/iri_ros_tools/module_service.h
+++ b/include/iri_ros_tools/module_service.h
@@ -4,6 +4,8 @@
#include <ros/ros.h>
#include <iri_ros_tools/module_common.h>
+#include <boost/any.hpp>
+
#define DEFAULT_SERVICE_MAX_RETRIES 5
/**
@@ -24,7 +26,7 @@
* <main node namespace>/<class namespace>/<class name>
*/
-template<class service_msg>
+template<class service_msg,class dyn_reconf_config>
class CModuleService
{
private:
@@ -110,6 +112,18 @@ class CModuleService
* \param namespace string with the base namespace for the service.
*/
CModuleService(const std::string &name,const std::string &name_space=std::string(""));
+ /**
+ * \ brief Handles the dynamic reconfigure parameters of the module service
+ *
+ * This functions handles all generic module service parameters for each instance
+ * and updates the internal attributes accordingly.
+ *
+ * \param config data object with all the module parameters.
+ * \param name prefix given to the module servie variables in order to locate and
+ * handle them.
+ */
+ void dynamice_reconfigure(dyn_reconf_config &config, std::string &name);
+
/**
* \brief Sets the maximum number of attempts to start the action
*
@@ -188,8 +202,8 @@ class CModuleService
~CModuleService();
};
-template<class service_msg>
-CModuleService<service_msg>::CModuleService(const std::string &name,const std::string &name_space):nh(name_space)
+template<class service_msg,class dyn_reconf_config>
+CModuleService<service_msg,dyn_reconf_config>::CModuleService(const std::string &name,const std::string &name_space):nh(name_space)
{
this->current_num_retries=0;
this->max_num_retries=DEFAULT_SERVICE_MAX_RETRIES;
@@ -202,50 +216,79 @@ CModuleService<service_msg>::CModuleService(const std::string &name,const std::s
this->enabled=true;
}
-template<class service_msg>
-bool CModuleService<service_msg>::default_call_check(service_msg &msg)
+template<class service_msg,class dyn_reconf_config>
+void CModuleService<service_msg,dyn_reconf_config>::dynamice_reconfigure(dyn_reconf_config &config, std::string &name)
+{
+ std::vector<typename dyn_reconf_config::AbstractParamDescriptionConstPtr> params;
+ boost::any value;
+
+ params=dyn_reconf_config::__getParamDescriptions__();
+
+ for(typename std::vector<typename dyn_reconf_config::AbstractParamDescriptionConstPtr>::iterator param=params.begin();param!=params.end();param++)
+ {
+ if((*param)->name==(name+"_max_num_retries"))
+ {
+ (*param)->getValue(config,value);
+ if(value.type()==typeid(int))
+ this->set_max_num_retries(boost::any_cast<int &>(value));
+ }
+ if((*param)->name==(name+"_enable"))
+ {
+ if(value.type()==typeid(bool))
+ {
+ if(boost::any_cast<bool &>(value))
+ this->enable();
+ else
+ this->disable();
+ }
+ }
+ }
+}
+
+template<class service_msg,class dyn_reconf_config>
+bool CModuleService<service_msg,dyn_reconf_config>::default_call_check(service_msg &msg)
{
return true;
}
-template<class service_msg>
-void CModuleService<service_msg>::set_max_num_retries(unsigned int max_retries)
+template<class service_msg,class dyn_reconf_config>
+void CModuleService<service_msg,dyn_reconf_config>::set_max_num_retries(unsigned int max_retries)
{
this->max_num_retries=max_retries;
}
-template<class service_msg>
-unsigned int CModuleService<service_msg>::get_max_num_retries(void)
+template<class service_msg,class dyn_reconf_config>
+unsigned int CModuleService<service_msg,dyn_reconf_config>::get_max_num_retries(void)
{
return this->max_num_retries;
}
-template<class service_msg>
-std::string CModuleService<service_msg>::get_name(void)
+template<class service_msg,class dyn_reconf_config>
+std::string CModuleService<service_msg,dyn_reconf_config>::get_name(void)
{
return this->name;
}
-template<class service_msg>
-void CModuleService<service_msg>::enable(void)
+template<class service_msg,class dyn_reconf_config>
+void CModuleService<service_msg,dyn_reconf_config>::enable(void)
{
this->enabled=true;
}
-template<class service_msg>
-void CModuleService<service_msg>::disable(void)
+template<class service_msg,class dyn_reconf_config>
+void CModuleService<service_msg,dyn_reconf_config>::disable(void)
{
this->enabled=false;
}
-template<class service_msg>
-bool CModuleService<service_msg>::is_enabled(void)
+template<class service_msg,class dyn_reconf_config>
+bool CModuleService<service_msg,dyn_reconf_config>::is_enabled(void)
{
return this->enabled;
}
-template<class service_msg>
-act_srv_status CModuleService<service_msg>::call(service_msg &msg)
+template<class service_msg,class dyn_reconf_config>
+act_srv_status CModuleService<service_msg,dyn_reconf_config>::call(service_msg &msg)
{
ROS_DEBUG_STREAM("CModuleService::call: Sending New Request to service " << this->service_client.getService());
@@ -295,14 +338,14 @@ act_srv_status CModuleService<service_msg>::call(service_msg &msg)
return ACT_SRV_SUCCESS;
}
-template<class service_msg>
-void CModuleService<service_msg>::set_call_check_function(const boost::function< bool(service_msg &msg) > &function)
+template<class service_msg,class dyn_reconf_config>
+void CModuleService<service_msg,dyn_reconf_config>::set_call_check_function(const boost::function< bool(service_msg &msg) > &function)
{
this->call_successfull=function;
}
-template<class service_msg>
-CModuleService<service_msg>::~CModuleService()
+template<class service_msg,class dyn_reconf_config>
+CModuleService<service_msg,dyn_reconf_config>::~CModuleService()
{
}
--
GitLab