Global planning using A* algorithm with GradientPath method #962
Conversation
use-_dijkstra parameter changed to false
Calculate Euclidean distance instead of Manhattan distance in A* algorithm
Goal adjacency tolerance increased from 0.5 to 1 to work while using A* algorithm and GradientPath method without error.
| @@ -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); | |||
There was a problem hiding this comment.
Generally, we try to avoid changing any of the default parameter values so you don't create unexpected behavior on someone else's system. For your use case, this could just be reconfigured at launch, right?
| @@ -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); | |||
There was a problem hiding this comment.
| //float distance = abs(end_x - x) + abs(end_y - y); |
Please just remove code instead of commenting it out.
There was a problem hiding this comment.
Sorry I'm a little new to Git and things. I would add a parameter to choose which metric to use.
| 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 |
There was a problem hiding this comment.
Can you add a parameter that changes which distance metric is used?
| @@ -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 | |||
There was a problem hiding this comment.
Can you give an example of where the combination A*/Gradient was NOT finding a path that came within the distance before?
There was a problem hiding this comment.
When I run "teb_local_planner_tutorials" demo launch file which uses a hierarchical approach to plan a trajectory using A*/Gradient for global planning, I get these errors:
[ERROR]: NO PATH!
[ERROR]: Failed to get a plan from potential when a legal potential was found. This shouldn't happen.
There was a problem hiding this comment.
I always had this error as well, this fixed it for me, thanks!
DLu
changed the title
There were some errors while using A* algorithm and the GradientPath method together. I made some minor changes to ignore them all. And also Manhattan distance heuristic changed to Euclidean distance. It could be useful for some future developers who want to plan a global path using both methods together. Thanks