Merge branch 'generic_dyn_reconf' into 'master'

Added the dynamic_reconfigure function to handle generic parameters to the...

See merge request !1

CMakeLists.txt

@@ -7,12 +7,12 @@ 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
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
+catkin_python_setup()
 
 ################################################
 ## Declare ROS messages, services and actions ##
@@ -77,7 +77,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp)
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES ${PROJECT_NAME}
-  CATKIN_DEPENDS roscpp 
+  CATKIN_DEPENDS roscpp rospy
 )
 
 ###########

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)
 {

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,97 @@ 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++)
+  {
+    (*param)->getValue(config,value);
+    if((*param)->name==(name+"_num_retries"))
+    {
+      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;
+      }
+    }
+    else if((*param)->name==(name+"_timeout_s"))
+    {
+      if(value.type()==typeid(double))
+        timeout=boost::any_cast<double &>(value);
+    }
+    else if((*param)->name==(name+"_feedback_watchdog_time_s"))
+    {
+      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);
+  else
+    this->disable_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 +700,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)
@@ -659,11 +739,14 @@ bool CModuleAction<action_ros>::is_watchdog_active(void)
     }
   }
   else
+  {
+    this->action_access.exit();
     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 +762,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 +790,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 +821,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 +862,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 +879,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 +896,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)
   {

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()
 {
 
 }

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 dynamic_reconfigure(dyn_reconf_config &config, const 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>::dynamic_reconfigure(dyn_reconf_config &config, const 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+"_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+"_enabled"))
+    {
+      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>