diff --git a/include/iri_nav_module/teb_lp_module.h b/include/iri_nav_module/teb_lp_module.h
index 21a07445d147b39d45b32d4550b071ca999edec2..c0b35c239263caefdabb6488e7a47fb2bc6fa439 100644
--- a/include/iri_nav_module/teb_lp_module.h
+++ b/include/iri_nav_module/teb_lp_module.h
@@ -204,6 +204,22 @@ class CTEBLPModule : public CNavPlannerModule<ModuleCfg>
       return DYN_RECONF_NO_SUCH_PARAM;
   }
 
+  template <class ModuleCfg>
+  dyn_reconf_status_t CTEBLPModule<ModuleCfg>::set_yaw_goal_tolerance(double &value)
+  {
+    this->planner_reconf.set_parameter("yaw_goal_tolerance",value);
+    return this->planner_reconf.get_status();
+  }
+
+  template <class ModuleCfg>
+  dyn_reconf_status_t CTEBLPModule<ModuleCfg>::get_yaw_goal_tolerance(double &value)
+  {
+    if(this->planner_reconf.get_parameter("yaw_goal_tolerance",value))
+      return DYN_RECONF_SUCCESSFULL;
+    else
+      return DYN_RECONF_NO_SUCH_PARAM;
+  }
+
   template <class ModuleCfg>
   dyn_reconf_status_t CTEBLPModule<ModuleCfg>::set_goal_tolerances(double &xy_tol, double &yaw_tol)
   {