From e8ef486dedfb753ec3cdbfbb3ad908cb120f124d Mon Sep 17 00:00:00 2001 From: Mohammad Date: Sun, 26 Jan 2020 06:09:58 +0330 Subject: [PATCH 1/3] Use A* algorithm by default use-_dijkstra parameter changed to false --- global_planner/src/planner_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/global_planner/src/planner_core.cpp b/global_planner/src/planner_core.cpp index c8d4e568e8..fe16574f20 100644 --- a/global_planner/src/planner_core.cpp +++ b/global_planner/src/planner_core.cpp @@ -113,7 +113,7 @@ void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, p_calc_ = new PotentialCalculator(cx, cy); bool use_dijkstra; - private_nh.param("use_dijkstra", use_dijkstra, true); + private_nh.param("use_dijkstra", use_dijkstra, false); if (use_dijkstra) { DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy); From b4a505b8d29de7252aaa3a768cdba5f2a82035b7 Mon Sep 17 00:00:00 2001 From: Mohammad Date: Sun, 26 Jan 2020 06:15:44 +0330 Subject: [PATCH 2/3] Euclidean instead of Manhattan Calculate Euclidean distance instead of Manhattan distance in A* algorithm --- global_planner/src/astar.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/global_planner/src/astar.cpp b/global_planner/src/astar.cpp index 696756f205..c3667ccaa5 100644 --- a/global_planner/src/astar.cpp +++ b/global_planner/src/astar.cpp @@ -89,7 +89,9 @@ void AStarExpansion::add(unsigned char* costs, float* potential, float prev_pote potential[next_i] = p_calc_->calculatePotential(potential, costs[next_i] + neutral_cost_, next_i, prev_potential); int x = next_i % nx_, y = next_i / nx_; - float distance = abs(end_x - x) + abs(end_y - y); + + //float distance = abs(end_x - x) + abs(end_y - y); + float distance = sqrt((end_x - x)*(end_x - x) + (end_y - y)*(end_y - y)); //Euclidean Distance queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_)); std::push_heap(queue_.begin(), queue_.end(), greater1()); From 785ae1f29b8712a80cf8b3270208bfb10c31f528 Mon Sep 17 00:00:00 2001 From: Mohammad Date: Sun, 26 Jan 2020 06:20:30 +0330 Subject: [PATCH 3/3] Goal adjacency tolerance increased Goal adjacency tolerance increased from 0.5 to 1 to work while using A* algorithm and GradientPath method without error. --- global_planner/src/gradient_path.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/global_planner/src/gradient_path.cpp b/global_planner/src/gradient_path.cpp index 009febecfb..b65718fb6f 100644 --- a/global_planner/src/gradient_path.cpp +++ b/global_planner/src/gradient_path.cpp @@ -81,7 +81,8 @@ bool GradientPath::getPath(float* potential, double start_x, double start_y, dou // check if near goal double nx = stc % xs_ + dx, ny = stc / xs_ + dy; - if (fabs(nx - start_x) < .5 && fabs(ny - start_y) < .5) { + //if (fabs(nx - start_x) < .5 && fabs(ny - start_y) < .5) { + if (fabs(nx - start_x) < 1 && fabs(ny - start_y) < 1) { //To be able to work with A* algorithm and GradientPath method current.first = start_x; current.second = start_y; path.push_back(current);