path_finder.cpp

Go to the documentation of this file.

#include <bwi_mapper/map_utils.h>
#include <bwi_mapper/path_finder.h>
#include <fstream>

namespace bwi_mapper {

  PathFinder::PathFinder(const nav_msgs::OccupancyGrid& map, const Point2d& start_pt) : width_(map.info.width) {

    search_space_.resize(map.info.height * map.info.width, NOT_CONNECTED);

    // Mark all the obstacles.
    for (int row = 0; row < map.info.height; ++row) {
      for (int col = 0; col < map.info.width; ++col) {
        int map_idx = MAP_IDX(width_, col, row);
        if (map.data[map_idx] == 100) {
          search_space_[map_idx] = OBSTACLE;
        }
      }
    }

    // Lets setup the initial condition.
    int start_idx = MAP_IDX(width_, start_pt.x, start_pt.y);
    if (search_space_[start_idx] != OBSTACLE) {
      search_space_[start_idx] = 0;
    } else {
      // The start point is in an obstacles. Nothing to see here. Move along!
      return;
    }

    std::set<int> current_points;
    current_points.insert(start_idx);

    while(!current_points.empty()) {
      int current_idx = *(current_points.begin());
      current_points.erase(current_points.begin());
      int row = current_idx / width_;
      int col = current_idx % width_;

      int step_x[] = {0, 1, 0, -1};
      int step_y[] = {1, 0, -1, 0};

      for (int pt = 0; pt < 4; ++pt) {
        int neighbor_x = col + step_x[pt]; 
        int neighbor_y = row + step_y[pt]; 
        if (neighbor_x >= 0 && neighbor_x < map.info.width && neighbor_y >= 0 && neighbor_y < map.info.height) {
          int neighbor_idx = MAP_IDX(width_, neighbor_x, neighbor_y);
          if ((search_space_[neighbor_idx] != OBSTACLE) && 
              (search_space_[neighbor_idx] == NOT_CONNECTED || (search_space_[neighbor_idx] > search_space_[current_idx] + 1))) {
            search_space_[neighbor_idx] = search_space_[current_idx] + 1; 
            current_points.insert(current_points.end(), neighbor_idx);
          }
        }
      }
    }

    // std::ofstream fout("/home/piyushk/test.txt");
    // for (int row = 0; row < map.info.height; ++row) {
    //   for (int col = 0; col < map.info.width; ++col) {
    //     int map_idx = MAP_IDX(width_, col, row);
    //     fout << std::setfill(' ') << std::setw(5) << search_space_[map_idx];
    //   }
    //   fout << std::endl;
    // }
    // fout.close();
    // throw std::runtime_error("blah!");

  }
      
  bool PathFinder::pathExists(const Point2d& pt) {
    return search_space_[MAP_IDX(width_, pt.x, pt.y)] >= 0;
  }

  int PathFinder::getManhattanDistance(const Point2d& pt) {
    return search_space_[MAP_IDX(width_, pt.x, pt.y)];
  }

} /* bwi_mapper */