Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Ft remove logging and directly visualize #19

Merged
merged 8 commits into from
Oct 28, 2023
Merged

Ft remove logging and directly visualize #19

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
7f6a086
Add grid visualizer
bilalkah Sep 19, 2023
298d9c1
Add tree visualizer
bilalkah Sep 19, 2023
293bf6b
apply new visualizer
bilalkah Sep 19, 2023
3e57bbd
Add time and change length
bilalkah Oct 22, 2023
b085304
Change 2dray with sf::line
bilalkah Oct 28, 2023
43f099a
apply unified visualizer
bilalkah Oct 28, 2023
8bd6085
New tests
bilalkah Oct 28, 2023
8972f3a
🎨 apply clang-format changes
bilalkah Oct 28, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion .vscode/launch.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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}",
Expand Down
103 changes: 26 additions & 77 deletions main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,20 +51,23 @@ int main(int argc, char **argv)
auto planner_name = config["planner_name"].as<std::string>();
PlannerType planner = GetPlanner(planner_name);
// Visualizer config
auto rescale = config["visualizer"]["rescale"].as<float>();
auto delay = config["visualizer"]["delay"].as<float>();
auto show = config["visualizer"]["show"].as<bool>();

tools::Visualizer visualizer(*map, rescale, rescale, delay, show);
auto tree_visualizer = std::make_shared<tools::Visualizer>(
map, tools::pair_double{2.0, 2.0}, 1u, "Tree Visualizer", planner_name);

// Path config
auto s = config["path"]["start"];
auto start_node = planning::Node(s["x"].as<int>(), s["y"].as<int>());
auto g = config["path"]["goal"];
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);

while (true)
{
tree_visualizer->SetGetLogFunction(
std::bind(&planning::IPlanningWithLogging::GetLog, planner));
std::cout << "Started" << std::endl;
// Get time
auto start_time{std::chrono::high_resolution_clock::now()};
planning::Path path = planner->FindPath(start_node, goal_node, map);
Expand All @@ -73,86 +76,32 @@ int main(int argc, char **argv)
end_time - start_time)
.count();
std::cout << "Planning Duration: " << duration << " ms" << std::endl;
// dynamic cast to get log

auto log = planner->GetLog();

std::cout << "Log size: " << log.size() << std::endl;
if (planner_name == "astar" || planner_name == "bfs" ||
planner_name == "dfs")
{
visualizer.VisualizeGridLog(log);

// sleep for 1 second
std::this_thread::sleep_for(std::chrono::milliseconds(1000));

if (!path.empty())
{
visualizer.VisualizeGridPath(path);
}
}
else if (planner_name == "rrt" || planner_name == "rrt_star")
{
if (planner_name == "rrt_star")
{
auto log_vector =
std::dynamic_pointer_cast<planning::tree_base::RRTStar>(
planner)
->GetLogVector();
for (auto &log : log_vector)
{
visualizer.VisualizeTreeLog(log.first, 0);
// sleep for 1 second
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
if (!log.second.empty())
{
visualizer.VisualizeTreePath(log.second);
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
visualizer.UpdateMap(*map);
}
}
else
{
visualizer.VisualizeTreeLog(log, 10);
}

// sleep for 1 second
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
if (!path.empty())
{
visualizer.VisualizeTreePath(path);
}
}

// sleep for 1 second
std::this_thread::sleep_for(std::chrono::milliseconds(1000));

visualizer.UpdateMap(*map);

// sleep for 1 second
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
std::cout << "\n\n\n-----------\n\n\n" << std::endl;
std::cout << "Finished" << std::endl;
// set null to get_log_function_ to stop logging
tree_visualizer->SetGetLogFunction(nullptr);
planner->ClearLog();
}

return 0;
}

struct FunctionMap
{
static std::unordered_map<std::string, PlannerType (*)(std::string)> map;
};

std::unordered_map<std::string, PlannerType (*)(std::string)> FunctionMap::map =
{{"astar", GetGridBasedPlanner},
{"bfs", GetGridBasedPlanner},
{"dfs", GetGridBasedPlanner},
{"rrt", GetTreeBasedPlanner},
{"rrt_star", GetTreeBasedPlanner}};

PlannerType GetPlanner(std::string planner_name)
{
PlannerType result = FunctionMap::map.at(planner_name)(planner_name);
PlannerType result{};
if (planner_name == "astar" || planner_name == "bfs" || planner_name == "dfs")
{
result = GetGridBasedPlanner(planner_name);
}
else if (planner_name == "rrt" || planner_name == "rrt_star")
{
result = GetTreeBasedPlanner(planner_name);
}
else
{
std::cout << "Invalid planner name" << std::endl;
exit(1);
}
return result;
}

Expand Down
28 changes: 13 additions & 15 deletions planning/grid_base/a_star/a_star.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,10 +11,12 @@

#include "a_star.h"

#include <chrono>
#include <cmath>
#include <iostream>
#include <memory>
#include <queue>
#include <thread>

namespace planning
{
Expand All @@ -26,7 +28,6 @@ auto Compare{
return lhs->cost.f > rhs->cost.f;
}};

