Change 2dray with sf::line

bilalkah · Oct 28, 2023 · b085304 · b085304
1 parent 3e57bbd
commit b085304
Show file tree

Hide file tree

Showing 4 changed files with 92 additions and 35 deletions.
diff --git a/.vscode/launch.json b/.vscode/launch.json
@@ -10,7 +10,8 @@
             "name": "Run gtest",
             "type": "cppdbg",
             "request": "launch",
-            "program": "${workspaceFolder}/build/test/test_dfs",
+            "preLaunchTask": "CMake: build all",
+            "program": "${workspaceFolder}/build/main",
             "args": [],
             "stopAtEntry": false,
             "cwd": "${workspaceFolder}",

diff --git a/planning/include/node_parent.h b/planning/include/node_parent.h
@@ -70,6 +70,7 @@ inline Path ReconstructPath(std::shared_ptr<NodeParent> current_node)
       path.insert(path.begin(), iterator_node->node);
       iterator_node = iterator_node->parent;
     }
+  path.insert(path.begin(), iterator_node->node);
   return path;
 }
 

diff --git a/tools/include/tree_visualizer.h b/tools/include/tree_visualizer.h
@@ -15,7 +15,9 @@
 #include "planning/tree_base/include/common_tree_base.h"
 #include "tools/include/i_visualize.h"
 #include <SFML/Graphics.hpp>
+#include <SFML/Graphics/RenderTexture.hpp>
 #include <memory>
+#include <thread>
 
 namespace tools
 {
@@ -34,16 +36,22 @@ class TreeVisualizer
   void SetStartAndGoal(const planning::Node &start_node,
                        const planning::Node &goal_node);
 
+  void MapToTexture();
+
+  void Run();
+
 private:
   sf::RenderWindow window_;
   std::string window_name_;
   std::shared_ptr<planning::Map> map;
   pair_double size_coeff_;
   std::size_t kDelay_;
+  sf::RenderTexture render_texture_;
 
   std::unordered_map<planning::NodeState, sf::Color> colors_;
   sf::CircleShape start_node_;
   sf::CircleShape goal_node_;
+  std::thread window_thread_;
 };
 
 } // namespace tools

diff --git a/tools/src/tree_visualizer.cpp b/tools/src/tree_visualizer.cpp
@@ -12,8 +12,9 @@
 #include "tools/include/tree_visualizer.h"
 #include "planning/include/data_types.h"
 #include "planning/tree_base/include/common_tree_base.h"
-#include <vector>
 #include <chrono>
+#include <thread>
+#include <vector>
 
 namespace tools
 {
@@ -28,6 +29,10 @@ TreeVisualizer::TreeVisualizer(std::shared_ptr<planning::Map> map,
                                map->GetWidth() * size_coeff_.second),
                  window_name_);
   colors_ = GetColorMap();
+  MapToTexture();
+
+  window_thread_ = std::thread(&TreeVisualizer::Run, this);
+  window_thread_.detach();
 };
 
 TreeVisualizer::~TreeVisualizer() { window_.close(); }
