Skip to content

Commit

Permalink
Greatly improved pathfinding by obstacle dilating
Browse files Browse the repository at this point in the history
  • Loading branch information
v01d committed Jan 9, 2012
1 parent 708dbc5 commit 89b16ac
Show file tree
Hide file tree
Showing 7 changed files with 65 additions and 24 deletions.
6 changes: 6 additions & 0 deletions exabot.cpp
View file
Expand Up @@ -40,8 +40,12 @@ ExaBot::ExaBot(void) : Singleton<ExaBot>(this), player_client("localhost"), lase
start_timer = std::time(NULL);
first_plot = false;

#ifdef ENABLE_PLOTS
cvStartWindowThread();
cv::namedWindow("grid");
cv::namedWindow("debug");

#endif

sleep(1);
}
Expand Down Expand Up @@ -107,6 +111,8 @@ void ExaBot::update(void) {
cv::Mat graph_big;
cv::resize(graph, graph_big, cv::Size(0,0), 4, 4, cv::INTER_NEAREST);
cv::imshow("grid", graph_big);

cv::imshow("debug", LocalExplorer::instance()->frontier_pathfinder.grid);
#endif
}

Expand Down
23 changes: 13 additions & 10 deletions exploration.geany
View file
Expand Up @@ -17,25 +17,28 @@ long_line_behaviour=1
long_line_column=120

[files]
current_page=9
FILE_NAME_0=3445;C++;0;16;0;1;1;/home/v01d/coding/exploration/exabot.cpp;0;2
FILE_NAME_1=4277;C++;0;16;0;1;1;/home/v01d/coding/exploration/metric_map.cpp;0;2
current_page=0
FILE_NAME_0=3369;C++;0;16;0;1;1;/home/v01d/coding/exploration/exabot.cpp;0;2
FILE_NAME_1=6581;C++;0;16;0;1;1;/home/v01d/coding/exploration/metric_map.cpp;0;2
FILE_NAME_2=836;C;0;16;0;1;1;/home/v01d/coding/exploration/occupancy_grid.h;0;2
FILE_NAME_3=1471;C;0;16;0;1;1;/home/v01d/coding/exploration/motion_planner.h;0;2
FILE_NAME_4=1416;C++;0;16;0;1;1;/home/v01d/coding/exploration/motion_planner.cpp;0;2
FILE_NAME_5=13416;C++;0;16;0;1;1;/home/v01d/coding/exploration/explorer.cpp;0;2
FILE_NAME_5=6798;C++;0;16;0;1;1;/home/v01d/coding/exploration/explorer.cpp;0;2
FILE_NAME_6=295;C;0;16;0;1;1;/home/v01d/coding/exploration/explorer.h;0;2
FILE_NAME_7=480;C;0;16;0;1;1;/home/v01d/coding/exploration/metric_map.h;0;2
FILE_NAME_8=2031;C++;0;16;0;1;1;/home/v01d/coding/exploration/occupancy_grid_frontiers.cpp;0;2
FILE_NAME_9=6715;C++;0;16;0;1;1;/home/v01d/coding/exploration/local_explorer.cpp;0;2
FILE_NAME_9=3204;C++;0;16;0;1;1;/home/v01d/coding/exploration/local_explorer.cpp;0;2
FILE_NAME_10=19;C++;0;16;0;1;1;/home/v01d/coding/exploration/main.cpp;0;2
FILE_NAME_11=0;CMake;0;16;0;1;1;/home/v01d/coding/exploration/CMakeLists.txt;0;2
FILE_NAME_12=705;C;0;16;0;1;1;/home/v01d/coding/exploration/exabot.h;0;2
FILE_NAME_13=0;C;0;16;0;1;1;/home/v01d/coding/exploration/local_explorer.h;0;2
FILE_NAME_14=742;None;0;16;0;1;1;/home/v01d/coding/exploration/TODO_BUGS;0;2
FILE_NAME_15=0;C++;0;16;0;1;1;/home/v01d/coding/exploration/super_matrix.cpp;0;2
FILE_NAME_16=0;C;0;16;0;1;1;/home/v01d/coding/exploration/graph.h;0;2
FILE_NAME_17=0;C++;0;16;0;1;1;/home/v01d/coding/exploration/occupancy_grid.cpp;0;2
FILE_NAME_13=742;None;0;16;0;1;1;/home/v01d/coding/exploration/TODO_BUGS;0;2
FILE_NAME_14=0;C++;0;16;0;1;1;/home/v01d/coding/exploration/super_matrix.cpp;0;2
FILE_NAME_15=0;C;0;16;0;1;1;/home/v01d/coding/exploration/graph.h;0;2
FILE_NAME_16=911;C++;0;16;0;1;1;/home/v01d/coding/exploration/pathfinder.cpp;0;2
FILE_NAME_17=0;C;0;16;0;1;1;/home/v01d/coding/exploration/pathfinder.h;0;2
FILE_NAME_18=581;C;0;16;0;1;1;/home/v01d/coding/exploration/local_explorer.h;0;2
FILE_NAME_19=0;C++;0;16;0;1;1;/home/v01d/coding/exploration/global_explorer.cpp;0;2
FILE_NAME_20=0;None;0;16;0;1;1;/home/v01d/coding/exploration/exploration.geany;0;2

