Skip to content
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

Open
wants to merge 3 commits into
base: melodic-devel
Choose a base branch
from

Conversation

MikeDegany
Copy link

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

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.
@mikeferguson mikeferguson requested a review from DLu March 3, 2020 17:41
@@ -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);
Copy link
Member

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?

Copy link
Author

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);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
//float distance = abs(end_x - x) + abs(end_y - y);

Please just remove code instead of commenting it out.

Copy link
Author

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
Copy link
Member

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
Copy link
Member

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?

Copy link
Author

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.

Copy link

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!

@DLu DLu changed the title Gloabal planning using A* algorithm with GradientPath method Global planning using A* algorithm with GradientPath method Mar 11, 2020
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants