diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp
index cb1f6e8..32344bd 100644
--- a/waypoint_nav/src/waypoint_nav.cpp
+++ b/waypoint_nav/src/waypoint_nav.cpp
@@ -49,7 +49,9 @@
         move_base_action_("move_base", true),
         rate_(10),
         last_moved_time_(0),
-        dist_err_(1.0)
+        dist_err_(1.0),
+        min_dist_err_(0.3),
+        min_yaw_err_(0.3)
     {
         while((move_base_action_.waitForServer(ros::Duration(1.0)) == false) && (ros::ok() == true))
         {
@@ -77,6 +79,8 @@
         } else {
             ROS_ERROR("waypoints file doesn't have name");
         }
+        min_dist_err_ = 0.3;
+        min_yaw_err_ = 0.5;
 
         ros::NodeHandle nh;
         start_server_ = nh.advertiseService("start_wp_nav", &WaypointsNavigation::startNavigationCallback, this);
@@ -155,6 +159,7 @@
                 point.x = pose.position.x;
                 point.y = pose.position.y;
                 point.z = pose.position.z;
+                point.stop = true;
                 waypoint_list_.push_back(point);
             } else {
                 return false;
@@ -273,7 +278,11 @@
     */
     bool navigationFinished()
     {
-        return move_base_action_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED;
+        if (move_base_action_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
+            ROS_WARN("Move base state is Goal reached");
+            return true;
+        }
+        return false;
     }
 
 
@@ -283,15 +292,20 @@
     */
     bool onNavigationPoint(const geometry_msgs::Point &dest, double dist_err=1.0)
     {
-        if (waypoint_list_[wp_num_.data-1].stop) {
-            return navigationFinished();
-        }
         tf::StampedTransform robot_gl = getRobotPosGL();
         const double wx = dest.x;
         const double wy = dest.y;
         const double rx = robot_gl.getOrigin().x();
         const double ry = robot_gl.getOrigin().y();
         const double dist = std::sqrt(std::pow(wx - rx, 2) + std::pow(wy - ry, 2));
+        if (waypoint_list_[wp_num_.data-1].stop) {
+            tf::Quaternion q = robot_gl.getRotation();
+            tf::Matrix3x3 m(q);
+            double r, p ,y;
+            m.getRPY(r, p, y);
+            double diff_yaw = std::abs(y - target_yaw_);
+            return ((dist < min_dist_err_) && (diff_yaw < min_yaw_err_)) || navigationFinished() ;
+        }
         return dist < dist_err;
     }
 
@@ -348,6 +362,13 @@
         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);
+        if (waypoint_list_[wp_num_.data-1].stop) {
+            tf::Quaternion q(dest.orientation.x, dest.orientation.y, dest.orientation.z, dest.orientation.w);
+            tf::Matrix3x3 m(q);
+            double r, p ,y;
+            m.getRPY(r, p, y);
+            target_yaw_ = y;
+        }
     }
 
 
@@ -380,7 +401,9 @@
             if (current_waypoint_ == finish_pose_) {
                 ROS_INFO_STREAM("Go to final goal");
                 startNavigationGL(*current_waypoint_);
-                while(!navigationFinished() && ros::ok()) sleep();
+                while(!onNavigationPoint(finish_pose_->position) && ros::ok()) {
+                  sleep();
+                }
                 ROS_INFO("Final goal reached!!");
                 has_activate_ = false;
                 continue;
@@ -396,21 +419,17 @@
                 // loop until reach waypoint
                 while(!onNavigationPoint(current_waypoint_->position, dist_err_)) {
                     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();
                     if (time - start_nav_time > 10.0 && time - last_moved_time_ > 10.0) {
                         ROS_WARN("Resend the navigation goal.");
                         std_srvs::Empty empty;
                         clear_costmaps_srv_.call(empty);
                         startNavigationGL(*current_waypoint_);
-                        resend_goal++;
-                        if (resend_goal == 3) {
-                            ROS_WARN("Skip waypoint.");
-                            break;
-                        }
+                        // resend_goal++;
+                        //if (resend_goal == 3) {
+                        //    ROS_WARN("Skip waypoint.");
+                        //    break;
+                        //}
                         start_nav_time = time;
                     }
                     wp_num_pub_.publish(wp_num_);
@@ -451,7 +470,7 @@
     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_;
+    double last_moved_time_, dist_err_, min_dist_err_, target_yaw_, min_yaw_err_;
     std_msgs::UInt16 wp_num_;
     std_msgs::Float32 max_vel_msg_;
 };