diff --git a/src/ackermann_planner.cpp b/src/ackermann_planner.cpp
index f1b6846fbdf57f15e6884210eb7eaff7497f7a17..4087d5c75f3ab9ecf25a33e8208b4601a1a5e6c1 100644
--- a/src/ackermann_planner.cpp
+++ b/src/ackermann_planner.cpp
@@ -241,8 +241,10 @@ void AckermannPlanner::update_plan_and_local_costs(geometry_msgs::PoseStamped &g
  */
 base_local_planner::Trajectory AckermannPlanner::find_best_path(geometry_msgs::PoseStamped &global_pose,ackermann_msgs::AckermannDrive &ackermann,geometry_msgs::PoseStamped& drive_velocities,std::vector<geometry_msgs::Point> footprint_spec) 
 {
-  unsigned int cell_x,cell_y,path_index=global_plan_.size()-1;
+  std::vector<geometry_msgs::Point> circular_footprint;
   geometry_msgs::PoseStamped goal_pose;
+  double min_radius,max_radius;
+  unsigned int path_index=global_plan_.size()-1;
 
   obstacle_costs_.setFootprint(footprint_spec);
 
@@ -251,20 +253,16 @@ 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);
+  costmap_2d::calculateMinAndMaxDistances(footprint_spec,min_radius,max_radius);
+  for(double angle=0;angle<2*3.14159;angle+=0.1)
+  {
+    geometry_msgs::Point new_point;
+    new_point.x=max_radius*cos(angle);
+    new_point.y=max_radius*sin(angle);
+    circular_footprint.push_back(new_point);
+  }
   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(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),circular_footprint)<0)
     {
       if(path_index>0)
         path_index--;