diff --git a/alib2graph_algo/src/generate/RandomGridFactory.hpp b/alib2graph_algo/src/generate/RandomGridFactory.hpp
index 7f2227c61cf9b15158d126b4e3dd7395134afece..27b50cd2ef2c834effa908020a1ec07f37eae643 100644
--- a/alib2graph_algo/src/generate/RandomGridFactory.hpp
+++ b/alib2graph_algo/src/generate/RandomGridFactory.hpp
@@ -54,19 +54,19 @@ RandomGridFactory::randomGrid(unsigned long height,
 
   TGrid grid(height, width);
 
-  // Add nodes
-  ext::vector<node_type> obstacles;
-
   // Generate start and goal node
   node_type start(random_height(e1), random_width(e1));
   node_type goal(random_height(e1), random_width(e1));
-  obstacles.push_back(start);
-  obstacles.push_back(goal);
-
+ 
   // Generate obstacles
   for (unsigned long i = 0; i < obstacle_cnt; ++i) {
     node_type obstacle(random_height(e1), random_width(e1));
-    obstacles.push_back(obstacle);
+
+    // Try to avoid creating obstacle on place of start or goal vertex
+    if (obstacle == start || obstacle == goal) {
+        continue;
+    }
+
     grid.addObstacle(std::move(obstacle));
   }