diff --git a/src/ackermann_planner_util.cpp b/src/ackermann_planner_util.cpp
index 5857485e46d6a2baae7fe1c71cee359cb39de70b..b2943b4d6f95bf13cda18525d20f2ca0ce12aa00 100644
--- a/src/ackermann_planner_util.cpp
+++ b/src/ackermann_planner_util.cpp
@@ -103,6 +103,11 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
     ROS_ERROR("AckermannPlannerUtil: planner utils have not been initialized, please call initialize() first");
     return false;
   }
+  if(orig_global_plan.size()<5)
+  {
+    ROS_ERROR("AckermannPlannerUtil: A valid path at least needs 5 points");
+    return false;
+  }
   ////divide plan by manuveurs
   subPathList.clear();
   subPath.clear();