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.

Sign up for GitHub

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
Open

Global planning using A* algorithm with GradientPath method #962

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
e8ef486
Use A* algorithm by default
MikeDegany Jan 26, 2020
b4a505b
Euclidean instead of Manhattan
MikeDegany Jan 26, 2020
785ae1f
Goal adjacency tolerance increased
MikeDegany Jan 26, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion global_planner/src/astar.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 = 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?


queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_));
std::push_heap(queue_.begin(), queue_.end(), greater1());
Expand Down
3 changes: 2 additions & 1 deletion global_planner/src/gradient_path.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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!

current.first = start_x;
current.second = start_y;
path.push_back(current);
Expand Down
2 changes: 1 addition & 1 deletion global_planner/src/planner_core.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.

if (use_dijkstra)
{
DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);
Expand Down