Solved a bug with the order in which the attributes of the class were...

Solved a bug with the order in which the attributes of the class were initialized: the wrong order caused a segmentation fault.
Catched the BT::RunTime exception while trying to import the tree.
Partially implemented the dynamic reconfigure callback.

include/iri_base_bt_client/iri_base_bt_client.h

@@ -37,6 +37,7 @@
 #include <boost/thread.hpp>
 #include <boost/bind.hpp>
 #include <boost/filesystem.hpp>
+#include <boost/any.hpp>
 
 // dynamic reconfigure server include
 #include <dynamic_reconfigure/server.h>
@@ -114,31 +115,6 @@ class IriBaseBTClient
     */
     BT::NodeStatus status;
 
-   /**
-    * \brief config object
-    *
-    * All template ConfigClass class will need a Config variable to allow ROS
-    * dynamic reconfigure. This config class will contain all algorithm 
-    * parameters which may be modified once the algorithm node is launched.
-    */
-    typedef ConfigClass Config;
-
-   /**
-    * \brief ros spin thread
-    * 
-    * This boost thread object will manage the ros::spin function. It is 
-    * instantiated and launched in the spin class method.
-    */
-    boost::shared_ptr<boost::thread> ros_thread_;
-
-   /**
-    * \brief dynamic reconfigure server
-    * 
-    * The dynamic reconfigure server is in charge to retrieve the parameters
-    * defined in the config cfg file through the reconfigureCallback.
-    */
-    dynamic_reconfigure::Server<Config> dsrv_;
-
    /**
     * \brief main thread loop rate
     * 
@@ -149,7 +125,6 @@ class IriBaseBTClient
     */
     ros::Rate loop_rate_;
 
-
   protected:
    /**
     * \brief define config type
@@ -203,6 +178,31 @@ class IriBaseBTClient
     */
     diagnostic_updater::Updater diagnostic_;
 
+   /**
+    * \brief ros spin thread
+    * 
+    * This boost thread object will manage the ros::spin function. It is 
+    * instantiated and launched in the spin class method.
+    */
+    boost::shared_ptr<boost::thread> ros_thread_;
+
+   /**
+    * \brief dynamic reconfigure server
+    * 
+    * The dynamic reconfigure server is in charge to retrieve the parameters
+    * defined in the config cfg file through the reconfigureCallback.
+    */
+    dynamic_reconfigure::Server<ConfigClass> dsrv_;
+
+   /**
+    * \brief config object
+    *
+    * All template ConfigClass class will need a Config variable to allow ROS
+    * dynamic reconfigure. This config class will contain all algorithm 
+    * parameters which may be modified once the algorithm node is launched.
+    */
+    ConfigClass config_;
+
   public:
    /**
     * \brief constructor
@@ -332,7 +332,7 @@ class IriBaseBTClient
     * \param level  integer referring the level in which the configuration
     *               has been changed.
     */
-    void reconfigureCallback(Config &config, uint32_t level);
+    void reconfigureCallback(ConfigClass &config, uint32_t level);
 
    /**
     * \brief dynamic reconfigure server callback
@@ -346,7 +346,7 @@ class IriBaseBTClient
     * \param level  integer referring the level in which the configuration
     *               has been changed.
     */
