diff --git a/waypoint_nav/CMakeLists.txt b/waypoint_nav/CMakeLists.txt
index b4c1003..7a3d636 100644
--- a/waypoint_nav/CMakeLists.txt
+++ b/waypoint_nav/CMakeLists.txt
@@ -59,3 +59,9 @@
   ${catkin_LIBRARIES}
 )
 
+
+catkin_install_python(PROGRAMS
+  scripts/tandem_run_manager.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
diff --git a/waypoint_nav/launch/waypoint_nav.launch b/waypoint_nav/launch/waypoint_nav.launch
index 4cd7678..c612308 100755
--- a/waypoint_nav/launch/waypoint_nav.launch
+++ b/waypoint_nav/launch/waypoint_nav.launch
@@ -28,4 +28,9 @@
         <param name="min_dist_err"  value="$(arg min_dist_err)"/>
         <param name="min_yaw_err"   value="$(arg min_yaw_err)"/>
     </node>
+
+
+    <node name="tandem_run_manager" pkg="waypoint_nav" type="tandem_run_manager.py" output="screen">
+        <param name="waypoints_file" value="$(arg waypoints_file)"/>
+    </node>
 </launch>
diff --git a/waypoint_nav/maps/map.pgm b/waypoint_nav/maps/map.pgm
deleted file mode 100644
index 10700a1..0000000
--- a/waypoint_nav/maps/map.pgm
+++ /dev/null
Binary files differ
diff --git a/waypoint_nav/maps/map.yaml b/waypoint_nav/maps/map.yaml
deleted file mode 100644
index 8cf0bd4..0000000
--- a/waypoint_nav/maps/map.yaml
+++ /dev/null
@@ -1,7 +0,0 @@
-image: ./map.pgm
-resolution: 0.050000
-origin: [-10.000000, -10.000000, 0.000000]
-negate: 0
-occupied_thresh: 0.65
-free_thresh: 0.196
-
diff --git a/waypoint_nav/maps/map_gakunai.pgm b/waypoint_nav/maps/map_gakunai.pgm
deleted file mode 100644
index 8a70604..0000000
--- a/waypoint_nav/maps/map_gakunai.pgm
+++ /dev/null
Binary files differ
diff --git a/waypoint_nav/maps/map_gakunai.yaml b/waypoint_nav/maps/map_gakunai.yaml
deleted file mode 100644
index 27872d5..0000000
--- a/waypoint_nav/maps/map_gakunai.yaml
+++ /dev/null
@@ -1,7 +0,0 @@
-image: map_gakunai.pgm
-resolution: 0.050000
-origin: [-100.000000, -100.000000, 0.000000]
-negate: 0
-occupied_thresh: 0.65
-free_thresh: 0.196
-
diff --git a/waypoint_nav/param/test.yaml b/waypoint_nav/param/test.yaml
deleted file mode 100644
index 4c9e554..0000000
--- a/waypoint_nav/param/test.yaml
+++ /dev/null
@@ -1,15 +0,0 @@
-waypoints:
-- point: {x: -1.094477, y: -0.52293, z: 0.0, vel: 1.0, rad: 0.3, stop: false}
-- point: {x: -0.468387, y: -0.529642, z: 0.0, vel: 1.0, rad: 0.3, stop: false}
-- point: {x: -0.497908, y: -1.858082, z: 0.0, vel: 1.0, rad: 0.5, stop: false}
-- point: {x: 0.505803, y: -1.841213, z: 0.0, vel: 1.0, rad: 0.8, stop: false}
-- point: {x: 0.592677, y: -0.561608, z: 0.0, vel: 0.8, rad: 0.2, stop: false}
-- point: {x: 1.753362, y: -0.536894, z: 0.0, vel: 1.0, rad: 0.3, stop: true}
-- point: {x: 1.749741, y: 0.526376, z: 0.0, vel: 1.0, rad: 0.3, stop: false}
-- point: {x: 0.631463, y: 0.530001, z: 0.0, vel: 0.5, rad: 0.3, stop: true}
-- point: {x: 0.629175, y: 1.860692, z: 0.0, vel: 0.5, rad: 0.2, stop: false}
-finish_pose:
-  header: {seq: 0.0, stamp: 1620367430.3803937, frame_id: base_link}
-  pose:
-    position: {x: -0.550981, y: 1.85, z: 0.0}
-    orientation: {x: 0.0, y: 0.0, z: 0.99749, w: 0.07074}
\ No newline at end of file
diff --git a/waypoint_nav/param/test2.yaml b/waypoint_nav/param/test2.yaml
deleted file mode 100644
index 3463e52..0000000
--- a/waypoint_nav/param/test2.yaml
+++ /dev/null
@@ -1,11 +0,0 @@
-waypoints:
-- point: {x: -0.996902, y: -0.499032, z: 0.0, vel: 1.0, rad: 0.3}
-- point: {x: -0.188321, y: -0.510442, z: 0.0, vel: 1.0, rad: 0.3}
-- point: {x: 0.568284, y: -0.501916, z: 0.0, vel: 0.8, rad: 0.2}
-- point: {x: 0.631463, y: 0.530001, z: 0.0, vel: 0.5, rad: 0.3}
-- point: {x: 0.629175, y: 1.860692, z: 0.0, vel: 0.5, rad: 0.2}
-finish_pose:
-  header: {seq: 0.0, stamp: 1620367430.3803937, frame_id: base_link}
-  pose:
-    position: {x: -0.550981, y: 1.85, z: 0.0}
-    orientation: {x: 0.0, y: 0.0, z: 0.99749, w: 0.07074}
\ No newline at end of file
diff --git a/waypoint_nav/param/test3.yaml b/waypoint_nav/param/test3.yaml
deleted file mode 100644
index 4d226ae..0000000
--- a/waypoint_nav/param/test3.yaml
+++ /dev/null
@@ -1,11 +0,0 @@
-waypoints:
-- point: {x: -1.094477, y: -0.52293, z: 0.0, vel: 1.0, rad: 0.3, stop: false}
-- point: {x: -0.23067, y: -0.546812, z: 0.0, vel: 1.0, rad: 0.5, stop: false}
-- point: {x: 0.578978, y: -0.568007, z: 0.0, vel: 0.8, rad: 0.2, stop: false}
-- point: {x: 0.631463, y: 0.530001, z: 0.0, vel: 0.5, rad: 0.3, stop: true}
-- point: {x: 0.629175, y: 1.860692, z: 0.0, vel: 0.5, rad: 0.2, stop: false}
-finish_pose:
-  header: {seq: 0.0, stamp: 1620367430.3803937, frame_id: base_link}
-  pose:
-    position: {x: -0.550981, y: 1.85, z: 0.0}
-    orientation: {x: 0.0, y: 0.0, z: 0.99749, w: 0.07074}
\ No newline at end of file
diff --git a/waypoint_nav/param/test4.yaml b/waypoint_nav/param/test4.yaml
deleted file mode 100644
index fb515e9..0000000
--- a/waypoint_nav/param/test4.yaml
+++ /dev/null
@@ -1,11 +0,0 @@
-waypoints:
-- point: {x: -1.789356, y: -0.636847, z: 0.0, vel: 1.0, rad: 0.3, stop: false}
-- point: {x: -1.052619, y: -1.811895, z: 0.0, vel: 1.0, rad: 0.5, stop: false}
-- point: {x: 0.542089, y: -1.86785, z: 0.0, vel: 0.8, rad: 0.2, stop: false}
-- point: {x: 0.551415, y: -0.105278, z: 0.0, vel: 0.5, rad: 0.3, stop: false}
-- point: {x: 0.629175, y: 1.860692, z: 0.0, vel: 0.5, rad: 0.2, stop: false}
-finish_pose:
-  header: {seq: 0.0, stamp: 1620367430.3803937, frame_id: base_link}
-  pose:
-    position: {x: -0.550981, y: 1.85, z: 0.0}
-    orientation: {x: 0.0, y: 0.0, z: 0.99749, w: 0.07074}
\ No newline at end of file
diff --git a/waypoint_nav/scripts/tandem_run_manager.py b/waypoint_nav/scripts/tandem_run_manager.py
new file mode 100644
index 0000000..fbe87c1
--- /dev/null
+++ b/waypoint_nav/scripts/tandem_run_manager.py
@@ -0,0 +1,75 @@
+#!/usr/bin/env python3
+
+import rospy
+import ruamel.yaml
+import numpy as np
+from std_msgs.msg import UInt16
+from sensor_msgs.msg import LaserScan
+from std_srvs.srv import Trigger
+
+
+class TandemManager():
+
+    def __init__(self):
+        yaml = ruamel.yaml.YAML()
+        waypoints_path = rospy.get_param("tandem_run_manager/waypoints_file")
+        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
+        self.wp_num_sub = rospy.Subscriber("/waypoint_num", UInt16, self.waypoint_num_callback)
+        self.scan_sub = rospy.Subscriber("/scan", LaserScan, self.laserscan_callback)
+        ##
+        self.stop_nav = rospy.ServiceProxy("/stop_wp_nav", Trigger)
+        self.resume_nav = rospy.ServiceProxy("/resume_nav", Trigger)
+        ##
+        self.front_range = None
+        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
+        elif (msg.data == self.tandem_end_list[self.tandem_id]):
+            self.in_tandem_area = False
+            self.tandem_id += 1
+        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)
+            range = int(round(np.deg2rad(15) / msg.angle_increment))
+            self.front_range = [front-range, front+range]
+        ##
+        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):
+            self.stop_nav()
+            self.stop = True
+        elif (self.stop) and (ave_range >= 0.7):
+            self.resume_nav()
+            self.stop = False
+        return
+
+
+
+
+
+if __name__ == '__main__':
+    rospy.init_node("map_changer")
+    tandem_manager = TandemManager()
+    rospy.spin()
\ No newline at end of file
diff --git a/waypoint_nav/src/velocity_controller.cpp b/waypoint_nav/src/velocity_controller.cpp
index 53735d7..d5a8406 100644
--- a/waypoint_nav/src/velocity_controller.cpp
+++ b/waypoint_nav/src/velocity_controller.cpp
@@ -27,8 +27,8 @@
 
         ros::NodeHandle nh;
         max_vel_sub_ = nh.subscribe("/max_vel", 1, &VelocityController::maxVelCallback, this);
