mountain pattern algorithm implementation

Author: ethanckim

include/full_coverage_path_planner/common.h

@@ -61,6 +61,15 @@ enum
   eNodeVisited = true
 };
 
+enum
+{
+  point = 0,
+  east = 1,
+  west = 2,
+  north = 3,
+  south = 4
+};
+
 /**
  * Find the distance from poi to the closest point in goals
  * @param poi Starting point
@@ -124,4 +133,23 @@ void printGrid(std::vector<std::vector<bool> > const& grid);
  * @return a list of points that have the given value_to_search
  */
 std::list<Point_t> map_2_goals(std::vector<std::vector<bool> > const& grid, bool value_to_search);
+
+/**
+ * Prints pathNodes in the terminal
+ */
+void printPathNodes(std::list<gridNode_t> pathNodes);
+
+/**
+ * returns true only if the desired move is valid
+ */
+bool validMove(int x2, int y2, int nCols, int nRows,
+               std::vector<std::vector<bool> > const& grid,
+               std::vector<std::vector<bool> > const& visited);
+
+/**
+ * Adds node in (x2, y2) into the list of pathNodes, and marks the node as visited
+ */
+bool addNodeToList(int x2, int y2, gridNode_t prev, std::list<gridNode_t> pathNodes,
+                   std::vector<std::vector<bool>>& visited);
+
 #endif  // FULL_COVERAGE_PATH_PLANNER_COMMON_H

src/common.cpp

@@ -309,3 +309,36 @@ std::list<Point_t> map_2_goals(std::vector<std::vector<bool> > const& grid, bool
   }
   return goals;
 }
+
+
+void printPathNodes(std::list<gridNode_t> pathNodes)
+{
+  for (gridNode_t node : pathNodes) {
+    std::cout << "(" << node.pos.x << ", " << node.pos.y << ")" << ": " << node.cost << " " << node.he << std::endl;
+  }
+  std:: cout << "--------------------------------------" << std::endl;
+
+}
+
+bool validMove(int x2, int y2, int nCols, int nRows,
+               std::vector<std::vector<bool>> const& grid,
+               std::vector<std::vector<bool>> const& visited)
+{
+  return (x2 >= 0 && x2 < nCols && y2 >= 0 && y2 < nRows) // path node is within the map
+        && (grid[y2][x2] == eNodeOpen && visited[y2][x2] == eNodeOpen); // the path node is unvisited
+}
+
+void addNodeToList(int x2, int y2, gridNode_t prev, std::list<gridNode_t>& pathNodes,
+                   std::vector<std::vector<bool>>& visited) {
+  Point_t new_point = { x2, y2 };
+  gridNode_t new_node =
+  {
+    new_point,  // Point: x,y
+    0,          // Cost
+    0,          // Heuristic
+  };
+  prev = pathNodes.back();
+  pathNodes.push_back(new_node);
+  visited[y2][x2] = eNodeVisited;  // Close node
+}
+