From 758c2b7a496407473c55eb82996a4527eb073436 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 23 Oct 2020 13:30:47 +0200
Subject: [PATCH] Added the dynamic_reconfigure function to handle generic
parameters to the module and module action classes.
---
include/iri_ros_tools/module.h | 31 +++++
include/iri_ros_tools/module_action.h | 181 ++++++++++++++++++-------
include/iri_ros_tools/module_service.h | 8 +-
3 files changed, 166 insertions(+), 54 deletions(-)
diff --git a/include/iri_ros_tools/module.h b/include/iri_ros_tools/module.h
index 0166c85..46b7d7f 100644
--- a/include/iri_ros_tools/module.h
+++ b/include/iri_ros_tools/module.h
@@ -160,6 +160,17 @@ class CModule
* and can be ignored.
*/
virtual void reconfigure_callback(ModuleCfg &config, uint32_t level)=0;
+ /**
+ * \ 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 dynamic_reconfigure(ModuleCfg &config, const std::string &name);
/**
* \brief function to start the operation of the class
*
@@ -307,6 +318,26 @@ void *CModule<ModuleCfg>::module_thread(void *param)
pthread_exit(NULL);
}
+template<class ModuleCfg>
+void CModule<ModuleCfg>::dynamic_reconfigure(ModuleCfg &config, const std::string &name)
+{
+ std::vector<typename ModuleCfg::AbstractParamDescriptionConstPtr> params;
+ boost::any value;
+
+ params=ModuleCfg::__getParamDescriptions__();
+
+ for(typename std::vector<typename ModuleCfg::AbstractParamDescriptionConstPtr>::iterator param=params.begin();param!=params.end();param++)
+ {
+ if((*param)->name==(name+"_rate_hz"))
+ {
+
+ (*param)->getValue(config,value);
+ if(value.type()==typeid(double))
+ this->set_rate(boost::any_cast<double &>(value));
+ }
+ }
+}
+
template<class ModuleCfg>
void CModule<ModuleCfg>::start_operation(void)
{
diff --git a/include/iri_ros_tools/module_action.h b/include/iri_ros_tools/module_action.h
index a34c7b3..4397ff5 100644
--- a/include/iri_ros_tools/module_action.h
+++ b/include/iri_ros_tools/module_action.h
@@ -56,7 +56,7 @@ typedef enum {ACTION_IDLE,
*
* <main node namespace>/<class namespace>/<class name>
*/
-template<class action_ros>
+template<class action_ros,class dyn_reconf_config>
class CModuleAction
{
private:
@@ -279,6 +279,17 @@ class CModuleAction
*/
CModuleAction(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 dynamic_reconfigure(dyn_reconf_config &config, const std::string &name);
/**
* \brief Sets the maximum number of attempts to start the action
*
@@ -457,9 +468,9 @@ class CModuleAction
ROS_DEBUG_STREAM("CModuleAction::make_request: Server " << this->name << " is Available!");
this->current_num_retries=0;
this->action_client->sendGoal(msg,
- boost::bind(&CModuleAction<action_ros>::action_done, this, _1, _2),
- boost::bind(&CModuleAction<action_ros>::action_active, this),
- boost::bind(&CModuleAction<action_ros>::action_feedback, this, _1));
+ boost::bind(&CModuleAction<action_ros,dyn_reconf_config>::action_done, this, _1, _2),
+ boost::bind(&CModuleAction<action_ros,dyn_reconf_config>::action_active, this),
+ boost::bind(&CModuleAction<action_ros,dyn_reconf_config>::action_feedback, this, _1));
this->feedback_watchdog.reset(ros::Duration(this->watchdog_time));
this->status=ACTION_RUNNING;
if(this->use_timeout)
@@ -559,8 +570,8 @@ class CModuleAction
~CModuleAction();
};
-template<class action_ros>
-CModuleAction<action_ros>::CModuleAction(const std::string &name,const std::string &name_space):nh(name_space)
+template<class action_ros,class dyn_reconf_config>
+CModuleAction<action_ros,dyn_reconf_config>::CModuleAction(const std::string &name,const std::string &name_space):nh(name_space)
{
this->name=this->nh.getNamespace()+"/"+name;
this->status=ACTION_IDLE;
@@ -582,28 +593,98 @@ CModuleAction<action_ros>::CModuleAction(const std::string &name,const std::stri
this->enabled=true;
}
-template<class action_ros>
-void CModuleAction<action_ros>::action_active(void)
+template<class action_ros,class dyn_reconf_config>
+void CModuleAction<action_ros,dyn_reconf_config>::dynamic_reconfigure(dyn_reconf_config &config, const std::string &name)
+{
+ std::vector<typename dyn_reconf_config::AbstractParamDescriptionConstPtr> params;
+ bool enable_timeout,enable_watchdog;
+ double timeout,watchdog;
+ 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+"_num_retries"))
+ {
+ (*param)->getValue(config,value);
+ if(value.type()==typeid(int))
+ this->set_max_num_retries(boost::any_cast<int &>(value));
+ }
+ else if((*param)->name==(name+"_enable_timeout"))
+ {
+ if(value.type()==typeid(bool))
+ {
+ if(boost::any_cast<bool &>(value))
+ enable_timeout=true;
+ else
+ enable_timeout=false;
+ }
+ }
+ if((*param)->name==(name+"_timeout_s"))
+ {
+ (*param)->getValue(config,value);
+ if(value.type()==typeid(double))
+ timeout=boost::any_cast<double &>(value);
+ }
+ if((*param)->name==(name+"_feedback_watchdog_time_s"))
+ {
+ (*param)->getValue(config,value);
+ if(value.type()==typeid(double))
+ watchdog=boost::any_cast<double &>(value);
+ }
+ else if((*param)->name==(name+"_enable_watchdog"))
+ {
+ if(value.type()==typeid(bool))
+ {
+ if(boost::any_cast<bool &>(value))
+ enable_watchdog=true;
+ else
+ enable_watchdog=false;
+ }
+ }
+ else if((*param)->name==(name+"_enabled"))
+ {
+ if(value.type()==typeid(bool))
+ {
+ if(boost::any_cast<bool &>(value))
+ this->enable();
+ else
+ this->disable();
+ }
+ }
+ }
+ if(enable_timeout)
+ this->enable_timeout(timeout);
+ else
+ this->disable_timeout();
+ if(enable_watchdog)
+ this->enable_watchdog(watchdog);
+}
+
+
+template<class action_ros,class dyn_reconf_config>
+void CModuleAction<action_ros,dyn_reconf_config>::action_active(void)
{
ROS_DEBUG_STREAM("CModuleAction::action_active: Goal on server " << this->name << " just went active!");
}
-template<class action_ros>
-void CModuleAction<action_ros>::set_max_num_retries(unsigned int max_retries)
+template<class action_ros,class dyn_reconf_config>
+void CModuleAction<action_ros,dyn_reconf_config>::set_max_num_retries(unsigned int max_retries)
{
this->action_access.enter();
this->max_num_retries=max_retries;
this->action_access.exit();
}
-template<class action_ros>
-unsigned int CModuleAction<action_ros>::get_max_num_retries(void)
+template<class action_ros,class dyn_reconf_config>
+unsigned int CModuleAction<action_ros,dyn_reconf_config>::get_max_num_retries(void)
{
return this->max_num_retries;
}
-template<class action_ros>
-void CModuleAction<action_ros>::enable_watchdog(double time_s)
+template<class action_ros,class dyn_reconf_config>
+void CModuleAction<action_ros,dyn_reconf_config>::enable_watchdog(double time_s)
{
this->action_access.enter();
if(time_s>0.0)
@@ -620,28 +701,28 @@ void CModuleAction<action_ros>::enable_watchdog(double time_s)
this->action_access.exit();
}
-template<class action_ros>
-void CModuleAction<action_ros>::disable_watchdog(void)
+template<class action_ros,class dyn_reconf_config>
+void CModuleAction<action_ros,dyn_reconf_config>::disable_watchdog(void)
{
this->action_access.enter();
this->use_watchdog=false;
this->action_access.exit();
}
-template<class action_ros>
-bool CModuleAction<action_ros>::is_watchdog_enabled(void)
+template<class action_ros,class dyn_reconf_config>
+bool CModuleAction<action_ros,dyn_reconf_config>::is_watchdog_enabled(void)
{
return this->use_watchdog;
}
-template<class action_ros>
-double CModuleAction<action_ros>::get_feedback_watchdog_time(void)
+template<class action_ros,class dyn_reconf_config>
+double CModuleAction<action_ros,dyn_reconf_config>::get_feedback_watchdog_time(void)
{
return this->watchdog_time;
}
-template<class action_ros>
-bool CModuleAction<action_ros>::is_watchdog_active(void)
+template<class action_ros,class dyn_reconf_config>
+bool CModuleAction<action_ros,dyn_reconf_config>::is_watchdog_active(void)
{
this->action_access.enter();
if(this->enabled)
@@ -662,8 +743,8 @@ bool CModuleAction<action_ros>::is_watchdog_active(void)
return false;
}
-template<class action_ros>
-void CModuleAction<action_ros>::enable_timeout(double time_s)
+template<class action_ros,class dyn_reconf_config>
+void CModuleAction<action_ros,dyn_reconf_config>::enable_timeout(double time_s)
{
this->action_access.enter();
if(time_s>0.0)
@@ -679,16 +760,16 @@ void CModuleAction<action_ros>::enable_timeout(double time_s)
this->action_access.exit();
}
-template<class action_ros>
-void CModuleAction<action_ros>::disable_timeout(void)
+template<class action_ros,class dyn_reconf_config>
+void CModuleAction<action_ros,dyn_reconf_config>::disable_timeout(void)
{
this->action_access.enter();
this->use_timeout=false;
this->action_access.exit();
}
-template<class action_ros>
-void CModuleAction<action_ros>::update_timeout(double time_s)
+template<class action_ros,class dyn_reconf_config>
+void CModuleAction<action_ros,dyn_reconf_config>::update_timeout(double time_s)
{
this->action_access.enter();
if(this->use_timeout)
@@ -707,22 +788,22 @@ void CModuleAction<action_ros>::update_timeout(double time_s)
this->action_access.exit();
}
-template<class action_ros>
-void CModuleAction<action_ros>::stop_timeout(void)
+template<class action_ros,class dyn_reconf_config>
+void CModuleAction<action_ros,dyn_reconf_config>::stop_timeout(void)
{
this->action_access.enter();
this->action_timeout.stop();
this->action_access.exit();
}
-template<class action_ros>
-bool CModuleAction<action_ros>::is_timeout_enabled(void)
+template<class action_ros,class dyn_reconf_config>
+bool CModuleAction<action_ros,dyn_reconf_config>::is_timeout_enabled(void)
{
return this->use_timeout;
}
-template<class action_ros>
-bool CModuleAction<action_ros>::is_timeout_active(void)
+template<class action_ros,class dyn_reconf_config>
+bool CModuleAction<action_ros,dyn_reconf_config>::is_timeout_active(void)
{
this->action_access.enter();
if(this->use_timeout && this->action_timeout.timed_out())
@@ -738,36 +819,36 @@ bool CModuleAction<action_ros>::is_timeout_active(void)
}
}
-template<class action_ros>
-void CModuleAction<action_ros>::enable(void)
+template<class action_ros,class dyn_reconf_config>
+void CModuleAction<action_ros,dyn_reconf_config>::enable(void)
{
this->action_access.enter();
this->enabled=true;
this->action_access.exit();
}
-template<class action_ros>
-void CModuleAction<action_ros>::disable(void)
+template<class action_ros,class dyn_reconf_config>
+void CModuleAction<action_ros,dyn_reconf_config>::disable(void)
{
this->action_access.enter();
this->enabled=false;
this->action_access.exit();
}
-template<class action_ros>
-bool CModuleAction<action_ros>::is_enabled(void)
+template<class action_ros,class dyn_reconf_config>
+bool CModuleAction<action_ros,dyn_reconf_config>::is_enabled(void)
{
return this->enabled;
}
-template<class action_ros>
-std::string CModuleAction<action_ros>::get_name(void)
+template<class action_ros,class dyn_reconf_config>
+std::string CModuleAction<action_ros,dyn_reconf_config>::get_name(void)
{
return this->name;
}
-template<class action_ros>
-void CModuleAction<action_ros>::cancel(void)
+template<class action_ros,class dyn_reconf_config>
+void CModuleAction<action_ros,dyn_reconf_config>::cancel(void)
{
actionlib::SimpleClientGoalState action_state(actionlib::SimpleClientGoalState::PENDING);
@@ -779,8 +860,8 @@ void CModuleAction<action_ros>::cancel(void)
}
}
-template<class action_ros>
-bool CModuleAction<action_ros>::is_finished(void)
+template<class action_ros,class dyn_reconf_config>
+bool CModuleAction<action_ros,dyn_reconf_config>::is_finished(void)
{
actionlib::SimpleClientGoalState action_state(actionlib::SimpleClientGoalState::PENDING);
@@ -796,8 +877,8 @@ bool CModuleAction<action_ros>::is_finished(void)
return true;
}
-template<class action_ros>
-action_status CModuleAction<action_ros>::get_state(void)
+template<class action_ros,class dyn_reconf_config>
+action_status CModuleAction<action_ros,dyn_reconf_config>::get_state(void)
{
if(this->enabled)
{
@@ -813,8 +894,8 @@ action_status CModuleAction<action_ros>::get_state(void)
return this->status;
}
-template<class action_ros>
-CModuleAction<action_ros>::~CModuleAction()
+template<class action_ros,class dyn_reconf_config>
+CModuleAction<action_ros,dyn_reconf_config>::~CModuleAction()
{
if(this->enabled)
{
diff --git a/include/iri_ros_tools/module_service.h b/include/iri_ros_tools/module_service.h
index 61fe04b..4c1cc96 100644
--- a/include/iri_ros_tools/module_service.h
+++ b/include/iri_ros_tools/module_service.h
@@ -122,7 +122,7 @@ class CModuleService
* \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);
+ void dynamic_reconfigure(dyn_reconf_config &config, const std::string &name);
/**
* \brief Sets the maximum number of attempts to start the action
@@ -217,7 +217,7 @@ CModuleService<service_msg,dyn_reconf_config>::CModuleService(const std::string
}
template<class service_msg,class dyn_reconf_config>
-void CModuleService<service_msg,dyn_reconf_config>::dynamice_reconfigure(dyn_reconf_config &config, std::string &name)
+void CModuleService<service_msg,dyn_reconf_config>::dynamic_reconfigure(dyn_reconf_config &config, const std::string &name)
{
std::vector<typename dyn_reconf_config::AbstractParamDescriptionConstPtr> params;
boost::any value;
@@ -226,13 +226,13 @@ void CModuleService<service_msg,dyn_reconf_config>::dynamice_reconfigure(dyn_rec
for(typename std::vector<typename dyn_reconf_config::AbstractParamDescriptionConstPtr>::iterator param=params.begin();param!=params.end();param++)
{
- if((*param)->name==(name+"_max_num_retries"))
+ if((*param)->name==(name+"_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"))
+ else if((*param)->name==(name+"_enabled"))
{
if(value.type()==typeid(bool))
{
--
GitLab