diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp
index cfbe79a..93eb296 100644
--- a/waypoint_nav/src/waypoint_nav.cpp
+++ b/waypoint_nav/src/waypoint_nav.cpp
@@ -89,7 +89,7 @@
         cmd_vel_sub_ = nh.subscribe("cmd_vel", 1, &WaypointsNavigation::cmdVelCallback, 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);
+        wp_num_pub_ = nh.advertise<std_msgs::UInt16>("waypoint_num", 5);
         clear_costmaps_srv_ = nh.serviceClient<std_srvs::Empty>("/move_base/clear_costmaps");
     }
 
@@ -304,7 +304,9 @@
             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_));
+            if ((dist < min_dist_err_) && (diff_yaw < min_yaw_err_)) return true;
+            else if ((dist < min_dist_err_ + 0.3) && navigationFinished()) return true;
+            else return false;
         }
         return dist < dist_err;
     }
@@ -414,9 +416,7 @@
             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_)) {