-
Notifications
You must be signed in to change notification settings - Fork 1.8k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Global planning using A* algorithm with GradientPath method #962
base: melodic-devel
Are you sure you want to change the base?
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Alright.
@@ -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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
//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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I always had this error as well, this fixed it for me, thanks!
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