search

rst-tu-dortmund / teb_local_planner

An optimal trajectory planner considering distinctive topologies for mobile robots based on Timed-Elastic-Bands (ROS Package)
http://wiki.ros.org/teb_local_planner
BSD 3-Clause "New" or "Revised" License
1.06k stars 550 forks source link

Fixed a bug on isGoalReached method #290

Open LucasNolasco opened 3 years ago

LucasNolasco commented 3 years ago

There was a bug on goal_reached_ flag where the robot wouldn't realize that a goal is reached. It was caused by a new global plan arriving before the move_base check if the goal is reached.

How the bug happens: The computeVelocityCommands method set the flag to true, then a new plan arrives and the setPlan method is called. Then, on the setPlan, the flag is set back to false before it was actually checked. So when the isGoalReached method is called, it keeps returning false.

Proposed Solution: The solution proposed here is to check the goal using the global plan, so it doesn't depend on the goal_reached_ flag anymore and even with a new plan be capable to realize that the goal is reached.

amakarow commented 3 years ago

Sounds good, hope to get some time to test it.

RohanDwivedi commented 2 years ago

Is there a particular reason why this is not merged yet? I have ran into the same issue.

VicenteQueiroz commented 9 months ago

I only had the early goal reach problem when I had planner_frequency > 0 With this solution, I would get a continuous error saying I had an empty global plan. My workaround was to, instead of clearing the global plan, I would return false, and the next iteration would have the correct goal.

if (isGoalReached())
{
  return false; // global_plan_.clear();
}
LucasNolasco commented 9 months ago

I haven't returned to this issue in a long time, but I'd be willing to try to replicate the problem mentioned by @VicenteQueiroz if there's still any interest in merging this PR