Fix wirenode and yaml changes

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 tree_visualizer = std::make_shared<tools::Visualizer>(
-      map, tools::pair_double{2.0, 2.0}, 1u, "Tree Visualizer", planner_name);
+  auto rescale_factor = config["visualizer"]["rescale"].as<double>();
+  auto delay = config["visualizer"]["delay"].as<double>();
+
+  auto visualizer = std::make_shared<tools::Visualizer>(
+      map, tools::pair_double{rescale_factor, rescale_factor}, delay,
+      "Tree Visualizer", planner_name);
 
   // Path config
   auto s = config["path"]["start"];
@@ -61,11 +66,11 @@ int main(int argc, char **argv)
   auto goal_node = planning::Node(g["x"].as<int>(), g["y"].as<int>());
 
   // Visualize start and goal nodes
-  tree_visualizer->SetStartAndGoal(start_node, goal_node);
+  visualizer->SetStartAndGoal(start_node, goal_node);
 
   while (true)
     {
-      tree_visualizer->SetGetLogFunction(
+      visualizer->SetGetLogFunction(
           std::bind(&planning::IPlanningWithLogging::GetLog, planner));
       std::cout << "Started" << std::endl;
       // Get time
@@ -79,7 +84,7 @@ int main(int argc, char **argv)
       std::this_thread::sleep_for(std::chrono::milliseconds(1000));
       std::cout << "Finished" << std::endl;
       // set null to get_log_function_ to stop logging
-      tree_visualizer->SetGetLogFunction(nullptr);
+      visualizer->SetGetLogFunction(nullptr);
       planner->ClearLog();
     }
 

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

@@ -195,6 +195,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 &&

tools/include/visualizer.h

@@ -46,8 +46,7 @@ class Visualizer
 {
 public:
   Visualizer(std::shared_ptr<planning::Map> map, pair_double size_coeff,
-             std::size_t kDelay, std::string window_name,
-             std::string planner_name);
+             double kDelay, std::string window_name, std::string planner_name);
   ~Visualizer() { window_.close(); }
 
   void SetStartAndGoal(const planning::Node &start_node,
@@ -68,7 +67,7 @@ class Visualizer
 private:
   std::shared_ptr<planning::Map> map;
   pair_double size_coeff_;
-  std::size_t kDelay_;
+  double kDelay_;
   std::string window_name_;
   std::string planner_name_;
 

tools/src/visualizer.cpp

@@ -20,7 +20,7 @@ namespace tools
 {
 
 Visualizer::Visualizer(std::shared_ptr<planning::Map> map,
-                       pair_double size_coeff, std::size_t kDelay,
+                       pair_double size_coeff, double kDelay,
                        std::string window_name, std::string planner_name)
     : map(map), size_coeff_(size_coeff), kDelay_(kDelay),
       window_name_(window_name), planner_name_(planner_name)