diff --git a/waypoint_nav/launch/waypoint_nav.launch b/waypoint_nav/launch/waypoint_nav.launch
old mode 100644
new mode 100755
index db60758..4cd7678
--- a/waypoint_nav/launch/waypoint_nav.launch
+++ b/waypoint_nav/launch/waypoint_nav.launch
@@ -1,20 +1,22 @@
 <?xml version="1.0"?>
 
 <launch>
-    <arg name="waypoints_file"    default="$(find waypoint_nav)/waypoints_cfg/waypoints.yaml"/>
-    <arg name="cmd_vel_topic_in"  default="/move_base/cmd_vel"/>
-    <arg name="cmd_vel_topic_out" default="/wp_nav/cmd_vel"/>
-    <arg name="robot_frame"       default="base_link"/>
-    <arg name="max_vel_param"     default="/move_base/TrajectoryPlannerROS/max_vel_x"/>
-    <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="waypoints_file"     default="$(find waypoint_nav)/waypoints_cfg/waypoints.yaml"/>
+    <arg name="StartFromTheMiddle" default="false"/>
+    <arg name="cmd_vel_topic_in"   default="/move_base/cmd_vel"/>
+    <arg name="cmd_vel_topic_out"  default="/wp_nav/cmd_vel"/>
+    <arg name="robot_frame"        default="base_link"/>
+    <arg name="max_vel_param"      default="/move_base/TrajectoryPlannerROS/max_vel_x"/>
+    <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"/>
 
 
     <node name="waypoint_nav" pkg="waypoint_nav" type="waypoint_nav" output="screen">
         <remap from="/cmd_vel"    to="$(arg cmd_vel_topic_out)"/>
-        <param name="filename"    value="$(arg waypoints_file)"/>
-        <param name="robot_frame" value="$(arg robot_frame)"/>
+        <param name="filename"           value="$(arg waypoints_file)"/>
+        <param name="StartFromTheMiddle" value="$(arg StartFromTheMiddle)"/>
+        <param name="robot_frame"        value="$(arg robot_frame)"/>
     </node>
 
 
diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp
old mode 100644
new mode 100755
index 93eb296..38ee0a8
--- a/waypoint_nav/src/waypoint_nav.cpp
+++ b/waypoint_nav/src/waypoint_nav.cpp
@@ -79,6 +79,14 @@
         } else {
             ROS_ERROR("waypoints file doesn't have name");
         }
+        
+        //Scripts of "start from the middle"
+        //Edited mori 2022/10/19
+        //#############################################################
+        StartFromTheMiddle = false;
+        private_nh.getParam("StartFromTheMiddle", StartFromTheMiddle);
+        //#############################################################
+        
         private_nh.param("min_dist_err", min_dist_err_, min_dist_err_);
         private_nh.param("min_yaw_err", min_yaw_err_, min_yaw_err_);
 
@@ -202,16 +210,72 @@
             response.success = false;
             return false;
         }
+        //Scripts of "start from the middle"
+        //Edited mori 2022/10/19
+        //#############################################################
+        wp_num_.data = 1;
+        init_waypoint_ = waypoints_.poses.begin();
+        compare_waypoint_ = waypoints_.poses.begin();
+        double min_dist_w = std::numeric_limits<int>::max();
+        if(StartFromTheMiddle){
+        	ROS_INFO("StartFromTheMiddle => True");
+        	tf::StampedTransform robot_init_pos = getRobotPosGL();
+        	double init_x = robot_init_pos.getOrigin().x();
+        	double init_y = robot_init_pos.getOrigin().y();
+        	double pre_waypoint_x;
+        	double pre_waypoint_y;
+        	double convert_x;
+        	double init_yaw = tf::getYaw(robot_init_pos.getRotation());
+        	double square_x;
+        	double square_y;
+        	double dist_w;
+        	double dir_w;
+        	int min_waypoint = 0;
+        	int i = 0;
+        	while(compare_waypoint_ != waypoints_.poses.end() - 1){
+        		i++;
+        		square_x = (compare_waypoint_ -> position.x - init_x) * (compare_waypoint_ -> position.x - init_x);
+        		square_y = (compare_waypoint_ -> position.y - init_y) * (compare_waypoint_ -> position.y - init_y);
+        		dist_w = sqrt(square_x + square_y);
+        		if(i == 1){
+        			dir_w = atan2(compare_waypoint_ -> position.y, compare_waypoint_ -> position.x);
+        			convert_x = (compare_waypoint_ -> position.x - init_x) * cos(-init_yaw) - (compare_waypoint_ -> position.y - init_y) * sin(-init_yaw);
+        			if(abs(dir_w - init_yaw) < (90 / 180.0 * M_PI) && convert_x > 0){
+        				min_dist_w = dist_w;
+        				init_waypoint_ = compare_waypoint_;
+        				min_waypoint = 1;
+        			}
+        		}else if(min_dist_w > dist_w){
+        			dir_w = atan2(compare_waypoint_ -> position.y - pre_waypoint_y, compare_waypoint_ -> position.x- pre_waypoint_x);
+        			convert_x = (compare_waypoint_ -> position.x - init_x) * cos(-init_yaw) - (compare_waypoint_ -> position.y - init_y) * sin(-init_yaw);
+        			if(abs(dir_w - init_yaw) < (90 / 180.0 * M_PI) && convert_x > 0){
+        				min_dist_w = dist_w;
+        				init_waypoint_ = compare_waypoint_;
+        				min_waypoint = i;
+        			}
+        		}
+        		pre_waypoint_x = compare_waypoint_ -> position.x;
+        		pre_waypoint_y = compare_waypoint_ -> position.y;
+        		compare_waypoint_++;
+        	}
+        	wp_num_.data = min_waypoint;
+        }
         std_srvs::Empty empty;
         while(!clear_costmaps_srv_.call(empty)) {
             ROS_WARN("Resend clear costmap service");
             sleep();
         }
