Fix infinite loop error and improved heading decision of path planning algorithm

Author: ethanckim

include/full_coverage_path_planner/common.h

@@ -136,11 +136,18 @@ std::list<Point_t> map_2_goals(std::vector<std::vector<bool> > const& grid, bool
 
 /**
  * Prints pathNodes in the terminal
+ * @param pathNodes pathNodes to be printed in the terminal
  */
 void printPathNodes(std::list<gridNode_t> pathNodes);
 
 /**
  * returns true only if the desired move is valid
+ * @param x2 x coordinate of desired position
+ * @param y2 y coordinate of desired position
+ * @param nCols
+ * @param nRows 
+ * @param grid internal map representation - 2D grid of bools. true == occupied/blocked/obstacle
+ * @param visited 2D grid of bools. true == visited
  */
 bool validMove(int x2, int y2, int nCols, int nRows,
                std::vector<std::vector<bool> > const& grid,
@@ -149,7 +156,16 @@ bool validMove(int x2, int y2, int nCols, int nRows,
 /**
  * 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,
+void addNodeToList(int x2, int y2, std::list<gridNode_t>& pathNodes,
                    std::vector<std::vector<bool>>& visited);
 
+/**
+ * Returns direction in which most free space is visible when given the robot's current location
+ * @param ignoreDir ignores a single direction specified. Pass 0 (point) to consider all four directions.
+ */
+int dirWithMostSpace(int x2, int y2, int nCols, int nRows,
+                     std::vector<std::vector<bool> > const& grid,
+                     std::vector<std::vector<bool> > const& visited,
+                     int ignoreDir);
+
 #endif  // FULL_COVERAGE_PATH_PLANNER_COMMON_H

include/full_coverage_path_planner/full_coverage_path_planner.h

@@ -85,6 +85,22 @@ class FullCoveragePathPlanner
   void parsePointlist2Plan(const geometry_msgs::PoseStamped& start, std::list<Point_t> const& goalpoints,
                            std::vector<geometry_msgs::PoseStamped>& plan);
 
+  /**
+   * Convert ROS Occupancy grid to internal grid representation, given the size of a single tile
+   * @param costmap_grid_ Costmap representation. Cells higher that 65 are considered occupied
+   * @param grid internal map representation
+   * @param tileSize size (in meters) of a cell. This can be the robot's size
+   * @param realStart Start position of the robot (in meters)
+   * @param scaledStart Start position of the robot on the grid
+   * @return success
+   */
+  bool parseCostmap(costmap_2d::Costmap2D* costmap_grid_,
+                    std::vector<std::vector<bool> >& grid,
+                    float robotRadius,
+                    float toolRadius,
+                    geometry_msgs::PoseStamped const& realStart,
+                    Point_t& scaledStart);
+
   /**
    * Convert ROS Occupancy grid to internal grid representation, given the size of a single tile
    * @param cpp_grid_ ROS occupancy grid representation. Cells higher that 65 are considered occupied
@@ -103,6 +119,9 @@ class FullCoveragePathPlanner
   ros::Publisher plan_pub_;
   ros::ServiceClient cpp_grid_client_;
   nav_msgs::OccupancyGrid cpp_grid_;
+  // Using costmap instead of Occupancy Grid from map server as the costmap updates periodically.
+  costmap_2d::Costmap2DROS* costmap_ros_;
+  costmap_2d::Costmap2D* costmap_;
   float robot_radius_;
   float tool_radius_;
   float plan_resolution_;

src/common.cpp

@@ -328,7 +328,7 @@ bool validMove(int x2, int y2, int nCols, int nRows,
         && (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,
+void addNodeToList(int x2, int y2, std::list<gridNode_t>& pathNodes,
                    std::vector<std::vector<bool>>& visited) {
   Point_t new_point = { x2, y2 };
   gridNode_t new_node =
@@ -337,8 +337,52 @@ void addNodeToList(int x2, int y2, gridNode_t prev, std::list<gridNode_t>& pathN
     0,          // Cost
     0,          // Heuristic
   };
-  prev = pathNodes.back();
   pathNodes.push_back(new_node);
   visited[y2][x2] = eNodeVisited;  // Close node
+  return;
 }
 
+int dirWithMostSpace(int x_init, int y_init, int nCols, int nRows,
+                     std::vector<std::vector<bool> > const& grid,
+                     std::vector<std::vector<bool> > const& visited,
+                     int ignoreDir) {
+  // this array stores how far the robot can travel in a straight line for each direction
+  int free_space_in_dir[5] = {0};
+  // for each direction
+  for (int i = 1; i < 5; i++) {
+    // start from starting pos
+    int x2 = x_init;
+    int y2 = y_init;
+    do {  // loop until hit wall
+      switch (i) {
+        case east:
+          x2++;
+          break;
+        case west:
+          x2--;
+          break;
+        case north:
+          y2++;
+          break;
+        case south:
+          y2--;
+          break;
+        default:
+          break;
+      }
+      free_space_in_dir[i]++;
+    } while (validMove(x2, y2, nCols, nRows, grid, visited));
+  }
+
+  // set initial direction towards direction with most travel possible
+  int robot_dir = 0;
+  int indexValue = 0;
+  for (int i = 1; i <= 4; i++) {
+      // std::cout << "free space in " << i << ": " << free_space_in_dir[i] << std::endl;
+      if (free_space_in_dir[i] > indexValue && i != ignoreDir) {
+          robot_dir = i;
+          indexValue = free_space_in_dir[i];
+      }
+  }
+  return robot_dir;
+}

src/full_coverage_path_planner.cpp

@@ -185,6 +185,67 @@ void FullCoveragePathPlanner::parsePointlist2Plan(const geometry_msgs::PoseStamp
   ROS_INFO("Plan ready containing %lu goals!", plan.size());
 }
 
+bool FullCoveragePathPlanner::parseCostmap(costmap_2d::Costmap2D* costmap_grid_,
+                                           std::vector<std::vector<bool> >& grid,
+                                           float robotRadius,
+                                           float toolRadius,
+                                           geometry_msgs::PoseStamped const& realStart,
+                                           Point_t& scaledStart)
+{
+  int ix, iy, nodeRow, nodeCol;
+  uint32_t nodeSize = dmax(floor(toolRadius / costmap_grid_->getResolution()), 1);  // Size of node in pixels/units
+  uint32_t nRows = costmap_grid_->getSizeInCellsY(), nCols = costmap_grid_->getSizeInCellsX();
+  ROS_INFO("nRows: %u nCols: %u nodeSize: %d", nRows, nCols, nodeSize);
+
+  if (nRows == 0 || nCols == 0)
+  {
+    return false;
+  }
+
+  // Save map origin and scaling
+  tile_size_ = nodeSize * costmap_grid_->getResolution();  // Size of a tile in meters
+  grid_origin_.x = costmap_grid_->getOriginX();  // x-origin in meters
+  grid_origin_.y = costmap_grid_->getOriginY();  // y-origin in meters
+  ROS_INFO("costmap resolution: %g", costmap_grid_->getResolution());
+  ROS_INFO("tile size: %g", tile_size_);
+  ROS_INFO("grid origin: (%g, %g)", grid_origin_.x, grid_origin_.y);
+
+  // Scale starting point
+  scaledStart.x = static_cast<unsigned int>(clamp((realStart.pose.position.x - grid_origin_.x) / tile_size_, 0.0,
+                             floor(nCols / tile_size_)));
+  scaledStart.y = static_cast<unsigned int>(clamp((realStart.pose.position.y - grid_origin_.y) / tile_size_, 0.0,
+                             floor(nRows / tile_size_)));
+  ROS_INFO("real start: (%g, %g)", realStart.pose.position.x, realStart.pose.position.y);
+  ROS_INFO("scaled start: (%u, %u)", scaledStart.x, scaledStart.y);
+
+  // Scale grid
+  for (iy = 0; iy < nRows; iy = iy + nodeSize)
+  {
+    std::vector<bool> gridRow;
+    for (ix = 0; ix < nCols; ix = ix + nodeSize)
+    {
+      bool nodeOccupied = false;
+      for (nodeRow = 0; (nodeRow < nodeSize) && ((iy + nodeRow) < nRows) && (nodeOccupied == false); ++nodeRow)
+      {
+        for (nodeCol = 0; (nodeCol < nodeSize) && ((ix + nodeCol) < nCols); ++nodeCol)
+        {
+          double mx = grid_origin_.x + ix + nodeCol;
+          double my = grid_origin_.y + iy + nodeRow;
+          if (costmap_grid_->getCost(mx, my) > costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+          {
+            nodeOccupied = true;
+            ROS_INFO("(%f, %f) marked occupied", mx, my);
+            break;
+          }
+        }
+      }
+      gridRow.push_back(nodeOccupied);
+    }
+    grid.push_back(gridRow);
+  }
+  return true;
+}
+
 bool FullCoveragePathPlanner::parseGrid(nav_msgs::OccupancyGrid const& cpp_grid_,
                                         std::vector<std::vector<bool> >& grid,
                                         float robotRadius,
@@ -207,12 +268,17 @@ bool FullCoveragePathPlanner::parseGrid(nav_msgs::OccupancyGrid const& cpp_grid_
   tile_size_ = nodeSize * cpp_grid_.info.resolution;  // Size of a tile in meters
   grid_origin_.x = cpp_grid_.info.origin.position.x;  // x-origin in meters
   grid_origin_.y = cpp_grid_.info.origin.position.y;  // y-origin in meters
+  ROS_INFO("costmap resolution: %g", cpp_grid_.info.resolution);
+  ROS_INFO("tile size: %g", tile_size_);
+  ROS_INFO("grid origin: (%g, %g)", grid_origin_.x, grid_origin_.y);
 
   // Scale starting point
   scaledStart.x = static_cast<unsigned int>(clamp((realStart.pose.position.x - grid_origin_.x) / tile_size_, 0.0,
                              floor(cpp_grid_.info.width / tile_size_)));
   scaledStart.y = static_cast<unsigned int>(clamp((realStart.pose.position.y - grid_origin_.y) / tile_size_, 0.0,
                              floor(cpp_grid_.info.height / tile_size_)));
+  ROS_INFO("real start: (%g, %g)", realStart.pose.position.x, realStart.pose.position.y);
+  ROS_INFO("scaled start: (%u, %u)", scaledStart.x, scaledStart.y);
 
   // Scale grid
   for (iy = 0; iy < nRows; iy = iy + nodeSize)