diff --git a/waypoint_nav/launch/waypoint_nav.launch b/waypoint_nav/launch/waypoint_nav.launch
index 4f5d4cb..e7b5ece 100644
--- a/waypoint_nav/launch/waypoint_nav.launch
+++ b/waypoint_nav/launch/waypoint_nav.launch
@@ -4,6 +4,8 @@
     <arg name="cmd_vel_topic_in" default="/move_base/cmd_vel"/>
     <arg name="cmd_vel_topic_out" default="/wp_nav/cmd_vel"/>
     <arg name="robot_frame"       default="base_link"/>
+    <arg name="max_vel_param"     default="/move_base/TrajectoryPlannerROS/max_vel_x"/>
+    <arg name="min_vel_param"     default="/move_base/TrajectoryPlannerROS/min_vel_x"/>
 
 
     <node name="waypoint_nav" pkg="waypoint_nav" type="waypoint_nav" output="screen">
@@ -16,5 +18,7 @@
     <node name="velocity_controller" pkg="waypoint_nav" type="velocity_controller" output="screen">
         <remap from="/cmd_vel_in" to="$(arg cmd_vel_topic_in)"/>
         <remap from="/cmd_vel_out" to="$(arg cmd_vel_topic_out)"/>
+        <param name="max_vel_param" value="$(arg max_vel_param)"/>
+        <param name="min_vel_param" value="$(arg min_vel_param)"/>
     </node>
 </launch>
diff --git a/waypoint_nav/src/velocity_controller.cpp b/waypoint_nav/src/velocity_controller.cpp
index d064beb..53735d7 100644
--- a/waypoint_nav/src/velocity_controller.cpp
+++ b/waypoint_nav/src/velocity_controller.cpp
@@ -19,21 +19,19 @@
             ROS_INFO("Waiting...");
         }
 
+        ros::NodeHandle private_nh("~");
+        std::string max_vel_param = "/move_base/TrajectoryPlannerROS/max_vel_x";
+        std::string min_vel_param = "/move_base/TrajectoryPlannerROS/min_vel_x";
+        private_nh.param("max_vel_param", max_vel_param, max_vel_param);
+        private_nh.param("min_vel_param", min_vel_param, min_vel_param);
+
         ros::NodeHandle nh;
         max_vel_sub_ = nh.subscribe("/max_vel", 1, &VelocityController::maxVelCallback, this);
         cmd_vel_sub_ = nh.subscribe("/cmd_vel_in", 10, &VelocityController::cmdVelCallback, this);
         cmd_vel_pub_ = nh.advertise<geometry_msgs::Twist>("/cmd_vel_out", 10);
 
-        std::string planner = "base_local_planner/TrajectoryPlannerROS";
-        std::string param_max = "/move_base/TrajectoryPlannerROS/max_vel_x";
-        std::string param_min = "/move_base/TrajectoryPlannerROS/min_vel_x";
-        nh.param("/move_base/base_local_planner", planner, planner);
-        if (planner == "dwa_local_planner/DWAPlannerROS") {
-            param_max = "/move_base/DWAPlannerROS/max_vel_trans";
-            param_min = "/move_base/DWAPlannerROS/min_vel_trans";
-        }
-        nh.param(param_max, standard_vel_, standard_vel_);
-        nh.param(param_min, min_vel_, min_vel_);
+        nh.param(max_vel_param, standard_vel_, standard_vel_);
+        nh.param(min_vel_param, min_vel_, min_vel_);
         ROS_INFO_STREAM("Standard max vel: " << standard_vel_);
     }
 
diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp
index 33b2ff4..120c34b 100644
--- a/waypoint_nav/src/waypoint_nav.cpp
+++ b/waypoint_nav/src/waypoint_nav.cpp
@@ -7,15 +7,12 @@
 #include <geometry_msgs/PoseArray.h>
 #include <geometry_msgs/Twist.h>
 #include <move_base_msgs/MoveBaseAction.h>
+#include <actionlib_msgs/GoalStatusArray.h>
 #include <actionlib/client/simple_action_client.h>
 #include <tf/tf.h>
 #include <tf/transform_listener.h>
 #include <visualization_msgs/MarkerArray.h>
 