-    virtual void node_config_update(Config &config, uint32_t level) = 0;
+    virtual void node_config_update(ConfigClass &config, uint32_t level) = 0;
    
    /**
     * \brief add diagnostics
@@ -397,6 +397,12 @@ IriBaseBTClient<ConfigClass>::IriBaseBTClient(const ros::NodeHandle &nh) :
 {
   ROS_DEBUG("IriBaseBTClient::Constructor");
 
+  if(!this->private_node_handle_.getParam("bt_client_rate", this->config_.bt_client_rate))
+  {
+    ROS_WARN("UserBtClientAlgNode::UserBtClientAlgNode: param 'bt_client_rate' not found");
+  }
+  else
+    this->setRate(this->config_.bt_client_rate);
   // allow Ctrl+C management
   signal(SIGHUP, &IriBaseBTClient<ConfigClass>::hupCalled);
 
@@ -494,7 +500,7 @@ void IriBaseBTClient<ConfigClass>::enable_minitrace_logger()
       }
       else
       {
-        ROS_INFO("IriBaseBTClient: Log directory created: %s", logs_path.c_str());
+        ROS_DEBUG("IriBaseBTClient: Log directory created: %s", logs_path.c_str());
         path_ok=true;
       }
     }
@@ -527,7 +533,7 @@ void IriBaseBTClient<ConfigClass>::enable_file_logger()
       }
       else
       {
-        ROS_INFO("IriBaseBTClient: Log directory created: %s", logs_path.c_str());
+        ROS_DEBUG("IriBaseBTClient: Log directory created: %s", logs_path.c_str());
         path_ok=true;
       }
     }
@@ -593,9 +599,44 @@ void IriBaseBTClient<ConfigClass>::disable_zmq_publisher()
 }
 
 template <class ConfigClass>
-void IriBaseBTClient<ConfigClass>::reconfigureCallback(Config &config, uint32_t level)
+void IriBaseBTClient<ConfigClass>::reconfigureCallback(ConfigClass &config, uint32_t level)
 {
+  std::vector<typename ConfigClass::AbstractParamDescriptionConstPtr> params;
+  boost::any value;
+  double new_rate;
+  bool new_enable;
+
   ROS_DEBUG("IriBaseBTClient::reconfigureCallback");
+
+  // process the iri_base_bt_client parameters
+  this->lock();
+  params=ConfigClass::__getParamDescriptions__();
+  for(typename std::vector<typename ConfigClass::AbstractParamDescriptionConstPtr>::iterator param=params.begin();param!=params.end();param++)
+  { 
+    (*param)->getValue(config,value);
+    if((*param)->name=="bt_client_rate")
+    {
+      new_rate=boost::any_cast<double &>(value);
+      if(new_rate!=this->getRate())
+        this->setRate(new_rate);
+    }
+    else if((*param)->name=="iri_bt_client_enable_cout_logger")
+    {
+      new_enable=boost::any_cast<bool &>(value);
+      if(new_enable)
+      {
+        if(this->logger_cout==NULL)
+          this->enable_cout_logger();
+      }
+      else
+      {
+        if(this->logger_cout!=NULL)
+          this->disable_cout_logger();
+      }
+    }
+  }
+  this->unlock();
+
   this->node_config_update(config, level);
 }
 
@@ -623,29 +664,28 @@ void *IriBaseBTClient<ConfigClass>::mainThread(void *param)
     std::string tree_string = iriNode->path + "/" + iriNode->tree_xml_file + ".xml";
     iriNode->tree = iriNode->factory.createTreeFromFile(tree_string);
     ROS_DEBUG("IriBaseBTClient: tree created from: '%s'", tree_string.c_str());
+    while(ros::ok())
+    {
+      try                        
+      {
+        iriNode->status=iriNode->tree.root_node->executeTick();
+        ROS_DEBUG_STREAM("Tree status: " << iriNode->status);          
+      } 
+      catch(BT::LogicError &e)
+      { 
+        ROS_ERROR_STREAM("EXCEPTION " << e.what());
+      }
+      // sleep remainder time
+      iriNode->loop_rate_.sleep();
+    }
   }
-  catch(BT::LogicError &e)
+  catch(BT::RuntimeError &e)
   {
     ROS_ERROR_STREAM("IriBaseBTClient: Exception: " << e.what());
     ROS_ERROR("IriBaseBTClient: couldn't create tree from file. Exiting");
     ros::shutdown();
   }
 
-  while(ros::ok())
-  {
-    try                        
-    {
-      iriNode->status=iriNode->tree.root_node->executeTick();
-      ROS_DEBUG_STREAM("Tree status: " << iriNode->status);          
-    } 
-    catch(BT::LogicError &e)
-    { 
-      ROS_ERROR_STREAM("EXCEPTION " << e.what());
-    }
-    // sleep remainder time
-    iriNode->loop_rate_.sleep();
-  }
-
   // kill main thread
   pthread_exit(NULL);
 }