teb local planner optimization jumps to goal ignoring obstacles

asked 2019-09-30 09:12:06 -0600

robotjohan
15 ●2 ●3 ●5

I am doing a project with a differential drive robot where I'ver recently started using teb local planner which has worked great except for a single issue.

When giving the robot a nav goal the robot starts out following the global plan more or less, which makes sense since teb optimizes the time and the goal is not inside the local costmap. However, while the goal is still not inside the local costmap the local planner suddenly plans all the way to the goal, in some cases straight through an obstacle. As the obstacle approaches and enters the local costmap teb replans and everything is fine, although jerky. While this works ok for the obstacles I have so far I wonder what will happen if the local planner does this with a horseshoe type obstacle which doesn't fit in the local costmap. Also the jerkyness is a bit of a problem.

To deal with this I tried to make the planner follow the global plan as through the tutorial on http://wiki.ros.org/teb_local_planner...

Local plan goes through obstacle which is outside of the local costmap.

I am using Ubuntu 18.04, ROS melodic and I compiled Teb local planner from source.

edit retag flag offensive close merge delete

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2020-01-09 05:03:47 -0600

dario
46 ●3 ●2

I had a similar issue and the problem was that I was providing a path with few via-points (they were far away), and the TEB algorithm seemed to look ahead a few points, thus finding the goal one and going for it. My solution was to provide intermediate via-points along the way (i.e. a straight line would have many intermediate points) and that solved the issue.

Separately, check the max_global_plan_lookahead_dist and give it a small value (say 0.5 or 1) to make sure it does not look many meters ahead, thus reaching for the goal position instead of your via-points.

edit flag offensive delete link

Comments

Thank you, this seems to help.

robotjohan ( 2020-01-28 05:37:23 -0600 )edit

add a comment

answered 2021-07-06 03:26:43 -0600

twdragon
28 ●6 ●10 ●14

For teb_local_planner built from the source, I discovered the same issue but without a possibility to provide intermediate waypoints. Exploring the code, I found the function bool TebLocalPlannerROS::transformGlobalPlan where the switching behaviour is implemented. There is break statement in file src/teb_local_planner_ros.cpp, line 717:

if (new_sq_dist > sq_dist_threshold)
        break;  // force stop if we have reached the costmap border

that blocks the planner from switching to the next waypoint if it is located outside the local costmap. Instead of this, the goal injection turns on, so the planner snaps itself to the goal point and then it starts to hang up.

It is up to you how to avoid this issue on the sparse global plan. In my case, I replaced this break statement to bloat the sq_dist_threshold value for the single given waypoint once it is out the border of the local costmap.

edit flag offensive delete link

add a comment

teb local planner optimization jumps to goal ignoring obstacles

2 Answers

Comments

Question Tools

Stats

Related questions

teb local planner optimization jumps to goal ignoring obstacles edit

2 Answers

Comments

Question Tools

Stats

Related questions

teb local planner optimization jumps to goal ignoring obstacles