frontier_search.cpp
Go to the documentation of this file.
00001 #include <frontier_exploration/frontier_search.h>
00002 
00003 #include <costmap_2d/costmap_2d.h>
00004 #include<costmap_2d/cost_values.h>
00005 #include <geometry_msgs/Point.h>
00006 #include <boost/foreach.hpp>
00007 
00008 #include <frontier_exploration/costmap_tools.h>
00009 #include <frontier_exploration/Frontier.h>
00010 
00011 namespace frontier_exploration{
00012 
00013 using costmap_2d::LETHAL_OBSTACLE;
00014 using costmap_2d::NO_INFORMATION;
00015 using costmap_2d::FREE_SPACE;
00016 
00017 FrontierSearch::FrontierSearch(costmap_2d::Costmap2D &costmap) : costmap_(costmap) { }
00018 
00019 std::list<Frontier> FrontierSearch::searchFrom(geometry_msgs::Point position){
00020 
00021     std::list<Frontier> frontier_list;
00022 
00023     //Sanity check that robot is inside costmap bounds before searching
00024     unsigned int mx,my;
00025     if (!costmap_.worldToMap(position.x,position.y,mx,my)){
00026         ROS_ERROR("Robot out of costmap bounds, cannot search for frontiers");
00027         return frontier_list;
00028     }
00029 
00030     //make sure map is consistent and locked for duration of search
00031     boost::unique_lock < boost::shared_mutex > lock(*(costmap_.getLock()));
00032 
00033     map_ = costmap_.getCharMap();
00034     size_x_ = costmap_.getSizeInCellsX();
00035     size_y_ = costmap_.getSizeInCellsY();
00036 
00037     //initialize flag arrays to keep track of visited and frontier cells
00038     std::vector<bool> frontier_flag(size_x_ * size_y_, false);
00039     std::vector<bool> visited_flag(size_x_ * size_y_, false);
00040 
00041     //initialize breadth first search
00042     std::queue<unsigned int> bfs;
00043 
00044     //find closest clear cell to start search
00045     unsigned int clear, pos = costmap_.getIndex(mx,my);
00046     if(nearestCell(clear, pos, FREE_SPACE, costmap_)){
00047         bfs.push(clear);
00048     }else{
00049         bfs.push(pos);
00050         ROS_WARN("Could not find nearby clear cell to start search");
00051     }
00052     visited_flag[bfs.front()] = true;
00053 
00054     while(!bfs.empty()){
00055         unsigned int idx = bfs.front();
00056         bfs.pop();
00057 
00058         //iterate over 4-connected neighbourhood
00059         BOOST_FOREACH(unsigned nbr, nhood4(idx, costmap_)){
00060             //add to queue all free, unvisited cells, use descending search in case initialized on non-free cell
00061             if(map_[nbr] <= map_[idx] && !visited_flag[nbr]){
00062                 visited_flag[nbr] = true;
00063                 bfs.push(nbr);
00064                 //check if cell is new frontier cell (unvisited, NO_INFORMATION, free neighbour)
00065             }else if(isNewFrontierCell(nbr, frontier_flag)){
00066                 frontier_flag[nbr] = true;
00067                 Frontier new_frontier = buildNewFrontier(nbr, pos, frontier_flag);
00068                 if(new_frontier.size > 1){
00069                     frontier_list.push_back(new_frontier);
00070                 }
00071             }
00072         }
00073     }
00074 
00075     return frontier_list;
00076 
00077 }
00078 
00079 Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell, unsigned int reference, std::vector<bool>& frontier_flag){
00080 
00081     //initialize frontier structure
00082     Frontier output;
00083     output.centroid.x = 0;
00084     output.centroid.y = 0;
00085     output.size = 1;
00086     output.min_distance = std::numeric_limits<double>::infinity();
00087 
00088     //record initial contact point for frontier
00089     unsigned int ix, iy;
00090     costmap_.indexToCells(initial_cell,ix,iy);
00091     costmap_.mapToWorld(ix,iy,output.initial.x,output.initial.y);
00092 
00093     //push initial gridcell onto queue
00094     std::queue<unsigned int> bfs;
00095     bfs.push(initial_cell);
00096 
00097     //cache reference position in world coords
00098     unsigned int rx,ry;
00099     double reference_x, reference_y;
00100     costmap_.indexToCells(reference,rx,ry);
00101     costmap_.mapToWorld(rx,ry,reference_x,reference_y);
00102 
00103     while(!bfs.empty()){
00104         unsigned int idx = bfs.front();
00105         bfs.pop();
00106 
00107         //try adding cells in 8-connected neighborhood to frontier
00108         BOOST_FOREACH(unsigned int nbr, nhood8(idx, costmap_)){
00109             //check if neighbour is a potential frontier cell
00110             if(isNewFrontierCell(nbr,frontier_flag)){
00111 
00112                 //mark cell as frontier
00113                 frontier_flag[nbr] = true;
00114                 unsigned int mx,my;
00115                 double wx,wy;
00116                 costmap_.indexToCells(nbr,mx,my);
00117                 costmap_.mapToWorld(mx,my,wx,wy);
00118 
00119                 //update frontier size
00120                 output.size++;
00121 
00122                 //update centroid of frontier
00123                 output.centroid.x += wx;
00124                 output.centroid.y += wy;
00125 
00126                 //determine frontier's distance from robot, going by closest gridcell to robot
00127                 double distance = sqrt(pow((double(reference_x)-double(wx)),2.0) + pow((double(reference_y)-double(wy)),2.0));
00128                 if(distance < output.min_distance){
00129                     output.min_distance = distance;
00130                     output.middle.x = wx;
00131                     output.middle.y = wy;
00132                 }
00133 
00134                 //add to queue for breadth first search
00135                 bfs.push(nbr);
00136             }
00137         }
00138     }
00139 
00140     //average out frontier centroid
00141     output.centroid.x /= output.size;
00142     output.centroid.y /= output.size;
00143     return output;
00144 }
00145 
00146 bool FrontierSearch::isNewFrontierCell(unsigned int idx, const std::vector<bool>& frontier_flag){
00147 
00148     //check that cell is unknown and not already marked as frontier
00149     if(map_[idx] != NO_INFORMATION || frontier_flag[idx]){
00150         return false;
00151     }
00152 
00153     //frontier cells should have at least one cell in 4-connected neighbourhood that is free
00154     BOOST_FOREACH(unsigned int nbr, nhood4(idx, costmap_)){
00155         if(map_[nbr] == FREE_SPACE){
00156             return true;
00157         }
00158     }
00159 
00160     return false;
00161 
00162 }
00163 
00164 }


frontier_exploration
Author(s): Paul Bovbel
autogenerated on Fri Aug 28 2015 10:43:32