diff --git a/waypoint_nav/launch/waypoint_nav.launch b/waypoint_nav/launch/waypoint_nav.launch
index c612308..a19d48e 100755
--- a/waypoint_nav/launch/waypoint_nav.launch
+++ b/waypoint_nav/launch/waypoint_nav.launch
@@ -10,6 +10,7 @@
     <arg name="min_vel_param"      default="/move_base/TrajectoryPlannerROS/min_vel_x"/>
     <arg name="min_dist_err"       default="0.3"/>
     <arg name="min_yaw_err"        default="0.3"/>
+    <arg name="tandem_scan"        default="scan"/>
 
 
     <node name="waypoint_nav" pkg="waypoint_nav" type="waypoint_nav" output="screen">
@@ -31,6 +32,7 @@
 
 
     <node name="tandem_run_manager" pkg="waypoint_nav" type="tandem_run_manager.py" output="screen">
+        <remap from="scan" to="$(arg tandem_scan)"/>
         <param name="waypoints_file" value="$(arg waypoints_file)"/>
     </node>
 </launch>
diff --git a/waypoint_nav/scripts/tandem_run_manager.py b/waypoint_nav/scripts/tandem_run_manager.py
old mode 100644
new mode 100755
index fbe87c1..d233808
--- a/waypoint_nav/scripts/tandem_run_manager.py
+++ b/waypoint_nav/scripts/tandem_run_manager.py
@@ -34,17 +34,22 @@
         self.resume_nav = rospy.ServiceProxy("/resume_nav", Trigger)
         ##
         self.front_range = None
+        self.danger_dist = 1.0
+        ## for debug
+        self.scan_pub = rospy.Publisher("/debug_scan", LaserScan, queue_size=10)
         return
 
 
     def waypoint_num_callback(self, msg):
         if (msg.data == self.waypoint_num) or (self.stop): return
         if (msg.data == self.tandem_start_list[self.tandem_id]):
-            rospy.loginfo("")
             self.in_tandem_area = True
+            rospy.loginfo("Enter tandem area.")
         elif (msg.data == self.tandem_end_list[self.tandem_id]):
             self.in_tandem_area = False
             self.tandem_id += 1
+            self.stop = (self.tandem_id >= len(self.tandem_start_list))  # if True, self.stop will never be False
+            rospy.loginfo("Exit from tandem area.")
         self.waypoint_num = msg.data
         return
     
@@ -52,17 +57,30 @@
     def laserscan_callback(self, msg):
         if self.front_range is None:
             front = round(-msg.angle_min / msg.angle_increment)
-            range = int(round(np.deg2rad(15) / msg.angle_increment))
-            self.front_range = [front-range, front+range]
+            ran = int(round(np.deg2rad(5) / msg.angle_increment))
+            self.front_range = [front-ran, front+ran]
+            return
         ##
         if not self.in_tandem_area: return
-        ave_range = np.mean(msg.ranges[self.front_range[0]:self.front_range[1]])
-        if (not self.stop) and (ave_range < 0.6):
+        #ave_range = np.mean(msg.ranges[self.front_range[0]:self.front_range[1]])
+        min_range = np.min(msg.ranges[self.front_range[0]:self.front_range[1]])
+        rospy.loginfo(min_range)
+        if (not self.stop) and (min_range < self.danger_dist):
             self.stop_nav()
             self.stop = True
-        elif (self.stop) and (ave_range >= 0.7):
+            rospy.loginfo("Stop because of obstacle within {}m ahead.".format(self.danger_dist))
+        elif (self.stop) and (min_range >= self.danger_dist+0.1):
             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)
         return
 
 
@@ -72,4 +90,4 @@
 if __name__ == '__main__':
     rospy.init_node("map_changer")
     tandem_manager = TandemManager()
-    rospy.spin()
\ No newline at end of file
+    rospy.spin()