Fix naming (#18)

- Fix naming - Unify custom costs --------- Co-authored-by: bilalkah <[email protected]>
bilalkah · Oct 18, 2023 · a9e272b · a9e272b
1 parent 4bc8730
commit a9e272b
Show file tree

Hide file tree

Showing 17 changed files with 146 additions and 182 deletions.
diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml
@@ -3,7 +3,7 @@ name: Linter
 on:
   pull_request:
     branches: [ "main" ]
-
+    types: [opened, labeled, unlabeled, synchronize]
 jobs:
   format:
     runs-on: ubuntu-latest

diff --git a/planning/grid_base/a_star/a_star.cpp b/planning/grid_base/a_star/a_star.cpp
@@ -21,10 +21,10 @@ namespace planning
 namespace grid_base
 {
 
-auto Compare{[](std::shared_ptr<NodeParent<Cost>> lhs,
-                std::shared_ptr<NodeParent<Cost>> rhs) {
-  return lhs->cost.f > rhs->cost.f;
-}};
+auto Compare{
+    [](std::shared_ptr<NodeParent> lhs, std::shared_ptr<NodeParent> rhs) {
+      return lhs->cost.f > rhs->cost.f;
+    }};
 
 // Euclidean distance.
 auto heuristic{[](const Node &lhs, const Node &rhs) {
@@ -61,46 +61,46 @@ Path AStar::FindPath(const Node &start_node, const Node &goal_node,
   map_copy->SetNodeState(goal_node, NodeState::kGoal);
 
   // Create priority queue for search list.
-  std::priority_queue<std::shared_ptr<NodeParent<Cost>>,
-                      std::vector<std::shared_ptr<NodeParent<Cost>>>,
+  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<Cost>>(
-      std::make_shared<Node>(start_node), nullptr,
+  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))
+  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))
+      if (!IsFree(current_node->node, map_copy))
         {
           continue;
         }
-      log_.emplace_back(LogType{*current_node->node, NodeState::kVisited});
+      log_.emplace_back(LogType{current_node->node, NodeState::kVisited});
       // Update map.
-      map_copy->SetNodeState(*current_node->node, NodeState::kVisited);
+      map_copy->SetNodeState(current_node->node, NodeState::kVisited);
 
       // Check neighbors.
       for (auto &direction : search_space_)
         {
-          int x = current_node->node->x_ + direction[0];
-          int y = current_node->node->y_ + direction[1];
+          int x = current_node->node.x_ + direction[0];
+          int y = current_node->node.y_ + direction[1];
 
           if (!IsFree(Node(x, y), map_copy))
             {
               continue;
             }
 
-          auto neighbor_node = std::make_shared<NodeParent<Cost>>(
-              std::make_shared<Node>(Node(x, y)), current_node,
+          auto neighbor_node = std::make_shared<NodeParent>(
+              Node(x, y), current_node,
               Cost(current_node->cost.g + 1, heuristic(Node(x, y), goal_node),
                    heuristic_weight_));
 

diff --git a/planning/grid_base/a_star/a_star.h b/planning/grid_base/a_star/a_star.h
@@ -26,16 +26,6 @@ namespace planning
 namespace grid_base
 {
 
-struct Cost
-{
-  Cost() : g(0), h(0), f(0) {}
-  Cost(double g, double h, double w) : g(g), h(h), f((1 - w) * g + w * h) {}
-
-  double g; // cost from start node
-  double h; // heuristic cost to goal node
-  double f; // total cost
-};
-
 /**
  * @brief A* path finding algorithm.
  *

diff --git a/planning/grid_base/bfs/bfs.cpp b/planning/grid_base/bfs/bfs.cpp
@@ -47,43 +47,42 @@ Path BFS::FindPath(const Node &start_node, const Node &goal_node,
   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<CostBFS>>> search_list;
+  std::queue<std::shared_ptr<NodeParent>> search_list;
 
   // Create start node.
-  auto start_node_info = std::make_shared<NodeParent<CostBFS>>(
-      std::make_shared<Node>(start_node), nullptr, CostBFS{0});
+  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))
+         !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))
+      if (!IsFree(current_node->node, map_copy))
         {
           continue;
         }
-      log_.emplace_back(LogType{*current_node->node, NodeState::kVisited});
+      log_.emplace_back(LogType{current_node->node, NodeState::kVisited});
       // Update map.
-      map_copy->SetNodeState(*current_node->node, NodeState::kVisited);
+      map_copy->SetNodeState(current_node->node, NodeState::kVisited);
 
       for (const auto &direction : search_space_)
         {
-          int x = current_node->node->x_ + direction[0];
-          int y = current_node->node->y_ + direction[1];
+          int x = current_node->node.x_ + direction[0];
+          int y = current_node->node.y_ + direction[1];
 
           if (!IsFree(Node(x, y), map_copy))
             {
               continue;
             }
 
-          auto neighbor_node = std::make_shared<NodeParent<CostBFS>>(
-              std::make_shared<Node>(Node(x, y)), current_node,
-              CostBFS{current_node->cost + 1});
+          auto neighbor_node =
+              std::make_shared<NodeParent>(Node(x, y), current_node, Cost{});
 
           // Add neighbor node to search list.
           search_list.push(neighbor_node);

diff --git a/planning/grid_base/bfs/bfs.h b/planning/grid_base/bfs/bfs.h
@@ -22,8 +22,6 @@ namespace planning
 namespace grid_base
 {
 
-using CostBFS = int;
-
 class BFS : public IPlanningWithLogging
 {
 

diff --git a/planning/grid_base/dfs/dfs.cpp b/planning/grid_base/dfs/dfs.cpp
@@ -46,43 +46,40 @@ Path DFS::FindPath(const Node &start_node, const Node &goal_node,
   map_copy->SetNodeState(goal_node, NodeState::kGoal);
 
   // Create stack for search list.
-  std::stack<std::shared_ptr<NodeParent<CostDFS>>> search_list;
+  std::stack<std::shared_ptr<NodeParent>> search_list;
 
   // Create start node.
-  std::shared_ptr<NodeParent<CostDFS>> start_node_info =
-      std::make_shared<NodeParent<CostDFS>>(std::make_shared<Node>(start_node),
-                                            nullptr, CostDFS{0});
+  std::shared_ptr<NodeParent> start_node_info =
+      std::make_shared<NodeParent>(start_node, nullptr, Cost{});
   search_list.push(start_node_info);
 
-  while (!search_list.empty() && !IsGoal(*search_list.top()->node, goal_node))
+  while (!search_list.empty() && !IsGoal(search_list.top()->node, goal_node))
     {
       auto current_node = search_list.top();
       search_list.pop();
 
       // Check if node is already visited.
-      if (map_copy->GetNodeState(*current_node->node) == NodeState::kVisited)
+      if (map_copy->GetNodeState(current_node->node) == NodeState::kVisited)
         {
           continue;
         }
-      log_.emplace_back(LogType{*current_node->node, NodeState::kVisited});
+      log_.emplace_back(LogType{current_node->node, NodeState::kVisited});
       // Set node state to visited.
-      map_copy->SetNodeState(*current_node->node, NodeState::kVisited);
+      map_copy->SetNodeState(current_node->node, NodeState::kVisited);
 
       for (const auto &direction : search_space_)
         {
-          int x = current_node->node->x_ + direction[0];
-          int y = current_node->node->y_ + direction[1];
+          int x = current_node->node.x_ + direction[0];
+          int y = current_node->node.y_ + direction[1];
 
           if (!IsFree(Node(x, y), map_copy))
             {
               continue;
             }
 
           // Create new node.
-          std::shared_ptr<NodeParent<CostDFS>> new_node_parent =
-              std::make_shared<NodeParent<CostDFS>>(
-                  std::make_shared<Node>(Node(x, y)), current_node,
-                  CostDFS{current_node->cost + 1});
+          std::shared_ptr<NodeParent> new_node_parent =
+              std::make_shared<NodeParent>(Node(x, y), current_node, Cost{});
 
           // Add new node to search list.
           search_list.push(new_node_parent);

diff --git a/planning/grid_base/dfs/dfs.h b/planning/grid_base/dfs/dfs.h
@@ -23,8 +23,6 @@ namespace planning
 namespace grid_base
 {
 
-using CostDFS = int;
-
 /**
  * @brief Depth First Search algorithm.
  *

diff --git a/planning/include/data_types.h b/planning/include/data_types.h
@@ -89,7 +89,7 @@ enum class NodeState : uint8_t
  * @brief Path type.
  *
  */
-using Path = std::vector<std::shared_ptr<Node>>;
+using Path = std::vector<Node>;
 // using Log = std::vector<std::pair<Node, NodeState>>;
 
 } // namespace planning

diff --git a/planning/include/i_planning.h b/planning/include/i_planning.h
@@ -31,9 +31,8 @@ class IPlanning
    * @param goal_node Goal node.
    * @param map Map to find path on.
    */
-  virtual std::vector<std::shared_ptr<Node>>
-  FindPath(const Node &start_node, const Node &goal_node,
-           const std::shared_ptr<Map> map) = 0;
+  virtual Path FindPath(const Node &start_node, const Node &goal_node,
+                        const std::shared_ptr<Map> map) = 0;
 
   virtual ~IPlanning() {}
 }; // class IPathFinding

diff --git a/planning/include/node_parent.h b/planning/include/node_parent.h
@@ -17,34 +17,47 @@
 namespace planning
 {
 
+/**
+ * @brief Common cost struct for planning algorithms.
+ *
+ */
+struct Cost
+{
+  Cost() : g(0), h(0), f(0) {}
+  Cost(double g, double h) : g(g), h(h), f(g + h) {}
+  Cost(double g, double h, double w) : g(g), h(h), f((1 - w) * g + w * h) {}
+
+  double g; // cost from start node
+  double h; // Euclidean distance to goal node or previous node
+  double f; // total cost
+};
+
 /**
  * @brief Node with parent and cost.
  *
  * @tparam T Cost type.
  */
-template <typename T> struct NodeParent
+struct NodeParent
 {
-  NodeParent() : node(nullptr), parent(nullptr), cost(0) {}
-  NodeParent(std::shared_ptr<Node> node_, std::shared_ptr<NodeParent> parent_)
-      : node(node_), parent(parent_), cost(0)
+  NodeParent() {}
+  NodeParent(Node node_, std::shared_ptr<NodeParent> parent_)
+      : node(node_), parent(parent_)
   {
   }
-  NodeParent(std::shared_ptr<Node> node_, std::shared_ptr<NodeParent> parent_,
-             T cost_)
+  NodeParent(Node node_, std::shared_ptr<NodeParent> parent_, Cost cost_)
       : node(node_), parent(parent_), cost(cost_)
   {
   }
-  std::shared_ptr<Node> node;
-  std::shared_ptr<NodeParent> parent;
-  T cost;
+  Node node{};
+  std::shared_ptr<NodeParent> parent{};
+  Cost cost{};
 };
 
 /**
  * @brief Backtrace path from goal to start.
  *
  */
-template <typename T>
-Path ReconstructPath(std::shared_ptr<NodeParent<T>> current_node)
+inline Path ReconstructPath(std::shared_ptr<NodeParent> current_node)
 {
   Path path;
   if (current_node == nullptr)

diff --git a/planning/src/common_planning.cpp b/planning/src/common_planning.cpp
@@ -81,7 +81,7 @@ void Map::UpdateMapWithPath(const Path &path)
 {
   for (const auto &node : path)
     {
-      map_[node->x_][node->y_] = NodeState::kPath;
+      map_[node.x_][node.y_] = NodeState::kPath;
     }
 }
-Original file line number
+Diff line change
@@ Expand Up / @@ -22,8 +22,6 @@ namespace planning @@
     namespace grid_base
     {
-    using CostBFS = int;
     class BFS : public IPlanningWithLogging
     {
@@ Expand Down @@