diff --git a/navfn/include/navfn/navfn_ros.h b/navfn/include/navfn/navfn_ros.h
index 9438007823..04fc56ec16 100644
--- a/navfn/include/navfn/navfn_ros.h
+++ b/navfn/include/navfn/navfn_ros.h
@@ -178,7 +178,7 @@ namespace navfn {
 
       void mapToWorld(double mx, double my, double& wx, double& wy);
       void clearRobotCell(const geometry_msgs::PoseStamped& global_pose, unsigned int mx, unsigned int my);
-      double planner_window_x_, planner_window_y_, default_tolerance_;
+      double planner_window_x_, planner_window_y_, default_tolerance_, plan_length_threshold_;
       boost::mutex mutex_;
       ros::ServiceServer make_plan_srv_;
       std::string global_frame_;
diff --git a/navfn/src/navfn_ros.cpp b/navfn/src/navfn_ros.cpp
index d78ca6d8a0..ec4a7d08a7 100644
--- a/navfn/src/navfn_ros.cpp
+++ b/navfn/src/navfn_ros.cpp
@@ -80,6 +80,7 @@ namespace navfn {
       private_nh.param("planner_window_x", planner_window_x_, 0.0);
       private_nh.param("planner_window_y", planner_window_y_, 0.0);
       private_nh.param("default_tolerance", default_tolerance_, 0.0);
+      private_nh.param("plan_length_threshold", plan_length_threshold_, 0.0);
 
       make_plan_srv_ =  private_nh.advertiseService("make_plan", &NavfnROS::makePlanService, this);
 
@@ -296,7 +297,7 @@ namespace navfn {
         plan.push_back(goal_copy);
       }
       else{
-        ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");
+        ROS_WARN("Failed to get a plan from potential when a legal potential was found. This is probably due to the plan lenght threshold.");
       }
     }
 
@@ -407,11 +408,21 @@ namespace navfn {
     int len = planner_->getPathLen();
     ros::Time plan_time = ros::Time::now();
 
+    double prev_wx, prev_wy;
+    double path_len = 0;
+
     for(int i = len - 1; i >= 0; --i){
       //convert the plan to world coordinates
       double world_x, world_y;
       mapToWorld(x[i], y[i], world_x, world_y);
 
+      if (i < len - 1){
+        path_len += hypot(world_x-prev_wx, world_y-prev_wy);
+      }
+
+      prev_wx = world_x;
+      prev_wy = world_y;
+
       geometry_msgs::PoseStamped pose;
       pose.header.stamp = plan_time;
       pose.header.frame_id = global_frame_;
@@ -424,6 +435,10 @@ namespace navfn {
       pose.pose.orientation.w = 1.0;
       plan.push_back(pose);
     }
+        
+    if (plan_length_threshold_ > 0.0 && path_len > plan_length_threshold_){
+      plan.clear();
+    }
 
     //publish the plan for visualization purposes
     publishPlan(plan, 0.0, 1.0, 0.0, 0.0);