diff --git a/dwb_local_planner/src/dwb_local_planner.cpp b/dwb_local_planner/src/dwb_local_planner.cpp
index d4139a3..e13e0ed 100644
--- a/dwb_local_planner/src/dwb_local_planner.cpp
+++ b/dwb_local_planner/src/dwb_local_planner.cpp
@@ -397,11 +397,31 @@ nav_2d_msgs::Path2D DWBLocalPlanner::transformGlobalPlan(const nav_2d_msgs::Pose
   nav_2d_msgs::Pose2DStamped stamped_pose;
   stamped_pose.header.frame_id = global_plan_.header.frame_id;
 
-  for (unsigned int i = 0; i < global_plan_.poses.size(); i++)
-  {
+  unsigned int begin_within = 0;
+  unsigned int closest_idx = 0;
+  double min_dist = getSquareDistance(robot_pose.pose, global_plan_.poses[0]);
+
+  for (unsigned int i = 1; i < global_plan_.poses.size(); ++i) {
+    double dist = getSquareDistance(robot_pose.pose, global_plan_.poses[i]);
+    if (dist < min_dist){
+      min_dist = dist;
+      closest_idx = i;
+    }//if
+  }//for
+
+  for (unsigned int i = closest_idx; i >= 0 && i < global_plan_.poses.size(); --i) {
+    double dist = getSquareDistance(robot_pose.pose, global_plan_.poses[i]);
+    if (dist > sq_dist_threshold){
+      begin_within = i;
+      break;
+    }//if
+  }//for
+
+  for (unsigned int i = begin_within; i < global_plan_.poses.size(); ++i) {
     bool should_break = false;
     if (getSquareDistance(robot_pose.pose, global_plan_.poses[i]) > sq_dist_threshold)
     {
+
       if (transformed_plan.poses.size() == 0)
       {
         // we need to skip to a point on the plan that is within a certain distance of the robot
@@ -412,7 +432,7 @@ nav_2d_msgs::Path2D DWBLocalPlanner::transformGlobalPlan(const nav_2d_msgs::Pose
         // we're done transforming points
         should_break = true;
       }
-    }
+    }//for
 
     // now we'll transform until points are outside of our distance threshold
     stamped_pose.pose = global_plan_.poses[i];