diff --git a/CMakeLists.txt b/CMakeLists.txt
index 97303cda18d3cfb637aede8650bbd3bed738c09c..c4f0ca9cf96347bfa5d8d2c6353ff029f2ce4562 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,6 +17,7 @@ find_package(catkin REQUIRED COMPONENTS
 	diagnostic_updater
   behaviortree_cpp_v3
   iri_behaviortree
+  tf2_ros
 )
 
 ## System dependencies are found with CMake's conventions
@@ -93,7 +94,7 @@ catkin_python_setup()
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES ${PROJECT_NAME}
-  CATKIN_DEPENDS roscpp dynamic_reconfigure diagnostic_updater behaviortree_cpp_v3 iri_behaviortree
+  CATKIN_DEPENDS roscpp dynamic_reconfigure diagnostic_updater behaviortree_cpp_v3 iri_behaviortree tf2_ros
 #  DEPENDS system_lib
   DEPENDS Boost
 )
@@ -113,7 +114,7 @@ include_directories(
 # add_library(foo
 #   src/${PROJECT_NAME}/foo.cpp
 # )
-add_library(${PROJECT_NAME} include/${PROJECT_NAME}/iri_base_bt_client.h src/iri_bt_basic_nodes.cpp)
+add_library(${PROJECT_NAME} include/${PROJECT_NAME}/iri_base_bt_client.h src/iri_bt_basic_nodes.cpp include/${PROJECT_NAME}/iri_bt_dyn_reconf.h)
 set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX)
 
 ## Declare a cpp executable
