costmap_tools.h
Go to the documentation of this file.
00001 #ifndef COSTMAP_TOOLS_H_
00002 #define COSTMAP_TOOLS_H_
00003 
00004 #include <geometry_msgs/PolygonStamped.h>
00005 #include <geometry_msgs/PointStamped.h>
00006 #include <costmap_2d/costmap_2d.h>
00007 #include <boost/foreach.hpp>
00008 #include <ros/ros.h>
00009 
00010 namespace frontier_exploration{
00011 
00018 std::vector<unsigned int> nhood4(unsigned int idx, const costmap_2d::Costmap2D& costmap){
00019     //get 4-connected neighbourhood indexes, check for edge of map
00020     std::vector<unsigned int> out;
00021 
00022     unsigned int size_x_ = costmap.getSizeInCellsX(), size_y_ = costmap.getSizeInCellsY();
00023 
00024     if (idx > size_x_ * size_y_ -1){
00025         ROS_WARN("Evaluating nhood for offmap point");
00026         return out;
00027     }
00028 
00029     if(idx % size_x_ > 0){
00030         out.push_back(idx - 1);
00031     }
00032     if(idx % size_x_ < size_x_ - 1){
00033         out.push_back(idx + 1);
00034     }
00035     if(idx >= size_x_){
00036         out.push_back(idx - size_x_);
00037     }
00038     if(idx < size_x_*(size_y_-1)){
00039         out.push_back(idx + size_x_);
00040     }
00041     return out;
00042 
00043 }
00044 
00051 std::vector<unsigned int> nhood8(unsigned int idx, const costmap_2d::Costmap2D& costmap){
00052     //get 8-connected neighbourhood indexes, check for edge of map
00053     std::vector<unsigned int> out = nhood4(idx, costmap);
00054 
00055     unsigned int size_x_ = costmap.getSizeInCellsX(), size_y_ = costmap.getSizeInCellsY();
00056 
00057     if (idx > size_x_ * size_y_ -1){
00058         return out;
00059     }
00060 
00061     if(idx % size_x_ > 0 && idx >= size_x_){
00062         out.push_back(idx - 1 - size_x_);
00063     }
00064     if(idx % size_x_ > 0 && idx < size_x_*(size_y_-1)){
00065         out.push_back(idx - 1 + size_x_);
00066     }
00067     if(idx % size_x_ < size_x_ - 1 && idx >= size_x_){
00068         out.push_back(idx + 1 - size_x_);
00069     }
00070     if(idx % size_x_ < size_x_ - 1 && idx < size_x_*(size_y_-1)){
00071         out.push_back(idx + 1 + size_x_);
00072     }
00073 
00074     return out;
00075 
00076 }
00077 
00086 bool nearestCell(unsigned int &result, unsigned int start, unsigned char val, const costmap_2d::Costmap2D& costmap){
00087 
00088     const unsigned char* map = costmap.getCharMap();
00089     const unsigned int size_x = costmap.getSizeInCellsX(), size_y = costmap.getSizeInCellsY();
00090 
00091     if(start >= size_x * size_y){
00092         return false;
00093     }
00094 
00095     //initialize breadth first search
00096     std::queue<unsigned int> bfs;
00097     std::vector<bool> visited_flag(size_x * size_y, false);
00098 
00099     //push initial cell
00100     bfs.push(start);
00101     visited_flag[start] = true;
00102 
00103     //search for neighbouring cell matching value
00104     while(!bfs.empty()){
00105         unsigned int idx = bfs.front();
00106         bfs.pop();
00107 
00108         //return if cell of correct value is found
00109         if(map[idx] == val){
00110             result = idx;
00111             return true;
00112         }
00113 
00114         //iterate over all adjacent unvisited cells
00115         BOOST_FOREACH(unsigned nbr, nhood8(idx, costmap)){
00116             if(!visited_flag[nbr]){
00117                 bfs.push(nbr);
00118                 visited_flag[nbr] = true;
00119             }
00120         }
00121     }
00122 
00123     return false;
00124 }
00125 
00126 }
00127 #endif


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