-        current_waypoint_ = waypoints_.poses.begin();
-        wp_num_.data = 1;
-        has_activate_ = true;
-        response.success = true;
-        return true;
+        if(min_dist_w == std::numeric_limits<int>::max()){
+        	ROS_WARN("Could not find the closest appropriate waypoint. Please check the direction of the robot again.");
+        	response.success = false;
+        	return false;
+        }else{
+        	current_waypoint_ = init_waypoint_;
+        	has_activate_ = true;
+        	response.success = true;
+        	return true;
+        }
+        //#############################################################
     }
 
 
@@ -233,7 +297,6 @@
     }
 
 
-
     /*
     ++++++++++ Callback function for service of "stop_wp_nav"  ++++++++++
     */
@@ -305,8 +368,8 @@
             m.getRPY(r, p, y);
             double diff_yaw = std::abs(y - target_yaw_);
             if ((dist < min_dist_err_) && (diff_yaw < min_yaw_err_)) return true;
-            else if ((dist < min_dist_err_ + 0.3) && navigationFinished()) return true;
-            else return false;
+            else if ((dist < min_dist_err_ * 2) && navigationFinished()) return true;
+            return false;
         }
         return dist < dist_err;
     }
@@ -393,6 +456,7 @@
     void run()
     {
         ROS_INFO("Waiting for waypoint navigation to start.");
+        int resend_goal;
         while(ros::ok()) {
             // has_activate_ is false, nothing to do
             if (!has_activate_) {
@@ -414,7 +478,7 @@
             ROS_INFO_STREAM("Go to waypoint " << wp_num_.data);
             ROS_INFO_STREAM("x: " << waypoint_list_[wp_num_.data-1].x << ", y: " << waypoint_list_[wp_num_.data-1].y);
             startNavigationGL(*current_waypoint_);
-            int resend_goal = 0;
+            resend_goal = 0;
             double start_nav_time = ros::Time::now().toSec();
             dist_err_ = waypoint_list_[wp_num_.data-1].rad;
             try {
@@ -446,6 +510,7 @@
                 // update current waypoint
                 current_waypoint_++;
                 wp_num_.data++;
+                wp_num_pub_.publish(wp_num_);
 
             } catch(const SwitchRunningStatus &e) {
                 ROS_INFO_STREAM("Running status switched");
@@ -462,6 +527,15 @@
     geometry_msgs::PoseArray waypoints_;
     visualization_msgs::MarkerArray marker_;
     std::vector<Waypoint> waypoint_list_;
+    
+    //Scripts of "start from the middle"
+    //Edited mori 2022/10/19
+    //#############################################################
+    bool StartFromTheMiddle;
+    std::vector<geometry_msgs::Pose>::iterator init_waypoint_;
+    std::vector<geometry_msgs::Pose>::iterator compare_waypoint_;
+    //#############################################################
+    
     std::vector<geometry_msgs::Pose>::iterator current_waypoint_;
     std::vector<geometry_msgs::Pose>::iterator finish_pose_;
     bool has_activate_, stopped_;