@@ -36,58 +41,64 @@ void TreeVisualizer::Visualize(
     const std::vector<std::shared_ptr<planning::NodeParent>> node_parent_vector,
     const std::shared_ptr<planning::NodeParent> goal_node_parent)
 {
-
-  window_.clear();
-  auto start_time = std::chrono::high_resolution_clock::now();
+
+  {
+    window_.clear();
+    sf::Sprite sprite(render_texture_.getTexture());
+    window_.draw(sprite);
+  }
   for (auto &node_parent : node_parent_vector)
     {
       if (node_parent->parent == nullptr)
         {
           continue;
         }
 
-      auto ray{planning::tree_base::Get2DRayBetweenNodes(
-          node_parent->parent->node, node_parent->node)};
-
-      for (auto &node : ray)
-        {
-          sf::RectangleShape rectangle;
-          rectangle.setSize(
-              sf::Vector2f(size_coeff_.second, size_coeff_.first));
-          rectangle.setPosition(node.y_ * size_coeff_.second,
-                                node.x_ * size_coeff_.first);
-          rectangle.setFillColor(
-              GetColorMap().at(planning::NodeState::kVisited));
-          window_.draw(rectangle);
-        }
+      // sf line
+      sf::Vertex line[] = {
+          sf::Vertex(
+              sf::Vector2f(node_parent->parent->node.y_ * size_coeff_.second,
+                           node_parent->parent->node.x_ * size_coeff_.first),
+              colors_.at(planning::NodeState::kVisited)),
+          sf::Vertex(sf::Vector2f(node_parent->node.y_ * size_coeff_.second,
+                                  node_parent->node.x_ * size_coeff_.first),
+                     colors_.at(planning::NodeState::kVisited))};
+      {
+        window_.draw(line, 2, sf::Lines);
+      }
     }
-auto end_time = std::chrono::high_resolution_clock::now();
-  auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(
-      end_time - start_time);
-  std::cout << "Visualize took " << duration.count() << " milliseconds"
-            << std::endl;
+
   if (goal_node_parent != nullptr)
     {
       auto path{planning::ReconstructPath(goal_node_parent)};
 
-      for (auto &node : path)
+      // sf line
+      for (auto i = 0u; i < path.size() - 1; i++)
         {
-          sf::RectangleShape rectangle;
-          rectangle.setSize(
-              sf::Vector2f(size_coeff_.second, size_coeff_.first));
-          rectangle.setPosition(node.y_ * size_coeff_.second,
-                                node.x_ * size_coeff_.first);
-          rectangle.setFillColor(GetColorMap().at(planning::NodeState::kPath));
-          window_.draw(rectangle);
+          sf::Vertex line[] = {
+              sf::Vertex(sf::Vector2f(path[i].y_ * size_coeff_.second,
+                                      path[i].x_ * size_coeff_.first),
+                         colors_.at(planning::NodeState::kPath)),
+              sf::Vertex(sf::Vector2f(path[i + 1].y_ * size_coeff_.second,
+                                      path[i + 1].x_ * size_coeff_.first),
+                         colors_.at(planning::NodeState::kPath))};
+
+          {
+            window_.draw(line, 2, sf::Lines);
+          }
         }
     }
 
   // Draw start and goal nodes as circles
-  window_.draw(start_node_);
-  window_.draw(goal_node_);
-
+  {
+    window_.draw(start_node_);
+    window_.draw(goal_node_);
+  }
   window_.display();
+  // sleep scheduler for kDelay_ ms
   sf::sleep(sf::milliseconds(kDelay_));
+  // std::this_thread::sleep_for(std::chrono::milliseconds(1000 / 120));
+  sf::sleep(sf::milliseconds(1000 / 60));
 }
 
 void TreeVisualizer::SetStartAndGoal(const planning::Node &start_node,
@@ -104,4 +115,40 @@ void TreeVisualizer::SetStartAndGoal(const planning::Node &start_node,
                          goal_node.x_ * size_coeff_.first);
 }
 
+void TreeVisualizer::MapToTexture()
+{
+  render_texture_.create(map->GetHeight() * size_coeff_.first,
+                         map->GetWidth() * size_coeff_.second);
+  render_texture_.clear();
+  for (auto i = 0u; i < map->GetHeight(); i++)
+    {
+      for (auto j = 0u; j < map->GetWidth(); j++)
+        {
+          sf::RectangleShape rectangle;
+          rectangle.setSize(
+              sf::Vector2f(size_coeff_.second, size_coeff_.first));
+          rectangle.setPosition(j * size_coeff_.second, i * size_coeff_.first);
+          auto key = map->GetNodeState(planning::Node(i, j));
+          rectangle.setFillColor(colors_.at(key));
+          render_texture_.draw(rectangle);
+        }
+    }
+  render_texture_.display();
+}
+
+void TreeVisualizer::Run()
+{
+  while (window_.isOpen())
+    {
+      sf::Event event;
+      while (window_.pollEvent(event))
+        {
+          if (event.type == sf::Event::Closed)
+            {
+              window_.close();
+            }
+        }
+    }
+}
+
 } // namespace tools