diff --git a/waypoint_nav/launch/waypoint_nav.launch b/waypoint_nav/launch/waypoint_nav.launch
index e7b5ece..db60758 100644
--- a/waypoint_nav/launch/waypoint_nav.launch
+++ b/waypoint_nav/launch/waypoint_nav.launch
@@ -1,24 +1,29 @@
 <?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="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"/>
 
 
     <node name="waypoint_nav" pkg="waypoint_nav" type="waypoint_nav" output="screen">
-        <param name="filename" value="$(arg waypoints_file)"/>
+        <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)"/>
-        <remap from="/cmd_vel" to="$(arg cmd_vel_topic_out)"/>
     </node>
 
 
     <node name="velocity_controller" pkg="waypoint_nav" type="velocity_controller" output="screen">
-        <remap from="/cmd_vel_in" to="$(arg cmd_vel_topic_in)"/>
-        <remap from="/cmd_vel_out" to="$(arg cmd_vel_topic_out)"/>
+        <remap from="/cmd_vel_in"   to="$(arg cmd_vel_topic_in)"/>
+        <remap from="/cmd_vel_out"  to="$(arg cmd_vel_topic_out)"/>
         <param name="max_vel_param" value="$(arg max_vel_param)"/>
         <param name="min_vel_param" value="$(arg min_vel_param)"/>
+        <param name="min_dist_err"  value="$(arg min_dist_err)"/>
+        <param name="min_yaw_err"   value="$(arg min_yaw_err)"/>
     </node>
 </launch>
diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp
index 36fbf7e..cfbe79a 100644
--- a/waypoint_nav/src/waypoint_nav.cpp
+++ b/waypoint_nav/src/waypoint_nav.cpp
@@ -48,10 +48,10 @@
         stopped_(false),
         move_base_action_("move_base", true),
         rate_(10),
-        after_start_(0.5),
         last_moved_time_(0),
         dist_err_(1.0),
-        min_dist_err_(0.3)
+        min_dist_err_(0.3),
+        min_yaw_err_(0.3)
     {
         while((move_base_action_.waitForServer(ros::Duration(1.0)) == false) && (ros::ok() == true))
         {
@@ -64,7 +64,6 @@
         double max_update_rate;
         private_nh.param("max_update_rate", max_update_rate, 10.0);
         rate_ = ros::Rate(max_update_rate);
-        after_start_ = ros::Rate(0.5);
         std::string filename = "";
         private_nh.param("filename", filename, filename);
         if (filename != "") {
@@ -80,7 +79,8 @@
         } else {
             ROS_ERROR("waypoints file doesn't have name");
         }
-        min_dist_err_ = 0.3;
+        private_nh.param("min_dist_err", min_dist_err_, min_dist_err_);
+        private_nh.param("min_yaw_err", min_yaw_err_, min_yaw_err_);
 
         ros::NodeHandle nh;
         start_server_ = nh.advertiseService("start_wp_nav", &WaypointsNavigation::startNavigationCallback, this);
@@ -159,6 +159,7 @@
                 point.x = pose.position.x;
                 point.y = pose.position.y;
                 point.z = pose.position.z;
+                point.stop = true;
                 waypoint_list_.push_back(point);
             } else {
                 return false;
@@ -277,8 +278,11 @@
     */
     bool navigationFinished()
     {
-        return onNavigationPoint(current_waypoint_->position, min_dist_err_);
-        //return move_base_action_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED;
+        if (move_base_action_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
+            ROS_WARN("Move base state is Goal reached");
+            return true;
+        }
+        return false;
     }
 
 
@@ -288,9 +292,6 @@
     */
     bool onNavigationPoint(const geometry_msgs::Point &dest, double dist_err=1.0)
     {
-        //if (waypoint_list_[wp_num_.data-1].stop) {
-        //    return navigationFinished();
-        //}
         tf::StampedTransform robot_gl = getRobotPosGL();
         const double wx = dest.x;
         const double wy = dest.y;
@@ -298,7 +299,12 @@
         const double ry = robot_gl.getOrigin().y();
         const double dist = std::sqrt(std::pow(wx - rx, 2) + std::pow(wy - ry, 2));
         if (waypoint_list_[wp_num_.data-1].stop) {
-            return dist < min_dist_err_;
+            tf::Quaternion q = robot_gl.getRotation();
+            tf::Matrix3x3 m(q);
+            double r, p ,y;
+            m.getRPY(r, p, y);
+            double diff_yaw = std::abs(y - target_yaw_);
+            return ((dist < min_dist_err_) && (diff_yaw < min_yaw_err_));
         }
         return dist < dist_err;
     }
@@ -356,7 +362,13 @@
         move_base_goal.target_pose.pose.position = dest.position;
         move_base_goal.target_pose.pose.orientation = dest.orientation;
         move_base_action_.sendGoal(move_base_goal);
-        after_start_.sleep();
+        if (waypoint_list_[wp_num_.data-1].stop) {
+            tf::Quaternion q(dest.orientation.x, dest.orientation.y, dest.orientation.z, dest.orientation.w);
+            tf::Matrix3x3 m(q);
+            double r, p ,y;
+            m.getRPY(r, p, y);
+            target_yaw_ = y;
+        }
     }
 
 
@@ -389,7 +401,7 @@
             if (current_waypoint_ == finish_pose_) {
                 ROS_INFO_STREAM("Go to final goal");
                 startNavigationGL(*current_waypoint_);
-                while(!navigationFinished() && ros::ok()) {
+                while(!onNavigationPoint(finish_pose_->position) && ros::ok()) {
                   sleep();
                 }
                 ROS_INFO("Final goal reached!!");
@@ -409,17 +421,13 @@
                 // loop until reach waypoint
                 while(!onNavigationPoint(current_waypoint_->position, dist_err_)) {
                     if (!has_activate_) throw SwitchRunningStatus();
-                    if ((!waypoint_list_[wp_num_.data-1].stop) && (navigationFinished())) {
-                        ROS_WARN("Reached default xy_goal_torelance.");
-                        break;
-                    }
                     double time = ros::Time::now().toSec();
                     if (time - start_nav_time > 10.0 && time - last_moved_time_ > 10.0) {
                         ROS_WARN("Resend the navigation goal.");
                         std_srvs::Empty empty;
                         clear_costmaps_srv_.call(empty);
                         startNavigationGL(*current_waypoint_);
-                        resend_goal++;
+                        // resend_goal++;
                         //if (resend_goal == 3) {
                         //    ROS_WARN("Skip waypoint.");
                         //    break;
@@ -459,12 +467,12 @@
     bool has_activate_, stopped_;
     std::string robot_frame_, world_frame_;
     tf::TransformListener tf_listener_;
-    ros::Rate rate_, after_start_;
+    ros::Rate rate_;
     ros::ServiceServer start_server_, stop_server_, resume_server_;
     ros::Subscriber cmd_vel_sub_, move_base_status_sub_;
     ros::Publisher wp_pub_, max_vel_pub_, wp_num_pub_;
     ros::ServiceClient clear_costmaps_srv_;
-    double last_moved_time_, dist_err_, min_dist_err_;
+    double last_moved_time_, dist_err_, min_dist_err_, target_yaw_, min_yaw_err_;
     std_msgs::UInt16 wp_num_;
     std_msgs::Float32 max_vel_msg_;
 };