diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp
index 38ee0a8..2e23fec 100755
--- a/waypoint_nav/src/waypoint_nav.cpp
+++ b/waypoint_nav/src/waypoint_nav.cpp
@@ -218,47 +218,47 @@
         compare_waypoint_ = waypoints_.poses.begin();
         double min_dist_w = std::numeric_limits<int>::max();
         if(StartFromTheMiddle){
-        	ROS_INFO("StartFromTheMiddle => True");
-        	tf::StampedTransform robot_init_pos = getRobotPosGL();
-        	double init_x = robot_init_pos.getOrigin().x();
-        	double init_y = robot_init_pos.getOrigin().y();
-        	double pre_waypoint_x;
-        	double pre_waypoint_y;
-        	double convert_x;
-        	double init_yaw = tf::getYaw(robot_init_pos.getRotation());
-        	double square_x;
-        	double square_y;
-        	double dist_w;
-        	double dir_w;
-        	int min_waypoint = 0;
-        	int i = 0;
-        	while(compare_waypoint_ != waypoints_.poses.end() - 1){
-        		i++;
-        		square_x = (compare_waypoint_ -> position.x - init_x) * (compare_waypoint_ -> position.x - init_x);
-        		square_y = (compare_waypoint_ -> position.y - init_y) * (compare_waypoint_ -> position.y - init_y);
-        		dist_w = sqrt(square_x + square_y);
-        		if(i == 1){
-        			dir_w = atan2(compare_waypoint_ -> position.y, compare_waypoint_ -> position.x);
-        			convert_x = (compare_waypoint_ -> position.x - init_x) * cos(-init_yaw) - (compare_waypoint_ -> position.y - init_y) * sin(-init_yaw);
-        			if(abs(dir_w - init_yaw) < (90 / 180.0 * M_PI) && convert_x > 0){
-        				min_dist_w = dist_w;
-        				init_waypoint_ = compare_waypoint_;
-        				min_waypoint = 1;
-        			}
-        		}else if(min_dist_w > dist_w){
-        			dir_w = atan2(compare_waypoint_ -> position.y - pre_waypoint_y, compare_waypoint_ -> position.x- pre_waypoint_x);
-        			convert_x = (compare_waypoint_ -> position.x - init_x) * cos(-init_yaw) - (compare_waypoint_ -> position.y - init_y) * sin(-init_yaw);
-        			if(abs(dir_w - init_yaw) < (90 / 180.0 * M_PI) && convert_x > 0){
-        				min_dist_w = dist_w;
-        				init_waypoint_ = compare_waypoint_;
-        				min_waypoint = i;
-        			}
-        		}
-        		pre_waypoint_x = compare_waypoint_ -> position.x;
-        		pre_waypoint_y = compare_waypoint_ -> position.y;
-        		compare_waypoint_++;
-        	}
-        	wp_num_.data = min_waypoint;
+            ROS_INFO("StartFromTheMiddle => True");
+            tf::StampedTransform robot_init_pos = getRobotPosGL();
+            double init_x = robot_init_pos.getOrigin().x();
+            double init_y = robot_init_pos.getOrigin().y();
+            double pre_waypoint_x;
+            double pre_waypoint_y;
+            double convert_x;
+            double init_yaw = tf::getYaw(robot_init_pos.getRotation());
+            double square_x;
+            double square_y;
+            double dist_w;
+            double dir_w;
+            int min_waypoint = 0;
+            int i = 0;
+            while(compare_waypoint_ != waypoints_.poses.end() - 1){
+                i++;
+                square_x = (compare_waypoint_ -> position.x - init_x) * (compare_waypoint_ -> position.x - init_x);
+                square_y = (compare_waypoint_ -> position.y - init_y) * (compare_waypoint_ -> position.y - init_y);
+                dist_w = sqrt(square_x + square_y);
+                if(i == 1){
+                    dir_w = atan2(compare_waypoint_ -> position.y, compare_waypoint_ -> position.x);
+                    convert_x = (compare_waypoint_ -> position.x - init_x) * cos(-init_yaw) - (compare_waypoint_ -> position.y - init_y) * sin(-init_yaw);
+                    if(abs(dir_w - init_yaw) < (90 / 180.0 * M_PI) && convert_x > 0){
+                        min_dist_w = dist_w;
+                        init_waypoint_ = compare_waypoint_;
+                        min_waypoint = 1;
+                    }
+                }else if(min_dist_w > dist_w){
+                    dir_w = atan2(compare_waypoint_ -> position.y - pre_waypoint_y, compare_waypoint_ -> position.x- pre_waypoint_x);
+                    convert_x = (compare_waypoint_ -> position.x - init_x) * cos(-init_yaw) - (compare_waypoint_ -> position.y - init_y) * sin(-init_yaw);
+                    if(abs(dir_w - init_yaw) < (90 / 180.0 * M_PI) && convert_x > 0){
+                        min_dist_w = dist_w;
+                        init_waypoint_ = compare_waypoint_;
+                        min_waypoint = i;
+                    }
+                }
+                pre_waypoint_x = compare_waypoint_ -> position.x;
+                pre_waypoint_y = compare_waypoint_ -> position.y;
+                compare_waypoint_++;
+            }
+            wp_num_.data = min_waypoint;
         }
         std_srvs::Empty empty;
         while(!clear_costmaps_srv_.call(empty)) {
@@ -266,14 +266,14 @@
             sleep();
         }
         if(min_dist_w == std::numeric_limits<int>::max()){
-        	ROS_WARN("Could not find the closest appropriate waypoint. Please check the direction of the robot again.");
-        	response.success = false;
-        	return false;
+            ROS_WARN("Could not find the closest appropriate waypoint. Please check the direction of the robot again.");
+            response.success = false;
+            return false;
         }else{
-        	current_waypoint_ = init_waypoint_;
-        	has_activate_ = true;
-        	response.success = true;
-        	return true;
+            current_waypoint_ = init_waypoint_;
+            has_activate_ = true;
+            response.success = true;
+            return true;
         }
         //#############################################################
     }