[VTE]
last_dir=/home/v01d
6 changes: 3 additions & 3 deletions explorer.cpp
View file
Expand Up @@ -140,13 +140,13 @@ void Explorer::update(void) {
if (!follow_path.empty()) {
// invalid path or unnecessary path
bool valid = true;
for (list<gsl::vector_int>::iterator it = follow_path.begin(); it != follow_path.end(); ++it) {
if (OccupancyGrid::valid_coordinates((*it)(0),(*it)(1)) && (*current_grid)((*it)(0), (*it)(1)) >= OccupancyGrid::Locc) {
/*for (list<gsl::vector_int>::iterator it = follow_path.begin(); it != follow_path.end(); ++it) {
if (OccupancyGrid::valid_coordinates((*it)(0),(*it)(1)) && LocalExplorer::instance()->get_occupancy((*it)(0), (*it)(1)) == 0) {
cout << "path crosses obstacle" << endl;
valid = false;
break;
}
}
}*/

// TODO: this does not work correctly. Frontiers should be re-detected periodically and this would use LocalExplorer's target_is_frontier()
// to check validity of the current target
Expand Down
42 changes: 32 additions & 10 deletions local_explorer.cpp
View file
Expand Up @@ -5,29 +5,29 @@ using namespace std;
const double wall_safety_radius = 0.4;

LocalPathfinder::LocalPathfinder(double v) : frontier_value_condition(v) {
grid.create(OccupancyGrid::CELLS, OccupancyGrid::CELLS, CV_8UC1);
}

list<gsl::vector_int> LocalPathfinder::neighbors(const gsl::vector_int& v, const gsl::vector_int& previous) {
OccupancyGrid& grid = *MetricMap::instance()->current_grid;
list<gsl::vector_int> neighbors;
gsl::vector_int n(2);
if ((uint)v(1) < OccupancyGrid::CELLS - 1 && grid(v[0],v[1]+1) < frontier_value_condition) { n = v; n(1) += 1; neighbors.push_back(n); } // up
if ((uint)v(0) > 0 && grid(v[0]-1,v[1]) < frontier_value_condition) { n = v; n(0) -= 1; neighbors.push_back(n); } // right
if ((uint)v(0) < OccupancyGrid::CELLS - 1 && grid(v[0]+1,v[1]) < frontier_value_condition) { n = v; n(0) += 1; neighbors.push_back(n); } // left
if ((uint)v(1) > 0 && grid(v[0],v[1]-1) < frontier_value_condition) { n = v; n(1) -= 1; neighbors.push_back(n); } // down
if ((uint)v(1) < OccupancyGrid::CELLS - 1 && get_occupancy(v[0],v[1]+1) == 255) { n = v; n(1) += 1; neighbors.push_back(n); } // up
if ((uint)v(0) > 0 && get_occupancy(v[0]-1,v[1]) == 255) { n = v; n(0) -= 1; neighbors.push_back(n); } // right
if ((uint)v(0) < OccupancyGrid::CELLS - 1 && get_occupancy(v[0]+1,v[1]) == 255) { n = v; n(0) += 1; neighbors.push_back(n); } // left
if ((uint)v(1) > 0 && get_occupancy(v[0],v[1]-1) == 255) { n = v; n(1) -= 1; neighbors.push_back(n); } // down
//cout << "neighbors of: " << v(0) << "," << v(1);
//LocalExplorer::instance()->print_path(neighbors);
//cout << endl;
return neighbors;
}

unsigned long LocalPathfinder::movement_cost(const gsl::vector_int& from, const gsl::vector_int& to, const gsl::vector_int& previous) {
OccupancyGrid& grid = *MetricMap::instance()->current_grid;
//OccupancyGrid& grid = *MetricMap::instance()->current_grid;
unsigned long cost = 1;

int safety_radius_cells = ceil(wall_safety_radius / OccupancyGrid::CELL_SIZE);

for (int i = -safety_radius_cells; i <= safety_radius_cells; i++) {
/*for (int i = -safety_radius_cells; i <= safety_radius_cells; i++) {
for (int j = -safety_radius_cells; j <= safety_radius_cells; j++) {
if ((i == 0) && (j == 0)) continue;
int x = to(0) + i;
Expand All @@ -38,7 +38,7 @@ unsigned long LocalPathfinder::movement_cost(const gsl::vector_int& from, const
else if ((uint)x == 0 || (uint)y == 0 || (uint)x == (OccupancyGrid::CELLS - 1) || (uint)y == (OccupancyGrid::CELLS - 1)) cost += 5;
}
}
}
}*/

// if there's a previous node in the path
if (previous != from) {
Expand All @@ -52,6 +52,28 @@ unsigned long LocalPathfinder::movement_cost(const gsl::vector_int& from, const
return cost;
}

void LocalPathfinder::prepare(void) {
process_current_grid();
}

void LocalPathfinder::process_current_grid(void) {
OccupancyGrid& current_grid = *MetricMap::instance()->current_grid;
grid = 255;

for (uint i = 0; i < OccupancyGrid::CELLS; i++) {
for (uint j = 0; j < OccupancyGrid::CELLS; j++) {
if (current_grid(i,j) >= frontier_value_condition)
cv::circle(grid, cv::Point(i,OccupancyGrid::CELLS - j - 1), MetricMap::ROBOT_RADIUS / OccupancyGrid::CELL_SIZE, 0, -1, 4);
}
}
}

uchar LocalPathfinder::get_occupancy(uint i, uint j) {
return grid.at<uchar>(OccupancyGrid::CELLS - j - 1,i);
}



ConnectivityPathfinder::ConnectivityPathfinder(void) : LocalPathfinder(0), x_range(2), y_range(2) { }

bool ConnectivityPathfinder::is_goal(const gsl::vector_int& pos) {
Expand Down Expand Up @@ -79,6 +101,7 @@ bool LocalExplorer::target_is_frontier(void) {
return (follow_path.size() > 0 && frontier_pathfinder.is_goal(follow_path.back()));
}


struct DistanceCost : public binary_function<list<gsl::vector_int>,list<gsl::vector_int>,bool> {
const gsl::vector_int& last_target;
DistanceCost(const gsl::vector_int& _last_target) : last_target(_last_target) { }
Expand Down Expand Up @@ -143,7 +166,7 @@ std::ostream& operator<<(std::ostream& out, const std::list<gsl::vector_int>& l)
void LocalExplorer::compute_frontier_paths(void) {
MetricMap::instance()->current_grid->update_frontiers();
if (MetricMap::instance()->current_grid->frontiers.empty()) { clear_paths(); found = false; }
else {
else {
gsl::vector_int grid_position = MetricMap::instance()->grid_position();
all_paths = frontier_pathfinder.findpath(grid_position, false);
if (!all_paths.empty()) { found = true; sort_paths(); follow_next_path(); }
Expand All @@ -154,7 +177,6 @@ void LocalExplorer::compute_frontier_paths(void) {
void LocalExplorer::compute_gateway_path(TopoMap::GatewayNode* gateway, bool follow) {
cout << "Looking for paths to gateway" << endl;
gsl::vector_int start = MetricMap::instance()->grid_position();

all_paths = connectivity_pathfinder.findpath(start, gateway->position(), true);
//gateway->get_ranges(connectivity_pathfinder.x_range, connectivity_pathfinder.y_range);
//all_paths = connectivity_pathfinder.findpath(start, true);
Expand Down
6 changes: 6 additions & 0 deletions local_explorer.h
View file
Expand Up @@ -13,9 +13,15 @@ namespace HybNav {
virtual ~LocalPathfinder(void) { }
std::list<gsl::vector_int> neighbors(const gsl::vector_int& current, const gsl::vector_int& previous);
unsigned long movement_cost(const gsl::vector_int& from, const gsl::vector_int& to, const gsl::vector_int& previous) ;
void prepare(void);

void process_current_grid(void);
uchar get_occupancy(uint i, uint j);
cv::Mat grid;

protected:
double frontier_value_condition;

};

class FrontierPathfinder : public LocalPathfinder {
Expand Down
4 changes: 3 additions & 1 deletion pathfinder.cpp
View file
Expand Up @@ -30,6 +30,8 @@ bool Pathfinder<T>::exists_path(const T& start, const T& goal) {
template<class T>
list< list<T> > Pathfinder<T>::findpath(const T& start, const T& goal, bool use_goal_function, bool first_solution)
{
prepare();

map<T,T> predecessors;
set<T> visited;
map<T,unsigned long> g;
Expand Down Expand Up @@ -100,4 +102,4 @@ template class Pathfinder<gsl::vector_int>;

namespace gsl {
bool operator<(const vector_int& a, const vector_int& b) { return (a(0) < b(0) || (a(0) == b(0) && a(1) < b(1))); }
}
}
2 changes: 2 additions & 0 deletions pathfinder.h
View file
Expand Up @@ -13,8 +13,10 @@ namespace HybNav {
virtual unsigned long distance_heuristic(const T& target) { return 0; }
virtual bool is_goal(const T& current) { return false; }
virtual ~Pathfinder(void) { }
virtual void prepare(void) { }

bool exists_path(const T& start, const T& goal);

std::list< std::list<T> > findpath(const T& start, const T& goal, bool first_solution = false);
std::list< std::list<T> > findpath(const T& start, bool first_solution = false);

Expand Down

0 comments on commit 89b16ac

Please sign in to comment.