map_utils.cpp

Go to the documentation of this file.

#include <bwi_mapper/map_utils.h>

namespace bwi_mapper {

  bool locationsInDirectLineOfSight(const Point2f& pt1, const Point2f& pt2,
      const nav_msgs::OccupancyGrid map) {

    int x0 = lrint(pt1.x), y0 = lrint(pt1.y);
    int x1 = lrint(pt2.x), y1 = lrint(pt2.y);
    int dx = abs(x1-x0), dy = abs(y1-y0);
    int sx = (x0 < x1) ? 1 : -1;
    int sy = (y0 < y1) ? 1 : -1;
    float err = dx - dy;
    bool is_occupied = false;
    while (!is_occupied) {
      is_occupied = map.data[MAP_IDX(map.info.width, x0, y0)] > 50;
      if (x0 == x1 && y0 == y1) break;
      float e2 = 2 * err;
      if (e2 > -dy) {
        err -= dy;
        x0 += sx;
      }
      if (x0 == x1 && y0 == y1) {
        is_occupied = map.data[MAP_IDX(map.info.width, x0, y0)] > 50;
        break;
      }
      if (e2 < dx) {
        err += dx;
        y0 += sy;
      }
    }
    return !is_occupied;
  }
  
  Point2f toGrid(const Point2f& pt, const nav_msgs::MapMetaData& info) {
    return (pt - Point2f(info.origin.position.x, info.origin.position.y)) * 
        (1.0 / info.resolution);
  }

  Point2f toMap(const Point2f& pt, const nav_msgs::MapMetaData& info) {
    return Point2f(info.origin.position.x, info.origin.position.y) + 
        info.resolution * pt;
  }

} /* bwi_mapper */