diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp
index 2ea78d7..e69c3f6 100755
--- a/waypoint_nav/src/waypoint_nav.cpp
+++ b/waypoint_nav/src/waypoint_nav.cpp
@@ -501,11 +501,11 @@
                         ROS_WARN("Resend the navigation goal.");
                         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_);