diff --git a/include/iri_nav_module/ackermann_lp_module.h b/include/iri_nav_module/ackermann_lp_module.h
index ec5030661439acc06148989186afb4eaaf234742..e9ba1e63a4a4100712f2ca0763e8de409b27ecbd 100644
--- a/include/iri_nav_module/ackermann_lp_module.h
+++ b/include/iri_nav_module/ackermann_lp_module.h
@@ -278,6 +278,36 @@ class CAckermannLPModule : public CNavPlannerModule<ModuleCfg>
      *
      */
     dyn_reconf_status_t set_wheel_radius(double radius);
+    /**
+     * \brief
+     *
+     */
+    dyn_reconf_status_t is_inverted_ackermann(bool &inverted);
+    /**
+     * \brief
+     *
+     */
+    dyn_reconf_status_t use_inverted_ackermann(bool inverted);
+    /**
+     * \brief
+     *
+     */
+    dyn_reconf_status_t is_steering_while_stopped(bool &enabled);
+    /**
+     * \brief
+     *
+     */
+    dyn_reconf_status_t use_steering_while_stopped(bool enabled,double angle_threshold);
+    /**
+     * \brief
+     *
+     */
+    dyn_reconf_status_t is_stuck_check_enabled(bool &enabled);
+    /**
+     * \brief
+     *
+     */
+    dyn_reconf_status_t use_stuck_check(bool enabled,double speed_threshold,unsigned int max_occurrences);
     /**
       * \brief Destructor
       *
@@ -349,8 +379,13 @@ class CAckermannLPModule : public CNavPlannerModule<ModuleCfg>
   template <class ModuleCfg>
   dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_trans_vel_samples(unsigned int &samples)
   {
-    if(this->planner_reconf.get_parameter("trans_vel_samples",samples))
+    int value;
+
+    if(this->planner_reconf.get_parameter("trans_vel_samples",value))
+    {
+      samples=value;
       return DYN_RECONF_SUCCESSFULL;
+    }
     else
       return DYN_RECONF_NO_SUCH_PARAM;
   }
@@ -358,15 +393,20 @@ class CAckermannLPModule : public CNavPlannerModule<ModuleCfg>
   template <class ModuleCfg>
   dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_trans_vel_samples(unsigned int samples)
   {
-    this->planner_reconf.set_parameter("trans_vel_samples",samples);
+    this->planner_reconf.set_parameter("trans_vel_samples",(int)samples);
     return this->planner_reconf.get_status();
   }
 
   template <class ModuleCfg>
   dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_steer_angle_samples(unsigned int &samples)
   {
-    if(this->planner_reconf.get_parameter("steer_angle_samples",samples))
+    int value;
+
+    if(this->planner_reconf.get_parameter("steer_angle_samples",value))
+    {
+      samples=value;
       return DYN_RECONF_SUCCESSFULL;
+    }
     else
       return DYN_RECONF_NO_SUCH_PARAM;
   }
@@ -374,15 +414,20 @@ class CAckermannLPModule : public CNavPlannerModule<ModuleCfg>
   template <class ModuleCfg>
   dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_steer_angle_samples(unsigned int samples)
   {
-    this->planner_reconf.set_parameter("steer_angle_samples",samples);
+    this->planner_reconf.set_parameter("steer_angle_samples",(int)samples);
     return this->planner_reconf.get_status();
   }
 
   template <class ModuleCfg>
   dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_angular_vel_samples(unsigned int &samples)
   {
-    if(this->planner_reconf.get_parameter("angular_vel_samples",samples))
+    int value;
+
+    if(this->planner_reconf.get_parameter("angular_vel_samples",value))
+    {
+      samples=value;
       return DYN_RECONF_SUCCESSFULL;
+    }
     else
       return DYN_RECONF_NO_SUCH_PARAM;
   }
@@ -390,15 +435,20 @@ class CAckermannLPModule : public CNavPlannerModule<ModuleCfg>
   template <class ModuleCfg>
   dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_angular_vel_samples(unsigned int samples)
   {
-    this->planner_reconf.set_parameter("angular_vel_samples",samples);
+    this->planner_reconf.set_parameter("angular_vel_samples",(int)samples);
     return this->planner_reconf.get_status();
   }
 
   template <class ModuleCfg>
   dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_cmd_vel_average_samples(unsigned int &samples)
   {
-    if(this->planner_reconf.get_parameter("cmd_vel_avg",samples))
+    int value;
+
+    if(this->planner_reconf.get_parameter("cmd_vel_avg",value))
+    {
+      samples=value;
       return DYN_RECONF_SUCCESSFULL;
+    }
     else
       return DYN_RECONF_NO_SUCH_PARAM;
   }
@@ -406,15 +456,20 @@ class CAckermannLPModule : public CNavPlannerModule<ModuleCfg>
   template <class ModuleCfg>
   dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_cmd_vel_average_samples(unsigned int samples)
   {
-    this->planner_reconf.set_parameter("cmd_vel_avg",samples);
+    this->planner_reconf.set_parameter("cmd_vel_avg",(int)samples);
     return this->planner_reconf.get_status();
   }
 
   template <class ModuleCfg>
   dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_odom_average_samples(unsigned int &samples)
   {
-    if(this->planner_reconf.get_parameter("odom_avg",samples))
+    int value;
+
+    if(this->planner_reconf.get_parameter("odom_avg",value))
+    {
+      samples=value;
       return DYN_RECONF_SUCCESSFULL;
+    }
     else
       return DYN_RECONF_NO_SUCH_PARAM;
   }
@@ -422,15 +477,20 @@ class CAckermannLPModule : public CNavPlannerModule<ModuleCfg>
   template <class ModuleCfg>
   dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_odom_average_samples(unsigned int samples)
   {
-    this->planner_reconf.set_parameter("odom_avg",samples);
+    this->planner_reconf.set_parameter("odom_avg",(int)samples);
     return this->planner_reconf.get_status();
   }
 
   template <class ModuleCfg>
   dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_heading_cost_points(unsigned int &points)
   {
-    if(this->planner_reconf.get_parameter("heading_points",points))
+    int num;
+
+    if(this->planner_reconf.get_parameter("heading_points",num))
+    {
+      points=num;
       return DYN_RECONF_SUCCESSFULL;
+    }
     else
       return DYN_RECONF_NO_SUCH_PARAM;
   }
@@ -438,7 +498,7 @@ class CAckermannLPModule : public CNavPlannerModule<ModuleCfg>
   template <class ModuleCfg>
   dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_heading_cost_points(unsigned int points)
   {
-    this->planner_reconf.set_parameter("heading_points",points);
+    this->planner_reconf.set_parameter("heading_points",(int)points);
     return this->planner_reconf.get_status();
   }
 
@@ -791,6 +851,72 @@ class CAckermannLPModule : public CNavPlannerModule<ModuleCfg>
     return this->planner_reconf.get_status();
   }
 
+  template <class ModuleCfg>
+  dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::is_inverted_ackermann(bool &inverted)
+  {
+    if(this->planner_reconf.get_parameter("invert_steer_angle_cmd",inverted))
+      return DYN_RECONF_SUCCESSFULL;
+    else
+      return DYN_RECONF_NO_SUCH_PARAM;
+  }
+
+  template <class ModuleCfg>
+  dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::use_inverted_ackermann(bool inverted)
+  {
+    this->planner_reconf.set_parameter("invert_steer_angle_cmd",inverted);
+    return this->planner_reconf.get_status();
+  }
+
+  template <class ModuleCfg>
+  dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::is_steering_while_stopped(bool &enabled)
+  {
+    if(this->planner_reconf.get_parameter("use_stopped_steering",enabled))
+      return DYN_RECONF_SUCCESSFULL;
+    else
+      return DYN_RECONF_NO_SUCH_PARAM;
+  }
+
+  template <class ModuleCfg>
+  dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::use_steering_while_stopped(bool enabled,double angle_threshold)
+  {
+    this->planner_reconf.set_parameter("stopped_steering_angle_threshold",angle_threshold);
+    if(this->planner_reconf.get_status()==DYN_RECONF_SUCCESSFULL)
+    {
+      this->planner_reconf.set_parameter("use_stopped_steering",enabled);
+      return this->planner_reconf.get_status();
+    }
+    else
+      return this->planner_reconf.get_status();
+  }
+
+  template <class ModuleCfg>
+  dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::is_stuck_check_enabled(bool &enabled)
+  {
+    if(this->planner_reconf.get_parameter("use_stuck_check",enabled))
+      return DYN_RECONF_SUCCESSFULL;
+    else
+      return DYN_RECONF_NO_SUCH_PARAM;
+  }
+
+  template <class ModuleCfg>
+  dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::use_stuck_check(bool enabled,double speed_threshold,unsigned int max_occurrences)
+  {
+    this->planner_reconf.set_parameter("stuck_check_vel_threshold",speed_threshold);
+    if(this->planner_reconf.get_status()==DYN_RECONF_SUCCESSFULL)
+    {
+      this->planner_reconf.set_parameter("stuck_check_max_occurrences",(int)max_occurrences);
+      if(this->planner_reconf.get_status()==DYN_RECONF_SUCCESSFULL)
+      {
+        this->planner_reconf.set_parameter("use_stuck_check",enabled);
+        return this->planner_reconf.get_status();
+      }
+      else
+        return this->planner_reconf.get_status();
+    }
+    else
+      return this->planner_reconf.get_status();
+  }
+
   template <class ModuleCfg>
   CAckermannLPModule<ModuleCfg>::~CAckermannLPModule()
   {
diff --git a/include/iri_nav_module/nav_costmap_module.h b/include/iri_nav_module/nav_costmap_module.h
index 36bea844ed4c2f1796082682f84e9dc50f05a8f0..982af2269cd3419fa9aa2984aaba13e13eaa08e7 100644
--- a/include/iri_nav_module/nav_costmap_module.h
+++ b/include/iri_nav_module/nav_costmap_module.h
@@ -136,7 +136,7 @@ class CNavCostmapModule
   template <class ModuleCfg>
   dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::get_width(unsigned int &width)
   {
-    if(this->costmap_reconf.get_parameter("width",width))
+    if(this->costmap_reconf.get_parameter("width",(int &)width))
       return DYN_RECONF_SUCCESSFULL;
     else
       return DYN_RECONF_NO_SUCH_PARAM;
@@ -145,14 +145,14 @@ class CNavCostmapModule
   template <class ModuleCfg>
   dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::set_width(unsigned int width)
   {
-    this->costmap_reconf.set_parameter("width",width);
+    this->costmap_reconf.set_parameter("width",(int)width);
     return this->costmap_reconf.get_status();
   }
 
   template <class ModuleCfg>
   dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::get_height(unsigned int &height)
   {
-    if(this->costmap_reconf.get_parameter("height",height))
+    if(this->costmap_reconf.get_parameter("height",(int &)height))
       return DYN_RECONF_SUCCESSFULL;
     else
       return DYN_RECONF_NO_SUCH_PARAM;
@@ -161,7 +161,7 @@ class CNavCostmapModule
   template <class ModuleCfg>
   dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::set_height(unsigned int height)
   {
-    this->costmap_reconf.set_parameter("height",height);
+    this->costmap_reconf.set_parameter("height",(int)height);
     return this->costmap_reconf.get_status();
   }
 
@@ -185,6 +185,7 @@ class CNavCostmapModule
   dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::set_footprint(std::vector<geometry_msgs::Point> &footprint)
   {
     std::stringstream footprint_string;
+    std::string value;
 
     footprint_string << "[";
     for(unsigned int i=0;i<footprint.size();i++)
@@ -194,7 +195,8 @@ class CNavCostmapModule
         std::cout << ",";
     }
     footprint_string << "]";
-    this->costmap_reconf.set_parameter("footprint",footprint_string.str());
+    value=footprint_string.str();
+    this->costmap_reconf.set_parameter("footprint",value);
     return this->costmap_reconf.get_status();
   }
 
diff --git a/include/iri_nav_module/nav_module.h b/include/iri_nav_module/nav_module.h
index 02f373223fd12e5ab8899ba0b94d2067793ae5e4..921edd8b5de2a0f5c1ebfd47de94840a7350fdb9 100644
--- a/include/iri_nav_module/nav_module.h
+++ b/include/iri_nav_module/nav_module.h
@@ -290,7 +290,13 @@ class CNavModule : public CModule<ModuleCfg>
      * 
      * \return The Euclidian distance to the goal. -1.0 when not calculated.
      */
