Fix random node and use yaml parameter (#25)

- RandomNode method returns without validation
- main reads yaml parameter

main.cpp

@@ -50,9 +50,14 @@ 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{2.0, 2.0}, 1u, "Tree Visualizer", planner_name);
+      map, tools::pair_double{rescale_factor, rescale_factor}, delay,
+      "Tree Visualizer", planner_name);
 
   // Path config
   auto s = config["path"]["start"];

planning/grid_base/dfs/dfs.cpp

@@ -81,6 +81,7 @@ 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())

planning/tree_base/src/common_tree_base.cpp

@@ -32,19 +32,11 @@ std::pair<double, double> RandomSampling()
 
 Node RandomNode(const std::shared_ptr<Map> map)
 {
-  bool is_valid{false};
   Node random_node;
-  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;
-        }
-    }
+  auto random_point{RandomSampling()};
+  random_node = Node(static_cast<int>(random_point.first * map->GetHeight()),
+                     static_cast<int>(random_point.second * map->GetWidth()));
+
   return random_node;
 }
 
@@ -195,6 +187,10 @@ 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 &&