diff --git a/src/ackermann_planner.cpp b/src/ackermann_planner.cpp
index f4c335a3775e027ec6c4ecada0dc983644cd4806..f1b6846fbdf57f15e6884210eb7eaff7497f7a17 100644
--- a/src/ackermann_planner.cpp
+++ b/src/ackermann_planner.cpp
@@ -251,11 +251,28 @@ base_local_planner::Trajectory AckermannPlanner::find_best_path(geometry_msgs::P
 
   Eigen::Vector3f pos(global_pose.pose.position.x, global_pose.pose.position.y, tf::getYaw(global_pose.pose.orientation));
   Eigen::Vector3f ack_state(ackermann.speed,ackermann.steering_angle,ackermann.steering_angle_velocity);
-  while(planner_util_->get_world_model()->footprintCost(global_plan_[path_index].pose.position.x,global_plan_[path_index].pose.position.y,tf::getYaw(global_plan_[path_index].pose.orientation),footprint_spec)<0 && path_index>0)
-  {
-    if(path_index>0)
-      path_index--;
-  }
+  do{
+    if(planner_util_->get_world_model()->footprintCost(global_plan_[path_index].pose.position.x,global_plan_[path_index].pose.position.y,tf::getYaw(global_plan_[path_index].pose.orientation),footprint_spec)<0)
+    {
+      if(path_index>0)
+        path_index--;
+      continue;
+    }
+    else if(planner_util_->get_world_model()->footprintCost(global_plan_[path_index].pose.position.x,global_plan_[path_index].pose.position.y,tf::getYaw(global_plan_[path_index].pose.orientation)+1.5707,footprint_spec)<0)
+    {
+      if(path_index>0)
+        path_index--;
+      continue;
+    }
+    else if(planner_util_->get_world_model()->footprintCost(global_plan_[path_index].pose.position.x,global_plan_[path_index].pose.position.y,tf::getYaw(global_plan_[path_index].pose.orientation)-1.5707,footprint_spec)<0)
+    {
+      if(path_index>0)
+        path_index--;
+      continue;
+    }
+    else
+      break;
+  }while(path_index>0);
   if(path_index==0)
   {
     std::cout << "empty path" << std::endl;