From cd17671053db0eee673168c4393091de69fd0d29 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 29 Oct 2021 19:09:23 +0200
Subject: [PATCH] Added functions to get the value of the parameters.
---
include/iri_ros_tools/module_dyn_reconf.h | 149 ++++++++++++++++++++++
1 file changed, 149 insertions(+)
diff --git a/include/iri_ros_tools/module_dyn_reconf.h b/include/iri_ros_tools/module_dyn_reconf.h
index f58982d..331a66a 100644
--- a/include/iri_ros_tools/module_dyn_reconf.h
+++ b/include/iri_ros_tools/module_dyn_reconf.h
@@ -3,6 +3,7 @@
#include <dynamic_reconfigure/Reconfigure.h>
#include <iri_ros_tools/module_service.h>
+#include <map>
typedef enum {DYN_RECONF_NO_SUCH_PARAM,
DYN_RECONF_NO_CHANGE,
@@ -35,6 +36,11 @@ class CModuleDynReconf : protected CModuleService<dynamic_reconfigure::Reconfigu
* sure the value has been changed correctly.
*/
bool bool_value;
+ /**
+ * \brief
+ *
+ */
+ std::map<std::string,bool> bool_params;
/**
* \brief Integer value to change
*
@@ -43,6 +49,11 @@ class CModuleDynReconf : protected CModuleService<dynamic_reconfigure::Reconfigu
* sure the value has been changed correctly.
*/
int int_value;
+ /**
+ * \brief
+ *
+ */
+ std::map<std::string,int> int_params;
/**
* \brief String value to change
*
@@ -51,6 +62,11 @@ class CModuleDynReconf : protected CModuleService<dynamic_reconfigure::Reconfigu
* sure the value has been changed correctly.
*/
std::string string_value;
+ /**
+ * \brief
+ *
+ */
+ std::map<std::string,std::string> string_params;
/**
* \brief Double value to change
*
@@ -59,12 +75,22 @@ class CModuleDynReconf : protected CModuleService<dynamic_reconfigure::Reconfigu
* sure the value has been changed correctly.
*/
double double_value;
+ /**
+ * \brief
+ *
+ */
+ std::map<std::string,double> double_params;
/**
* \brief Status of the last service call
*
*
*/
dyn_reconf_status_t dyn_reconf_status;
+ /**
+ * \brief
+ *
+ */
+ bool read_done;
protected:
/**
* \brief Service data check callback
@@ -80,6 +106,16 @@ class CModuleDynReconf : protected CModuleService<dynamic_reconfigure::Reconfigu
*
*/
bool check_dyn_reconf(dynamic_reconfigure::Reconfigure &msg);
+ /**
+ * \brief
+ *
+ */
+ bool check_always_true(dynamic_reconfigure::Reconfigure &msg);
+ /**
+ * \brief
+ *
+ */
+ void read_params(void);
public:
/**
* \brief Constructor
@@ -104,6 +140,11 @@ class CModuleDynReconf : protected CModuleService<dynamic_reconfigure::Reconfigu
* changed (true) or not (false)
*/
bool set_parameter(const std::string &name,bool value);
+ /**
+ * \brief
+ *
+ */
+ bool get_parameter(const std::string &name,bool &value);
/**
* \brief Sets a parameter of type integer
*
@@ -123,6 +164,11 @@ class CModuleDynReconf : protected CModuleService<dynamic_reconfigure::Reconfigu
* changed (true) or not (false)
*/
bool set_parameter(const std::string &name,int value);
+ /**
+ * \brief
+ *
+ */
+ bool get_parameter(const std::string &name,int &value);
/**
* \brief Sets a parameter of type string
*
@@ -142,6 +188,11 @@ class CModuleDynReconf : protected CModuleService<dynamic_reconfigure::Reconfigu
* changed (true) or not (false)
*/
bool set_parameter(const std::string &name,std::string &value);
+ /**
+ * \brief
+ *
+ */
+ bool get_parameter(const std::string &name,std::string &value);
/**
* \brief Sets a parameter of type double
*
@@ -161,6 +212,11 @@ class CModuleDynReconf : protected CModuleService<dynamic_reconfigure::Reconfigu
* changed (true) or not (false)
*/
bool set_parameter(const std::string &name,double value);
+ /**
+ * \brief
+ *
+ */
+ bool get_parameter(const std::string &name,double &value);
/**
* \brief returns the las call status
*
@@ -183,6 +239,8 @@ CModuleDynReconf<dyn_reconf_config>::CModuleDynReconf(const std::string &name,co
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));
+ this->read_done=false;
+ this->read_params();
}
template<class dyn_reconf_config>
@@ -259,6 +317,37 @@ bool CModuleDynReconf<dyn_reconf_config>::check_dyn_reconf(dynamic_reconfigure::
return false;
}
+template<class dyn_reconf_config>
+bool CModuleDynReconf<dyn_reconf_config>::check_always_true(dynamic_reconfigure::Reconfigure &msg)
+{
+ return true;
+}
+
+template<class dyn_reconf_config>
+void CModuleDynReconf<dyn_reconf_config>::read_params(void)
+{
+ dynamic_reconfigure::Reconfigure set_params_srv;
+ unsigned int i=0;
+
+ this->set_call_check_function(boost::bind(&CModuleDynReconf::check_always_true,this,_1));
+ // make an empty service call to get the current values
+ if(this->call(set_params_srv)==ACT_SRV_SUCCESS)
+ {
+ for(i=0;i<set_params_srv.response.config.bools.size();i++)
+ this->bool_params[set_params_srv.response.config.bools[i].name]=set_params_srv.response.config.bools[i].value;
+ for(i=0;i<set_params_srv.response.config.ints.size();i++)
+ this->bool_params[set_params_srv.response.config.ints[i].name]=set_params_srv.response.config.ints[i].value;
+ for(i=0;i<set_params_srv.response.config.strs.size();i++)
+ this->bool_params[set_params_srv.response.config.strs[i].name]=set_params_srv.response.config.strs[i].value;
+ for(i=0;i<set_params_srv.response.config.doubles.size();i++)
+ this->bool_params[set_params_srv.response.config.doubles[i].name]=set_params_srv.response.config.doubles[i].value;
+ this->read_done=true;
+ }
+ else
+ this->read_done=false;
+ this->set_call_check_function(boost::bind(&CModuleDynReconf::check_dyn_reconf,this,_1));
+}
+
template<class dyn_reconf_config>
bool CModuleDynReconf<dyn_reconf_config>::set_parameter(const std::string &name,bool value)
{
@@ -267,6 +356,8 @@ bool CModuleDynReconf<dyn_reconf_config>::set_parameter(const std::string &name,
ROS_DEBUG_STREAM("CModuleDynReconf::set_parameter: Sending new request on service " << this->get_name());
ROS_DEBUG_STREAM("CModuleDynReconf::set boolean parameter " << name << " to " << value);
+ if(!this->read_done)
+ this->read_params();
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;
@@ -275,6 +366,7 @@ bool CModuleDynReconf<dyn_reconf_config>::set_parameter(const std::string &name,
switch(this->call(set_params_srv))
{
case ACT_SRV_SUCCESS: ROS_DEBUG_STREAM("CModule::set_parameters: Request successfull on server: " << this->get_name());
+ this->bool_params[name]=set_params_srv.response.config.bools[0].value;
return true;
break;
case ACT_SRV_PENDING:
@@ -285,6 +377,18 @@ bool CModuleDynReconf<dyn_reconf_config>::set_parameter(const std::string &name,
}
}
+template<class dyn_reconf_config>
+bool CModuleDynReconf<dyn_reconf_config>::get_parameter(const std::string &name,bool &value)
+{
+ if(this->bool_params.find(name)!=this->bool_params.end())
+ {
+ value=this->bool_params[name];
+ return true;
+ }
+ else
+ return false;
+}
+
template<class dyn_reconf_config>
bool CModuleDynReconf<dyn_reconf_config>::set_parameter(const std::string &name,int value)
{
@@ -293,6 +397,8 @@ bool CModuleDynReconf<dyn_reconf_config>::set_parameter(const std::string &name,
ROS_DEBUG_STREAM("CModuleDynReconf::set_parameter: Sending new request on service " << this->get_name());
ROS_DEBUG_STREAM("CModule::set integer parameter " << name << " to " << value);
+ if(!this->read_done)
+ this->read_params();
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;
@@ -301,6 +407,7 @@ bool CModuleDynReconf<dyn_reconf_config>::set_parameter(const std::string &name,
switch(this->call(set_params_srv))
{
case ACT_SRV_SUCCESS: ROS_DEBUG_STREAM("CModule::set_parameters: Request successfull on server: " << this->get_name());
+ this->int_params[name]=set_params_srv.response.config.ints[0].value;
return true;
break;
case ACT_SRV_PENDING:
@@ -311,6 +418,18 @@ bool CModuleDynReconf<dyn_reconf_config>::set_parameter(const std::string &name,
}
}
+template<class dyn_reconf_config>
+bool CModuleDynReconf<dyn_reconf_config>::get_parameter(const std::string &name,int &value)
+{
+ if(this->int_params.find(name)!=this->int_params.end())
+ {
+ value=this->int_params[name];
+ return true;
+ }
+ else
+ return false;
+}
+
template<class dyn_reconf_config>
bool CModuleDynReconf<dyn_reconf_config>::set_parameter(const std::string &name,std::string &value)
{
@@ -319,6 +438,8 @@ bool CModuleDynReconf<dyn_reconf_config>::set_parameter(const std::string &name,
ROS_DEBUG_STREAM("CModuleDynReconf::set_parameter: Sending new request on service " << this->get_name());
ROS_DEBUG_STREAM("CModuleDynReconf::set string parameter " << name << " to " << value);
+ if(!this->read_done)
+ this->read_params();
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;
@@ -327,6 +448,7 @@ bool CModuleDynReconf<dyn_reconf_config>::set_parameter(const std::string &name,
switch(this->call(set_params_srv))
{
case ACT_SRV_SUCCESS: ROS_DEBUG_STREAM("CModule::set_parameters: Request successfull on server: " << this->get_name());
+ this->string_params[name]=set_params_srv.response.config.strs[0].value;
return true;
break;
case ACT_SRV_PENDING:
@@ -337,6 +459,18 @@ bool CModuleDynReconf<dyn_reconf_config>::set_parameter(const std::string &name,
}
}
+template<class dyn_reconf_config>
+bool CModuleDynReconf<dyn_reconf_config>::get_parameter(const std::string &name,std::string &value)
+{
+ if(this->string_params.find(name)!=this->string_params.end())
+ {
+ value=this->string_params[name];
+ return true;
+ }
+ else
+ return false;
+}
+
template<class dyn_reconf_config>
bool CModuleDynReconf<dyn_reconf_config>::set_parameter(const std::string &name,double value)
{
@@ -345,6 +479,8 @@ bool CModuleDynReconf<dyn_reconf_config>::set_parameter(const std::string &name,
ROS_DEBUG_STREAM("CModule::set_parameter: Sending new request on service " << this->get_name());
ROS_DEBUG_STREAM("CModule::set double parameter " << name << " to " << value);
+ if(!this->read_done)
+ this->read_params();
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;
@@ -353,6 +489,7 @@ bool CModuleDynReconf<dyn_reconf_config>::set_parameter(const std::string &name,
switch(this->call(set_params_srv))
{
case ACT_SRV_SUCCESS: ROS_DEBUG_STREAM("CModule::set_parameters: Request successfull on server: " << this->get_name());
+ this->double_params[name]=set_params_srv.response.config.doubles[0].value;
return true;
break;
case ACT_SRV_PENDING:
@@ -363,6 +500,18 @@ bool CModuleDynReconf<dyn_reconf_config>::set_parameter(const std::string &name,
}
}
+template<class dyn_reconf_config>
+bool CModuleDynReconf<dyn_reconf_config>::get_parameter(const std::string &name,double &value)
+{
+ if(this->double_params.find(name)!=this->double_params.end())
+ {
+ value=this->double_params[name];
+ return true;
+ }
+ else
+ return false;
+}
+
template<class dyn_reconf_config>
dyn_reconf_status_t CModuleDynReconf<dyn_reconf_config>::get_status(void)
{
--
GitLab