diff --git a/waypoint_nav/scripts/tandem_run_manager.py b/waypoint_nav/scripts/tandem_run_manager.py
index d233808..0dc16cd 100755
--- a/waypoint_nav/scripts/tandem_run_manager.py
+++ b/waypoint_nav/scripts/tandem_run_manager.py
@@ -1,6 +1,7 @@
 #!/usr/bin/env python3
 
 import rospy
+import dynamic_reconfigure.client
 import ruamel.yaml
 import numpy as np
 from std_msgs.msg import UInt16
@@ -11,76 +12,92 @@
 class TandemManager():
 
     def __init__(self):
+        ## Read waypoints file
         yaml = ruamel.yaml.YAML()
         waypoints_path = rospy.get_param("tandem_run_manager/waypoints_file")
+        with open(waypoints_path) as file:
+            waypoints_yaml = yaml.load(file)
+        ## Register waypoint number of start/end tandem area 
         self.tandem_start_list = []
         self.tandem_end_list = []
         self.tandem_id = 0
-        with open(waypoints_path) as file:
-            waypoints_yaml = yaml.load(file)
         for i, data in enumerate(waypoints_yaml["waypoints"]):
             if ("tandem_start" in data["point"]):
                 self.tandem_start_list.append(i+2)
             if ("tandem_end" in data["point"]):
                 self.tandem_end_list.append(i+2)
-        self.in_tandem_area = False
-        self.stop = False
-        ##
-        self.waypoint_num = 0
+        ## Subscribers
         self.wp_num_sub = rospy.Subscriber("/waypoint_num", UInt16, self.waypoint_num_callback)
         self.scan_sub = rospy.Subscriber("/scan", LaserScan, self.laserscan_callback)
-        ##
+        ## Waypoint navigation service clients
         self.stop_nav = rospy.ServiceProxy("/stop_wp_nav", Trigger)
         self.resume_nav = rospy.ServiceProxy("/resume_nav", Trigger)
-        ##
+        ## Dynamic reconfigure clients
+        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.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)
+        # self.scan_pub = rospy.Publisher("/debug_scan", LaserScan, queue_size=10)
         return
 
 
+    ## Subscribe current waypoint number
     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]):
             self.in_tandem_area = True
+            self.costmap_client1.update_configuration({"enabled": False})
+            self.costmap_client2.update_configuration({"enabled": False})
             rospy.loginfo("Enter tandem area.")
+        
         elif (msg.data == self.tandem_end_list[self.tandem_id]):
             self.in_tandem_area = False
+            self.costmap_client1.update_configuration({"enabled": True})
+            self.costmap_client2.update_configuration({"enabled": True})
             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
     
 
+
     def laserscan_callback(self, msg):
-        if self.front_range is None:
-            front = round(-msg.angle_min / msg.angle_increment)
-            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]])
-        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
-            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)
+        try:
+            if self.front_range is None:
+                front = round(-msg.angle_min / msg.angle_increment)
+                ran = int(round(np.deg2rad(5) / msg.angle_increment))
+                self.front_range = [front-ran, front+ran]
+                return
+            
+            if not self.in_tandem_area: return
+            
+            min_range = np.min(msg.ranges[self.front_range[0]:self.front_range[1]])
+            if (not self.stop) and (min_range < self.danger_dist):
+                self.stop_nav()
+                self.stop = True
+                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)
+        except AttributeError:
+            pass
         return