diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp
index cb1f6e8..36fbf7e 100644
--- a/waypoint_nav/src/waypoint_nav.cpp
+++ b/waypoint_nav/src/waypoint_nav.cpp
@@ -48,8 +48,10 @@
         stopped_(false),
         move_base_action_("move_base", true),
         rate_(10),
+        after_start_(0.5),
         last_moved_time_(0),
-        dist_err_(1.0)
+        dist_err_(1.0),
+        min_dist_err_(0.3)
     {
         while((move_base_action_.waitForServer(ros::Duration(1.0)) == false) && (ros::ok() == true))
         {
@@ -62,6 +64,7 @@
         double max_update_rate;
         private_nh.param("max_update_rate", max_update_rate, 10.0);
         rate_ = ros::Rate(max_update_rate);
+        after_start_ = ros::Rate(0.5);
         std::string filename = "";
         private_nh.param("filename", filename, filename);
         if (filename != "") {
@@ -77,6 +80,7 @@
         } else {
             ROS_ERROR("waypoints file doesn't have name");
         }
+        min_dist_err_ = 0.3;
 
         ros::NodeHandle nh;
         start_server_ = nh.advertiseService("start_wp_nav", &WaypointsNavigation::startNavigationCallback, this);
@@ -273,7 +277,8 @@
     */
     bool navigationFinished()
     {
-        return move_base_action_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED;
+        return onNavigationPoint(current_waypoint_->position, min_dist_err_);
+        //return move_base_action_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED;
     }
 
 
@@ -283,15 +288,18 @@
     */
     bool onNavigationPoint(const geometry_msgs::Point &dest, double dist_err=1.0)
     {
-        if (waypoint_list_[wp_num_.data-1].stop) {
-            return navigationFinished();
-        }
+        //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) {
+            return dist < min_dist_err_;
+        }
         return dist < dist_err;
     }
 
@@ -348,6 +356,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);
+        after_start_.sleep();
     }
 
 
@@ -380,7 +389,9 @@
             if (current_waypoint_ == finish_pose_) {
                 ROS_INFO_STREAM("Go to final goal");
                 startNavigationGL(*current_waypoint_);
-                while(!navigationFinished() && ros::ok()) sleep();
+                while(!navigationFinished() && ros::ok()) {
+                  sleep();
+                }
                 ROS_INFO("Final goal reached!!");
                 has_activate_ = false;
                 continue;
@@ -391,13 +402,15 @@
             startNavigationGL(*current_waypoint_);
             int resend_goal = 0;
             double start_nav_time = ros::Time::now().toSec();
+            ROS_WARN("error");
             dist_err_ = waypoint_list_[wp_num_.data-1].rad;
+            ROS_WARN("error");
             try {
                 // 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.");
+                        ROS_WARN("Reached default xy_goal_torelance.");
                         break;
                     }
                     double time = ros::Time::now().toSec();
@@ -407,10 +420,10 @@
                         clear_costmaps_srv_.call(empty);
                         startNavigationGL(*current_waypoint_);
                         resend_goal++;
-                        if (resend_goal == 3) {
-                            ROS_WARN("Skip waypoint.");
-                            break;
-                        }
+                        //if (resend_goal == 3) {
+                        //    ROS_WARN("Skip waypoint.");
+                        //    break;
+                        //}
                         start_nav_time = time;
                     }
                     wp_num_pub_.publish(wp_num_);
@@ -446,12 +459,12 @@
     bool has_activate_, stopped_;
     std::string robot_frame_, world_frame_;
     tf::TransformListener tf_listener_;
-    ros::Rate rate_;
+    ros::Rate rate_, after_start_;
     ros::ServiceServer start_server_, stop_server_, resume_server_;
     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_;
     std_msgs::UInt16 wp_num_;
     std_msgs::Float32 max_vel_msg_;
 };