// Euclidean distance.
auto heuristic{[](const Node &lhs, const Node &rhs) {
return std::hypot(lhs.x_ - rhs.x_, lhs.y_ - rhs.y_);
}};
Expand All @@ -52,43 +53,37 @@ AStar::AStar(const double &heuristic_weight, const int search_space)
Path AStar::FindPath(const Node &start_node, const Node &goal_node,
const std::shared_ptr<Map> map)
{
// Clear log.
log_.clear();
log_.emplace_back(LogType{start_node, NodeState::kStart});
log_.emplace_back(LogType{goal_node, NodeState::kGoal});
// Copy map to avoid changing it.
ClearLog();
std::shared_ptr<Map> map_copy = std::make_shared<Map>(*map);
map_copy->SetNodeState(goal_node, NodeState::kGoal);

// Create priority queue for search list.
std::priority_queue<std::shared_ptr<NodeParent>,
std::vector<std::shared_ptr<NodeParent>>,
decltype(Compare)>
search_list(Compare);

// Create start node.
auto start_node_info = std::make_shared<NodeParent>(
start_node, nullptr,
Cost(0, heuristic(start_node, goal_node), heuristic_weight_));

// Add start node to search list.
search_list.push(start_node_info);

while (!search_list.empty() && !IsGoal(search_list.top()->node, goal_node))
{
// Get the node with the lowest cost.
auto current_node = search_list.top();
search_list.pop();

if (!IsFree(current_node->node, map_copy))
{
continue;
}
log_.emplace_back(LogType{current_node->node, NodeState::kVisited});
// Update map.
{
std::lock_guard<std::mutex> lock(log_mutex_);
log_.first.emplace_back(current_node);
}

map_copy->SetNodeState(current_node->node, NodeState::kVisited);

// Check neighbors.
for (auto &direction : search_space_)
{
int x = current_node->node.x_ + direction[0];
Expand All @@ -106,6 +101,7 @@ Path AStar::FindPath(const Node &start_node, const Node &goal_node,

search_list.push(neighbor_node);
}
std::this_thread::sleep_for(std::chrono::microseconds(100));
}

if (search_list.empty())
Expand All @@ -115,12 +111,14 @@ Path AStar::FindPath(const Node &start_node, const Node &goal_node,
}

auto current_node = search_list.top();
{
std::lock_guard<std::mutex> lock(log_mutex_);
log_.second = current_node;
}
auto path = ReconstructPath(current_node);
map_copy->UpdateMapWithPath(path);
return path;
}

Log AStar::GetLog() { return log_; }

} // namespace grid_base
} // namespace planning
20 changes: 16 additions & 4 deletions planning/grid_base/a_star/a_star.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
#include "planning/include/i_planning.h"

#include <memory>
#include <mutex>
#include <string>
#include <utility>
#include <vector>
Expand All @@ -36,13 +37,24 @@ class AStar : public IPlanningWithLogging
AStar(const double &heuristic_weight, const int search_space);
Path FindPath(const Node &start_node, const Node &goal_node,
const std::shared_ptr<Map> map) override;
Log GetLog() override;
Log GetLog() override
{
std::lock_guard<std::mutex> lock(log_mutex_);
return log_;
}
void ClearLog() override
{
std::lock_guard<std::mutex> lock(log_mutex_);
log_.first.clear();
log_.second = nullptr;
}

private:
Log log_;
SearchSpace search_space_;
Log log_{};

double heuristic_weight_;
SearchSpace search_space_{};
double heuristic_weight_{};
std::mutex log_mutex_{};
}; // class AStar

} // namespace grid_base
Expand Down
27 changes: 12 additions & 15 deletions planning/grid_base/bfs/bfs.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,10 +10,12 @@
*/

#include "bfs.h"
#include <chrono>
#include <iostream>
#include <memory>
#include <queue>
#include <stdexcept>
#include <thread>

namespace planning
{
Expand All @@ -39,36 +41,32 @@ BFS::BFS(const int search_space)
Path BFS::FindPath(const Node &start_node, const Node &goal_node,
const std::shared_ptr<Map> map)
{
// Clear log.
log_.clear();
log_.emplace_back(LogType{start_node, NodeState::kStart});
log_.emplace_back(LogType{goal_node, NodeState::kGoal});
// Copy map to avoid changing it.
ClearLog();

std::shared_ptr<Map> map_copy = std::make_shared<Map>(*map);
map_copy->SetNodeState(goal_node, NodeState::kGoal);
// Create queue for search list.
std::queue<std::shared_ptr<NodeParent>> search_list;

// Create start node.
std::queue<std::shared_ptr<NodeParent>> search_list;
auto start_node_info =
std::make_shared<NodeParent>(start_node, nullptr, Cost{});

// Add start node to search list.
search_list.push(start_node_info);

while (!search_list.empty() &&
!IsGoal((search_list.front()->node), goal_node))
{
// Get first node from search list.
auto current_node = search_list.front();
search_list.pop();

if (!IsFree(current_node->node, map_copy))
{
continue;
}
log_.emplace_back(LogType{current_node->node, NodeState::kVisited});
// Update map.

{
std::lock_guard<std::mutex> lock(log_mutex_);
log_.first.emplace_back(current_node);
}
map_copy->SetNodeState(current_node->node, NodeState::kVisited);

for (const auto &direction : search_space_)
Expand All @@ -84,9 +82,9 @@ Path BFS::FindPath(const Node &start_node, const Node &goal_node,
auto neighbor_node =
std::make_shared<NodeParent>(Node(x, y), current_node, Cost{});

// Add neighbor node to search list.
search_list.push(neighbor_node);
}
std::this_thread::sleep_for(std::chrono::microseconds(100));
}
if (search_list.empty())
{
Expand All @@ -95,13 +93,12 @@ Path BFS::FindPath(const Node &start_node, const Node &goal_node,
}

auto current_node = search_list.front();
log_.second = current_node;
auto path = ReconstructPath(current_node);
map_copy->UpdateMapWithPath(path);

return path;
}

Log BFS::GetLog() { return log_; }

} // namespace grid_base
} // namespace planning
Loading