Revision history [back]

So your problem is that the local trajectory planner/controller gets confused about which portion of the path it should follow when the path segments are too close to each other and when they intersect.

I think the easier solution is to first generate a path in one direction (say path A) and execute this navigation. And generate and execute the next path (path B) only after path A has been completed.