path_finder.cpp
Go to the documentation of this file.
00001 
00035 #include <bwi_mapper/map_utils.h>
00036 #include <bwi_mapper/path_finder.h>
00037 #include <fstream>
00038 
00039 namespace bwi_mapper {
00040 
00041   PathFinder::PathFinder(const nav_msgs::OccupancyGrid& map, const Point2d& start_pt) : width_(map.info.width) {
00042 
00043     search_space_.resize(map.info.height * map.info.width, NOT_CONNECTED);
00044 
00045     // Mark all the obstacles.
00046     for (int row = 0; row < map.info.height; ++row) {
00047       for (int col = 0; col < map.info.width; ++col) {
00048         int map_idx = MAP_IDX(width_, col, row);
00049         if (map.data[map_idx] == 100) {
00050           search_space_[map_idx] = OBSTACLE;
00051         }
00052       }
00053     }
00054 
00055     // Lets setup the initial condition.
00056     int start_idx = MAP_IDX(width_, start_pt.x, start_pt.y);
00057     if (search_space_[start_idx] != OBSTACLE) {
00058       search_space_[start_idx] = 0;
00059     } else {
00060       // The start point is in an obstacles. Nothing to see here. Move along!
00061       return;
00062     }
00063 
00064     std::set<int> current_points;
00065     current_points.insert(start_idx);
00066 
00067     while(!current_points.empty()) {
00068       int current_idx = *(current_points.begin());
00069       current_points.erase(current_points.begin());
00070       int row = current_idx / width_;
00071       int col = current_idx % width_;
00072 
00073       int step_x[] = {0, 1, 0, -1};
00074       int step_y[] = {1, 0, -1, 0};
00075 
00076       for (int pt = 0; pt < 4; ++pt) {
00077         int neighbor_x = col + step_x[pt]; 
00078         int neighbor_y = row + step_y[pt]; 
00079         if (neighbor_x >= 0 && neighbor_x < map.info.width && neighbor_y >= 0 && neighbor_y < map.info.height) {
00080           int neighbor_idx = MAP_IDX(width_, neighbor_x, neighbor_y);
00081           if ((search_space_[neighbor_idx] != OBSTACLE) && 
00082               (search_space_[neighbor_idx] == NOT_CONNECTED || (search_space_[neighbor_idx] > search_space_[current_idx] + 1))) {
00083             search_space_[neighbor_idx] = search_space_[current_idx] + 1; 
00084             current_points.insert(current_points.end(), neighbor_idx);
00085           }
00086         }
00087       }
00088     }
00089 
00090     // std::ofstream fout("/home/piyushk/test.txt");
00091     // for (int row = 0; row < map.info.height; ++row) {
00092     //   for (int col = 0; col < map.info.width; ++col) {
00093     //     int map_idx = MAP_IDX(width_, col, row);
00094     //     fout << std::setfill(' ') << std::setw(5) << search_space_[map_idx];
00095     //   }
00096     //   fout << std::endl;
00097     // }
00098     // fout.close();
00099     // throw std::runtime_error("blah!");
00100 
00101   }
00102       
00103   bool PathFinder::pathExists(const Point2d& pt) {
00104     return search_space_[MAP_IDX(width_, pt.x, pt.y)] >= 0;
00105   }
00106 
00107   int PathFinder::getManhattanDistance(const Point2d& pt) {
00108     return search_space_[MAP_IDX(width_, pt.x, pt.y)];
00109   }
00110 
00111 } /* bwi_mapper */


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