diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp
index 120c34b..cb1f6e8 100644
--- a/waypoint_nav/src/waypoint_nav.cpp
+++ b/waypoint_nav/src/waypoint_nav.cpp
@@ -45,11 +45,11 @@
     */
     WaypointsNavigation() :
         has_activate_(false),
+        stopped_(false),
         move_base_action_("move_base", true),
         rate_(10),
         last_moved_time_(0),
-        dist_err_(1.0),
-        move_base_status_(0)
+        dist_err_(1.0)
     {
         while((move_base_action_.waitForServer(ros::Duration(1.0)) == false) && (ros::ok() == true))
         {
@@ -83,7 +83,6 @@
         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);
@@ -270,18 +269,6 @@
 
 
     /*
-    ++++++++++ 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()
@@ -294,7 +281,7 @@
     /*
     ++++++++++ Check if robot reached current waypoint ++++++++++
     */
-    bool onNavigationPoint(const geometry_msgs::Point &dest, double dist_err = 0.8)
+    bool onNavigationPoint(const geometry_msgs::Point &dest, double dist_err=1.0)
     {
         if (waypoint_list_[wp_num_.data-1].stop) {
             return navigationFinished();
@@ -361,7 +348,6 @@
         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);
     }
 
 
@@ -409,11 +395,9 @@
             try {
                 // loop until reach waypoint
                 while(!onNavigationPoint(current_waypoint_->position, dist_err_)) {
-                    if (!has_activate_) {
-                        throw SwitchRunningStatus();
-                    }
-                    if (move_base_status_ == 3) {
-                        ROS_WARN("Reached default xy_gaol_tolerance.");
+                    if (!has_activate_) throw SwitchRunningStatus();
+                    if ((!waypoint_list_[wp_num_.data-1].stop) && (navigationFinished())) {
+                        ROS_WARN("Reached default yaw_goal_torelance.");
                         break;
                     }
                     double time = ros::Time::now().toSec();
@@ -459,7 +443,7 @@
     std::vector<Waypoint> waypoint_list_;
     std::vector<geometry_msgs::Pose>::iterator current_waypoint_;
     std::vector<geometry_msgs::Pose>::iterator finish_pose_;
-    bool has_activate_;
+    bool has_activate_, stopped_;
     std::string robot_frame_, world_frame_;
     tf::TransformListener tf_listener_;
     ros::Rate rate_;
@@ -468,7 +452,6 @@
     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_;
 };