diff --git a/src/opendrive_planner.cpp b/src/opendrive_planner.cpp
index 01206358b9ec19b158c552b11ffe81d554c0b36a..36ed297f3c7f5601d2a22b4948f0e23e27a478e9 100644
--- a/src/opendrive_planner.cpp
+++ b/src/opendrive_planner.cpp
@@ -271,6 +271,8 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
       std::cout << std::endl;
     }
     best_path_index=this->roadmap.get_best_path(best_cost,best_path);
+    if(best_path_index==-1)
+      return false;
     std::cout << "best path index: " << best_path_index << std::endl;
     for(unsigned int i=0;i<best_path.size();i++)
       std::cout << best_path[i] << ",";