-#include <dynamic_reconfigure/client.h>
-#include <base_local_planner/BaseLocalPlannerConfig.h>
-#include <dwa_local_planner/DWAPlannerConfig.h>
-
 #include <yaml-cpp/yaml.h>
 #include <waypoint_saver.h>  // include Waypoint class
 
@@ -51,7 +48,8 @@
         move_base_action_("move_base", true),
         rate_(10),
         last_moved_time_(0),
-        dist_err_(0.8)
+        dist_err_(1.0),
+        move_base_status_(0)
     {
         while((move_base_action_.waitForServer(ros::Duration(1.0)) == false) && (ros::ok() == true))
         {
@@ -85,6 +83,7 @@
         stop_server_ = nh.advertiseService("stop_wp_nav", &WaypointsNavigation::stopNavigationCallback, this);
         resume_server_ = nh.advertiseService("resume_nav", &WaypointsNavigation::resumeNavigationCallback, this);
         cmd_vel_sub_ = nh.subscribe("cmd_vel", 1, &WaypointsNavigation::cmdVelCallback, this);
+        move_base_status_sub_ = nh.subscribe("/move_base/status", 10, &WaypointsNavigation::navStatusCallBack, this);
         wp_pub_ = nh.advertise<geometry_msgs::PoseArray>("waypoints", 10);
         max_vel_pub_ = nh.advertise<std_msgs::Float32>("max_vel", 5);
         wp_num_pub_ = nh.advertise<std_msgs::UInt16>("waypoint_number", 5);
@@ -250,7 +249,7 @@
 
 
     /*
-    ++++++++++ Callback function for cmd_vel topic  ++++++++++
+    ++++++++++ Callback function for /cmd_vel topic  ++++++++++
     */
     void cmdVelCallback(const geometry_msgs::Twist &msg)
     {
@@ -271,6 +270,18 @@
 
 
     /*
+    ++++++++++ Callback function for /move_base/status topic  ++++++++++
+    */
+    void navStatusCallBack(const actionlib_msgs::GoalStatusArray &status)
+    {
+        if (!status.status_list.empty()) {
+            actionlib_msgs::GoalStatus goalStatus = status.status_list[0];
+            move_base_status_ = goalStatus.status;
+        }
+    }
+
+
+    /*
     ++++++++++ Check if robot reached the goal sent to move_base ++++++++++
     */
     bool navigationFinished()
@@ -350,6 +361,7 @@
         move_base_goal.target_pose.pose.position = dest.position;
         move_base_goal.target_pose.pose.orientation = dest.orientation;
         move_base_action_.sendGoal(move_base_goal);
+        // move_base_action_.sendGoalAndWait(move_base_goal);
     }
 
 
@@ -371,6 +383,7 @@
     */
     void run()
     {
+        ROS_INFO("Waiting for waypoint navigation to start.");
         while(ros::ok()) {
             // has_activate_ is false, nothing to do
             if (!has_activate_) {
@@ -399,6 +412,10 @@
                     if (!has_activate_) {
                         throw SwitchRunningStatus();
                     }
+                    if (move_base_status_ == 3) {
+                        ROS_WARN("Reached default xy_gaol_tolerance.");
+                        break;
+                    }
                     double time = ros::Time::now().toSec();
                     if (time - start_nav_time > 10.0 && time - last_moved_time_ > 10.0) {
                         ROS_WARN("Resend the navigation goal.");
@@ -408,9 +425,7 @@
                         resend_goal++;
                         if (resend_goal == 3) {
                             ROS_WARN("Skip waypoint.");
-                            current_waypoint_++;
-                            wp_num_.data++;
-                            startNavigationGL(*current_waypoint_);
+                            break;
                         }
                         start_nav_time = time;
                     }
@@ -449,10 +464,11 @@
     tf::TransformListener tf_listener_;
     ros::Rate rate_;
     ros::ServiceServer start_server_, stop_server_, resume_server_;
-    ros::Subscriber cmd_vel_sub_;
+    ros::Subscriber cmd_vel_sub_, move_base_status_sub_;
     ros::Publisher wp_pub_, max_vel_pub_, wp_num_pub_;
     ros::ServiceClient clear_costmaps_srv_;
     double last_moved_time_, dist_err_;
+    int move_base_status_;
     std_msgs::UInt16 wp_num_;
     std_msgs::Float32 max_vel_msg_;
 };