diff --git a/include/iri_nav_module/teb_lp_module.h b/include/iri_nav_module/teb_lp_module.h
index c0b35c239263caefdabb6488e7a47fb2bc6fa439..a0989591a120982cabba4e9bc61015c9ab81f1c4 100644
--- a/include/iri_nav_module/teb_lp_module.h
+++ b/include/iri_nav_module/teb_lp_module.h
@@ -20,7 +20,7 @@ class CTEBLPModule : public CNavPlannerModule<ModuleCfg>
     CTEBLPModule(const std::string &name,const std::string &name_space=std::string(""));
 
     /**
-     * \brief Function to set the parameter
+     * \brief Function to set the parameter xy_goal_tolerance.
      *
      * \param value The value to set.
      * 
@@ -29,7 +29,7 @@ class CTEBLPModule : public CNavPlannerModule<ModuleCfg>
     dyn_reconf_status_t set_xy_goal_tolerance(double &value);
 
     /**
-     * \brief Function to get the parameter
+     * \brief Function to get the parameter xy_goal_tolerance.
      *
      * \param value The parameter value.
      * 
@@ -38,7 +38,7 @@ class CTEBLPModule : public CNavPlannerModule<ModuleCfg>
     dyn_reconf_status_t get_xy_goal_tolerance(double &value);
 
     /**
-     * \brief Function to set the parameter
+     * \brief Function to set the parameter yaw_goal_tolerance
      *
      * \param value The value to set.
      * 
@@ -47,7 +47,7 @@ class CTEBLPModule : public CNavPlannerModule<ModuleCfg>
     dyn_reconf_status_t set_yaw_goal_tolerance(double &value);
 
     /**
-     * \brief Function to get the parameter
+     * \brief Function to get the parameter yaw_goal_tolerance
      *
      * \param value The parameter value.
      * 
@@ -78,7 +78,7 @@ class CTEBLPModule : public CNavPlannerModule<ModuleCfg>
     dyn_reconf_status_t set_goal_tolerances(double &xy_tol, double &yaw_tol);
 
     /**
-     * \brief Function to set the parameter
+     * \brief Function to set the parameter max_vel_x
      *
      * \param value The value to set.
      * 
@@ -87,7 +87,7 @@ class CTEBLPModule : public CNavPlannerModule<ModuleCfg>
     dyn_reconf_status_t set_max_vel_x(double &value);
 
     /**
-     * \brief Function to get the parameter
+     * \brief Function to get the parameter max_vel_x
      *
      * \param value The parameter value.
      * 
@@ -96,25 +96,25 @@ class CTEBLPModule : public CNavPlannerModule<ModuleCfg>
     dyn_reconf_status_t get_max_vel_x(double &value);
 
     /**
-     * \brief Function to set the parameter
+     * \brief Function to set the parameter max_vel_x_backwards.
      *
      * \param value The value to set.
      * 
      * \return The status of the operation.
      */
-    dyn_reconf_status_t set_mav_vel_x_backwards(double &value);
+    dyn_reconf_status_t set_max_vel_x_backwards(double &value);
 
     /**
-     * \brief Function to get the parameter
+     * \brief Function to get the parameter max_vel_x_backwards.
      *
      * \param value The parameter value.
      * 
      * \return The status of the operation.
      */