-        cmd_vel_sub_ = nh.subscribe("/cmd_vel_in", 10, &VelocityController::cmdVelCallback, this);
-        cmd_vel_pub_ = nh.advertise<geometry_msgs::Twist>("/cmd_vel_out", 10);
+        cmd_vel_sub_ = nh.subscribe("/cmd_vel_in", 100, &VelocityController::cmdVelCallback, this);
+        cmd_vel_pub_ = nh.advertise<geometry_msgs::Twist>("/cmd_vel_out", 100);
 
         nh.param(max_vel_param, standard_vel_, standard_vel_);
         nh.param(min_vel_param, min_vel_, min_vel_);
@@ -76,4 +76,4 @@
 
     ros::spin();
     return 0;
-}
\ No newline at end of file
+}
diff --git a/waypoint_nav/waypoints_cfg/waypoints.yaml b/waypoint_nav/waypoints_cfg/waypoints.yaml
deleted file mode 100644
index 92f70d6..0000000
--- a/waypoint_nav/waypoints_cfg/waypoints.yaml
+++ /dev/null
@@ -1,10 +0,0 @@
-waypoints:
-- point: {x: -0.45, y: -0.499032, z: 0.0, vel: 1.0, rad: 0.3, stop: false}
-- point: {x: 0.50, y: -0.576395, z: 0.0, vel: 0.5, rad: 0.1, stop: true}
-- point: {x: 0.50, y: 0.80, z: 0.0, vel: 1.0, rad: 0.3, stop: false}
-- point: {x: 0.515219, y: 1.8, z: 0.0, vel: 0.3, rad: 0.1, stop: true}
-finish_pose:
-  header: {seq: 0.0, stamp: 1620367430.3803937, frame_id: base_link}
-  pose:
-    position: {x: -0.550981, y: 1.8, z: 0.0}
-    orientation: {x: 0.0, y: 0.0, z: 0.99749, w: 0.07074}
\ No newline at end of file
diff --git a/waypoint_nav/waypoints_cfg/waypoints_gakunai.yaml b/waypoint_nav/waypoints_cfg/waypoints_gakunai.yaml
deleted file mode 100644
index 1b42abe..0000000
--- a/waypoint_nav/waypoints_cfg/waypoints_gakunai.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-waypoints:
-- point: {x: 15.549, y: 1.99887, z: 0}
-- point: {x: 27.7033, y: 0.740999, z: 0}
-- point: {x: 44.367489, y: -3.73285, z: 0.0}
-- point: {x: 53.4473, y: -1.98026, z: 0}
-- point: {x: 53.5381, y: 13.9875, z: 0}
-- point: {x: 53.0202, y: 25.7033, z: 0}
-- point: {x: 52.624, y: 40.8326, z: 0}
-- point: {x: 52.4651, y: 56.1678, z: 0}
-- point: {x: 47.4651, y: 55.9678, z: 0}
-- point: {x: 41.1833, y: 55.7508, z: 0}
-- point: {x: 30.9773, y: 56.3972, z: 0}
-- point: {x: 19.605, y: 55.9349, z: 0}
-- point: {x: 9.74244, y: 57.775, z: 0}
-- point: {x: 9.04664, y: 53.1814, z: 0}
-- point: {x: 12.2474, y: 45.1187, z: 0}
-- point: {x: 17.5568, y: 32.2592, z: 0}
-- point: {x: 19.6331, y: 27.0299, z: 0}
-- point: {x: 23.7635, y: 17.3945, z: 0}
-- point: {x: 27.6797, y: 6.92171, z: 0}
-- point: {x: 29.8247, y: 1.30448, z: 0}
-- point: {x: 12.7818, y: 1.13019, z: 0}
-finish_pose:
-  header: {seq: 0, stamp: 1623386648.025251, frame_id: base_link}
-  pose:
-    position: {x: -0.8863756, y: 0.5020624, z: 0}
-    orientation: {x: 0, y: 0, z: 0.999797, w: -0.0201511}
\ No newline at end of file
diff --git a/waypoint_nav/waypoints_cfg/waypoints_gakunai_edit.yaml b/waypoint_nav/waypoints_cfg/waypoints_gakunai_edit.yaml
deleted file mode 100644
index 5d13f3a..0000000
--- a/waypoint_nav/waypoints_cfg/waypoints_gakunai_edit.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-waypoints:
-- point: {x: 15.549, y: 1.99887, z: 0}
-- point: {x: 27.7033, y: 0.740999, z: 0}
-- point: {x: 44.5033, y: -3.88193, z: 0}
-- point: {x: 53.4473, y: -1.98026, z: 0}
-- point: {x: 53.5381, y: 13.9875, z: 0}
-- point: {x: 53.0202, y: 25.7033, z: 0}
-- point: {x: 52.624, y: 40.8326, z: 0}
-- point: {x: 52.4651, y: 56.1678, z: 0}
-- point: {x: 47.4651, y: 55.9678, z: 0}
-- point: {x: 41.1833, y: 55.7508, z: 0}
-- point: {x: 30.9773, y: 56.3972, z: 0}
-- point: {x: 19.605, y: 55.9349, z: 0}
-- point: {x: 9.74244, y: 57.775, z: 0}
-- point: {x: 9.04664, y: 53.1814, z: 0}
-- point: {x: 12.2474, y: 45.1187, z: 0}
-- point: {x: 17.5568, y: 32.2592, z: 0}
-- point: {x: 19.6331, y: 27.0299, z: 0}
-- point: {x: 23.7635, y: 17.3945, z: 0}
-- point: {x: 27.6797, y: 6.92171, z: 0}
-- point: {x: 29.8247, y: 1.30448, z: 0}
-- point: {x: 12.7818, y: 1.13019, z: 0}
-finish_pose:
-  header: {seq: 0, stamp: 1623386648.025251, frame_id: base_link}
-  pose:
-    position: {x: -0.8863756, y: 0.5020624, z: 0}
-    orientation: {x: 0, y: 0, z: 0.999797, w: -0.0201511}
\ No newline at end of file
diff --git a/waypoint_nav/waypoints_cfg/waypoints_test.yaml b/waypoint_nav/waypoints_cfg/waypoints_test.yaml
deleted file mode 100644
index 713cc07..0000000
--- a/waypoint_nav/waypoints_cfg/waypoints_test.yaml
+++ /dev/null
@@ -1,13 +0,0 @@
-waypoints:
-- point: {x: -1.01321, y: -0.528956, z: -0.000870705, vel: 1, rad: 0.8, stop: 0}
-- point: {x: -0.0284393, y: -0.471859, z: 0.00520468, vel: 1, rad: 0.8, stop: 0}
-- point: {x: 0.981428, y: -0.5452, z: 0.00261879, vel: 1, rad: 0.8, stop: 0}
-- point: {x: 1.90456, y: -0.536331, z: 0.00246334, vel: 1, rad: 0.8, stop: 0}
-- point: {x: 1.95224, y: 0.538929, z: 0.00476408, vel: 1, rad: 0.8, stop: 0}
-- point: {x: 0.603558, y: 0.551442, z: 0.00079298, vel: 1, rad: 0.8, stop: 0}
-- point: {x: 0.539775, y: 1.787, z: 0.00275993, vel: 1, rad: 0.8, stop: 0}
-finish_pose:
-  header: {seq: 0, stamp: 244.465000000, frame_id: map}
-  pose: 
-    position: {x: -0.488029, y: 1.82672, z: 0}
-    orientation: {x: 0, y: 0, z: 0.999992, w: 0.00399584}