Use costmap instead of static map

Author: ethanckim

src/full_coverage_path_planner.cpp

@@ -229,12 +229,12 @@ bool FullCoveragePathPlanner::parseCostmap(costmap_2d::Costmap2D* costmap_grid_,
       {
         for (nodeCol = 0; (nodeCol < nodeSize) && ((ix + nodeCol) < nCols); ++nodeCol)
         {
-          double mx = grid_origin_.x + ix + nodeCol;
-          double my = grid_origin_.y + iy + nodeRow;
+          double mx = ix + nodeCol;
+          double my = iy + nodeRow;
           if (costmap_grid_->getCost(mx, my) > costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
           {
             nodeOccupied = true;
-            ROS_INFO("(%f, %f) marked occupied", mx, my);
+            // ROS_INFO("(%f, %f) marked occupied", mx, my);
             break;
           }
         }

src/spiral_stc.cpp

@@ -191,7 +191,7 @@ std::list<gridNode_t> SpiralSTC::spiral(std::vector<std::vector<bool> > const& g
     }
   }
   // Log
-  printPathNodes(pathNodes);
+  // printPathNodes(pathNodes);
   return pathNodes;
 }
 
@@ -313,9 +313,8 @@ bool SpiralSTC::makePlan(const geometry_msgs::PoseStamped& start, const geometry
   }
   ROS_INFO("grid recieved!!");
 
-//parseCostmap(costmap_, grid, robot_radius_ * 2, tool_radius_ * 2, start, startPoint)
   ROS_INFO("Parsing grid to internal representation...");
-  if (!parseGrid(grid_req_srv.response.map, grid, robot_radius_ * 2, tool_radius_ * 2, start, startPoint))
+  if (!parseCostmap(costmap_, grid, robot_radius_ * 2, tool_radius_ * 2, start, startPoint))
   {
     ROS_ERROR("Could not parse retrieved grid");
     return false;