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

added default tolerance and fixed behavior when a goal is obstructed #1040

Open
wants to merge 1 commit into
base: melodic-devel
Choose a base branch
from

Conversation

jungladicitta
Copy link

@jungladicitta jungladicitta commented Oct 16, 2020

As I remember, I had a problem when I was running navigation and explore_lite:
explore_lite sends the goal -> as robot moves closer to goal explore_lite calculates and sends next goal, robot's velocity is not zero -> this next goal is obstructed, somewhere in the wall -> plan is not published, velocity is not zero -> robot moves with old velocity and hits the wall.

To fix this behavior I tried to use tolerance for global_planner and found out that it is not realized, and some similar issues was submitted.
This fix simply does:

  1. calculates 8 points on tolerance distance around the origin goal

    auto goals = findToleratedPoints(wx, wy, tolerance, 8);

    this vector also contains origin goal.

  2. sorts them by distance to current robot position

  3. iterates them:

    for (auto goal : goals) {
           //bool found_legal = calculatePlan(...)
           //if (found_legal) {
           //        publishPlan(...);
           //        break;
           //}
           //else {
           //   ROS_ERROR("Failed to get a plan.");
           //   publishVelocity(0.0);
           //}
    }

@doisyg
Copy link
Contributor

doisyg commented Oct 16, 2020

Thanks for this PR. I ended up re-implementing it targeting the last noetic-devel branch, #1041
Also I am not sure this PR was enabling a replanning to the closest valid point from the origin goal

@jungladicitta
Copy link
Author

jungladicitta commented Oct 16, 2020

@doisyg sorry for not providing description of this PR at first.
Now I have added it. I hope it helps

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants