map_inflator.cpp
Go to the documentation of this file.
00001 
00038 #include <bwi_mapper/map_inflator.h>
00039 
00040 namespace bwi_mapper {
00041   
00046   void inflateMap(double threshold, const nav_msgs::OccupancyGrid& map, 
00047       nav_msgs::OccupancyGrid& inflated_map) {
00048 
00049     inflated_map.header = map.header;
00050     inflated_map.info = map.info;
00051 
00052     // expand the map out based on the circumscribed robot distance
00053     int expand_pixels = ceil(threshold / map.info.resolution);
00054     inflated_map.data.resize(map.info.height * map.info.width);
00055     for (int i = 0; i < (int)map.info.height; ++i) {
00056       for (int j = 0; j < (int)map.info.width; ++j) {
00057         int low_i = (i - expand_pixels < 0) ? 0 : i - expand_pixels;
00058         int high_i = (i + expand_pixels >= (int)map.info.height) ? 
00059           map.info.height - 1 : i + expand_pixels;
00060         int max = 0;
00061         for (int k = low_i; k <= high_i; ++k) {
00062           int diff_j = floor(sqrtf(expand_pixels * expand_pixels - (i - k) * (i - k)));
00063           int low_j = (j - diff_j < 0) ? 0 : j - diff_j;
00064           int high_j = (j + diff_j >= (int)map.info.width) ? 
00065             map.info.width - 1 : j + diff_j;
00066           for (int l = low_j; l <= high_j; ++l) {
00067             if (map.data[k * map.info.width + l] > max) {
00068               max = map.data[k * map.info.width + l];
00069             }
00070           }
00071         }
00072         inflated_map.data[i * map.info.width + j] = max;
00073       }
00074     }
00075   }
00076 
00077 } /* bwi_mapper */


bwi_mapper
Author(s): Piyush Khandelwal
autogenerated on Thu Jun 6 2019 17:57:21