diff --git a/main.cpp b/main.cpp
index d67bff3..2e0bdfe 100644
--- a/main.cpp
+++ b/main.cpp
@@ -50,14 +50,9 @@ int main(int argc, char **argv)
   // Planner config
   auto planner_name = config["planner_name"].as<std::string>();
   PlannerType planner = GetPlanner(planner_name);
-
   // Visualizer config
-  auto rescale_factor = config["visualizer"]["rescale"].as<double>();
-  auto delay = config["visualizer"]["delay"].as<size_t>();
-
   auto tree_visualizer = std::make_shared<tools::Visualizer>(
-      map, tools::pair_double{rescale_factor, rescale_factor}, delay,
-      "Tree Visualizer", planner_name);
+      map, tools::pair_double{2.0, 2.0}, 1u, "Tree Visualizer", planner_name);
 
   // Path config
   auto s = config["path"]["start"];
diff --git a/planning/grid_base/dfs/dfs.cpp b/planning/grid_base/dfs/dfs.cpp
index 3a6cd3f..fa712f9 100644
--- a/planning/grid_base/dfs/dfs.cpp
+++ b/planning/grid_base/dfs/dfs.cpp
@@ -81,7 +81,6 @@ Path DFS::FindPath(const Node &start_node, const Node &goal_node,
 
           search_list.push(new_node_parent);
         }
-      std::this_thread::sleep_for(std::chrono::microseconds(100));
     }
 
   if (search_list.empty())
diff --git a/planning/tree_base/src/common_tree_base.cpp b/planning/tree_base/src/common_tree_base.cpp
index 9be73d7..86c77cc 100644
--- a/planning/tree_base/src/common_tree_base.cpp
+++ b/planning/tree_base/src/common_tree_base.cpp
@@ -32,11 +32,19 @@ std::pair<double, double> RandomSampling()
 
 Node RandomNode(const std::shared_ptr<Map> map)
 {
+  bool is_valid{false};
   Node random_node;
-  auto random_point{RandomSampling()};
-  random_node = Node(static_cast<int>(random_point.first * map->GetHeight()),
-                     static_cast<int>(random_point.second * map->GetWidth()));
-
+  while (!is_valid)
+    {
+      auto random_point{RandomSampling()};
+      random_node =
+          Node(static_cast<int>(random_point.first * map->GetHeight()),
+               static_cast<int>(random_point.second * map->GetWidth()));
+      if (map->GetNodeState(random_node) == NodeState::kFree)
+        {
+          is_valid = true;
+        }
+    }
   return random_node;
 }
 
@@ -187,10 +195,6 @@ WireNewNode(const int max_branch_length, const int min_branch_length,
     {
       return std::make_shared<NodeParent>(new_node, nearest_node, Cost{});
     }
-  if (index == 0)
-    {
-      return std::nullptr_t();
-    }
   auto isLengthValid{EuclideanDistance(ray[index - 1], nearest_node->node) >
                      min_branch_length};
   auto isNodeValid{map->GetNodeState(ray[index]) != NodeState::kOccupied &&