directed_dfs.cpp
Go to the documentation of this file.
00001 
00038 #include <bwi_mapper/directed_dfs.h>
00039 #include <bwi_mapper/map_utils.h>
00040 
00041 namespace bwi_mapper {
00042 
00046   bool DirectedDFS::searchForPath(const Point2d &start, const Point2d &goal,
00047       uint32_t depth, bool in_obstacle_space) {
00048 
00049     std::vector<bool> visited(map_.info.height * map_.info.width, false);
00050     return searchForPath(start, goal, depth, visited, in_obstacle_space);
00051   }
00052 
00056   bool DirectedDFS::searchForPath(const Point2d &start, const Point2d &goal, 
00057       uint32_t depth, std::vector<bool> &visited, bool in_obstacle_space) {
00058 
00059     //std::cout << start.x << " " << start.y << std::endl;
00060 
00061     // Termination crit
00062     if (start == goal) {
00063       return true;
00064     }
00065     if (depth == 0) {
00066       return false;
00067     }
00068 
00069     uint32_t start_idx = MAP_IDX(map_.info.width, start.x, start.y);
00070     visited[start_idx] = true;
00071 
00072     std::vector<Point2d> neighbours;
00073     getOrderedNeighbours(start, goal, visited, neighbours, in_obstacle_space);
00074     for (size_t i = 0; i < neighbours.size(); ++i) {
00075       Point2d& n = neighbours[i];
00076       // Check if it has been visited again - quite likely that one of the 
00077       // previous loop iterations have covered this already
00078       uint32_t n_idx = MAP_IDX(map_.info.width, n.x, n.y);
00079       if (visited[n_idx]) {
00080         continue;
00081       }
00082       bool success = searchForPath(n, goal, depth - 1, visited);
00083       if (success)
00084         return true;
00085     }
00086 
00087     return false; // disconnected components
00088   }
00089 
00090 
00095   void DirectedDFS::getOrderedNeighbours(const Point2d &from, 
00096       const Point2d &goal, const std::vector<bool> &visited, 
00097       std::vector<Point2d> &neighbours, bool in_obstacle_space) {
00098 
00099     size_t neighbour_count = 8;
00100     uint32_t x_offset[] = {-1, 0, 1, -1, 1, -1, 0, 1};
00101     uint32_t y_offset[] = {-1, -1, -1, 0, 0, 1, 1, 1};
00102     neighbours.clear();
00103     for (size_t i = 0; i < neighbour_count; ++i) {
00104       // Check if neighbours are still on map
00105       Point2d p = from + Point2d(x_offset[i],y_offset[i]);
00106       // covers negative case as well (unsigned)
00107       if (p.x >= (int) map_.info.width || p.y >= (int) map_.info.height ||
00108           p.x <= 0 || p.y <= 0) { 
00109         continue;
00110       }
00111       uint32_t map_idx = MAP_IDX(map_.info.width, p.x, p.y);
00112       if (visited[map_idx] || (in_obstacle_space && map_.data[map_idx] == 0) ||
00113           (!in_obstacle_space && map_.data[map_idx] != 0)) {
00114         continue;
00115       }
00116       p.distance_from_ref = norm(p - goal); 
00117       neighbours.push_back(p);
00118     }
00119     std::sort(neighbours.begin(), neighbours.end(), Point2dDistanceComp());
00120   }
00121 
00122 } /* bwi_mapper */


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