@@ -457,6 +457,8 @@
     {
         ROS_INFO("Waiting for waypoint navigation to start.");
         int resend_goal;
+        double start_nav_time, time;
+        std_srvs::Empty empty;
         while(ros::ok()) {
             // has_activate_ is false, nothing to do
             if (!has_activate_) {
@@ -467,8 +469,16 @@
             if (current_waypoint_ == finish_pose_) {
                 ROS_INFO_STREAM("Go to final goal");
                 startNavigationGL(*current_waypoint_);
+                start_nav_time = ros::Time::now().toSec();
                 while(!onNavigationPoint(finish_pose_->position) && ros::ok()) {
-                  sleep();
+                    time = ros::Time::now().toSec();
+                    if (time - start_nav_time > 10.0 && time - last_moved_time_ > 10.0) {
+                        ROS_WARN("Resend the navigation goal.");
+                        clear_costmaps_srv_.call(empty);
+                        startNavigationGL(*current_waypoint_);
+                        start_nav_time = time;
+                    }
+                    sleep();
                 }
                 ROS_INFO("Final goal reached!!");
                 has_activate_ = false;
@@ -479,16 +489,15 @@
             ROS_INFO_STREAM("x: " << waypoint_list_[wp_num_.data-1].x << ", y: " << waypoint_list_[wp_num_.data-1].y);
             startNavigationGL(*current_waypoint_);
             resend_goal = 0;
-            double start_nav_time = ros::Time::now().toSec();
+            start_nav_time = ros::Time::now().toSec();
             dist_err_ = waypoint_list_[wp_num_.data-1].rad;
             try {
                 // loop until reach waypoint
                 while(!onNavigationPoint(current_waypoint_->position, dist_err_)) {
                     if (!has_activate_) throw SwitchRunningStatus();
-                    double time = ros::Time::now().toSec();
+                    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++;