map_utils.cpp
Go to the documentation of this file.
00001 
00038 #include <bwi_mapper/map_utils.h>
00039 
00040 namespace bwi_mapper {
00041 
00042   bool locationsInDirectLineOfSight(const Point2f& pt1, const Point2f& pt2,
00043       const nav_msgs::OccupancyGrid map) {
00044 
00045     int x0 = lrint(pt1.x), y0 = lrint(pt1.y);
00046     int x1 = lrint(pt2.x), y1 = lrint(pt2.y);
00047     int dx = abs(x1-x0), dy = abs(y1-y0);
00048     int sx = (x0 < x1) ? 1 : -1;
00049     int sy = (y0 < y1) ? 1 : -1;
00050     float err = dx - dy;
00051     bool is_occupied = false;
00052     while (!is_occupied) {
00053       is_occupied = map.data[MAP_IDX(map.info.width, x0, y0)] > 50;
00054       if (x0 == x1 && y0 == y1) break;
00055       float e2 = 2 * err;
00056       if (e2 > -dy) {
00057         err -= dy;
00058         x0 += sx;
00059       }
00060       if (x0 == x1 && y0 == y1) {
00061         is_occupied = map.data[MAP_IDX(map.info.width, x0, y0)] > 50;
00062         break;
00063       }
00064       if (e2 < dx) {
00065         err += dx;
00066         y0 += sy;
00067       }
00068     }
00069     return !is_occupied;
00070   }
00071   
00072   Point2f toGrid(const Point2f& pt, const nav_msgs::MapMetaData& info) {
00073     return (pt - Point2f(info.origin.position.x, info.origin.position.y)) * 
00074         (1.0 / info.resolution);
00075   }
00076 
00077   Point2f toMap(const Point2f& pt, const nav_msgs::MapMetaData& info) {
00078     return Point2f(info.origin.position.x, info.origin.position.y) + 
00079         info.resolution * pt;
00080   }
00081 
00082 } /* bwi_mapper */


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