ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

teb local planner optimization jumps to goal ignoring obstacles

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

robotjohan gravatar image

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

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

2 Answers

Sort by ยป oldest newest most voted

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

dario gravatar image

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 more


Thank you, this seems to help.

robotjohan gravatar image robotjohan  ( 2020-01-28 05:37:23 -0500 )edit

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

twdragon gravatar image

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 more

Question Tools

1 follower


Asked: 2019-09-30 09:12:06 -0500

Seen: 2,238 times

Last updated: Jul 06 '21