Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

Greatly improved pathfinding by obstacle dilating

  • Loading branch information...
commit 89b16ac6d410bd5b39516af0d4e50ffc9b83c4d5 1 parent 708dbc5
@v01d v01d authored
View
6 exabot.cpp
@@ -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);
}
@@ -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
}
View
23 exploration.geany
@@ -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
View
6 explorer.cpp
@@ -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
View
42 local_explorer.cpp
@@ -5,16 +5,16 @@ 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;
@@ -22,12 +22,12 @@ list<gsl::vector_int> LocalPathfinder::neighbors(const gsl::vector_int& v, const
}
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;
@@ -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) {
@@ -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) {
@@ -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) { }
@@ -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(); }
@@ -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);
View
6 local_explorer.h
@@ -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 {
View
4 pathfinder.cpp
@@ -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;
@@ -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))); }
-}
+}
View
2  pathfinder.h
@@ -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);
Please sign in to comment.
Something went wrong with that request. Please try again.