-    dyn_reconf_status_t get_mav_vel_x_backwards(double &value);
+    dyn_reconf_status_t get_max_vel_x_backwards(double &value);
 
     /**
-     * \brief Function to set the parameter
+     * \brief Function to set the parameter max_vel_theta.
      *
      * \param value The value to set.
      * 
@@ -123,7 +123,7 @@ class CTEBLPModule : public CNavPlannerModule<ModuleCfg>
     dyn_reconf_status_t set_max_vel_theta(double &value);
 
     /**
-     * \brief Function to get the parameter
+     * \brief Function to get the parameter max_vel_theta.
      *
      * \param value The parameter value.
      * 
@@ -132,7 +132,7 @@ class CTEBLPModule : public CNavPlannerModule<ModuleCfg>
     dyn_reconf_status_t get_max_vel_theta(double &value);
 
     /**
-     * \brief Function to set the parameter
+     * \brief Function to set the parameter acc_lim_x.
      *
      * \param value The value to set.
      * 
@@ -141,7 +141,7 @@ class CTEBLPModule : public CNavPlannerModule<ModuleCfg>
     dyn_reconf_status_t set_acc_lim_x(double &value);
 
     /**
-     * \brief Function to get the parameter
+     * \brief Function to get the parameter acc_lim_x.
      *
      * \param value The parameter value.
      * 
@@ -150,7 +150,7 @@ class CTEBLPModule : public CNavPlannerModule<ModuleCfg>
     dyn_reconf_status_t get_acc_lim_x(double &value);
 
     /**
-     * \brief Function to set the parameter
+     * \brief Function to set the parameter acc_lim_theta.
      *
      * \param value The value to set.
      * 
@@ -159,7 +159,7 @@ class CTEBLPModule : public CNavPlannerModule<ModuleCfg>
     dyn_reconf_status_t set_acc_lim_theta(double &value);
 
     /**
-     * \brief Function to get the parameter
+     * \brief Function to get the parameter acc_lim_theta.
      *
      * \param value The parameter value.
      * 
@@ -257,14 +257,14 @@ class CTEBLPModule : public CNavPlannerModule<ModuleCfg>
   }
 
   template <class ModuleCfg>
-  dyn_reconf_status_t CTEBLPModule<ModuleCfg>::set_mav_vel_x_backwards(double &value)
+  dyn_reconf_status_t CTEBLPModule<ModuleCfg>::set_max_vel_x_backwards(double &value)
   {
     this->planner_reconf.set_parameter("mav_vel_x_backwards",value);
     return this->planner_reconf.get_status();
   }
 
   template <class ModuleCfg>
-  dyn_reconf_status_t CTEBLPModule<ModuleCfg>::get_mav_vel_x_backwards(double &value)
+  dyn_reconf_status_t CTEBLPModule<ModuleCfg>::get_max_vel_x_backwards(double &value)
   {
     if(this->planner_reconf.get_parameter("mav_vel_x_backwards",value))
       return DYN_RECONF_SUCCESSFULL;
diff --git a/include/iri_nav_module/teb_lp_module_bt.h b/include/iri_nav_module/teb_lp_module_bt.h
index fc2b2a009ea5491d51b92e52bd853e9b97d2a295..505edeb1cbe96255e434bb51b29b69f101eb6704 100644
--- a/include/iri_nav_module/teb_lp_module_bt.h
+++ b/include/iri_nav_module/teb_lp_module_bt.h
@@ -52,8 +52,8 @@ class CTEBLPModuleBT
     BT::NodeStatus get_yaw_goal_tolerance(BT::TreeNode& self);
     BT::NodeStatus set_max_vel_x(BT::TreeNode& self);
     BT::NodeStatus get_max_vel_x(BT::TreeNode& self);
-    BT::NodeStatus set_mav_vel_x_backwards(BT::TreeNode& self);
-    BT::NodeStatus get_mav_vel_x_backwards(BT::TreeNode& self);  
+    BT::NodeStatus set_max_vel_x_backwards(BT::TreeNode& self);
+    BT::NodeStatus get_max_vel_x_backwards(BT::TreeNode& self);  
     BT::NodeStatus set_max_vel_theta(BT::TreeNode& self);
     BT::NodeStatus get_max_vel_theta(BT::TreeNode& self); 
   };
@@ -94,8 +94,8 @@ class CTEBLPModuleBT
     factory.registerSimpleAction(this->name+"_teb_get_yaw_goal_tolerance", std::bind(&CTEBLPModuleBT::get_yaw_goal_tolerance, this, std::placeholders::_1), get_yaw_goal_tol_ports);
     factory.registerSimpleAction(this->name+"_teb_set_max_vel_x", std::bind(&CTEBLPModuleBT::set_max_vel_x, this, std::placeholders::_1), set_max_vel_x_ports);
     factory.registerSimpleAction(this->name+"_teb_get_max_vel_x", std::bind(&CTEBLPModuleBT::get_max_vel_x, this, std::placeholders::_1), get_max_vel_x_ports);
-    factory.registerSimpleAction(this->name+"_teb_set_mav_vel_x_backwards", std::bind(&CTEBLPModuleBT::set_mav_vel_x_backwards, this, std::placeholders::_1), set_max_vel_x_back_ports);
-    factory.registerSimpleAction(this->name+"_teb_get_mav_vel_x_backwards", std::bind(&CTEBLPModuleBT::get_mav_vel_x_backwards, this, std::placeholders::_1), get_max_vel_x_back_ports);
+    factory.registerSimpleAction(this->name+"_teb_set_max_vel_x_backwards", std::bind(&CTEBLPModuleBT::set_max_vel_x_backwards, this, std::placeholders::_1), set_max_vel_x_back_ports);
+    factory.registerSimpleAction(this->name+"_teb_get_max_vel_x_backwards", std::bind(&CTEBLPModuleBT::get_max_vel_x_backwards, this, std::placeholders::_1), get_max_vel_x_back_ports);
     factory.registerSimpleAction(this->name+"_teb_set_max_vel_theta", std::bind(&CTEBLPModuleBT::set_max_vel_theta, this, std::placeholders::_1), set_max_vel_theta_ports);
     factory.registerSimpleAction(this->name+"_teb_get_max_vel_theta", std::bind(&CTEBLPModuleBT::get_max_vel_theta, this, std::placeholders::_1), get_max_vel_theta_ports);
 
@@ -256,30 +256,30 @@ class CTEBLPModuleBT
   }
 
   template <class ModuleCfg>
-  BT::NodeStatus CTEBLPModuleBT<ModuleCfg>::set_mav_vel_x_backwards(BT::TreeNode& self)
+  BT::NodeStatus CTEBLPModuleBT<ModuleCfg>::set_max_vel_x_backwards(BT::TreeNode& self)
   {
-    ROS_DEBUG("CTEBLPModuleBT::set_mav_vel_x_backwards-> set_mav_vel_x_backwards");
+    ROS_DEBUG("CTEBLPModuleBT::set_max_vel_x_backwards-> set_max_vel_x_backwards");
 
     BT::Optional<double> bt_vel = self.getInput<double>("max_vel_x_back");
     if (!bt_vel)
     {
-      ROS_ERROR("CTEBLPModuleBT::set_mav_vel_x_backwards-> Incorrect or missing input. It needs the following input ports: max_vel_x_back(double)");
+      ROS_ERROR("CTEBLPModuleBT::set_max_vel_x_backwards-> Incorrect or missing input. It needs the following input ports: max_vel_x_back(double)");
       return BT::NodeStatus::FAILURE;
     }
     double vel=bt_vel.value();
-    if(this->module.set_mav_vel_x_backwards(vel)==DYN_RECONF_SUCCESSFULL)
+    if(this->module.set_max_vel_x_backwards(vel)==DYN_RECONF_SUCCESSFULL)
       return BT::NodeStatus::SUCCESS;
     else
       return BT::NodeStatus::FAILURE;
   }
 
   template <class ModuleCfg>
-  BT::NodeStatus CTEBLPModuleBT<ModuleCfg>::get_mav_vel_x_backwards(BT::TreeNode& self)
+  BT::NodeStatus CTEBLPModuleBT<ModuleCfg>::get_max_vel_x_backwards(BT::TreeNode& self)
   {
-    ROS_DEBUG("CTEBLPModuleBT::get_mav_vel_x_backwards-> get_mav_vel_x_backwards");
+    ROS_DEBUG("CTEBLPModuleBT::get_max_vel_x_backwards-> get_max_vel_x_backwards");
 
     double vel;
-    if(this->module.get_mav_vel_x_backwards(vel)==DYN_RECONF_SUCCESSFULL)
+    if(this->module.get_max_vel_x_backwards(vel)==DYN_RECONF_SUCCESSFULL)
     {
       self.setOutput("max_vel_x_back", vel);
       return BT::NodeStatus::SUCCESS;
diff --git a/src/xml/bt_definitions.xml b/src/xml/bt_definitions.xml
index 388c071cb0ee2f12ab02b5b9addd588130b0be92..0eaaf91bd9d47dc30b250902dc7523c01cf9023b 100644
--- a/src/xml/bt_definitions.xml
+++ b/src/xml/bt_definitions.xml
@@ -147,10 +147,10 @@
         <Action ID="nav_module_teb_get_max_vel_x">
             <output_port name="max_vel_x"> Local planner max vel x</output_port>
         </Action>
-        <Action ID="nav_module_teb_set_mav_vel_x_backwards">
+        <Action ID="nav_module_teb_set_max_vel_x_backwards">
             <input_port name="max_vel_x_back"> Local planner max vel x backwards</input_port>
         </Action>
-        <Action ID="nav_module_teb_get_mav_vel_x_backwards">
+        <Action ID="nav_module_teb_get_max_vel_x_backwards">
             <output_port name="max_vel_x_back"> Local planner max vel x backwards</output_port>
         </Action>
         <Action ID="nav_module_teb_set_max_vel_theta">