test_dfs.cpp

Go to the documentation of this file.

#include <iostream>
#include <string>

#include <bwi_mapper/voronoi_approximator.h>

int main(int argc, char** argv) {

  nav_msgs::OccupancyGrid grid;
  grid.info.height = 5;
  grid.info.width = 5;
  grid.data.resize(25);
  for (int i = 0; i < 5; ++i) {
    for (int j = 0; j < 5; ++j) {
      grid.data[5*j + i] = 0;
    }
  }

  grid.data[5*1 + 1] = 100;
  grid.data[5*2 + 1] = 100;
  grid.data[5*3 + 1] = 100;

  grid.data[5*1 + 3] = 100;
  grid.data[5*2 + 3] = 100;
  grid.data[5*3 + 3] = 100;

  bwi_mapper::DirectedDFS dfs(grid);
  bwi_mapper::Point2d p1, p2;
  p1.x = 1, p1.y = 1;
  p2.x = 1, p2.y = 3;
  std::cout << dfs.searchForPath(p1, p2, 5) << std::endl; // 1
  p2.x = 3, p2.y = 3;
  std::cout << dfs.searchForPath(p1, p2, 5) << std::endl; // 0

  grid.data[5*4 + 2] = 100;
  std::cout << dfs.searchForPath(p1, p2, 4) << std::endl; // 1
  std::cout << dfs.searchForPath(p1, p2, 3) << std::endl; // 0

  return 0;
}