-    double getGoalDistance(void);
+    double get_goal_distance(void);
+    /**
+     * \brief Function to get the euclidian distance to the path.
+     * 
+     * \return The Euclidian distance to the path. -1.0 when not calculated.
+     */
+    double get_path_length(void);
     /* parameter functions */
     /**
      * \brief
@@ -618,7 +624,62 @@ class CNavModule : public CModule<ModuleCfg>
   }
 
   template <class ModuleCfg>
-  double CNavModule<ModuleCfg>::getGoalDistance(void)
+  double CNavModule<ModuleCfg>::get_goal_distance(void)
+  {
+    double min_d=std::numeric_limits<double>::max();
+    geometry_msgs::TransformStamped transform;
+    geometry_msgs::PoseStamped pose;
+    unsigned int min_index=0;
+    bool tf2_exists;
+    
+    this->lock();
+    if (!this->path_available)
+    {
+      ROS_WARN("CNavModule: Goal distance not calculed. No path received.");
+      this->unlock();
+      return std::numeric_limits<double>::quiet_NaN();
+    }
+    double dist = 0.0;
+    if (this->path_msg.poses.size() < 2)
+    {
+      ROS_WARN("CNavModule: Goal distance not calculed. Not enough points.");
+      this->unlock();
+      return std::numeric_limits<double>::quiet_NaN();
+    }
+    tf2_exists=this->tf2_buffer.canTransform(this->goal_frame_id,this->current_pose.header.frame_id,this->current_pose.header.stamp,ros::Duration(this->tf_timeout_time_s));
+    transform = this->tf2_buffer.lookupTransform(this->goal_frame_id,this->current_pose.header.frame_id,this->current_pose.header.stamp);
+    if(tf2_exists)
+      tf2::doTransform(this->current_pose,pose,transform);
+    else
+    {
+      ROS_WARN("CNavModule: Goal distance not calculed. Impossible to transform to goal frame.");
+      this->unlock();
+      return std::numeric_limits<double>::quiet_NaN();
+    }
+    for (unsigned int i=0; i< this->path_msg.poses.size()-1; i++)
+    {
+      double dx = this->path_msg.poses[i].pose.position.x - pose.pose.position.x;
+      double dy = this->path_msg.poses[i].pose.position.y - pose.pose.position.y;
+      double d = std::sqrt(std::pow(dx,2) + std::pow(dy,2));
+      if(d<min_d)
+      {
+        min_d=d;
+        min_index=i;
+      }
+    }
+    for (unsigned int i=min_index; i< this->path_msg.poses.size()-1; i++)
+    {
+      double dx = this->path_msg.poses[i].pose.position.x - this->path_msg.poses[i+1].pose.position.x;
+      double dy = this->path_msg.poses[i].pose.position.y - this->path_msg.poses[i+1].pose.position.y;
+      double d = std::sqrt(std::pow(dx,2) + std::pow(dy,2));
+      dist += d;
+    }
+    this->unlock();
+    return dist;
+  }
+
+  template <class ModuleCfg>
+  double CNavModule<ModuleCfg>::get_path_length(void)
   {
     this->lock();
     if (!this->path_available)
@@ -776,7 +837,7 @@ class CNavModule : public CModule<ModuleCfg>
   template <class ModuleCfg>
   dyn_reconf_status_t CNavModule<ModuleCfg>::get_max_num_retries(unsigned int &retries)
   {
-    if(this->move_base_reconf.get_parameter("max_planning_retries",retries))
+    if(this->move_base_reconf.get_parameter("max_planning_retries",(int&)retries))
       return DYN_RECONF_SUCCESSFULL;
     else
       return DYN_RECONF_NO_SUCH_PARAM;
@@ -785,7 +846,7 @@ class CNavModule : public CModule<ModuleCfg>
   template <class ModuleCfg>
   dyn_reconf_status_t CNavModule<ModuleCfg>::set_max_num_retries(unsigned int &retries)
   {
-    this->move_base_reconf.set_parameter("max_planning_retries",retries);
+    this->move_base_reconf.set_parameter("max_planning_retries",(int)retries);
     return this->move_base_reconf.get_status();
   }
 
diff --git a/include/iri_nav_module/opendrive_gp_module.h b/include/iri_nav_module/opendrive_gp_module.h
index 844ca66b7a2fa0fe4024650c9c4aabd9de5becf8..0e331486cf27919818b97f5a40fae9d41b986cca 100644
--- a/include/iri_nav_module/opendrive_gp_module.h
+++ b/include/iri_nav_module/opendrive_gp_module.h
@@ -5,8 +5,8 @@
 #include <iri_ros_tools/module_service.h>
 #include <iri_nav_module/nav_planner_module.h>
 #include <nav_msgs/OccupancyGrid.h>
-#include <iri_adc_msgs/get_opendrive_map.h>
-#include <iri_adc_msgs/get_opendrive_nodes.h>
+#include <iri_nav_module/get_opendrive_map.h>
+#include <iri_nav_module/get_opendrive_nodes.h>
 
 typedef enum {OD_GP_DIST_COST=0,OD_GP_TIME_COST=1} cost_type_t;
 
@@ -18,8 +18,8 @@ template <class ModuleCfg>
 class COpendriveGPModule : public CNavPlannerModule<ModuleCfg>
 {
   private:
-    CModuleService<iri_adc_msgs::get_opendrive_map,ModuleCfg> get_map;
-    CModuleService<iri_adc_msgs::get_opendrive_nodes,ModuleCfg> get_nodes;
+    CModuleService<iri_nav_module::get_opendrive_map,ModuleCfg> get_map;
+    CModuleService<iri_nav_module::get_opendrive_nodes,ModuleCfg> get_nodes;
   public:
     /**
       * \brief Constructor
@@ -230,7 +230,7 @@ class COpendriveGPModule : public CNavPlannerModule<ModuleCfg>
   template<class ModuleCfg>
   bool COpendriveGPModule<ModuleCfg>::get_opendrive_map(nav_msgs::OccupancyGrid &map)
   {
-    iri_adc_msgs::get_opendrive_map get_map_req;
+    iri_nav_module::get_opendrive_map get_map_req;
 
     switch(this->get_map.call(get_map_req))
     {
@@ -250,7 +250,7 @@ class COpendriveGPModule : public CNavPlannerModule<ModuleCfg>
   template<class ModuleCfg>
   bool COpendriveGPModule<ModuleCfg>::get_opendrive_nodes(std::vector<unsigned int> &nodes)
   {
-    iri_adc_msgs::get_opendrive_nodes get_nodes_req;
+    iri_nav_module::get_opendrive_nodes get_nodes_req;
 
     switch(this->get_nodes.call(get_nodes_req))
     {