Revision history [back]

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.