diff --git a/include/iri_base_bt_client/iri_bt_dyn_reconf.h b/include/iri_base_bt_client/iri_bt_dyn_reconf.h
new file mode 100644
index 0000000000000000000000000000000000000000..40d475054e7675f5ff559621dfa76f7708c2d369
--- /dev/null
+++ b/include/iri_base_bt_client/iri_bt_dyn_reconf.h
@@ -0,0 +1,356 @@
+#ifndef IRI_BT_DYN_RECONF_H
+#define IRI_BT_DYN_RECONF_H
+
+#include "iri_bt_factory.h"
+
+#include <dynamic_reconfigure/server.h>
+#include <dynamic_reconfigure/Reconfigure.h>
+
+template<class Config>
+class IriBTDynReconf
+{
+  public:
+    IriBTDynReconf();
+
+    /**
+      * \brief Register all basic conditions and actions
+      *
+      * This function registers all basic conditions and actions needed for
+      * any generic application.
+      *
+      * \param factory (IriBehaviorTreeFactory)
+      *
+      */
+    void init(IriBehaviorTreeFactory &factory);
+
+  private:
+    bool get_string(Config &config, const std::string &name,std::string &value);
+    bool get_bool(Config &config, const std::string &name, bool &value);
+    bool get_integer(Config &config, const std::string &name, int &value);
+    bool get_unsigned_integer(Config &config, const std::string &name, unsigned int &value);
+    bool get_double(Config &config, const std::string &name, double &value);
+    // variables to know whether start or stop have been demanded by the user
+
+  protected:
+    BT::NodeStatus get_dyn_reconf_string(BT::TreeNode& self);
+    BT::NodeStatus get_dyn_reconf_bool(BT::TreeNode& self);
+    BT::NodeStatus get_dyn_reconf_integer(BT::TreeNode& self);
+    BT::NodeStatus get_dyn_reconf_unsigned_integer(BT::TreeNode& self);
+    BT::NodeStatus get_dyn_reconf_double(BT::TreeNode& self);
+    BT::NodeStatus get_dyn_reconf_double_vector(BT::TreeNode& self);
+};
+
+template<class Config>
+IriBTDynReconf<Config>::IriBTDynReconf()
+{
+}
+
+template<class Config>
+bool IriBTDynReconf<Config>::get_string(Config &config, const std::string &name,std::string &value)
+{
+  std::vector<typename Config::AbstractParamDescriptionConstPtr> params;
+  boost::any dyn_value;
+
+  params=Config::__getParamDescriptions__();
+
+  for(typename std::vector<typename Config::AbstractParamDescriptionConstPtr>::iterator param=params.begin();param!=params.end();param++)
+  {
+    if((*param)->name==(name))
+    {
+      (*param)->getValue(config,dyn_value);
+      if(dyn_value.type()==typeid(std::string))
+      {
+        value=boost::any_cast<std::string &>(dyn_value);
+        return true;
+      }
+      else
+        return false;
+    }
+  }
+  return false;
+}
+
+template<class Config>
+bool IriBTDynReconf<Config>::get_bool(Config &config, const std::string &name, bool &value)
+{
+  std::vector<typename Config::AbstractParamDescriptionConstPtr> params;
+  boost::any dyn_value;
+
+  params=Config::__getParamDescriptions__();
+
+  for(typename std::vector<typename Config::AbstractParamDescriptionConstPtr>::iterator param=params.begin();param!=params.end();param++)
+  {
+    if((*param)->name==(name))
+    {
+      (*param)->getValue(config,dyn_value);
+      if(dyn_value.type()==typeid(bool))
+      {
+        value=boost::any_cast<bool &>(dyn_value);
+        return true;
+      }
+      else
+        return false;
+    }
+  }
+  return false;
+}
+
+template<class Config>
+bool IriBTDynReconf<Config>::get_integer(Config &config, const std::string &name, int &value)
+{
+  std::vector<typename Config::AbstractParamDescriptionConstPtr> params;
+  boost::any dyn_value;
+
+  params=Config::__getParamDescriptions__();
+
+  for(typename std::vector<typename Config::AbstractParamDescriptionConstPtr>::iterator param=params.begin();param!=params.end();param++)
+  {
+    if((*param)->name==(name))
+    {
+      (*param)->getValue(config,dyn_value);
+      if(dyn_value.type()==typeid(int))
+      {
+        value=boost::any_cast<int &>(dyn_value);
+        return true;
+      }
+      else
+        return false;
+    }
+  }
+  return false;
+}
+
+template<class Config>
+bool IriBTDynReconf<Config>::get_unsigned_integer(Config &config, const std::string &name, unsigned int &value)
+{
+  std::vector<typename Config::AbstractParamDescriptionConstPtr> params;
+  boost::any dyn_value;
+
+  params=Config::__getParamDescriptions__();
+
+  for(typename std::vector<typename Config::AbstractParamDescriptionConstPtr>::iterator param=params.begin();param!=params.end();param++)
+  {
+    if((*param)->name==(name))
+    {
+      (*param)->getValue(config,dyn_value);
+      if(dyn_value.type()==typeid(int))
+      {
+        value=(unsigned int)boost::any_cast<int &>(dyn_value);
+        return true;
+      }
+      else
+        return false;
+    }
+  }
+  return false;
+}
+
+template<class Config>
+bool IriBTDynReconf<Config>::get_double(Config &config, const std::string &name, double &value)
+{
+  std::vector<typename Config::AbstractParamDescriptionConstPtr> params;
+  boost::any dyn_value;
+
+  params=Config::__getParamDescriptions__();
+
+  for(typename std::vector<typename Config::AbstractParamDescriptionConstPtr>::iterator param=params.begin();param!=params.end();param++)
+  {
+    if((*param)->name==(name))
+    {
+      (*param)->getValue(config,dyn_value);
+      if(dyn_value.type()==typeid(double))
+      {
+        value=boost::any_cast<double &>(dyn_value);
+        return true;
+      }
+      else
+        return false;
+    }
+  }
+  return false;
+}
+
+template<class Config>
+void IriBTDynReconf<Config>::init(IriBehaviorTreeFactory &factory)
+{
+  BT::PortsList get_string_ports = {BT::InputPort<Config>("dyn_config"),BT::InputPort<std::string>("param_name"),BT::OutputPort<std::string>("param_value")};
+  BT::PortsList get_integer_ports = {BT::InputPort<Config>("dyn_config"),BT::InputPort<std::string>("param_name"),BT::OutputPort<int>("param_value")};
+  BT::PortsList get_unsigned_integer_ports = {BT::InputPort<Config>("dyn_config"),BT::InputPort<std::string>("param_name"),BT::OutputPort<unsigned int>("param_value")};
+  BT::PortsList get_double_ports = {BT::InputPort<Config>("dyn_config"),BT::InputPort<std::string>("param_name"),BT::OutputPort<double>("param_value")};
+  BT::PortsList get_double_vector_ports = {BT::InputPort<Config>("dyn_config"),BT::InputPort<std::string>("param_name"),BT::OutputPort<std::vector<double>>("param_value")};
+  BT::PortsList get_bool_ports = {BT::InputPort<Config>("dyn_config"),BT::InputPort<std::string>("param_name"),BT::OutputPort<bool>("param_value")};
+
+  factory.registerSimpleAction("dyn_reconf_get_string", std::bind(&IriBTDynReconf::get_dyn_reconf_string, this, std::placeholders::_1),get_string_ports);
+  factory.registerSimpleAction("dyn_reconf_get_integer", std::bind(&IriBTDynReconf::get_dyn_reconf_integer, this, std::placeholders::_1),get_integer_ports);
+  factory.registerSimpleAction("dyn_reconf_get_unsigned_integer", std::bind(&IriBTDynReconf::get_dyn_reconf_unsigned_integer, this, std::placeholders::_1),get_unsigned_integer_ports);
+  factory.registerSimpleAction("dyn_reconf_get_double", std::bind(&IriBTDynReconf::get_dyn_reconf_double, this, std::placeholders::_1),get_double_ports);
+  factory.registerSimpleAction("dyn_reconf_get_double_vector", std::bind(&IriBTDynReconf::get_dyn_reconf_double_vector, this, std::placeholders::_1),get_double_vector_ports);
+  factory.registerSimpleAction("dyn_reconf_get_bool", std::bind(&IriBTDynReconf::get_dyn_reconf_bool, this, std::placeholders::_1),get_bool_ports);
+}
+
+template<class Config>
+BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_string(BT::TreeNode& self)
+{
+  ROS_DEBUG("IriBTDynReconf::get_dyn_reconf_string");
+  BT::Optional<Config> config = self.getInput<Config>("dyn_config");
+  BT::Optional<std::string> name = self.getInput<std::string>("param_name");
+
+  if ((!config) || (!name))
+  {
+    ROS_ERROR("IriBTDynReconf::get_dyn_reconf_string -> Incorrect or missing input. It needs the following input ports: name (std::string) value (std::string)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  Config config_value=config.value();
+  std::string name_value = name.value();
+  std::string value;
+  if(this->get_string(config_value,name_value,value))
+  {
+    self.setOutput("param_value", value);
+    return BT::NodeStatus::SUCCESS;
+  }
+  else
+    return BT::NodeStatus::FAILURE;
+}
+
+template<class Config>
+BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_bool(BT::TreeNode& self)
+{
+  ROS_DEBUG("IriBTDynReconf::get_dyn_reconf_bool");
+  BT::Optional<Config> config = self.getInput<Config>("dyn_config");
+  BT::Optional<std::string> name = self.getInput<std::string>("param_name");
+
+  if ((!config) || (!name))
+  {
+    ROS_ERROR("IriBTDynReconf::get_dyn_reconf_bool -> Incorrect or missing input. It needs the following input ports: name (std::string) value (std::string)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  Config config_value=config.value();
+  std::string name_value = name.value();
+  bool value;
+  if(this->get_bool(config_value,name_value,value))
+  {
+    self.setOutput("param_value", value);
+    return BT::NodeStatus::SUCCESS;
+  }
+  else
+    return BT::NodeStatus::FAILURE;
+}
+
+template<class Config>
+BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_integer(BT::TreeNode& self)
+{
+  ROS_DEBUG("IriBTDynReconf::get_dyn_reconf_integer");
+  BT::Optional<Config> config = self.getInput<Config>("dyn_config");
+  BT::Optional<std::string> name = self.getInput<std::string>("param_name");
+
+  if ((!config) || (!name))
+  {
+    ROS_ERROR("IriBTDynReconf::get_dyn_reconf_integer -> Incorrect or missing input. It needs the following input ports: name (std::string) value (int)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  Config config_value=config.value();
+  std::string name_value = name.value();
+  int value;
+  if(this->get_integer(config_value,name_value,value))
+  {
+    self.setOutput("param_value", value);
+    return BT::NodeStatus::SUCCESS;
+  }
+  else
+    return BT::NodeStatus::FAILURE;
+}
+
+template<class Config>
+BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_unsigned_integer(BT::TreeNode& self)
+{
+  ROS_DEBUG("IriBTDynReconf::get_dyn_reconf_unsigned_integer");
+  BT::Optional<Config> config = self.getInput<Config>("dyn_config");
+  BT::Optional<std::string> name = self.getInput<std::string>("param_name");
+
+  if ((!config) || (!name))
+  {
+    ROS_ERROR("IriBTDynReconf::get_dyn_reconf_unsigned_integer -> Incorrect or missing input. It needs the following input ports: name (std::string) value (int)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  Config config_value=config.value();
+  std::string name_value = name.value();
+  unsigned int value;
+  if(this->get_unsigned_integer(config_value,name_value,value))
+  {
+    self.setOutput("param_value", value);
+    return BT::NodeStatus::SUCCESS;
+  }
+  else
+    return BT::NodeStatus::FAILURE;
+}
+
+template<class Config>
+BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_double(BT::TreeNode& self)
+{
+  ROS_DEBUG("IriBTDynReconf::get_dyn_reconf_double");
+  BT::Optional<Config> config = self.getInput<Config>("dyn_config");
+  BT::Optional<std::string> name = self.getInput<std::string>("param_name");
+
+  if ((!config) || (!name))
+  {
+    ROS_ERROR("IriBTDynReconf::get_dyn_reconf_double -> Incorrect or missing input. It needs the following input ports: name (std::string) value (std::string)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  Config config_value=config.value();
+  std::string name_value = name.value();
+  double value;
+  if(this->get_double(config_value,name_value,value))
+  {
+    self.setOutput("param_value", value);
+    return BT::NodeStatus::SUCCESS;
+  }
+  else
+    return BT::NodeStatus::FAILURE;
+}
+
+template<class Config>
+BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_double_vector(BT::TreeNode& self)
+{
+  ROS_DEBUG("IriBTDynReconf::get_dyn_reconf_double_vector");
+  BT::Optional<Config> config = self.getInput<Config>("dyn_config");
+  BT::Optional<std::string> name = self.getInput<std::string>("param_name");
+
+  if ((!config) || (!name))
+  {
+    ROS_ERROR("IriBTDynReconf::get_dyn_reconf_double_vector -> Incorrect or missing input. It needs the following input ports: name (std::string) value (std::vector<double>)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  Config config_value=config.value();
+  std::string name_value = name.value();
+  std::stringstream text;
+  unsigned char dummy;
+  std::string string_value;
+  std::vector<double> values;
+  double value; 
+  if(this->get_string(config_value,name_value,string_value))
+  {
+    text.str(string_value);
+    text >> dummy; // read [
+    do{
+      text >> value >> dummy;// read "value," or "value]"
+      values.push_back(value);
+    }while(dummy!=']' && !text.fail());
+    if(text.fail())
+      return BT::NodeStatus::FAILURE;
+    else
+    {
+      self.setOutput("param_value", values);
+      return BT::NodeStatus::SUCCESS;
+    }
+  }
+  else
+    return BT::NodeStatus::FAILURE;
+}
+
+#endif
diff --git a/package.xml b/package.xml
index 47b5bd93c59beedf574bc1ad1451a5f5e4b9ffe8..ffbd00b6fb0e066f3672b17135578b9ca9c47a7b 100644
--- a/package.xml
+++ b/package.xml
@@ -18,6 +18,7 @@
   <depend>diagnostic_updater</depend>
   <depend>behaviortree_cpp_v3</depend>
   <depend>iri_behaviortree</depend>
+  <depend>tf2_ros</depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->