diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp
index b019047..723b4db 100755
--- a/waypoint_nav/src/waypoint_nav.cpp
+++ b/waypoint_nav/src/waypoint_nav.cpp
@@ -369,6 +369,7 @@
             float y[laserscan.ranges.size()];
             float waypoint_x = dest.position.x - robot_pos.getOrigin().x();
             float waypoint_y = dest.position.y - robot_pos.getOrigin().y();
+            float robot_yaw = tf::getYaw(robot_pos.getRotation());
             float waypoint_x_update = dest.position.x - robot_pos.getOrigin().x();
             float waypoint_y_update = dest.position.y - robot_pos.getOrigin().y();
             float r = std::sqrt(std::pow(dest.position.x - robot_pos.getOrigin().x(), 2) + std::pow(dest.position.y - robot_pos.getOrigin().y(), 2));
@@ -377,7 +378,7 @@
             bool pass_bool = false;
             for(int i = 0; i < laserscan.ranges.size(); i++){
                 if(!(laserscan.ranges[i] < laserscan.range_min || laserscan.ranges[i] > laserscan.range_max || std::isnan(laserscan.ranges[i]))){
-                    float theta = laserscan.angle_min + i * laserscan.angle_increment;
+                    float theta = laserscan.angle_min + i * laserscan.angle_increment + robot_yaw;
                     x[j] = laserscan.ranges[i] * cosf(theta);
                     y[j] = laserscan.ranges[i] * sinf(theta);
                     j++;
@@ -400,6 +401,8 @@
                 float min = *std::min_element(dist_waypoint, dist_waypoint + size);
                 if((i == 0 && min > first_thresh) || (i == 0 && std::sqrt(std::pow(waypoint_x_update, 2) + std::pow(waypoint_y_update, 2)) < first_thresh)){
                     pass_bool = true;
+                    ROS_WARN("Min dist_waypoint: %f", min);
+                    ROS_WARN("Pass");
                     break;
                 }else if(i == 0 && min < first_thresh){
                     ROS_WARN("There is an obstacle near waypoint.");
@@ -533,8 +536,8 @@
     {
         setMaxVel();
         bool FindGoalWithoutObstacle = true;
-        float first_thresh = 0.5;
-        float after_second_thresh = 0.7;
+        float first_thresh = 0.7;
+        float after_second_thresh = 1.0;
         float goal_thresh = 0.5;
         if(FindGoalWithoutObstacle){
             sendNoObstacleGoal(first_thresh, after_second_thresh, goal_thresh, dest);
@@ -620,7 +623,7 @@
                         clear_costmaps_srv_.call(empty);
                         startNavigationGL(*current_waypoint_);
                         resend_goal++;
-                        if (resend_goal == 3) {
+                        if (!waypoint_list_[wp_num_.data-1].stop && resend_goal == 3) {
                             ROS_WARN("Skip waypoint.");
                             break;
                         }