Merge pull request #1 from MapaRobo/path_planner_algorithm

ethanckim · web-flow · commit 95db6339a936 · 2021-06-22T12:47:32.000-06:00
Path planner algorithm Implementation
diff --git a/include/full_coverage_path_planner/common.h b/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
diff --git a/maps/square.yaml b/maps/square.yaml
@@ -0,0 +1,6 @@
+image: square.png
+resolution: 0.05
+origin: [-5, -5, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
diff --git a/src/common.cpp b/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
+}
+
diff --git a/src/spiral_stc.cpp b/src/spiral_stc.cpp
@@ -40,64 +40,167 @@ void SpiralSTC::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_r
 std::list<gridNode_t> SpiralSTC::spiral(std::vector<std::vector<bool> > const& grid, std::list<gridNode_t>& init,
                                         std::vector<std::vector<bool> >& visited)
 {
-  int dx, dy, dx_prev, x2, y2, i, nRows = grid.size(), nCols = grid[0].size();
-  // Spiral filling of the open space
+  int dx, dy, x2, y2, i, nRows = grid.size(), nCols = grid[0].size();
+  // Mountain pattern filling of the open space
   // Copy incoming list to 'end'
-  std::list<gridNode_t> pathNodes(init);
-  // Create iterator for gridNode_t list and let it point to the last element of end
-  std::list<gridNode_t>::iterator it = --(pathNodes.end());
-  if (pathNodes.size() > 1)  // if list is length 1, keep iterator at end
-    it--;                    // Let iterator point to second to last element
+  std::list<gridNode_t> pathNodes(init);  
+
+  // set initial direction towards longest side
+  int robot_dir = point;
+  int pattern_dir = point;
+  if (nRows >= nCols) {
+    if (pathNodes.back().pos.y < nRows / 2) {
+      robot_dir = north;
+      dy = 1;
+    }
+    else {
+      robot_dir = south;
+      dy = -1;
+    }
+  } else {
+    if (pathNodes.back().pos.x < nCols / 2) {
+      robot_dir = east;
+      dx = 1;
+    }
+    else {
+      robot_dir = west;
+      dx = -1;
+    }
+  }
 
-  gridNode_t prev = *(it);
   bool done = false;
   while (!done)
   {
-    if (it != pathNodes.begin())
-    {
-      // turn ccw
-      dx = pathNodes.back().pos.x - prev.pos.x;
-      dy = pathNodes.back().pos.y - prev.pos.y;
-      dx_prev = dx;
-      dx = -dy;
-      dy = dx_prev;
-    }
-    else
-    {
-      // Initialize spiral direction towards y-axis
-      dx = 0;
-      dy = 1;
-    }
-    done = true;
-
-    for (int i = 0; i < 4; ++i)
-    {
+    // 1. drive straight until hit wall
+    bool hitWall = false;
+    while(!hitWall) {
       x2 = pathNodes.back().pos.x + dx;
       y2 = pathNodes.back().pos.y + dy;
-      if (x2 >= 0 && x2 < nCols && y2 >= 0 && y2 < nRows)
+      if (!validMove(x2, y2, nCols, nRows, grid, visited))        
       {
-        if (grid[y2][x2] == eNodeOpen && visited[y2][x2] == eNodeOpen)
-        {
+        hitWall = true;
+        x2 = pathNodes.back().pos.x;
+        y2 = pathNodes.back().pos.y;
+        break;
+      }
+      if (!hitWall) {
           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);
-          it = --(pathNodes.end());
           visited[y2][x2] = eNodeVisited;  // Close node
-          done = false;
-          break;
+      }
+    }
+
+    // 2. check left and right after hitting wall, then change direction
+    if (robot_dir == north || robot_dir == south)
+    {
+      if (!validMove(x2 + 1, y2, nCols, nRows, grid, visited)
+        && !validMove(x2 - 1, y2, nCols, nRows, grid, visited)) {
+        // dead end, exit
+        done = true;
+        break;
+      } else if (!validMove(x2 + 1, y2, nCols, nRows, grid, visited)) {
+        // east is occupied, travel towards west
+        x2--;
+        pattern_dir = west;
+      } else if (!validMove(x2 - 1, y2, nCols, nRows, grid, visited)) {
+        // west is occupied, travel towards east
+        x2++;
+        pattern_dir = east;
+      } else {
+        // both sides are opened. If don't have a prefered turn direction, travel towards farthest edge
+        if (!(pattern_dir == east || pattern_dir == west)) {
+          if (x2 < nCols / 2) { // east
+            pattern_dir = east;
+          } else { // west
+            pattern_dir = west;
+          }
+        }
+        if (pattern_dir = east) {
+            x2++;
+        } else if (pattern_dir = west) {
+            x2--;
         }
       }
-      // try next direction cw
-      dx_prev = dx;
-      dx = dy;
-      dy = -dx_prev;
+
+      // add Nodes to List
+      Point_t new_point = { x2, y2 };
+      gridNode_t new_node =
+      {
+        new_point,  // Point: x,y
+        0,          // Cost
+        0,          // Heuristic
+      };
+      pathNodes.push_back(new_node);
+      visited[y2][x2] = eNodeVisited;  // Close node
+
+      // change direction 180 deg
+      if (robot_dir == north) {
+        robot_dir = south;
+        dy = -1;
+      } else if (robot_dir == south) {
+        robot_dir = north;
+        dy = 1;
+      }
     }
+    else if (robot_dir == east || robot_dir == west)
+    {
+      if (!validMove(x2, y2 + 1, nCols, nRows, grid, visited)
+        && !validMove(x2, y2 - 1, nCols, nRows, grid, visited)) {
+        // dead end, exit
+        done = true;
+        break;
+      } else if (!validMove(x2, y2 + 1, nCols, nRows, grid, visited)) {
+        // north is occupied, travel towards south
+        y2--;
+        pattern_dir = south;
+      } else if (!validMove(x2, y2 - 1, nCols, nRows, grid, visited)) {
+        // south is occupied, travel towards north
+        y2++;
+        pattern_dir = north;
+      } else {
+        // both sides are opened. If don't have a prefered turn direction, travel towards farthest edge
+        if (!(pattern_dir == north || pattern_dir == south)) {
+          if (y2 < nRows / 2) { // north
+            pattern_dir = north;
+          } else { // south
+            pattern_dir = south;
+          }
+        }
+        if (pattern_dir = north) {
+            y2++;
+        } else if (pattern_dir = south) {
+            y2--;
+        }
+      }
+
+      // add Nodes to List
+      Point_t new_point = { x2, y2 };
+      gridNode_t new_node =
+      {
+        new_point,  // Point: x,y
+        0,          // Cost
+        0,          // Heuristic
+      };
+      pathNodes.push_back(new_node);
+      visited[y2][x2] = eNodeVisited;  // Close node
+      
+      // change direction 180 deg
+      if (robot_dir == east) {
+        robot_dir = west;
+        dx = -1;
+      } else if (robot_dir == west) {
+        robot_dir = east;
+        dx = 1;
+      }
+    }
+    // Log
+    printPathNodes(pathNodes);
   }
   return pathNodes;
 }
@@ -117,6 +220,7 @@ std::list<Point_t> SpiralSTC::spiral_stc(std::vector<std::vector<bool> > const&
   x = init.x;
   y = init.y;
 
+  // add initial point to pathNodes
   Point_t new_point = { x, y };
   gridNode_t new_node =
   {
@@ -129,31 +233,34 @@ std::list<Point_t> SpiralSTC::spiral_stc(std::vector<std::vector<bool> > const&
   pathNodes.push_back(new_node);
   visited[y][x] = eNodeVisited;
 
+  std::list<Point_t> goals = map_2_goals(visited, eNodeOpen);  // Retrieve all goalpoints (Cells not visited)
+  std::list<gridNode_t>::iterator it;
+
 #ifdef DEBUG_PLOT
   ROS_INFO("Grid before walking is: ");
   printGrid(grid, visited, fullPath);
 #endif
 
-  pathNodes = SpiralSTC::spiral(grid, pathNodes, visited);                // First spiral fill
-  std::list<Point_t> goals = map_2_goals(visited, eNodeOpen);  // Retrieve remaining goalpoints
-  // Add points to full path
-  std::list<gridNode_t>::iterator it;
-  for (it = pathNodes.begin(); it != pathNodes.end(); ++it)
+  while (goals.size() != 0)
   {
-    Point_t newPoint = { it->pos.x, it->pos.y };
-    visited_counter++;
-    fullPath.push_back(newPoint);
-  }
-  // Remove all elements from pathNodes list except last element
-  pathNodes.erase(pathNodes.begin(), --(pathNodes.end()));
+    // Spiral fill from current position
+    // TODO: Convert to U-turn pattern
+    pathNodes = spiral(grid, pathNodes, visited);
 
 #ifdef DEBUG_PLOT
-  ROS_INFO("Current grid after first spiral is");
-  printGrid(grid, visited, fullPath);
-  ROS_INFO("There are %d goals remaining", goals.size());
+    ROS_INFO("Visited grid updated after spiral:");
+    printGrid(grid, visited, pathNodes, SpiralStart, pathNodes.back());
 #endif
-  while (goals.size() != 0)
-  {
+
+    goals = map_2_goals(visited, eNodeOpen);  // Retrieve remaining goalpoints
+
+    for (it = pathNodes.begin(); it != pathNodes.end(); ++it)
+    {
+      Point_t newPoint = { it->pos.x, it->pos.y };
+      visited_counter++;
+      fullPath.push_back(newPoint);
+    }
+
     // Remove all elements from pathNodes list except last element.
     // The last point is the starting point for a new search and A* extends the path from there on
     pathNodes.erase(pathNodes.begin(), --(pathNodes.end()));
@@ -189,23 +296,7 @@ std::list<Point_t> SpiralSTC::spiral_stc(std::vector<std::vector<bool> > const&
     gridNode_t SpiralStart = pathNodes.back();
     printGrid(grid, visited, pathNodes, pathNodes.front(), pathNodes.back());
 #endif
-
-    // Spiral fill from current position
-    pathNodes = spiral(grid, pathNodes, visited);
-
-#ifdef DEBUG_PLOT
-    ROS_INFO("Visited grid updated after spiral:");
-    printGrid(grid, visited, pathNodes, SpiralStart, pathNodes.back());
-#endif
-
-    goals = map_2_goals(visited, eNodeOpen);  // Retrieve remaining goalpoints
-
-    for (it = pathNodes.begin(); it != pathNodes.end(); ++it)
-    {
-      Point_t newPoint = { it->pos.x, it->pos.y };
-      visited_counter++;
-      fullPath.push_back(newPoint);
-    }
+    
   }
 
   return fullPath;
diff --git a/test/full_coverage_path_planner/test_full_coverage_path_planner.launch b/test/full_coverage_path_planner/test_full_coverage_path_planner.launch
@@ -1,14 +1,14 @@
 <?xml version="1.0"?>
 
 <launch>
-    <arg name="map" default="$(find full_coverage_path_planner)/maps/grid.yaml"/>
+    <arg name="map" default="$(find full_coverage_path_planner)/maps/square.yaml"/>
     <arg name="coverage_area_offset" default="-2.5 -2.5 0 0 0 0"/>
     <arg name="coverage_area_size_x" default="10"/>
     <arg name="coverage_area_size_y" default="10"/>
     <arg name="target_x_vel" default="0.5"/>
     <arg name="target_yaw_vel" default="0.4"/>
     <arg name="robot_radius" default="0.3"/>
-    <arg name="tool_radius" default="0.3"/>
+    <arg name="tool_radius" default="0.2"/>
     <arg name="rviz" default="true"/>
 
     <!--Move base flex, using the full_coverage_path_planner-->
diff --git a/test/full_coverage_path_planner/test_full_coverage_path_planner_plugin.launch b/test/full_coverage_path_planner/test_full_coverage_path_planner_plugin.launch
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 
 <launch>
-    <arg name="map" default="$(find full_coverage_path_planner)/maps/grid.yaml"/>
+    <arg name="map" default="$(find full_coverage_path_planner)/maps/square.yaml"/>
     <arg name="robot_radius" default="0.3"/>
     <arg name="tool_radius" default="0.2"/>
 
