From 26c31832c953766724c370e034b14059e2bb0e68 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Mon, 14 Nov 2022 18:25:22 +0100
Subject: [PATCH] Added a check of the path length in the set_plan function.

---
 src/ackermann_planner_util.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/ackermann_planner_util.cpp b/src/ackermann_planner_util.cpp
index 5857485..b2943b4 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();
-- 
GitLab