diff --git a/waypoint_nav/scripts/tandem_run_manager.py b/waypoint_nav/scripts/tandem_run_manager.py
index 7182457..c402e00 100755
--- a/waypoint_nav/scripts/tandem_run_manager.py
+++ b/waypoint_nav/scripts/tandem_run_manager.py
@@ -36,13 +36,12 @@
         self.costmap_client1 = dynamic_reconfigure.client.Client("/move_base/global_costmap/obstacle_layer1")
         self.costmap_client2 = dynamic_reconfigure.client.Client("/move_base/global_costmap/obstacle_layer2")
         ## Variable
+        self.front_angle = 40 # degree
+        self.danger_dist = 1.0 # meter
         self.waypoint_num = 0
         self.front_range = None
-        self.danger_dist = 1.0
         self.in_tandem_area = False
         self.stop = False
-        ## for debug
-        # self.scan_pub = rospy.Publisher("/debug_scan", LaserScan, queue_size=10)
         return
 
 
@@ -72,14 +71,18 @@
         try:
             if self.front_range is None:
                 front = round(-msg.angle_min / msg.angle_increment)
-                ran = int(round(np.deg2rad(10) / msg.angle_increment))
+                ran = int(round(np.deg2rad(self.front_angle/2) / msg.angle_increment))
                 self.front_range = [front-ran, front+ran]
                 return
             
             if not self.in_tandem_area: return
-            
-            sort_ranges = np.sort(msg.ranges[self.front_range[0]:self.front_range[1]])
-            min_range = np.mean(sort_ranges[:5])
+            ranges = np.array(msg.ranges[self.front_range[0]:self.front_range[1]])
+            ## Use simply minimum
+            ranges[ranges <= msg.range_min] = msg.range_max
+            min_range = min(ranges)
+            ## or use sort
+            #sort_ranges = np.sort(ranges)
+            #min_range = np.mean(sort_ranges[:5])
             if (not self.stop) and (min_range < self.danger_dist):
                 self.stop_nav()
                 self.stop = True
@@ -89,14 +92,7 @@
                 self.resume_nav()
                 self.stop = False
                 rospy.loginfo("Resumed navigation because the obstacle ahead was more than {}m away.".format(self.danger_dist+0.1))
-            
-            ## for debug
-            # debug_scan = msg
-            # debug_scan.ranges = np.array(msg.ranges)
-            # debug_scan.ranges[:self.front_range[0]] = 0
-            # debug_scan.ranges[self.front_range[1]:] = 0
-            # debug_scan.ranges = tuple(debug_scan.ranges)
-            # self.scan_pub.publish(debug_scan)
+        
         except AttributeError:
             pass
         return
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_);